彈箭定姿系統(tǒng)的改進卡爾曼濾波算法研究

李 茜，賈旭東，馬寅峰，崔 敏，王 高，楊 磊

(1.中北大學電子測試技術國家重點實驗室，太原 030051;2.山西省郵電建設工程有限公司，太原 030051;3.上海索錄工程有限公司，上海 200235)

0 引言

在最近幾次海灣戰(zhàn)爭和科索沃局部戰(zhàn)爭中，精密制導武器憑借其精確打擊能力控制和主宰了整個戰(zhàn)場［1］。所有的武器包括從槍炮到導彈及衛(wèi)星的攔截彈都利用制導技術來提高性能和精度，姿態(tài)測試技術是其中的核心［2］。隨著精密制導武器向著遠程化、智能化和低成本發(fā)展，人們對小型彈箭姿態(tài)測試系統(tǒng)的精度、可靠性和抗高過載性等的要求不斷提高。單一傳感器已遠不能滿足要求，測姿技術已向多傳感器組合方向發(fā)展，通過信息融合增強姿態(tài)測試系統(tǒng)對彈箭復雜環(huán)境的適應性［3］。

就單一的MEMS陀螺測量而言，使用改進的小波閾值方法可有效地抑制陀螺的隨機漂移［4］。文獻［5］中首次提出利用磁強計和陀螺姿態(tài)估計擴展卡爾曼濾波器設計。將陀螺和磁強計組合使用，利用卡爾曼濾波進行數(shù)據(jù)融合，可以從精度和穩(wěn)定性兩方面提高系統(tǒng)性能。文獻［6］提出自適應卡爾曼濾波解決慣性/地磁組合導航系統(tǒng)的姿態(tài)解算，通過實時估計和修正系統(tǒng)噪聲以及觀測噪聲的統(tǒng)計特性達到降低模型誤差，抑制濾波發(fā)散的目的。粒子濾波和H∞濾波等方法也逐漸應用于以陀螺測量為基礎的組合導航中［7－8］。

傳統(tǒng)卡爾曼濾波算法噪聲模型只能是高斯白噪聲，并不適用陀螺/磁強計這一非線性系統(tǒng)，十分容易造成數(shù)據(jù)發(fā)散［9］。本文利用小波分析方法在對信號進行3層分解，重構出陀螺與磁強計的測量誤差信號，誤差信號與卡爾曼濾波估計值作為BP神經(jīng)網(wǎng)絡的訓練樣本對，建立誤差模型，修正卡爾曼濾波的姿態(tài)信息。該方法具有容錯性強、測量精度較高的特點。

1 總體模型和傳統(tǒng)卡爾曼定姿算法

1.1 總體模型

本文采用的是磁強計與IMU組合測量。用較實用的工程方法—四元數(shù)法表示陀螺的姿態(tài)運動學方程。卡爾曼濾波器的狀態(tài)轉移矩陣利用基于四元數(shù)的等效旋轉矢量來計算姿態(tài)。記地面坐標系oξηζ是系統(tǒng)參考坐標系，假設地磁強度為H，在地理坐標系三軸上的分量分別為Hξ、Hη、Hζ，磁阻傳感器磁測探頭沿載體坐標系ObXbYbZb三軸方向安裝，y為彈軸地磁強度在載體坐標系三軸上的分量，分別為 Hbξ、Hbη、Hbζ。四元數(shù)的四階近似算法為

根據(jù)式(1)求出各個時刻四元數(shù)的值后，可以利用狀態(tài)轉移矩陣中的有關元素解算出載體的姿態(tài)角，如式(2)所示。

要求解基于陀螺傳感器的彈道平面內的高自旋轉彈丸的姿態(tài)角，則可以假設偏航角ψ保持不變，cosψ=1，sinψ=0，則磁強計的數(shù)學模型可化解為

實測時，方程左邊的值由測試系統(tǒng)實時給出，記任意瞬時的值為，因而包含γ、θ的方程組可以表示為

這是包含飛行體姿態(tài)角的數(shù)學模型，在每一瞬時求解方程組，即可求得此瞬時的姿態(tài)角。利用阻尼最小二乘法可以求出一組γ、θ的值，求出γ、θ值后，根據(jù)求解四元數(shù)的方法，求出一組 q0、q1、q2、q3的值，然后再結合陀螺傳感器解算出的q0、q1、q2、q3值，利用卡爾曼濾波對兩組值進行信息融合，使q0、q1、q2、q3的值更接近實際值。

1.2 卡爾曼濾波

由四元數(shù)的微分方程構造了離散化的狀態(tài)方程為

設被估計系統(tǒng)的狀態(tài)方程和觀測方程為

卡爾曼濾波器的時間更新方程為

卡爾曼濾波器的狀態(tài)更新方程為

