捷聯(lián)慣性基組合導(dǎo)航魯棒UKF方法

張夢(mèng)得，胡柏青，趙仁杰，田佳玉

(海軍工程大學(xué) 導(dǎo)航工程教研室，湖北 武漢 430033)

0 引 言

組合導(dǎo)航系統(tǒng)是將2 種或者2 種以上的非相似導(dǎo)航系統(tǒng)的輸出進(jìn)行綜合處理,獲得更高的導(dǎo)航精度與可靠性[1-2]。最常見的組合導(dǎo)航系統(tǒng)是以慣導(dǎo)為核心的慣性基組合導(dǎo)航系統(tǒng)[3-4]。捷聯(lián)慣導(dǎo)（Strapdown Inertial Navigation，SINS）具有用數(shù)字平臺(tái)代替實(shí)體平臺(tái)，結(jié)構(gòu)簡(jiǎn)單，體積小，重量輕等特點(diǎn)，成為研究的熱門[5]。然而SINS 卻存在導(dǎo)航精度隨時(shí)間發(fā)散的問(wèn)題，無(wú)法僅僅依靠自身校正誤差[6]。這就是SINS 需要與其他導(dǎo)航設(shè)備進(jìn)行組合導(dǎo)航的原因，常見的基于捷聯(lián)慣導(dǎo)的組合系統(tǒng)可以應(yīng)用在陸用導(dǎo)航（GPS、羅蘭C、里程計(jì)）、水下導(dǎo)航（多普勒計(jì)程儀、基線系統(tǒng)）、航空航天以及無(wú)人系統(tǒng)導(dǎo)航等領(lǐng)域[7-9]，然而，實(shí)現(xiàn)慣性基組合導(dǎo)航的關(guān)鍵技術(shù)就是組合濾波方法[10]。

在現(xiàn)代濾波技術(shù)中，1995 年由S.J.Julier 和J.K.Uhlmann 提出的無(wú)跡卡爾曼濾波算法（Unscented Kalman Filter，UKF）一直是研究的熱點(diǎn)，該算法通過(guò)UT 變換可以不忽略泰勒展開的高階項(xiàng)，從而在非線性系統(tǒng)中有較高的估計(jì)精度和穩(wěn)定性，受到了廣泛的關(guān)注[11]。在捷聯(lián)慣性基組合導(dǎo)航的實(shí)際應(yīng)用中，估計(jì)的精度與實(shí)時(shí)性都是需要考慮的問(wèn)題，UKF 不太大的計(jì)算量與較高的精度導(dǎo)致其成為了一種可行非線性的濾波方法，這也是其成為研究熱點(diǎn)的原因之一。

在捷聯(lián)慣性基組合導(dǎo)航實(shí)際應(yīng)用中，捷聯(lián)慣導(dǎo)自身比較穩(wěn)定，不易受到干擾，而環(huán)境情況復(fù)雜多變，使得量測(cè)量難以避免的會(huì)被污染、干擾，這會(huì)對(duì)UKF的精度造成較大的影響，甚至導(dǎo)致濾波器的發(fā)散，進(jìn)而影響到組合導(dǎo)航的效果。比如在城市中使用SINS/GPS 系統(tǒng)，可能存在衛(wèi)星受到信號(hào)遮擋、信號(hào)受到干擾的情況；在水下使用SINS/DVL 系統(tǒng)，可能存在水流速或者水底地形的變化導(dǎo)致的測(cè)速異常等問(wèn)題。如何提高濾波的抗干擾能力，提升組合導(dǎo)航系統(tǒng)的穩(wěn)定性是UKF 在實(shí)際應(yīng)用中必須要解決的問(wèn)題。傳統(tǒng)的魯棒方法是通過(guò)增加量測(cè)噪聲矩陣的權(quán)重以削弱野值對(duì)濾波的影響，但是這樣會(huì)犧牲濾波一定的估計(jì)精度[12]。文獻(xiàn)[13]對(duì)魯棒濾波在初始對(duì)準(zhǔn)動(dòng)基座中的應(yīng)用進(jìn)行了研究，文獻(xiàn)[14]對(duì)改進(jìn)的Huber 魯棒濾波進(jìn)行了研究，通過(guò)文獻(xiàn)[15]使用支持向量回歸的方法提高組合導(dǎo)航系統(tǒng)的魯棒性。

