基于改進Huber-M的組合導航魯棒濾波算法

王進達,魯 浩,程海彬,李群生

(中航工業(yè)空空導彈研究院， 河南 洛陽 471009)

提高SINS/GNSS緊組合導航系統(tǒng)的魯棒性是當前導航領(lǐng)域的研究熱點[1]。在緊組合的彈載應(yīng)用中，大過載、高機動以及衛(wèi)星缺失等復雜條件經(jīng)常出現(xiàn)，這使得緊組合必須具備很好的魯棒特性和狀態(tài)跟蹤特性。

目前提高組合導航濾波器魯棒性的方法主要集中在線性/非線性濾波算法框架內(nèi)進行時間更新或量測更新的改進。如H∞濾波器[2]、自適應(yīng)濾波器[3-4]和抗差Kalman濾波器[5]，其中H∞濾波器是基于放寬噪聲容限來對未知噪聲進行范圍估計，自適應(yīng)濾波器是直接將未知噪聲統(tǒng)計量作為狀態(tài)量進行估計的，抗差Kalman是在傳統(tǒng)Kalman基礎(chǔ)上對未知噪聲方差矩陣進行等效替換。三者是魯棒線性濾波框架的代表。非線性濾波框架內(nèi)的魯棒改進更是廣泛，基于貝葉斯理論基礎(chǔ)的容積卡爾曼濾波、粒子濾波、擴展卡爾曼濾波以及無跡卡爾曼濾波等[6]，其中容積卡爾曼算法(Cubature Kalman Filters，CKF)是較新的非線性濾波算法。

基于魯棒濾波器的改進，目前應(yīng)用較多的是在CKF框架內(nèi)引入強跟蹤機制(Strong Tracking Filters, STF)，并在此基礎(chǔ)上進行多種簡化[7-10]。而傳統(tǒng)STF的關(guān)鍵是如何準確地確定漸消因子，并且漸消因子的引入條件具有一定的隨機性，導致濾波結(jié)果不夠穩(wěn)定，出現(xiàn)誤判概率過高的現(xiàn)象[11]。而引入Huber-M估計[12]雖然能夠解決這個問題，但是目前大多數(shù)基于Huber-M估計的魯棒濾波算法應(yīng)用在目標跟蹤領(lǐng)域中[13-15]，在導航領(lǐng)域應(yīng)用較少。

GNSS/SINS緊組合導航系統(tǒng)在高動態(tài)條件下其非線性特性表現(xiàn)明顯，此時采用線性框架下的魯棒濾波器已不再適用。因此，本文提出一種基于改進Huber-M估計的CKF導航算法。對Huber-M估計進行改進，應(yīng)用于非線性的導航系統(tǒng)觀測方程，不進行線性近似，保持了CKF在方差傳遞過程中原有的精度；其次，并不直接通過Huber條件下的最小二乘迭代得到狀態(tài)估計，而是利用Huber估計出新的量測噪聲方差矩陣對一步預測協(xié)方差矩陣進行二次正交化處理，既能提高系統(tǒng)魯棒性，又可避免噪聲方差矩陣的變化可能導致的模型不確定性；最后引入強跟蹤機制，采用模糊推理方式計算弱化因子，保證量測噪聲統(tǒng)計特性發(fā)生突變或狀態(tài)突變時，系統(tǒng)濾波器仍能夠?qū)Ш较到y(tǒng)姿態(tài)誤差實現(xiàn)良好跟蹤。

1 基于改進Huber-M估計的容積卡爾曼濾波

1.1 Huber-M估計與強跟蹤機制的統(tǒng)一

關(guān)于狀態(tài)量xk和m維觀測量yk建立非線性系統(tǒng)模型如下：

(1)

式(1)中，f(·)和h(·)分別為非線性狀態(tài)函數(shù)和量測函數(shù)；vk和wk分別為狀態(tài)噪聲和量測噪聲，其方差矩陣分別為Qk和Rk。

廣義M估計的Huber代價函數(shù)的定義如下：

(2)

式(2)中，ρ(υi)為關(guān)于υi的任意函數(shù)；υi為觀測殘差。由此可以得到式(2)的解為：

(3)

式(3)中，φ(υi)=(ρ(υi))′，影響函數(shù)為ψ(υi)=φ(υi)/υi。

構(gòu)造Huber條件下的回歸模型，假設(shè)狀態(tài)一步預測殘差為δxk+1/k，于是有

(4)

