基于自適應(yīng)分類容錯濾波的SINS/GNSS組合導(dǎo)航方法

郭慧娟，劉慧英，石 靜，孫景峰

（西北工業(yè)大學(xué) 自動化學(xué)院，西安 710072）

捷聯(lián)慣性導(dǎo)航系統(tǒng)（Strapdown Inertial Navigation System, SINS）作為一種自主式導(dǎo)航系統(tǒng)得到了廣泛應(yīng)用，通常將其與全球定位系統(tǒng)（Global Navigation Satellite System, GNSS）組合使用，通過濾波算法進(jìn)行數(shù)據(jù)融合，來保證慣性導(dǎo)航系統(tǒng)的長期精度[1-3]。而在實際應(yīng)用中系統(tǒng)的數(shù)學(xué)模型往往是非線性的，所以EKF算法被廣泛應(yīng)用于SINS/GNSS組合導(dǎo)航系統(tǒng)中估計、補償導(dǎo)航誤差。雖然將GNSS與SINS組合使用并通過 EKF對兩者輸出的信息進(jìn)行濾波可以克服慣性器件誤差不斷累積的缺陷，提高系統(tǒng)精度，但實際情況下GNSS量測信號極易受到周圍環(huán)境、多路徑效應(yīng)等多種因素的影響，如環(huán)境中的無線電和強磁場會使GNSS測量值產(chǎn)生較大的速度誤差和至少百米級位置誤差，因此對濾波產(chǎn)生不利影響，嚴(yán)重時將導(dǎo)致狀態(tài)的估計值明顯偏離真實值，從而導(dǎo)致系統(tǒng)導(dǎo)航定位的準(zhǔn)確性、可靠性變差[4]。所以，當(dāng)GNSS量測輸出中存在異常量測信號等故障時，如果仍采用EKF算法將SINS和GNSS輸出的數(shù)據(jù)相融合，會導(dǎo)致精度降低甚至發(fā)散。為了解決GNSS信號異常這類軟故障，很多學(xué)者對自適應(yīng)濾波和魯棒濾波進(jìn)行研究。文獻(xiàn)[4]利用系統(tǒng)的量測殘差序列構(gòu)造基于自適應(yīng)多重漸消因子的魯棒EKF算法（REKF）來解決系統(tǒng)中出現(xiàn)的任何類型故障；文獻(xiàn)[5]利用量測殘差提出了估計量測噪聲的自適應(yīng)濾波算法（AEKF），通過仿真證明了該算法在GNSS信號存在較大的位置誤差時的有效性；文獻(xiàn)[6-7]利用系統(tǒng)的殘差序列或新息序列的協(xié)方差矩陣構(gòu)造自適應(yīng)濾波器來修正量測噪聲方差陣和解決系統(tǒng)噪聲不確定的問題，得到了較好的濾波效果。文獻(xiàn)[4-5]中的REKF和AEKF在存在較大速度、位置誤差時的濾波結(jié)果仍有明顯發(fā)散，若故障時間較長就無法保障導(dǎo)航的精度；文獻(xiàn)[5-6]利用單一的加權(quán)因子，無法對GNSS量測輸出各通道的故障進(jìn)行精確的加權(quán)。

本文針對SINS/GNSS組合導(dǎo)航系統(tǒng)中存在GNSS信號異常這類問題，提出一種基于EKF的自適應(yīng)分類容錯濾波算法。首先，通過比較系統(tǒng)殘差協(xié)方差矩陣的實際值與理論值來檢測 GNSS信號中是否存在異常，并區(qū)分異常的類型，然后針對不同程度的異常信號計算來進(jìn)行修正，并且在濾波過程中加入UD分解來改善濾波的穩(wěn)定性能。

1 SINS/GNSS組合系統(tǒng)非線性數(shù)學(xué)模型

考慮SINS/GNSS組合導(dǎo)航系統(tǒng)非線性數(shù)學(xué)模型[8]，

式中：δk,j為克羅內(nèi)克（Kronecker）符號；Qk、Rk分別為系統(tǒng)噪聲方差陣和量測噪聲方差陣。

系統(tǒng)量測殘差向量為

式中，Pk+1/k為一步預(yù)測均方誤差陣，在EKF濾波方程中滿足：

其中，Pk為估計均方誤差陣：

且Kk滿足：

2 自適應(yīng)分類容錯濾波設(shè)計

2.1 問題提出