針對(duì)捷聯(lián)慣性基直接法組合導(dǎo)航系統(tǒng)實(shí)際使用中出現(xiàn)的量測(cè)量中出現(xiàn)野值的問(wèn)題，本文提出一種基于一類支持向量機(jī)輔助的適用于捷聯(lián)慣性基組合導(dǎo)航的魯棒UKF 算法（Support Vector Machine-Adaptive UKF，SVM-UKF）。本方法首先使用一類支持向量機(jī)（Support Vector Machine，SVM）建立滑動(dòng)窗，來(lái)辨別濾波中的新息是否為異常，對(duì)于正常新息不予處理，對(duì)于異常的新息采用指數(shù)加權(quán)的方法得到新的“正確”新息以替代野值新息，這樣不但可以修正量測(cè)值，還可以提高該情況下濾波的估計(jì)精度，提高系統(tǒng)的魯棒性，根據(jù)江試實(shí)驗(yàn)數(shù)據(jù)，對(duì)SINS/GPS 系統(tǒng)使用SVM-UKF 與常規(guī)UKF，RUKF 濾波組合導(dǎo)航的精度進(jìn)行比較。實(shí)驗(yàn)結(jié)果表明，在量測(cè)量有野值污染的情況下，SVM-UKF 具有更高的估計(jì)精度。

1 基于一類SVM 的野值識(shí)別與處理

對(duì)于量測(cè)量中野值的辨別，一類SVM 具有適用于小樣本，非線性等特點(diǎn)，只需要正常工作的小樣本數(shù)據(jù)就可以進(jìn)行模型的訓(xùn)練，非常適用于野值的辨識(shí)。一類SVM 本質(zhì)是通過(guò)在特征空間中尋找超平面對(duì)正常樣本與異常樣本進(jìn)行分離，如圖1 所示。

圖1 一類SVM 分類基本原理圖Fig.1 Graphical illustration of 1-class SVM

1.1 訓(xùn)練方法

一類SVM 的在線訓(xùn)練流程如下：

1）構(gòu)建新息樣本滑動(dòng)窗

選取n個(gè)連續(xù)的不含野值的新息數(shù)據(jù)組作為訓(xùn)練信息樣本滑動(dòng)窗X=[x1,x2,···,xn]T,式中x表示新息。規(guī)范化訓(xùn)練新息滑動(dòng)窗中每一個(gè)向量x。對(duì)規(guī)范化后的新息滑動(dòng)窗進(jìn)行主元分析得到目標(biāo)向量矩陣Y：

式中：PPC=[p1,p2,···,pn]∈RN×l表示主元矩陣；y為支持向量；l為特征空間的維數(shù)。

2）訓(xùn)練一類SVM 模型

根據(jù)新息樣本滑動(dòng)窗訓(xùn)練得到的目標(biāo)向量矩陣Y，訓(xùn)練一類SVM 模型為：

約束條件為：

式中：α=[α1,α2,···,αn]T表示優(yōu)化權(quán)值向量；Hij=K(yi,yj)，yi,yj∈Y，K(yi,yj)=e-‖xi-x j‖2/2σ2為核函數(shù)；v∈{0,1}是置信水平，用來(lái)表示允許訓(xùn)練新息樣本出現(xiàn)錯(cuò)誤被分類的程度。

3）計(jì)算偏移量

式中：b為偏移量；ns為y的個(gè)數(shù)。設(shè)定置信水平v的值，將新息樣本滑動(dòng)窗中的新息按從大到小的順序排列，若驗(yàn)證數(shù)據(jù)樣本的大小為n，則取第v×n個(gè)新息值為閾值Jn。

1.2 檢測(cè)方法與野值處理

1）計(jì)算目標(biāo)向量

將待檢測(cè)新息數(shù)據(jù)x′，加入到新息樣本滑動(dòng)窗X中得到新樣本，對(duì)x′進(jìn)行規(guī)范化，根據(jù)式（1）得到目標(biāo)向量：

