INS／GPS緊耦合系統中的模糊自應SRUKF算法

袁建國，袁艷濤，劉飛龍，龐 宇，林金朝

（重慶郵電大學光纖通信技術重點實驗室，400065重慶）

INS／GPS緊耦合系統中的模糊自應SRUKF算法

袁建國，袁艷濤，劉飛龍，龐 宇，林金朝

（重慶郵電大學光纖通信技術重點實驗室，400065重慶）

為解決慣性導航系統（INS）與全球定位系統（GPS）緊耦合中標準無跡卡爾曼濾波（UKF）由于計算舍入誤差使協方差矩陣負定和實際應用中由于量測噪聲時變而嚴重影響濾波精度的問題，提出了基于模糊控制理論的自適應平方根無跡卡爾曼濾波（NASRUKF）算法.該算法在濾波過程中不是直接計算協方差矩陣，而是計算協方差矩陣的平方根，從而可以保證協方差矩陣的非負定性；然后根據實時得到的量測信息的實際方差與理論方差的比值，通過設計的模糊控制系統（FCS）實時調整量測噪聲矩陣.實驗表明：該算法對時變的噪聲具有很好的自適應性，相比于UKF算法具有更高的精度并使得系統具有更高的穩定性和魯棒性.

INS／GPS；卡爾曼濾波；SRUKF；模糊控制；自適應

慣性導航系統（INS）是一種完全自主的導航系統，不需要從外部接收信號，具有更新頻率高、抗干擾能力強等優點，但其誤差會隨時間不斷積累，不適合長時間工作；全球定位系統（GPS）能夠實現高精度長時間的定位，但需要依賴外界信息，因此易受外界環境干擾，且由于更新頻率低不適合高動態性能的環境.INS／GPS組合導航可以彌補各自的缺點，提供更優的導航［1］.

INS／GPS緊耦合相比松耦合具有精度更高、抗干擾能力更強等優點.在緊耦合組合導航中，擴展卡爾曼濾波器EKF（extented kalman filter）、無跡卡爾曼濾波器UKF（unscented kalman filter）等得到廣泛的應用［2］.UKF引入了UT變換，無需計算雅克比矩陣，且不存在高階截斷誤差，但UKF要求量測噪聲先驗已知，而實際應用中量測噪聲是時變且未知的，這就會導致系統不能很好地工作，甚至會使UKF發散［3-4］.一般自適應UKF算法能對量測噪聲自適應，但在數值計算中會存在舍入誤差，使協方差矩陣逐漸失去非負定，尤其當測量向量中某個或某幾個分量很準確，而數值計算的有效位數卻相對較少時這種現象較易發生.協方差矩陣非負定性的逐漸喪失將會使增益矩陣計算失真，從殘差中提取對一步預測值的補償信息就會越來越錯誤，最后導致濾波器發散不能正常工作［5-7］.針對上述問題，本文將模糊控制理論引入平方根無跡卡爾曼濾波器SRUKF（square-root unscented kalman filter），提出一種模糊自適應SRUKF算法，不僅可以保證協方差矩陣的非負定性而且還可以對時變的測量噪聲進行跟蹤和調整.實驗表明本文算法相比于傳統的UKF算法精度更高，可靠性更強.

1 INS／GPS緊耦合狀態方程和測量方程

INS與GPS狀態矢量如下：

式中：δφe，δφn，δφu，δve，δvn，δvu，δL，δλ，δh分別為INS三維姿態角誤差、速度誤差和位置誤差；εrx，εry，εrz為陀螺儀常值漂移；?αx，?αy，?αz為加速度計零偏；δtu為時鐘偏差等效距離誤差；δtru為時鐘頻漂等效距離變化誤差．INS／GPS組合導航狀態方程為

式中：WI（t）為3個方向上陀螺的白噪聲和加速度計的白噪聲，WG（t）為GPS時鐘鐘差噪聲和時鐘頻率噪聲．系統狀態方程離散數學模型為

式中：Wk為系統噪聲，Qk為協方差陣．

選取偽距差Δρ，偽距率差Δ˙ρ和航偏角差Δφ作為觀測向量，則系統觀測方程離散數學模型為［8］

式中：Vk為測量噪聲，Rk為協方差陣．

2 基于模糊控制理論的自適應SRUKF算法

2.1 SRUKF濾波算法

SRUKF濾波算法非線性系統模型如下：

式中：f為非線性函數；h為關于狀態的一階連續偏導數；wk和vk為過程噪聲和測量噪聲，其協方差陣分別為Qk和Rk．則基于SRUKF的濾波步驟為：