根據(jù)Huber原理和式(2)定義新的殘差及代價函數(shù)，如式(5)、式(6)所示：

(5)

(6)

(7)

從式(7)可以看出，Huber-M估計的關(guān)鍵是構(gòu)造偽觀測值或重新構(gòu)造觀測噪聲方差矩陣，將原非線性觀測值截斷，從而定義修正后的殘差。這樣做的好處是能夠自適應(yīng)地對不同大小的殘差配以權(quán)重，減小量測異常的影響。引入正交原理來使得濾波器輸出的殘差序列處處正交，能夠進一步減小觀測噪聲方差矩陣的變化對狀態(tài)方程的影響。

在SINS/GNSS緊組合數(shù)學模型中，由于量測方程的非線性特性，采用線性濾波框架下的強跟蹤機制會造成系統(tǒng)模型失配，導致濾波迭代的不穩(wěn)定，因此需要避免對觀測方程進行線性化。強跟蹤算法中漸消因子的計算如下：

λk=max(1,λ0)

(8)

(9)

(10)

(11)

(12)

(13)

1.2 弱化因子的求解

在式(10)、式(11)中β′>1為弱化因子，取值通常靠經(jīng)驗選取，其作用是使狀態(tài)估計值更加平滑，但取值過大，會明顯增加量測噪聲矩陣對漸消因子選取的影響，人為地調(diào)節(jié)弱化因子不僅降低了對動態(tài)條件下狀態(tài)平滑的作用，還增加了濾波發(fā)散的風險。對此，本文采用模糊邏輯對弱化因子的取值進行改進，在強跟蹤原理的基礎(chǔ)上，建立模糊規(guī)則，從而避免人為因素的影響。

(14)

(15)

式(14)中，υk,i為新息第i個分量。輸入輸出采用平滑度更好的高斯型隸屬函數(shù)，如圖1所示。

圖1 輸入輸出的高斯型隸屬函數(shù)

設(shè)定該模糊系統(tǒng)的模糊子集合為{A,B,C}={Z(0),S(小),L(大)}，則根據(jù)T-S模型可制定模糊規(guī)則如表1所示。

表1 模糊規(guī)則

根據(jù)表1的模糊規(guī)則，可采用重心法進行解模糊求出弱化因子。

1.3 改進的Huber-M STCKF算法步驟

通過1.1節(jié)～1.2節(jié)的分析和計算，可以得到基于改進Huber-M估計的強跟蹤容積卡爾曼濾波算法步驟如下。

步驟1：時間更新，計算容積卡爾曼濾波的容積點[17]；

步驟3：根據(jù)模糊原理得出弱化因子，求漸消因子λk+1；

步驟7：計算濾波增益、狀態(tài)估計值與預測方差。

2 基于偽距-偽距率差的緊組合模型

設(shè)定組合導航系統(tǒng)的狀態(tài)方程如下：

(16)

式(16)中，X=[φxφyφzδVEδVNδVUδLδλδhΔtdΔtv]T，各狀態(tài)量依次為失準角、速度誤差、位置誤差、接收機鐘差和鐘漂。系統(tǒng)狀態(tài)矩陣的定義如下：

(17)

式(17)中，F(xiàn)ij(i,j=1,2,3)的具體定義可參見文獻[16]，F(xiàn)G為GNSS誤差方程的狀態(tài)驅(qū)動矩陣。

由于接收機鐘差的時間導數(shù)為鐘漂項，那么采用鐘差項δtd和鐘漂項δtv作為狀態(tài)量的GNSS誤差方程為：

(18)

式(18)中，Tc為相關(guān)時間系數(shù)；ωd、ωv分別為噪聲項。因此有

量測模型為：

下面推導包括偽距和偽距率差的非線性觀測方程，假設(shè)慣導給出的ECEF系下的導航系統(tǒng)位置為(xI,yI,zI)，衛(wèi)星定位組件共跟蹤k顆衛(wèi)星，其中第i顆衛(wèi)星在ECEF系下的位置是(xSi,ySi,zSi)，則相應(yīng)于慣導給出的導航系統(tǒng)位置到第i顆衛(wèi)星的偽距觀測差為：

(19)

式(19)中，(δx,δy,δz)為慣導給出的導航系統(tǒng)在地球坐標系中的位置誤差。其與狀態(tài)量中的位置參數(shù)之間的關(guān)系為：

(20)

那么式(20)減去第i顆衛(wèi)星觀測到的偽距值，可以得到關(guān)于偽距差的非線性模型為:

δρi=ρIi-ρGi=