2）計(jì)算距離檢測(cè)量

本文采用文獻(xiàn)[16]提出的使用一類SVM 模型的相反數(shù)作為檢測(cè)量的方法，計(jì)算距離檢測(cè)量為：

2）野值的識(shí)別與處理

1）新息的野值識(shí)別

若F(t)＜Jn，則新加入的新息x′為正常值；若F(t)＞Jn，則可以認(rèn)定新息x′為野值，證明相對(duì)應(yīng)的量測(cè)值為野值。

2）野值處理

傳統(tǒng)的處理方法大多為將野值剔除，如果有數(shù)量較多的野值，剔除的方法有可能造成濾波的估計(jì)精度下降甚至發(fā)散。本文采用指數(shù)加權(quán)的方法，通過(guò)對(duì)滑動(dòng)窗內(nèi)正常的新息進(jìn)行賦權(quán)，估計(jì)出野值新息處的“正確”新息，以替代野值新息。按照時(shí)間軸距離野值越近的新息權(quán)值越高，越遠(yuǎn)的新息權(quán)值越低，目的是使得越靠近野值的正常新息越能在對(duì)野值新息的估計(jì)中占有更大的比重。

權(quán)值系數(shù)應(yīng)滿足

權(quán)值系數(shù)因子為d=[(1-r)/(1-rn-2)]，其中r為遺忘因子，根據(jù)經(jīng)驗(yàn)通常取0.95～0.99。

這樣可以得到滑動(dòng)窗內(nèi)所有權(quán)值系數(shù)計(jì)算公式

通過(guò)權(quán)值系數(shù)可以估計(jì)出“正確”新息替代野值新息

這樣通過(guò)指數(shù)加權(quán)就可以消除量測(cè)值野值對(duì)濾波效果影響。與傳統(tǒng)的直接對(duì)量測(cè)值野值進(jìn)行剔除的方法相比，該方法不影響濾波的量測(cè)更新，避免了濾波由于缺少量測(cè)更新造成的估計(jì)精度急劇下降甚至發(fā)散的情況。野值識(shí)別與處理的示意圖如圖2 所示。

圖2 野值識(shí)別與處理方法示意圖Fig.2 Identification and processing of outliers

1.3 SVM-UKF 算法

式中，xk表示k時(shí)刻的狀態(tài)量，yk表示k時(shí)刻的量測(cè)量，wk-1～N(0,QK-1)為 系統(tǒng)噪聲，rk-1～N(0,RK-1)為量測(cè)噪聲，f(·)表 示捷聯(lián)慣導(dǎo)基本方程，h(·)為量測(cè)模型，對(duì)h(·) 進(jìn)行離散化處理可以得到H(·)。

SVM-UKF 算法流程如下：

1）時(shí)間更新

使用UT 變化構(gòu)造sigma 點(diǎn)χk-1(i)=sigma(,Pk-1)，

通過(guò)系統(tǒng)模型更新得到 χk/k-1(i)，

狀態(tài)預(yù)測(cè)均值與協(xié)方差為：

2）量測(cè)更新

計(jì)算新息并代入滑動(dòng)窗：

若新息為野值，使用指數(shù)加權(quán)計(jì)算替代值后代入量測(cè)更新：

若新息為正常值，直接進(jìn)入量測(cè)更新：

2 實(shí)驗(yàn)及分析

通過(guò)實(shí)驗(yàn)對(duì)所研究的SVM-UKF 方法估計(jì)效果進(jìn)行評(píng)價(jià)。實(shí)驗(yàn)采用位置作為量測(cè)量SINS/GPS 松組合方式。

實(shí)驗(yàn)采用激光陀螺捷聯(lián)慣導(dǎo)與GPS 組合實(shí)驗(yàn)數(shù)據(jù)來(lái)比較3 種算法的估計(jì)效果。該實(shí)驗(yàn)中采用激光捷聯(lián)慣導(dǎo)系統(tǒng)的輸出數(shù)據(jù)，激光陀螺漂移約為0.01°/h，加速度計(jì)零偏約為5×10-5g。慣導(dǎo)IMU 更新率為200 Hz，GPS 接收機(jī)更新率為1 Hz，水平速度誤差小于0.1 m/s，位置誤差小于2 m。