1）選定濾波初值x0和p0，即

對k=1，2，3，…，執行步驟．

2）計算k-1時刻的2n+1個σ樣本點，即

3）計算時刻的k一步預測模型，即

在SRUKF算法中狀態方程是非線性的，由于INS／GPS組合導航系統經過精對準后平臺誤差角很小，濾波周期也比較小，在小角度誤差的情況下，建立起來的誤差方程是線性的，這樣用卡爾曼濾波的方法直接得到狀態方程的一步預測，然后進行以下的計算，這樣不僅可以減少算法復雜度還可以減少計算量.

4）誤差協方差平方根一步預測，即

5）計算k時刻的一步預測增廣樣本點，即

6）殘差協方差平方根陣和互協方差陣測量更新，即

7）計算濾波值，即

2.2 基于模糊理論的自適應控制

SRUKF濾波算法可以保證協方差矩陣的非負定性，但系統測量噪聲特性需先驗已知.系統運行環境的不確定性使測量噪聲會隨時變化，所以測量噪聲的協方差陣Rk也會不斷變化，這樣可能會導致卡爾曼濾波不能正常工作，甚至會發散.本文引入控制模糊理論對系統觀測噪聲進行自適應，對濾波模型進行實時修正，提高系統的性能.

基于模糊控制理論的自適應SRUKF算法將實時得到的量測新信息的實際方差與理論方差的比值作為模糊控制系統（FCS）輸入，有FCS的輸出權值在線實時調整量測噪聲矩陣.INS／GPS緊耦合中基于模糊自適應的SRUKF系統結構如圖1所示.

圖1 模糊自適應SRUKF系統結構

圖1 中cr／pr表示新信息實測殘差方差與理論方差的比值，FCS模塊根據模糊規則得到修正權值Wk，用來修正偽距、偽距率和航向角的觀測噪聲矩陣，從而當系統的觀測噪聲未知時，可以對觀測噪聲進行實時、準確估計，提高系統的性能．

殘差的理論值定義為［9-10］

殘差的實測方差定義為

式中：m=k-M+1，M根據具體情況而定．殘差實測方差與理論方差的比值定義為

式中Tr（·）表示對矩陣求跡，當濾波模型準確時qk應該接近1，如果測量噪聲增大，則cr增大，進而qk將大于1，這時增大Rk可以使qk回到1附近；如果測量噪聲減小，則cr減小，進而qk將小于1，這時減小Rk使qk回到1附近．用FCS模塊的輸入值來調節Wk大小，進而調節Rk的大小．FCS輸入、輸出函數的隸屬模糊度如圖2與圖3所示．

圖2 輸入函數

圖3 輸出函數

其模糊控制規則如下：

if qk∈［0.2，0.7］，then Wk=1-0.5μ（qk）．

if qk∈［0.7，1.3］，then Wk=1+0.2（1-μ（qk））．

if qk∈［1.3，2.0］，then Wk=1+0.5μ（qk）．

模糊控制系統最終輸出權值Wk，然后對測量噪聲協方差矩陣自適應調整：Rk=WkRk／k-1．

3 實驗與結果分析

基于實際采集數據用MATLAB軟件在計算機上進行仿真.試驗中INS系統的數據更新頻率為100 Hz，GPS系統為1 Hz，初始位置經緯度誤差為1′，高度誤差為5m，初始姿態角誤差為0.1°，初始速度誤差為0，蛇螺儀漂移為20（°）／h，加速度計的噪聲為5×10-4g，偽距初始測量噪聲方差為10 m2，偽距率初始測量噪聲方差為0.5（m／s）2.卡爾曼濾波的系統噪聲可以通過對系統的大量反復實驗得到，其測量噪聲在時刻發生變化.分別用UKF算法與模糊自適應SRUKF算法作對比分析，真結果如圖4～6所示.

圖4 位置誤差

圖5 速度誤差

圖6 姿態角誤差

從圖4～6中可以看出，由于測量噪聲不斷變化，模型初始的R不能準確描述測量噪聲的統計特性，所以由于模型誤差使普通的UKF濾波誤差明顯大于NASRUKF濾波的誤差.而對于NASRUKF，由于此算法能夠自適應調整R陣逼近真實的量測噪聲水平，保證模型的準確性，故濾波誤差明顯小于普通的UKF濾波誤差.

逐漸增大測量噪聲的方差再次用普通的UKF和NASRUKF進行仿真，各自誤差曲線如圖7～10所示.

圖7 NASRUKF位置誤差

圖8 UKF位置誤差

圖9 NASRUKF速度誤差