(21)

式(21)中，ρ為衛(wèi)星與接收機之間的理想偽距；w1i為觀測噪聲。

式(20)與式(21)組成GNSS/SINS緊組合導航系統(tǒng)的雙差非線性量測模型。

3 仿真驗證

本文導航坐標系采用東-北-天當?shù)氐乩碜鴺讼怠＜僭O(shè)導航系統(tǒng)初始位置為北緯34.2°，東經(jīng)108.9°，高度380 m；初始速度為340 m/s。傳感器采樣周期為0.01 s，濾波周期0.1 s；陀螺常值漂移為3°/h，加速度常值誤差為0.1 mg，GNSS隨機偽距率誤差為0.05 m/s，進行165 s的仿真時間，飛行軌跡包括加速、爬升、轉(zhuǎn)彎等機動過程，如圖2所示。

無量測異常條件下，常用的濾波算法能夠保持較好的導航精度，因此本文只考慮有量測異常情況下所提出算法的導航精度和魯棒特性。選用傳統(tǒng)KF(Kalman Filter)算法、ACKF(Adaptive Cubature Kalman Filter)算法和本文提出的Huber-STCKF三種方法的導航結(jié)果進行對比。設(shè)置量測異常，在t=80～120 s設(shè)置量測噪聲方差為初始狀態(tài)的1.2倍，并將其設(shè)置為狀態(tài)突變。

圖2 導航系統(tǒng)運動軌跡

考慮三種方法的導航精度，其三種方法的姿態(tài)誤差角與速度誤差分別如圖3、圖4所示。

圖3 量測異常條件下3種方法的姿態(tài)誤差角對比

圖4 量測異常條件下3種方法的速度誤差對比

從圖3可以看出，當出現(xiàn)量測異常時，3種方法x和y向失準角估計值在量測異常后能夠保持穩(wěn)定，而z軸失準角除本文方法外的兩種方法均出現(xiàn)了不同程度的發(fā)散現(xiàn)象，濾波器此時無法保證穩(wěn)態(tài)精度。導致這種現(xiàn)象的原因是前兩種方法對量測異常過于敏感，導致在濾波估計過程中增益矩陣跟隨異常值發(fā)生突變，使得濾波器對狀態(tài)量無法保持收斂，而本文提出的方法因為強跟蹤機制的引入使得系統(tǒng)同時能夠保證濾波新息的正交化，也就是說能夠最大限度的提取異常量測量中的有用信息并進行濾波更新，具有較高的魯棒性能。

同樣，在圖4的速度誤差估計也能夠看出，東向和天向的速度誤差相差不大，而北向速度誤差則在量測異常時出現(xiàn)不同程度的發(fā)散現(xiàn)象。說明速度分量在系統(tǒng)模型中三種方法的非線性度相似，而北向速度誤差在量測異常時導致模型非線性特性增強，導致AKF和KF方法均出現(xiàn)精度下降現(xiàn)象，而本文的Huber-M STCKF則能夠較好地保持導航精度。

對該條件進行50次蒙特卡羅仿真，對比三種方法的失準角和速度誤差大小，如表2所示。可以明顯看出，本文提出的魯棒估計算法導航精度均高于ACKF和KF算法。

表2 本文方法與其他方法的估計誤差比較

總的來說，量測異常的出現(xiàn)導致三種算法對姿態(tài)誤差的估計精度都會有所下降，但本文提出的方法能夠較好地保持估計精度。這是由于在量測異常發(fā)生時，由Huber-M估計出的噪聲方差矩陣發(fā)生同步變化，并且強跟蹤機制的引入能夠?qū)α繙y信息突變保持較高的魯棒性能。

4 結(jié)論

基于改進Huber-M估計的容積卡爾曼濾波導航算法(Huber-STCKF)通過將Huber-M估計應(yīng)用于非線性的導航系統(tǒng)觀測方程，避免了非線性模型的線性化；引入強跟蹤機制，利用新的量測噪聲方差矩陣對一步預測協(xié)方差矩陣進行正交化處理，采用模糊推理方式計算弱化因子，提高了系統(tǒng)的魯棒性。通過仿真驗證，與傳統(tǒng)卡爾曼濾波算法(KF)和自適應(yīng)容積卡爾曼濾波算法(CKF)進行對比，新的算法在量測異常條件下具有更好的魯棒性、更高估計精度，能夠?qū)Ш较到y(tǒng)姿態(tài)誤差實現(xiàn)良好的跟蹤性。