式中:vk是均值為零、方差為Rk的白噪聲;wk是均值為零、方差為Qk的白噪聲;且vk和wk不相關［8］。若已經(jīng)得到第(k－1)時刻的最優(yōu)(k－1)融合估計及其方差P(k－1)，則融合第k時刻的量測z(k)后，可得到第k時刻的最優(yōu)融合估計(k)及其方差P(k)。選這個系統(tǒng)的狀態(tài)矢量為x= (q0q1q2q3)。

將磁阻傳感器解算的四元數(shù)值作為卡爾曼濾波的觀測量，陀螺解算的四元數(shù)值作為狀態(tài)變量。在卡爾曼濾波算法中，過程的狀態(tài)不隨時間變化，A=1;u=0;包含噪聲的觀測值是狀態(tài)變量的直接體現(xiàn)，所以H=1;同時，為了使數(shù)據(jù)更接近現(xiàn)實的狀態(tài)，我們可以在狀態(tài)變量中加入一個0.1 V的白噪聲。過程激勵噪聲方差 Q，這里取 Q=10－5。類似地，選擇 Pk－1的初值P0=1，幾乎任何P0≠0都會使濾波器最終收斂。由以上公式估計了q的值后，再用卡爾曼融合后的q值求出狀態(tài)轉移矩陣中的各個元素值，然后利用狀態(tài)轉移矩陣求解飛行體的姿態(tài)角。

2 陀螺/磁強計信號的多尺度小波分析

小波變換是空間和頻率的局部變換，能有效地從信號中提取信息。通過伸縮和平移等運算功能可對函數(shù)或信號進行多尺度的細化分析［10］。

選取適合的小波基函數(shù)能夠既保持重構信號的局部特性又實現(xiàn)信號的平滑特性。一般在導航多傳感器組合應用中，若f(t)的正交性較高，選擇高消失矩的小波函數(shù)，若f(t)的奇異點較多，可選擇短支撐集的小波函數(shù)［11］。

Mallat算法是一種快速二進小波變換算法，主要通過用不同分辨層上的投影函數(shù)的疊加來表示信號，這一分解重構算法是基于對各層分解系數(shù)之間關系的研究而構造出的結晶。在不同尺度下把信號分解成細節(jié)信號和逼近信號，分辨率j反映了頻帶的位置和帶寬。

Djf(x)是信號f(t)在Vj空間的正交投影，是分辨率為j時的細節(jié)部分，Aj是逼近信號。Aj－1f(x)是在分辨率為j－1時的近似信號，Dj屬于高頻部分，Aj屬于低頻部分。

Mallat算法分解信號過程見圖1。其中↓2表示1/2抽樣，即從到和，樣點數(shù)減少一半［12－13］。

圖1 小波分解的Mallat算法Fig.1 The Mallat algorithm of the wavelet decomposition

上述過程表示為

式中:hk和gk分別是低通濾波器H和高通濾波器G的沖擊響應序列和為前面所介紹的逼近信號和細節(jié)信號。Mallat算法的重構過程為分解的逆過程。給出重構公式［9］為

選擇db3雙正交小波基能夠很好滿足陀螺/磁強計組合測姿系統(tǒng)的實時性和快速性要求。信號在尺度3下分解如圖2所示。

圖2 陀螺/磁強計信息在尺度3下小波分解Fig.2 Decomposing the information of the gyro and the magnetometer in three dimensions

對陀螺和磁強計的信息分別在尺度3下進行分解，然后將每層的細節(jié)系數(shù)和近似部分的系數(shù)比較獲得每層的誤差信號，最后再把每層的誤差信號系數(shù)進行小波變換重構得到陀螺與磁強計的測量誤差。

3 數(shù)據(jù)融合模型

本文采用BP神經(jīng)網(wǎng)絡建立了誤差網(wǎng)絡模型，用于對卡爾曼濾波的修正。將誤差信號與卡爾曼濾波估計值作為訓練樣本對。

磁強計輸出有效時，利用求得的磁強計與陀螺的測量誤差和對應的由卡爾曼濾波器輸出的磁強計與陀螺估計值構成BP神經(jīng)網(wǎng)絡的訓練樣本對進行單隱層神經(jīng)網(wǎng)絡訓練。當磁強計失效時，利用訓練得到的BP神經(jīng)網(wǎng)絡模型預測磁強計與陀螺的測量誤差，最后將經(jīng)過處理的該測量誤差數(shù)據(jù)和經(jīng)卡爾曼濾波器得到的測量數(shù)據(jù)相加輸出更為精準的測量值。

在進行神經(jīng)網(wǎng)絡訓練前對樣本集，即陀螺/磁強計誤差信號和磁強計有效時卡爾曼濾波的最佳估計值進行歸一化處理，以提高速度和穩(wěn)定性。BP神經(jīng)網(wǎng)絡設計為單隱層結構，其網(wǎng)絡數(shù)學模型為