在SINS/GNSS中，如果GNSS信號出現(xiàn)異常則會導(dǎo)致系統(tǒng)量測存在較大的誤差，所以實際系統(tǒng)的量測噪聲方差Rk+1并不能用固定值表示，否則會使EKF對量測值Zk+1和一步估計值利用的比例程度失衡，從而導(dǎo)致濾波發(fā)散。因此，為了方便實驗觀察，本文將量測噪聲分為兩部分：一部分為有固定方差系統(tǒng)量測噪聲一部分為時變量測噪聲且設(shè)兩部分噪聲互不相關(guān)，所以式(1)中的系統(tǒng)量測方程和式(5)可分別等價于

式中，Υk+1和Λk+1同為m×m維對角矩陣，分別為時變量測噪聲和其方差的加權(quán)修正矩陣，非負(fù)定。設(shè)

同時，式(10)中：

基于上述分析與計算，本文提出的自適應(yīng)分類容錯濾波方法的濾波方程如式(15)～ (22)所示：

式中：Λk+1為加權(quán)因子矩陣，如式(11)所示。

在上述計算過程中，如果估計均方誤差陣Pk為病矩陣或輕度負(fù)定會使Pk+1/k失去非負(fù)定性，再求取時Sk+1的求逆運算將會不存在或產(chǎn)生巨大的誤差，從而導(dǎo)致的估計誤差較大。針對上述問題，Thornton等[9]提出一種基于UD分解的濾波方法。本文也將UD分解應(yīng)用到自適應(yīng)分類容錯濾波方法中，將Pk和Pk+1/k分別分解為TUDU 的形式：

2.2 加權(quán)矩陣Λk+1的確定

根據(jù)1.1節(jié)的分析，為降低GNSS信號異常對組合導(dǎo)航系統(tǒng)濾波精度和穩(wěn)定性的不利影響，適當(dāng)?shù)剡x取Λk+1的值是設(shè)計自適應(yīng)分類容錯濾波算法的關(guān)鍵。

所以為了提高濾波精度，本文提出的自適應(yīng)分類容錯濾波算法在進(jìn)行自適應(yīng)分類加權(quán)矩陣的設(shè)計時，首先利用異常信號檢測公式來求取η。根據(jù)η的歸屬范圍，將GNSS出現(xiàn)的異常信號分為兩類（A類異常和B類異常），并根據(jù)異常信號的分類來定義量測噪聲方差加權(quán)陣Λk+1的求取方法。η的判定公式為：

式中，1η和ξ的值可根據(jù)系統(tǒng)的實際情況選取，在大量試驗的基礎(chǔ)上，對常規(guī)SINS/GNSS組合導(dǎo)航系統(tǒng)1η取值在100～500之間，ξ的取值不大于0.1。

根據(jù)上述分析，GNSS信號被分為三種情況：

（a）當(dāng)由η的值判斷 GNSS信號無異常時，

（b）當(dāng)判斷GNSS有A類異常信號時，由式(10)可知，Λk+1滿足下列不等式：

所以，Λk+1取值的必要條件為α滿足：

若α＜0，則α=0。當(dāng)時，由式(27)推出Λk+1取值的充分條件為iα滿足：

由上述推導(dǎo)可知，iα必須同時滿足式(28)和式(29)，即所以，為了精確的加權(quán)修正異常信號，針對本文算法中Λk+1取值的充要條件為其每一個元素滿足：

（c）當(dāng)GNSS有B類異常信號輸出時，利用殘差序列協(xié)方差的無偏估計來求取Λk+1的值，殘差序列協(xié)方差無偏估計表示為

式中，N為滑動窗口長度。式(10)和式(31)都包含了殘差信息，所以本文提出的自適應(yīng)容錯濾波算法中的最小值可以由F范數(shù)來確定。設(shè)

式中：Λk+1為非負(fù)定矩陣，表示F范數(shù)。當(dāng)J(Λ)取得最小值時，殘差序列協(xié)方差無偏估計M就越接近殘差序列協(xié)方差矩陣Sk+1(式(10))的值。所以，如何求出當(dāng)J(Λ)取最小值時Λk+1的值是下文中首先要解決的問題。將式(32)展開如下：

且J（Λ）取得最小值的必要條件為

所以，對式(34)求偏導(dǎo)得：

由式(35)和式(36)得：

若α＜0，則α=0。當(dāng)時，式(33)等價于

取得最小值。所以，αi的取值為

若αi＜0，則αi=0。為了精確的加權(quán)修正異常信號，由式(37)和式(40)可知，針對本文算法中Λk+1取值的充要條件為其每一個元素滿足：

