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

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

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

?

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

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

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

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

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

0 引言

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

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

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

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

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

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

(1)式中：Xk為k時(shí)刻的狀態(tài)向量；Φk,k-1為從(k-1)時(shí)刻到k時(shí)刻的狀態(tài)轉(zhuǎn)移矩陣；Wk為k時(shí)刻的系統(tǒng)誤差向量；Zk為k時(shí)刻的觀測向量；Ak為k時(shí)刻的系數(shù)矩陣；Vk為k時(shí)刻的觀測誤差向量。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時(shí)刻的過程噪聲方差陣；∑k為k時(shí)刻的測量噪聲方差陣。

(2)

(3)

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

(4)

(5)

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

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

(6)

3 算例分析

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

(8)

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

圖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時(shí)，載體分別在作加速、減速和機(jī)動(dòng)運(yùn)動(dòng)，使濾波模型的狀態(tài)方程與實(shí)際運(yùn)動(dòng)的狀態(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能有效地控制載體異常擾動(dòng)，且經(jīng)過AKF1處理后的位置均方根誤差為0.446 8 m，遠(yuǎn)小于經(jīng)CKF處理后的位置均方根誤差。

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

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

4 結(jié)束語

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

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

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

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

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

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

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

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

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

[8] 陳遠(yuǎn),張小紅,郭斐,等.自適應(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濾波的動(dòng)態(tài)精密單點(diǎn)定位[J].自動(dòng)化儀表,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é)基金資助項(xiàng)目(編號：51207117)；湖北省教育廳科學(xué)研究基金資助項(xiàng)目(編號：Q20141503)。

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

TH86;TP274

A

10.16086/j.cnki.issn 1000-0380.201608001

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