基于改進(jìn)Huber-M的組合導(dǎo)航魯棒濾波算法

王進(jìn)達(dá),魯 浩,程海彬,李群生

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

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

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

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

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

1 基于改進(jìn)Huber-M估計(jì)的容積卡爾曼濾波

1.1 Huber-M估計(jì)與強(qiáng)跟蹤機(jī)制的統(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估計(jì)的Huber代價(jià)函數(shù)的定義如下：

(2)

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

(3)

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

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

(4)

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

(5)

(6)

(7)

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

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

λk=max(1,λ0)

(8)

(9)

(10)

(11)

(12)

(13)

1.2 弱化因子的求解

在式(10)、式(11)中β′>1為弱化因子，取值通?？拷?jīng)驗(yàn)選取，其作用是使?fàn)顟B(tài)估計(jì)值更加平滑，但取值過大，會明顯增加量測噪聲矩陣對漸消因子選取的影響，人為地調(diào)節(jié)弱化因子不僅降低了對動態(tài)條件下狀態(tài)平滑的作用，還增加了濾波發(fā)散的風(fēng)險(xiǎn)。對此，本文采用模糊邏輯對弱化因子的取值進(jìn)行改進(jìn)，在強(qiáng)跟蹤原理的基礎(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ī)則，可采用重心法進(jìn)行解模糊求出弱化因子。

1.3 改進(jìn)的Huber-M STCKF算法步驟

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

步驟1：時間更新，計(jì)算容積卡爾曼濾波的容積點(diǎn)[17]；

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

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

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

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

(16)

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

(17)

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

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

(18)

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

量測模型為：

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

(19)

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

(20)

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

δρi=ρIi-ρGi=

(21)

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

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

3 仿真驗(yàn)證

本文導(dǎo)航坐標(biāo)系采用東-北-天當(dāng)?shù)氐乩碜鴺?biāo)系。假設(shè)導(dǎo)航系統(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隨機(jī)偽距率誤差為0.05 m/s，進(jìn)行165 s的仿真時間，飛行軌跡包括加速、爬升、轉(zhuǎn)彎等機(jī)動過程，如圖2所示。

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

圖2 導(dǎo)航系統(tǒng)運(yùn)動軌跡

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

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

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

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

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

對該條件進(jìn)行50次蒙特卡羅仿真，對比三種方法的失準(zhǔn)角和速度誤差大小，如表2所示?？梢悦黠@看出，本文提出的魯棒估計(jì)算法導(dǎo)航精度均高于ACKF和KF算法。

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

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

4 結(jié)論

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