初始速度姿態(tài)位置由激光捷聯(lián)慣導(dǎo)提供。

實(shí)驗(yàn)過(guò)程共10 000 s，本文選取5 800～6 700 s 的900 s 數(shù)據(jù)進(jìn)行研究，該段中航行軌跡見圖3，五角星表示船的初始位置，圓點(diǎn)表示船的末端位置。在GPS輸出中每隔60 s 加入800 m 的位置誤差模擬GPS 出現(xiàn)野值，以未加入野值的SINS/GPS 組合導(dǎo)航系統(tǒng)的結(jié)果作為參考基準(zhǔn)，由于航海更關(guān)注位置與航向，因此給出組合導(dǎo)航的位置與航向估計(jì)效果如圖4～圖6 所示。

圖3 實(shí)驗(yàn)船行軌跡圖Fig.3 Trajectory map of river trial

圖4 3 種濾波方法的經(jīng)度估計(jì)誤差Fig.4 Longitude estimation error of three filters

圖5 3 種濾波方法的緯度估計(jì)誤差Fig.5 Latitude eestimation error of three filters

圖6 3 種濾波方法的航向估計(jì)誤差Fig.6 Yaw estimation error of three filters

圖4 與圖5 表示了位置的估計(jì)結(jié)果，可以看到UKF受到野值的干擾已經(jīng)發(fā)散，RUKF 與SVM-UKF 則表現(xiàn)出較好的估計(jì)效果。表1 給出了不同算法的位置、航向、東向、北向速度的RMSE 統(tǒng)計(jì)數(shù)據(jù)結(jié)果。由表1可以看出，RUKF 方法的位置誤差為4.968 m，航向誤差5.43′，北向速度估計(jì)誤差0.08/s，東向速度估計(jì)誤差0.07 m/s，SVM-UKF 方法的更好，位置誤差和航向誤差分別為2.474 m 和3.34′，北向、東向速度估計(jì)誤差為0.04 m/s 與0.06 m/s。

表1 位置估計(jì)誤差均方根誤差Tab.1 RMSE of position and yaw error

根據(jù)實(shí)驗(yàn)結(jié)果，可以得出結(jié)論。在位置估計(jì)中，雖然RUKF 有一定的魯棒性，但野值以及魯棒算法還是影響了濾波器的估計(jì)精度，SVM-UKF 的估計(jì)效果比RUKF 提高50.2%。分析航向的估計(jì)結(jié)果可以發(fā)現(xiàn)，RUKF 雖然具有一定的魯棒性，由于受到野值的干擾，SVM-UKF 的估計(jì)效比RUKF 提高38%。在速度估計(jì)方面，北向速度的估計(jì)精度SVM-UKF 比RUKF提高了50%，東向速度的估計(jì)精度SVM-UKF 比RUKF 提高了14.2%。

3 結(jié) 語(yǔ)

捷聯(lián)慣性基組合導(dǎo)航中，量測(cè)值存在野值的情況時(shí)有發(fā)生，這個(gè)問(wèn)題都會(huì)導(dǎo)致UKF 的估計(jì)精度下降。針對(duì)這些問(wèn)題，本文提出了一種基SVM-UKF 算法。通過(guò)使用一類支持向量機(jī)訓(xùn)練滑動(dòng)窗，按照新息是否為進(jìn)行分類，對(duì)于異常的新息采用指數(shù)加權(quán)的方法進(jìn)行估計(jì)，使用新的估計(jì)值替換野值。進(jìn)行了船載實(shí)驗(yàn)對(duì)SINS/GPS 系統(tǒng)進(jìn)行算法的性能檢驗(yàn)。通過(guò)與UKF，RUKF 相比較，證明了算法的有效性，得到結(jié)果：在實(shí)驗(yàn)中，SVM-UKF 對(duì)比于RUKF，位置估計(jì)精度、航向估計(jì)精度分別提高了50.2%與38%，北向、東向速度對(duì)比于RUKF 分別位置估計(jì)精度，航向估計(jì)精度提高了50%與14.2%。