周 敏 劉 健 楊 林 王宇龍 桑青松

(武漢工程大學(xué)電氣信息學(xué)院，湖北 武漢 430205)

?

一種自適應(yīng)卡爾曼濾波在動態(tài)導(dǎo)航定位中的應(yīng)用

周 敏 劉 健 楊 林 王宇龍 桑青松

(武漢工程大學(xué)電氣信息學(xué)院，湖北 武漢 430205)

在實際運行過程中，往往很難預(yù)測目標的運動狀態(tài)，這就會導(dǎo)致應(yīng)用在動態(tài)導(dǎo)航定位中的卡爾曼濾波算法難以收斂。為了解決這個問題，提出了一種新的自適應(yīng)卡爾曼濾波算法。該算法采用卡方分布構(gòu)造統(tǒng)計量，通過檢驗動力學(xué)模型是否出現(xiàn)異常，確定了一種平衡觀測方程和狀態(tài)方程的自適應(yīng)因子。仿真結(jié)果表明，在目標狀態(tài)發(fā)生變化的情況下，該方法優(yōu)于標準卡爾曼濾波算法和兩段函數(shù)確定的自適應(yīng)因子的卡爾曼濾波算法，是一種魯棒性好、性能可靠、精度高的濾波算法，可廣泛應(yīng)用于動態(tài)導(dǎo)航定位中。

動態(tài)導(dǎo)航 定位 卡爾曼濾波 自適應(yīng) 動力學(xué)模型 算法

0 引言

卡爾曼濾波算法被廣泛應(yīng)用于動態(tài)載體的導(dǎo)航與定位中。當載體進行規(guī)則運動且系統(tǒng)噪聲的統(tǒng)計特性已知時，卡爾曼濾波將會是最優(yōu)濾波。但是載體在實際運行過程中，很難確定噪聲的統(tǒng)計特性，而且一般也很難保證規(guī)則運動，這往往會導(dǎo)致系統(tǒng)濾波模型與真實模型之間存在一定的偏差，使導(dǎo)航解算的解嚴重偏離真實解。

為了解決以上問題，提出了自適應(yīng)卡爾曼濾波算法并將其應(yīng)用于動態(tài)導(dǎo)航定位中，同時涌現(xiàn)出了許多求解自適應(yīng)因子的方法。文獻[1]介紹了一種利用相關(guān)法實現(xiàn)的QR自適應(yīng)濾波方法；文獻[2]提出了一種改進的Sage自適應(yīng)濾波方法，這類方法需要觀測信息同維、同類；楊元喜等提出了一種更為靈活的自適應(yīng)濾波方法[3],利用四種判別統(tǒng)計量和四種自適應(yīng)因子構(gòu)造出用于平衡觀測值和一步預(yù)測值的自適應(yīng)濾波方法，但是當觀測信息嚴重不足時，采用該方法確定的自適應(yīng)因子不能很好地控制載體異常擾動；文獻[4]提出了一種直接對動力學(xué)模型誤差參數(shù)進行在線估計的方法。

合理的最優(yōu)自適應(yīng)因子應(yīng)該使自適應(yīng)濾波器的理論統(tǒng)計特性與實際觀測值的統(tǒng)計特性盡量保持一致[5],如狀態(tài)向量濾波值的協(xié)方差陣、狀態(tài)向量預(yù)測值的協(xié)方差陣以及新息向量的協(xié)方差陣。而最優(yōu)自適應(yīng)因子的求解可以利用卡方分布構(gòu)造一個統(tǒng)計量來實現(xiàn)。由此，可以在一定的置信水平下，通過假設(shè)檢驗來判斷濾波是否出現(xiàn)異常，從而對模型進行自適應(yīng)調(diào)節(jié)。

為了解決動態(tài)導(dǎo)航定位中存在的觀測信息嚴重不足、系統(tǒng)動力學(xué)模型與真實動力學(xué)模型之間存在偏差等問題，本文提出了一種新的自適應(yīng)卡爾曼濾波算法(adaptive Kalman filtering algorithm,AKTA)。該算法在自適應(yīng)卡爾曼濾波原理的基礎(chǔ)上，利用卡方分布構(gòu)造一個統(tǒng)計量，同時給出了卡方分布檢驗準則以及確定自適應(yīng)因子的計算方法。通過仿真，將該方法與經(jīng)典卡爾曼濾波算法(conventional Kalman filtering algorithm,CKFA)和兩段函數(shù)確定的自適應(yīng)因子的卡爾曼濾波算法進行對比，驗證了該算法在動態(tài)導(dǎo)航與定位中的有效性及其濾波精度高的優(yōu)點。

1 自適應(yīng)卡爾曼濾波原理

