袁建國(guó)，袁艷濤，劉飛龍，龐 宇，林金朝

（重慶郵電大學(xué)光纖通信技術(shù)重點(diǎn)實(shí)驗(yàn)室，400065重慶）

INS／GPS緊耦合系統(tǒng)中的模糊自應(yīng)SRUKF算法

袁建國(guó)，袁艷濤，劉飛龍，龐 宇，林金朝

（重慶郵電大學(xué)光纖通信技術(shù)重點(diǎn)實(shí)驗(yàn)室，400065重慶）

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

INS／GPS；卡爾曼濾波；SRUKF；模糊控制；自適應(yīng)

慣性導(dǎo)航系統(tǒng)（INS）是一種完全自主的導(dǎo)航系統(tǒng)，不需要從外部接收信號(hào)，具有更新頻率高、抗干擾能力強(qiáng)等優(yōu)點(diǎn)，但其誤差會(huì)隨時(shí)間不斷積累，不適合長(zhǎng)時(shí)間工作；全球定位系統(tǒng)（GPS）能夠?qū)崿F(xiàn)高精度長(zhǎng)時(shí)間的定位，但需要依賴外界信息，因此易受外界環(huán)境干擾，且由于更新頻率低不適合高動(dòng)態(tài)性能的環(huán)境.INS／GPS組合導(dǎo)航可以彌補(bǔ)各自的缺點(diǎn)，提供更優(yōu)的導(dǎo)航［1］.

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

1 INS／GPS緊耦合狀態(tài)方程和測(cè)量方程

INS與GPS狀態(tài)矢量如下：

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

式中：WI（t）為3個(gè)方向上陀螺的白噪聲和加速度計(jì)的白噪聲，WG（t）為GPS時(shí)鐘鐘差噪聲和時(shí)鐘頻率噪聲．系統(tǒng)狀態(tài)方程離散數(shù)學(xué)模型為

式中：Wk為系統(tǒng)噪聲，Qk為協(xié)方差陣．

選取偽距差Δρ，偽距率差Δ˙ρ和航偏角差Δφ作為觀測(cè)向量，則系統(tǒng)觀測(cè)方程離散數(shù)學(xué)模型為［8］

式中：Vk為測(cè)量噪聲，Rk為協(xié)方差陣．

2 基于模糊控制理論的自適應(yīng)SRUKF算法

2.1 SRUKF濾波算法

SRUKF濾波算法非線性系統(tǒng)模型如下：

式中：f為非線性函數(shù)；h為關(guān)于狀態(tài)的一階連續(xù)偏導(dǎo)數(shù)；wk和vk為過程噪聲和測(cè)量噪聲，其協(xié)方差陣分別為Qk和Rk．則基于SRUKF的濾波步驟為：

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

對(duì)k=1，2，3，…，執(zhí)行步驟．

2）計(jì)算k-1時(shí)刻的2n+1個(gè)σ樣本點(diǎn)，即

3）計(jì)算時(shí)刻的k一步預(yù)測(cè)模型，即

在SRUKF算法中狀態(tài)方程是非線性的，由于INS／GPS組合導(dǎo)航系統(tǒng)經(jīng)過精對(duì)準(zhǔn)后平臺(tái)誤差角很小，濾波周期也比較小，在小角度誤差的情況下，建立起來(lái)的誤差方程是線性的，這樣用卡爾曼濾波的方法直接得到狀態(tài)方程的一步預(yù)測(cè)，然后進(jìn)行以下的計(jì)算，這樣不僅可以減少算法復(fù)雜度還可以減少計(jì)算量.

4）誤差協(xié)方差平方根一步預(yù)測(cè)，即

5）計(jì)算k時(shí)刻的一步預(yù)測(cè)增廣樣本點(diǎn)，即

6）殘差協(xié)方差平方根陣和互協(xié)方差陣測(cè)量更新，即

7）計(jì)算濾波值，即

2.2 基于模糊理論的自適應(yīng)控制

SRUKF濾波算法可以保證協(xié)方差矩陣的非負(fù)定性，但系統(tǒng)測(cè)量噪聲特性需先驗(yàn)已知.系統(tǒng)運(yùn)行環(huán)境的不確定性使測(cè)量噪聲會(huì)隨時(shí)變化，所以測(cè)量噪聲的協(xié)方差陣Rk也會(huì)不斷變化，這樣可能會(huì)導(dǎo)致卡爾曼濾波不能正常工作，甚至?xí)l(fā)散.本文引入控制模糊理論對(duì)系統(tǒng)觀測(cè)噪聲進(jìn)行自適應(yīng)，對(duì)濾波模型進(jìn)行實(shí)時(shí)修正，提高系統(tǒng)的性能.