所以，上述為GNSS信號出現(xiàn)不同情況時，加權(quán)矩陣Λk+1的取值方法。原理為當(dāng)GNSS信號出現(xiàn)A類異常時，Λk+1的取值越大，濾波增益矩陣Kk+1的值就會越小，在確定時對Ζk+1的利用權(quán)重減??；當(dāng)GNSS信號出現(xiàn)B類異常時，本文通過F范數(shù)的方法求取Λk+1，從而使其將估計的故障噪聲修正到正常范圍，但若此時殘差序列協(xié)方差無偏估計樣本的選取仍包含當(dāng)前值，則式(31)中M的取值仍會包含較大的量測誤差，致使Λk+1的取值達(dá)不到修正系統(tǒng)量測噪聲的目的，所以本文將M修正為由殘差序列的歷史值組成的樣本來推導(dǎo)，即將式(31)修正為

3 自適應(yīng)分類容錯算法在SINS/GNSS中的應(yīng)用

3.1 SINS/GNSS系統(tǒng)濾波模型

針對SINS/GNSS組合導(dǎo)航系統(tǒng)，選取一組15維的系統(tǒng)狀態(tài)變量：

式中：δΦE、δΦN、δΦU為姿態(tài)角誤差；δVE、δVN、δVU、δL、δλ、δh分別為東北天方向的速度誤差和經(jīng)緯高度誤差；ε為陀螺常值零漂；?為加速度常值零偏。

系統(tǒng)誤差模型[10]：

系統(tǒng)量測模型：

3.2 仿真實驗及分析

實驗平臺為模擬的SINS/GNSS組合導(dǎo)航系統(tǒng)。進(jìn)行仿真的SINS輸出頻率為200 Hz，其陀螺儀和加速度計的數(shù)據(jù)為：陀螺儀常值零漂和隨機(jī)漂移分別為0.1(°)/h 和0.002(°)/h1/2，加速度計常值零偏和隨機(jī)零偏分別為和。GNSS導(dǎo)航系統(tǒng)的輸出周期為1 s，GNSS接收機(jī)東、北、天方向的速度誤差均方根為0.5 m/s，位置誤差均方根為10 m。假設(shè)飛機(jī)初始速度和初始航向角分別為0 m/s和0°，初始緯度、經(jīng)度、高度分別為L0=34.27°、0λ=108.95°、h0=380 m，飛行過程中包括滑跑、爬升、轉(zhuǎn)彎、勻速平飛等階段，其飛行軌跡如圖1所示。

為了對文中提到的算法進(jìn)行更直觀的比較，在已獲得GNSS信號中，根據(jù)不同情況加入GNSS信號出現(xiàn)異常時的速度誤差均方根值和位置誤差均方根值，如表1所示。仿真時間為1000 s，且針對上述SINS/GNSS組合導(dǎo)航系統(tǒng)進(jìn)行仿真時，取1η=334.2，ξ=0.1。

用上述數(shù)據(jù)對本文提出的自適應(yīng)分類容錯濾波算法（ACEKF）進(jìn)行驗證。為了進(jìn)行比較，本文使用同一組數(shù)據(jù)對文獻(xiàn)[4]提出的REKF和文獻(xiàn)[5]提出的AEKF進(jìn)行仿真，所得仿真實驗結(jié)果如圖2和圖3所示。

由表1可知，本文在仿真的6個不同時間段內(nèi)在GNSS正常信號的基礎(chǔ)上分別加入不同程度的位置、速度誤差，且根據(jù)本文異常信號檢測分類算法，在時間段1和時間段4加入誤差的GNSS信號，大部分時REKF算法出現(xiàn)了一定程度的發(fā)散狀態(tài)，在時間段5，間內(nèi)歸屬于A類異常，而在時間段2、3、5、6加入誤差的GNSS信號則大部分時間內(nèi)屬于B類異常。圖2和圖3結(jié)果表明：本文提出的ACEKF與REKF、AEKF相比導(dǎo)航精度較高，且ACEKF與AEKF都優(yōu)于文獻(xiàn)[4]的REKF算法，其中在時間段1、2、4、5，REKF算法出現(xiàn)了一定程度的發(fā)散狀態(tài)，在時間段5，AEKF的經(jīng)、緯度誤差與東、北向速度誤差呈現(xiàn)了一定程度的發(fā)散狀態(tài)，在其它時間段REKF和AEKF都能將速度誤差和位置分別維持在0.5 m/s和10 m以內(nèi)，而本文提出的ACEKF算法則可以一直維持上述精度。所以整體上，ACEKF相比上述兩種算法誤差小，在濾波過程中能夠一直保持較高的精度，也更穩(wěn)定。

圖1 飛機(jī)飛行軌跡Fig.1 Flight trajectory

表1 加入的GNSS信號的速度、位置誤差均方根值Tab.1 Additional velocity and position errors of GNSS

圖2 SINS/GNSS在GNSS信號異常時的位置誤差Fig.2 Position errors of SINS/GNSS when there is abnormal GNSS signal

圖3 SINS/GNSS在GNSS信號異常時的速度誤差Fig.3 Velocity errors of SINS/GNSS when there is abnormal GNSS signal