在動態(tài)導(dǎo)航與定位中，卡爾曼濾波器一般用線性化后的離散模型來描述[6-7]：

(1)式中：Xk為k時刻的狀態(tài)向量；Φk,k-1為從(k-1)時刻到k時刻的狀態(tài)轉(zhuǎn)移矩陣；Wk為k時刻的系統(tǒng)誤差向量；Zk為k時刻的觀測向量；Ak為k時刻的系數(shù)矩陣；Vk為k時刻的觀測誤差向量。Wk、Vk須滿足以下性質(zhì)：E(Wk)=0,D(Wk)=∑Wk；E(Vk)=0,D(Vk)=∑k;Cov(Wk, Wj)=0, k≠j; Cov(Vk, Vj)=0, k≠j, Cov(Wk, Vj)=0。式中：∑Wk為k時刻的過程噪聲方差陣；∑k為k時刻的測量噪聲方差陣。

(2)

(3)

自適應(yīng)濾波原理為[8]：

(4)

(5)

2 動態(tài)導(dǎo)航中的自適應(yīng)濾波

目前，卡爾曼濾波算法是在動態(tài)導(dǎo)航應(yīng)用中較常用的方法。濾波器在某時刻輸出的結(jié)果是在觀測值和預(yù)測值之間的一個權(quán)衡。當動力學(xué)模型發(fā)生擾動而觀測值比較準確時，如果預(yù)測值仍然分配較大的權(quán)，這將會使定位結(jié)果嚴重偏離真實值，甚至造成濾波無法收斂。和文中第1節(jié)類似地引入一個自適應(yīng)因子αk，使預(yù)測值的權(quán)分配得小一些，只是求解αk的方法不同。因此，在式(2)的基礎(chǔ)上可作如下改動，并得到第k個歷元的預(yù)測誤差及其新息向量方差為：

(6)

3 算例分析

為了驗證本文所提出的自適應(yīng)卡爾曼濾波在動態(tài)導(dǎo)航與定位中應(yīng)用的有效性，采用以下條件進行仿真測試。假設(shè)載體在X軸方向運動，在0～100 s以0.5 m/s2的加速度作勻加速運動，在100～400 s以大約50 m/s的速度作勻速運動，在400～500 s以-0.3 m/s2的加速度作勻減速運動，在500～800 s以大約20 m/s的速度作勻速運動，在800～900 s以加速度方差為1 m/s2作機動運動，在900～1 500 s以大約26 m/s速度作勻速運動。直接在軌跡發(fā)器的位置和速度輸出上疊加白噪聲信號來模擬X軸方向上的位置和速度。位置誤差方差取為1 m2,速度誤差的標準差為0.1m/s。

(8)

式中：c為常量，一般在1.0～2.5之間；ΔVk為預(yù)測殘差統(tǒng)計量。

圖1是各種方案在X軸方向的位置殘差(Ex)；圖2是方案二和方案三的自適應(yīng)因子對比圖；表1是三種濾波算法的均方根誤差(root mean square error,RMS)比較。圖1(c)和圖2(b)是選取置信水平在0.01情況下的基于卡方分布的濾波殘差波形和自適應(yīng)因子的波形。

根據(jù)圖1和表1可以看出，在0～100 s、400～500 s以及800～900 s時，載體分別在作加速、減速和機動運動，使濾波模型的狀態(tài)方程與實際運動的狀態(tài)方程不相符，導(dǎo)致CKF后的殘差較大，整體效果欠佳。

圖1 X軸方向的位置殘差對比圖

圖2 自適應(yīng)因子對比圖

RMSCKFAKF1AKF2γ=0.01γ=0.05γ=0.10位置/m0.81550.44680.38160.41450.4207速度/(m/s)0.15360.12900.09300.09310.0823

根據(jù)圖1和表1可以看出：相對于CKF,AKF1能有效地控制載體異常擾動，且經(jīng)過AKF1處理后的位置均方根誤差為0.446 8 m，遠小于經(jīng)CKF處理后的位置均方根誤差。

圖1表明:AKF2可以有效控制運動載體在三個“特殊”時段濾波誤差的增大，阻止了卡爾曼濾波的發(fā)散，但這是以降低濾波精度為代價的。當動態(tài)解算的觀測信息不足時，AKF2的解算殘差要小于AKF1。由表1可知：AKF1的位置均方根誤差為0.446 8 m，而AKF2的位置均方根誤差為0.381 6 m。但是AKF2的計算量要比AKF1大一些，這是因為AKF2在計算自適應(yīng)因子時采用了迭代計算。