圖10 UKF速度誤差

從圖7～10可以明顯看出，經過普通UKF濾波后輸出的位置和速度誤差在不斷變大，這是由于隨著濾波步數的增加測量噪聲不斷增大，UKF不能實時統計測量噪聲的特性，使模型與獲得的測量值不匹配，導致濾波器發散.輸出的估計值相對實際的被估計值的偏差越來越大，誤差的不斷積累又會導致估計的均方誤差陣失去非負定性，使增益矩陣的計算值逐漸失去合適的加權作用從而又進一步加快了濾波的發散.NASRUKF能夠自適應調整R跟隨真實測量噪聲的變化，保證了模型的正確性和均方誤差陣的非負定性，所以在強噪聲的環境中也能有很好的濾波效果，大大提高了系統的抗干擾能力.

4 結 語

基于模糊控制理論的自適應SRUKF算法通過間接地計算均方誤差矩陣的平方根來計算其值，保證了非負定性，并通過引入模糊控制理論來實時自適應地觀測噪聲.實驗表明本文算法可以有效提高系統的精度和抗干擾能力.

［1］羅建軍，馬衛華，袁建平.組合導航原理與應用［M］.西安：西北工業大學出版社，2012.

［2］LIU YU，WANG HONG，HOU Chaohuan.UKF based nonlinear filtering using minimum entropy criterion［J］. IEEE Transactions on Signal Processing，2013，61（20）：4988-4999.

［3］LUO Zhen，FANG Huajing.Modified state prediction algorithm based on UKF［J］.Journal of Systems Engineering and Electronics，2013，24（1）：135-140.

［4］馬春來，朱立新，孟龔.混合SRUKF在GPS／INS組合導航中的應用［J］.電子信息對抗技術，2013，28（2）：24-29.

［5］HU Haidong，HUANG Xianlin.SINS／CNS／GPS integrated navigation algorithm based on UKF［J］.Journal of Systems Engineering and Electronics，2010，21（1）：102-109.

［6］GUSTAFSSON F，HENDEBY G.Some relations between extended and unscented Kalman filters［J］.IEEE Transactions on Signal Processing，2012，60（2）：545-555.

［7］DENG Fang，CHEN Jie，CHEN Chen.Adaptive unscented Kalman filter for parameter and state estimation of nonlinear high-speed objects［J］.Journal of Systems Engineering and Electronics，2013，24（4）：655-665.

［8］陳劍，孫金海，李金海，等.GPS／AHRS緊耦合系統中改進的SRUKF算法［J］.哈爾濱工程大學學報，2012，33（10）：1300-1303.

［9］田易，孫金海，李金海，等.航姿參考系統中一種自適應卡爾曼濾波算法［J］.西安電子科技大學學報：自然科學版，2011，38（6）：103-107.

［10］JWO Dahjing，YANG Chifan，CHUANG ChihHsun，et al. Performance enhancement for ultra-tight GPS／INS integration using a fuzzy adaptive strong tracking unscented Kalman filter［J］.Journal of Nonlinear Dynamics，2013，73（1）：377-395.

（編輯王小唯）

A novel fuzzy adaptive SRUKF algorithm in tightly-coupled INS／GPS systems

YUAN Jianguo，YUAN Yantao，LIU Feilong，PANG Yu，LIN Jinzhao
（Key Lab of Optical Fiber Communication Technology，Chongqing University of Posts and Telecommunications，400065 Chongqing，China）

In tightly-coupled Inertial Navigation System（INS）／Global Position System（GPS）systems，the filtering precision of Kalman filter（UKF）will be affected seriously by the calculation rounding error leading to the covariancematrix negative definiteness and the time-varying measurement noise in practical application.To deal with this problem，an adaptive square root unscented Kalman filter（NASRUKF）based on fuzzy control theory algorithm is proposed.This algorithm not only can ensure the covariancematrix non-negative definiteness，but also can track and adjust the time-varyingmeasurement noise.Experimental results show that the algorithm has a good adaptability for the time-varying noise with higher accuracy，which makes the system have higher stability and robustness in comparison with UKF algorithm.

INS／GPS；Kalman filter；SRUKF；fuzzy control；adaptive

TP23

：A

：0367-6234（2015）11-0108-05

10.11918／j.issn.0367-6234.2015.11.019

2014-11-08.

重慶高校創新團隊建設計劃；國家自然科學基金項目（61472464）；重慶郵電大學（重慶市）研究生科研創新項目（CYS14144）.

袁建國（1968—），男，教授.

袁建國，yyyyjg＠126.com.