基于模糊控制理論的自適應(yīng)SRUKF算法將實(shí)時(shí)得到的量測(cè)新信息的實(shí)際方差與理論方差的比值作為模糊控制系統(tǒng)（FCS）輸入，有FCS的輸出權(quán)值在線實(shí)時(shí)調(diào)整量測(cè)噪聲矩陣.INS／GPS緊耦合中基于模糊自適應(yīng)的SRUKF系統(tǒng)結(jié)構(gòu)如圖1所示.

圖1 模糊自適應(yīng)SRUKF系統(tǒng)結(jié)構(gòu)

圖1 中cr／pr表示新信息實(shí)測(cè)殘差方差與理論方差的比值，F(xiàn)CS模塊根據(jù)模糊規(guī)則得到修正權(quán)值Wk，用來(lái)修正偽距、偽距率和航向角的觀測(cè)噪聲矩陣，從而當(dāng)系統(tǒng)的觀測(cè)噪聲未知時(shí)，可以對(duì)觀測(cè)噪聲進(jìn)行實(shí)時(shí)、準(zhǔn)確估計(jì)，提高系統(tǒng)的性能．

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

殘差的實(shí)測(cè)方差定義為

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

式中Tr（·）表示對(duì)矩陣求跡，當(dāng)濾波模型準(zhǔn)確時(shí)qk應(yīng)該接近1，如果測(cè)量噪聲增大，則cr增大，進(jìn)而qk將大于1，這時(shí)增大Rk可以使qk回到1附近；如果測(cè)量噪聲減小，則cr減小，進(jìn)而qk將小于1，這時(shí)減小Rk使qk回到1附近．用FCS模塊的輸入值來(lái)調(diào)節(jié)Wk大小，進(jìn)而調(diào)節(jié)Rk的大?。瓼CS輸入、輸出函數(shù)的隸屬模糊度如圖2與圖3所示．

圖2 輸入函數(shù)

圖3 輸出函數(shù)

其模糊控制規(guī)則如下：

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）．

模糊控制系統(tǒng)最終輸出權(quán)值Wk，然后對(duì)測(cè)量噪聲協(xié)方差矩陣自適應(yīng)調(diào)整：Rk=WkRk／k-1．

3 實(shí)驗(yàn)與結(jié)果分析

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

圖4 位置誤差

圖5 速度誤差

圖6 姿態(tài)角誤差

從圖4～6中可以看出，由于測(cè)量噪聲不斷變化，模型初始的R不能準(zhǔn)確描述測(cè)量噪聲的統(tǒng)計(jì)特性，所以由于模型誤差使普通的UKF濾波誤差明顯大于NASRUKF濾波的誤差.而對(duì)于NASRUKF，由于此算法能夠自適應(yīng)調(diào)整R陣逼近真實(shí)的量測(cè)噪聲水平，保證模型的準(zhǔn)確性，故濾波誤差明顯小于普通的UKF濾波誤差.

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

圖7 NASRUKF位置誤差

圖8 UKF位置誤差

圖9 NASRUKF速度誤差

圖10 UKF速度誤差

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

4 結(jié) 語(yǔ)

基于模糊控制理論的自適應(yīng)SRUKF算法通過間接地計(jì)算均方誤差矩陣的平方根來(lái)計(jì)算其值，保證了非負(fù)定性，并通過引入模糊控制理論來(lái)實(shí)時(shí)自適應(yīng)地觀測(cè)噪聲.實(shí)驗(yàn)表明本文算法可以有效提高系統(tǒng)的精度和抗干擾能力.

［1］羅建軍，馬衛(wèi)華，袁建平.組合導(dǎo)航原理與應(yīng)用［M］.西安：西北工業(yè)大學(xué)出版社，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，F(xiàn)ANG Huajing.Modified state prediction algorithm based on UKF［J］.Journal of Systems Engineering and Electronics，2013，24（1）：135-140.

［4］馬春來(lái)，朱立新，孟龔.混合SRUKF在GPS／INS組合導(dǎo)航中的應(yīng)用［J］.電子信息對(duì)抗技術(shù)，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緊耦合系統(tǒng)中改進(jìn)的SRUKF算法［J］.哈爾濱工程大學(xué)學(xué)報(bào)，2012，33（10）：1300-1303.

［9］田易，孫金海，李金海，等.航姿參考系統(tǒng)中一種自適應(yīng)卡爾曼濾波算法［J］.西安電子科技大學(xué)學(xué)報(bào)：自然科學(xué)版，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.

重慶高校創(chuàng)新團(tuán)隊(duì)建設(shè)計(jì)劃；國(guó)家自然科學(xué)基金項(xiàng)目（61472464）；重慶郵電大學(xué)（重慶市）研究生科研創(chuàng)新項(xiàng)目（CYS14144）.

袁建國(guó)（1968—），男，教授.

袁建國(guó)，yyyyjg＠126.com.