根據(jù)圖2可知：在動力學(xué)模型異常的三個時段，AKF1的自適應(yīng)因子最大衰減可以達到0.4，平均衰減可以達到0.5左右；而AKF2的自適應(yīng)因子最大衰減可以達到0.2，平均衰減在0.3左右。因此在相同模型誤差的情況下，AKF2更能較好地遺忘動力學(xué)模型誤差。而在其他時段，基于卡方分布檢驗的自適應(yīng)因子比兩段函數(shù)確定的自適應(yīng)因子衰減更小，這將會增加AKF2在導(dǎo)航解算時對動力學(xué)模型的依賴，使濾波結(jié)果更接近真實值。

4 結(jié)束語

本文研究了一種自適應(yīng)卡爾曼濾波算法在動態(tài)導(dǎo)航與定位中的應(yīng)用，并且將該算法與標準卡爾曼濾波、兩段函數(shù)法進行了比較與分析。仿真結(jié)果表明，當運動目標狀態(tài)發(fā)生變化且觀測信息不充足時，該算法能更好地控制模型誤差對導(dǎo)航解的影響，并更有效地控制載體的異常擾動、提高濾波的精度。該方法比文獻

[5]計算自適應(yīng)因子的方法更易于實現(xiàn)。因此，該方法可被廣泛應(yīng)用于動態(tài)導(dǎo)航與定位，對于解決觀測異常和狀態(tài)異常等問題具有重要的研究價值。

[1] 寧少武,史治宇. QR分解的最小二乘格型自適應(yīng)濾波算法在噪聲主動控制中的應(yīng)用[J].振動工程學(xué)報,2013,26(3):363-373.

[2] 徐婷婷. VTS系統(tǒng)船舶跟蹤和預(yù)測的新技術(shù)研究[D].大連：大連海事大學(xué),2012.

[3] 楊元喜,任夏,許艷.自適應(yīng)抗差濾波理論及應(yīng)用的主要進展[J].導(dǎo)航定位報,2013,1(1):9-15.

[4] 高為廣,張曉東.基于Kalman濾波的動力學(xué)模型誤差估計算法[J].測繪科學(xué),2011,36(2):56-58.

[5] 宋迎春.抗差自適應(yīng)濾波中帶有置信度的自適應(yīng)因子設(shè)計[C]//第一屆中國衛(wèi)星導(dǎo)航學(xué)術(shù)年會論文集,北京：2010,1310-1316.

[6] 于靜文,薛蕙,溫渤嬰.基于卡爾曼濾波的電能質(zhì)量分析方法綜述[C]//電網(wǎng)技術(shù),北京：2010,34(2):97-103．

[7] 王笑天,楊志家，王英男，等.雙卡爾曼濾波算法在鋰電池SOC估算中的應(yīng)用[J].儀器儀表學(xué)報,2013,34(8):1732-1738.

[8] 陳遠,張小紅,郭斐,等.自適應(yīng)卡爾曼濾波在GPS/DR組合導(dǎo)航中的應(yīng)用[J].測繪科學(xué),2010,35(3):169-170.

[9] HIDE C,MOORE T,SMITH M. Adaptive Kalman filtering algorithms for integrating GPS and low cost INS[J]．Position Location and Navigation Symposium,2004:227-233.[10]Da R.Failure detection of dynamical systems with the state chi-square test [J].Journal of Guidance Control and Dynamics,1994,17(2):271-277.

[11]王雙紅,聶建亮.自適應(yīng)交互集合Kalman濾波的動態(tài)精密單點定位[J].自動化儀表,2013,34(5):10-12.

Application of an Adaptive Kalman Filtering Algorithm in Dynamic Navigation Positioning

In actual operation process of target,the dynamic motion state of the target is difficult to be predicted,which makes the Kalman filter algorithm applying in dynamic navigation positioning difficult to converge. In order to solve the problem,a new adaptive Kalman filtering algorithm is proposed. The chi square distribution is used to construct statistics,through checking if the dynamics model is abnormal or not,the adaptive factor of equilibrium observation equation and state equation is designed. The results of simulation show that in the case of the target state charging,this method outperforms the standard Kalman filtering algorithm and the Kalman filtering algorithm that the adaptive factor is determined by two-segment functions,so it is a kind of filtering algorithm with good robustness,reliable performance and high accuracy,and can be used widely in dynamic navigation positioning.

Dynamic navigation Positioning Kalman filter Self-adaption Dynamics model Algorithm

國家自然科學(xué)基金資助項目(編號：51207117)；湖北省教育廳科學(xué)研究基金資助項目(編號：Q20141503)。

周敏(1990—)，女，現(xiàn)為武漢工程大學(xué)檢測技術(shù)與自動化裝置專業(yè)在讀碩士研究生；主要從事模式識別與智能系統(tǒng)方向的研究。

TH86;TP274

A

10.16086/j.cnki.issn 1000-0380.201608001

修改稿收到日期：2015-12-22。