陀螺/磁強計誤差信號作為輸入層神經(jīng)元，卡爾曼濾波的最佳估計值為輸出層神經(jīng)元。對神經(jīng)網(wǎng)絡的訓練輸入/輸出樣本對進行預處理對預測的輸入/輸出樣本對進行后處理 p=pmin+

基于小波變換、卡爾曼濾波和BP神經(jīng)網(wǎng)絡預測模型的組合導航系統(tǒng)框圖如圖3所示。

圖3 BP神經(jīng)網(wǎng)絡預測模型Fig.3 Prediction model of BP neural network

4 仿真與結論

在進行神經(jīng)網(wǎng)絡訓練前對樣本集進行歸一化處理，以提高速度和穩(wěn)定性。其傳遞函數(shù)是雙正切S型函數(shù)，輸出層的傳遞函數(shù)是線性函數(shù)，隱層的神經(jīng)元個數(shù)選為15個。在經(jīng)過98次學習后完成訓練。

本文在三軸無磁轉臺上完成了模擬仿真實驗，采用上述算法處理數(shù)據(jù)后與轉臺實際輸出值進行對比。經(jīng)度 112°44';緯度 38°01';滾轉角速度 3.5 r/s。圖 4是解算三維姿態(tài)角圖，圖5中虛線是改進前的誤差曲線，實線是改進后的誤差曲線，三維姿態(tài)角的誤差均在2°～3°以內。

圖4 姿態(tài)角解算Fig.4 Calculation of the attitude angles

圖5 姿態(tài)角誤差曲線對比圖Fig.5 Error of the attitude angles

結果表明:10 s的測試時間內，俯仰角誤差在±1.5°，偏航角誤差為 ±2.2°，滾轉角誤差為 ±2.9°，3個角最大累計誤差均在3°以內，與傳統(tǒng)卡爾曼濾波算法比較，精度提高了2°。該算法解決了卡爾曼濾波由測量誤差帶來的發(fā)散失效問題，實現(xiàn)了卡爾曼估計值的修正，使得卡爾曼濾波適應了陀螺/磁強計這一非線性系統(tǒng)，達到了提高系統(tǒng)解算精度的目的。

本文提出的利用小波變換和BP神經(jīng)網(wǎng)絡建立誤差網(wǎng)絡預測模型，具有很好的應用前景。由于BP神經(jīng)網(wǎng)絡的不穩(wěn)定性，泛化能力有待提高，在優(yōu)化神經(jīng)網(wǎng)絡的基礎上，測量精度進一步提高，為今后的研究提出了新的挑戰(zhàn)。

［1］梁薇，張科.精確制導武器發(fā)展及其關鍵技術［J］.火力與指揮控制，2008，33(12):5-7.

［2］劉勇，沈毅，胡恒章，等.精確制導武器及其支持系統(tǒng)中的信息融合技術［J］.系統(tǒng)工程與電子技術，1999，21(4):42-48.

［3］RICH H，KELMENSON S，MENDELSONHN L.Geophysical navigation technologies and applications［C］//IEEE PLANS Position Location and Navigation Syinposium，Apirl 2004:618-624.

［4］杜繼永，黃國榮，程洪炳，等.基于改進小波閾值法處理MEMS陀螺信號噪聲［J］.電光與控制，2009，16(12):61-64.

［5］黃旭，王常虹，伊國興，等.利用磁強計及微機械加速度計和陀螺的姿態(tài)估計擴展卡爾曼濾波器［J］.中國慣性技術學報，2005，13(2):27-30.

［6］晏登洋，任建新，牛爾卓.慣性/地磁組合導航系統(tǒng)自適應卡爾曼濾波算法研究［J］.電光與控制，2007，14(6):74-77.

［7］姚智穎，劉冬，劉光斌.基于粒子濾波的INS/TAN組合導航［J］.電光與控制，2006，13(6):54-56.

［8］郭寧博，宋艷君，黃國榮.H∞濾波在GFSINS/GPS組合導航系統(tǒng)中的應用［J］.電光與控制，2009，16(9):63-66.

［9］DONOHO D L，JOHNSTONE I.Adapting to unknown smoothness via wavelet shrinking［J］.Journal of American Stat Assoc，1995(12):1200-1224.

［10］劉傳艷，朱斌，樊鍵.系統(tǒng)中陀螺信號的三種濾波方法［J］.電光與控制，2008，15(6):71-74.

［11］魏敏，魏武.基于小波分析與神經(jīng)網(wǎng)絡的組合導航的研究［J］.信息技術，2009(11):49-52.

［12］MALLAT S.A theory for muhiresolution signal decomposition:The wavelet representation［J］.IEEE Trans Part Ana1 Machine intel1，1989，11(7):674-693.

［13］MALLAT S.Multifrequency channel decompositions of Images and wavelet models［J］.IEEE Trans Aeoust Speech Si Processing，1989，37(12):2091-2100.