表2和表3的數(shù)據(jù)比較了三種算法和EKF算法的位置和速度的平均絕對誤差與均方根誤差，可以看出采用本文提出的ACEKF方法具有較高的導(dǎo)航精度與穩(wěn)定性：在GNSS信號存在異常情況下，其導(dǎo)航精度相比對基于實時估計量測噪聲方差的 EKF至少提高95.6%，相比REKF和AEKF分別至少提高44.5%和24.6%。結(jié)果證明本文提出的ACEKF算法對GNSS異常信號有更強的容錯能力。

表2 速度、位置平均絕對誤差Tab.2 Mean absolute errors of velocity and position

表3 速度、位置誤差均方根誤差Tab. 3 RMSEs of velocity and position

4 結(jié) 論

本文針對 SINS/GNSS組合導(dǎo)航系統(tǒng)容易出現(xiàn)GNSS信號異常進(jìn)而影響導(dǎo)航精度及系統(tǒng)穩(wěn)定性等問題，提出一種基于EKF的自適應(yīng)分類容錯濾波方法，并在濾波過程中加入UD分解，最后將其應(yīng)用到組合導(dǎo)航系統(tǒng)中。本文方法主要對GNSS異常信號進(jìn)行檢測分類，對不同類別的異常信號采用不同方法計算量測噪聲的加權(quán)修正矩陣，以減弱異常數(shù)據(jù)對系統(tǒng)導(dǎo)航精度的影響。仿真結(jié)果表明：本文提出的ACEKF算法能準(zhǔn)確地檢測出GNSS異常信號，在GNSS信號出現(xiàn)異常情況下，其導(dǎo)航精度相比基于實時估計量測噪聲方差的 EKF算法至少提高 95.6%，相比REKF和AEKF分別至少提高44.5%和24.6%，對異常量測值有較好的修正能力，且ACEKF算法的容錯能力強，用其優(yōu)化后的系統(tǒng)能夠一直保持正常穩(wěn)定的狀態(tài)。

參考文獻(xiàn)（References）：

[1]Zhao L, Qiu H Y, Feng Y M. Study of robust filtering application in loosely coupled INS/GNSS system[J].Mathematical Problems in Engineering, 2014: 1-10.

[2]Lim C H, Lim T S, Koo V C. New GNSS-aided SINU system modeling using an autoregressive model[J]. International Journal of Advance Robotic Systems, 2015, 12:1-13.

[3]Zhang B, Chu H, Sun T. Error prediction for SINS/GNSS after GNSS outage based on hybrid KF-UKF[J]. Mathematical Problems in Engineering, 2015: 1-10.

[4]Soken H E, Hajiyev C. REKF and RUKF for pico satellite attitude estimation in the presence of measurement faults[J]. Journal of Systems Engineering and Electronics, 2014, 25(2): 288-297.

[5]Yu M J. INS/GNSS integration system using adaptive filter for estimating measurement noise variance[J]. IEEE Transactions on Aerospace and Electronic Systems, 2012,48(2): 1786-1792.

[6]朱占龍, 單友東. 基于新息正交性自適應(yīng)濾波的慣性/地磁組合導(dǎo)航方法[J]. 中國慣性技術(shù)學(xué)報, 2015, 23(1):66-70.Zhu Z L, Shan Y D. INS/GNSS integrated method based on innovation orthogonality adaptive Kalman filter[J].Journal of Chinese Inertial Technology, 2015, 23(1): 66-70.

[7]董寧, 徐玉嬌, 劉向東. 一種帶自適應(yīng)因子的IMM-UKF 的 GNSS/BD-2 導(dǎo)航方法[J]. 宇航學(xué)報,2015, 36(6): 676-683.Dong N, Xu Y J, Liu X D. An IMM-UKF with adaptive factor for GNSS/BD-2 satellite navigation system[J].Journal of Astronautics, 2015, 36(6): 676-683. ()

[8]秦永元, 張洪鉞, 汪叔華. 卡爾曼濾波與組合導(dǎo)航原理[M]. 西安：西北工業(yè)大學(xué)出版社, 2015: 198-205.Qin Y Y, Zhnag H Y, Wang S H. Kalman filtering and integrated navigation theory[M]. Xi’an: Northwestern Polytechnical University Press, 2015: 198-205.

[9]Thornton C L, Bierman G J. UDUT covariance factorization for Kalman filtering[J]. Control and dynamic systems, 1980: 65(4): 177-248.

[10]Ali J, Mirza M. Performance comparison among some nonlinear filters for a low cost SINS/GNSS integrated solution[J]. Nonlinear Dynamics, 2010, 61(3): 491-502.