基于自適應(yīng)卡爾曼濾波的GPS/INS位置組合導(dǎo)航

許根源，王直，王志強(qiáng)

（江蘇科技大學(xué)電子與信息學(xué)院，江蘇鎮(zhèn)江212003）

在控制工程領(lǐng)域中各種系統(tǒng)都需要能夠完成對系統(tǒng)的有效控制，精確有效的控制系統(tǒng)無一例外的都是閉環(huán)控制系統(tǒng)，閉環(huán)控制中系統(tǒng)狀態(tài)反饋的準(zhǔn)確把握是系統(tǒng)能否有效控制的主要判定標(biāo)準(zhǔn)。實(shí)際的工程控制領(lǐng)域包含多種控制方法，其中對系統(tǒng)的過去、未來及現(xiàn)在的狀態(tài)的控制檢測為常見的典型控制系統(tǒng)。對特定系統(tǒng)的精確控制需要特定的觀測手段，只有通過這些特定的觀測手段才可以估計(jì)出系統(tǒng)過去、現(xiàn)在、未來的狀態(tài)，進(jìn)而達(dá)到對系統(tǒng)已知狀態(tài)的評估，實(shí)時(shí)狀態(tài)的精確控制，控制趨勢的準(zhǔn)確預(yù)測等。在估計(jì)控制系統(tǒng)的狀態(tài)的過程中最優(yōu)估計(jì)顯得尤為重要，實(shí)際系統(tǒng)在觀測過程中存在來自系統(tǒng)本身的誤差，檢測元件誤差，系統(tǒng)外部干擾等，這些干擾在控制理論中被定義為噪聲。尋求濾除噪聲干擾的方法直接影響系統(tǒng)的穩(wěn)定性及精確性，故最佳系統(tǒng)狀態(tài)估計(jì)必須解決噪聲干擾的問題[1]。

為解決系統(tǒng)噪聲干擾的問題多種控制算法應(yīng)運(yùn)而生，其中Kalman濾波等一些濾波算法在解決噪聲干擾方面效果明顯。Kalman濾波采用實(shí)時(shí)遞推其數(shù)據(jù)存儲(chǔ)量較小同時(shí)濾波算法簡單易行方便移植多種控制器，在工程應(yīng)用中成為一種備受重視的最優(yōu)估計(jì)理論與方法[2-3]，在信號采集處理、工業(yè)軌跡控制、GPS導(dǎo)航定位等各種工控領(lǐng)域被廣泛的應(yīng)用。

1 自適應(yīng)Kalman濾波

1.1 Kalman濾波方程

設(shè)控制對象的離散模型為：

上式u（k）為模型的m維控制向量，x（k）為模型的n維狀態(tài)向量，v（k）為離散模型的n維過程干擾向量，y（k）為模型的r維輸出向量，w（k）為r維測量噪聲向量。假設(shè)W為正定對稱陣，V為非負(fù)定對稱陣，并且v（k）、w（k）不相關(guān)，w（k）為離散的高斯白噪聲。v（k）為離散的高斯白噪聲序列。

一步預(yù)測方程：

一步預(yù)測均方誤差方程：

狀態(tài)估值計(jì)算方程：

其中，K（k）為狀態(tài)估計(jì)器或Ka1man濾波增益矩陣是x（k）的一步最優(yōu)預(yù)報(bào)估計(jì)是 關(guān) 于y（k）的 一 步 預(yù) 報(bào) 估 計(jì) ，是 關(guān) 于y（k）的 一 步預(yù)報(bào)估計(jì)誤差。為了獲得x（k）最小狀態(tài)估計(jì)值，令狀態(tài)估計(jì)誤差的協(xié)方差最小，則有：

濾波增益方程：

上列式（2）～（6）構(gòu)成了Ka1man濾波遞推公式。

1.2 自適應(yīng)卡爾曼

在應(yīng)用中理論特性應(yīng)與實(shí)際的余項(xiàng)相匹配，實(shí)際的余項(xiàng)應(yīng)該在濾波的過程中被實(shí)時(shí)的檢測，以保證濾波過程中實(shí)際的余項(xiàng)不會(huì)出現(xiàn)異常。實(shí)際的余項(xiàng)不相容原假設(shè)Q（k-1）、R（k-1）時(shí)，原來假設(shè)的Q（k-1）、R（k-1）應(yīng)該用Q（k）、R（k）的重新估計(jì)來取代。其常用判據(jù)為[4]：式中γ＞1為儲(chǔ)備系數(shù)；tr為矩陣的跡；v（k）為信息序列。

v（k）方差的理論值為：

v（k）的實(shí)測方差為：

可得到濾波嚴(yán)格收斂判據(jù)為[5]：

上式中，Var(k)為濾波狀態(tài)因子，Var(k)≠1時(shí)稱為濾波異常，Var(k)=1時(shí)稱為濾波嚴(yán)格收斂。Var(k)＞1或Var(k)＜1的量測噪聲增大或減小，調(diào)節(jié)（k）增大或減小，可以使Var（k）在1附近變化，這樣就可以實(shí)現(xiàn)濾波的多種適應(yīng)性[2]。

2 狀態(tài)、觀測方程的建立

在航空空間技術(shù)中卡爾曼濾波也因其優(yōu)越的特性被廣泛的應(yīng)用。在測軌問題和慣性導(dǎo)航等方面卡爾曼濾波發(fā)揮這重要的作用。所謂測軌問題就是通過觀測飛行器在不同時(shí)刻的狀態(tài)數(shù)據(jù)，然后應(yīng)用卡爾曼濾波方法對這些數(shù)據(jù)分析，估計(jì)出飛行器各個(gè)時(shí)刻的位置、速度、加速度及阻力系數(shù)等物理量這些都被稱作狀態(tài)變量。這些狀態(tài)變量作為系統(tǒng)的反饋為控制系統(tǒng)提供精確的控制數(shù)據(jù)，這樣控制系統(tǒng)便可以控制飛行器進(jìn)行導(dǎo)航、制導(dǎo)和攔截等動(dòng)作。文中主要研究在GPS/INS組合導(dǎo)航的方法中如何更好的運(yùn)用濾波算法提高飛行軌跡精度。

在GPS/INS組合導(dǎo)航系統(tǒng)中使用的是線型卡爾曼濾波器，線型卡爾曼濾波器應(yīng)用于組合導(dǎo)航系統(tǒng)時(shí)多用間接估計(jì)法。組合導(dǎo)航系統(tǒng)中的誤差狀態(tài)較多且類型不一，在導(dǎo)航系統(tǒng)中將這些誤差統(tǒng)稱為誤差組合體。濾波器根據(jù)系統(tǒng)誤差估算出的誤差估值ΔX是控制系統(tǒng)的重要控制參數(shù)，在慣性導(dǎo)航系統(tǒng)中需要用誤差估值ΔX來對導(dǎo)航系統(tǒng)的控制參數(shù)進(jìn)行精確的矯正。根據(jù)導(dǎo)航系統(tǒng)中最典型的平臺(tái)誤差，速度誤差以及位置誤差可以建立導(dǎo)航系統(tǒng)的模型方程。

INS/GPS組合模型的逼近線性離散時(shí)間系統(tǒng)如下：

式中，Xk和Zk分別為被估計(jì)的狀態(tài)向量和觀測向量：Φk,k-1和Hk為狀態(tài)轉(zhuǎn)移矩陣和觀測矩陣；Wk為導(dǎo)航系統(tǒng)的過程噪聲，Vk為導(dǎo)航系統(tǒng)的量測噪聲，過程噪聲和量測噪聲是相互獨(dú)立的，同時(shí)是帶時(shí)變均值和協(xié)方差的正態(tài)白噪聲序列。

其中，q(k)、Q(k)為導(dǎo)航系統(tǒng)的過程噪聲，Wk時(shí)變均值和協(xié)方差，r(k)、R(k)為導(dǎo)航系統(tǒng)的量測噪聲，Vk時(shí)變均值和協(xié)方差。

導(dǎo)航系統(tǒng)多為多傳感器組合導(dǎo)航系統(tǒng)，為了便于對導(dǎo)航系統(tǒng)做深入的研究，導(dǎo)航基準(zhǔn)坐標(biāo)系采用東、北、天坐標(biāo)系，用此坐標(biāo)系作為車輛導(dǎo)航定位[3]的基準(zhǔn)坐標(biāo)系。在分析研究INS及GPS組合導(dǎo)航系統(tǒng)的性能及誤差數(shù)據(jù)時(shí)，取15維狀態(tài)矩陣、9維觀測矩陣，并采用位置、姿態(tài)組合觀測[6-7]：

其中，δPe、δPn、δPh為相對切平面位置誤差，δVe、δVn為速度誤差，δα、δβ、δβ為平臺(tái)角姿態(tài)誤差，?cx、?cy、?cz為加速度計(jì)零偏，εcx、εcy、εcz為陀螺常值誤差[4]。

3 算例仿真

3.1 仿真流程圖

仿真流程圖如圖1所示。

圖1 仿真流程圖

3.2 仿真結(jié)果

3.2.1 普通Kalman濾波

根據(jù)仿真分析先利用飛行軌跡發(fā)生器產(chǎn)生一組狀態(tài)數(shù)據(jù)，對這組狀態(tài)數(shù)據(jù)進(jìn)行Kalman濾波，如圖2是飛行軌跡發(fā)生器產(chǎn)生狀態(tài)數(shù)據(jù)得到的軌跡圖。

分析位置誤差需要先將位置誤差進(jìn)行放大，放大比較可以將不明顯的位置誤差變?yōu)橐子谟^察。

圖2 飛行軌跡圖

圖3是放大的誤差比較圖，從圖中很容易觀察出Kalman濾波后誤差比濾波前小很多。通過觀察圖4速度誤差比較圖，Kalman濾波[5]后速度追蹤誤差減小很多。通過對比可得出采用Kalman濾波后的導(dǎo)航系統(tǒng)精確度優(yōu)于只用INS的導(dǎo)航系統(tǒng)，綜合分析Kalman濾波整體濾波效果明顯。

圖3 位置誤差比較圖

圖4 速度誤差比較

3.2.2 自適應(yīng)Kalman濾波

自適應(yīng)Kalman濾波如圖5、6所示。

3.2.3 自適應(yīng)與非自適應(yīng)Kalman濾波比較

可以看出隨著時(shí)間的推移，自適應(yīng)Kalman濾波[6]有較少的誤差，誤差的波動(dòng)也比非自適應(yīng)小。

圖5 位置誤差比較圖

圖6 速度誤差比較

圖7 自適應(yīng)與非自適應(yīng)位置誤差比較

根據(jù)以上的仿真數(shù)據(jù)分析，采用自適應(yīng)Kalman濾波的導(dǎo)航系統(tǒng)相比于單純采用標(biāo)準(zhǔn)Kalman濾波的導(dǎo)航系統(tǒng)有更好的濾波效果。

4 結(jié)論

圖8 自適應(yīng)與非自適應(yīng)速度誤差比較

Kalman[16-17]濾波器的最優(yōu)估計(jì)不僅可以應(yīng)用于對系統(tǒng)狀態(tài)估計(jì)而且在智能控制算法中的可以利用Kalman濾波器的最優(yōu)估計(jì)參數(shù)調(diào)整加快系統(tǒng)速度。Kalman濾波算法會(huì)有更多的擴(kuò)展應(yīng)用及繼續(xù)發(fā)展，能夠擴(kuò)展的地方還有很多。標(biāo)準(zhǔn)Kalman濾波在實(shí)際使用中會(huì)存在一些局限性[7]，此時(shí)就應(yīng)當(dāng)進(jìn)行相應(yīng)的優(yōu)化，在一些特定的控制系統(tǒng)中采用自適應(yīng)Kalman濾波將會(huì)比較好的控制系統(tǒng)誤差。自適應(yīng)Kalman濾波具有很高的工程價(jià)值。

[1]韓崇昭，朱洪艷，段戰(zhàn)勝.多源信息融合[M].北京：清華大學(xué)出版社，2010.

[2]周衛(wèi)東，喬相偉.QUKF算法及其在SINS初始對準(zhǔn)中的應(yīng)用[J].儀器儀表學(xué)報(bào)，2010（2）：264-269.

[3]王甫紅.高精度星載GPS實(shí)時(shí)定軌卡爾曼濾波模型[J].武漢大學(xué)學(xué)報(bào)：信息科學(xué)版，2010，35（6）：653-656.

[4]錢華明，葛磊，彭宇.多漸消因子卡爾曼濾波及其SINS初始對準(zhǔn)中的應(yīng)用[J].中國慣性技術(shù)學(xué)報(bào)，2012，10（3）：287-291．

[5]許長輝，高井祥，王堅(jiān)．GNSS/INS組合系統(tǒng)的抗差卡爾曼濾波[J].中國礦業(yè)大學(xué)學(xué)報(bào)，2010，39（5）：773-778.

[6]徐彥田，程鵬飛，蔡艷輝，等.估計(jì)對流層延遲的單頻RTK卡爾曼濾波算法[J].測繪通報(bào)，2012（8）：4-6.

[7]宋迎春.GPS動(dòng)態(tài)導(dǎo)航定位的當(dāng)前統(tǒng)計(jì)模型與自適應(yīng)濾波[J].湖南人文科技學(xué)院學(xué)報(bào)，2010，10（5）：7-9.

[8]蘇宛新，黃春梅.自適應(yīng)Kalman濾波在SINS初始對準(zhǔn)中的應(yīng)用[J].中國慣性技術(shù)學(xué)報(bào)，2010，18（1）：44-47．

[9]孫楓，唐李軍．基于CubatureKalmanfilter的INS/GPS組合導(dǎo)航濾波算法[J].控制與決策，2012，27（7）：1032-1036.

[10]Rigatos G G.Sensor fusion for UAV navigation based onderivative-free nonlinear Kalman Filtering[C]//Proceedingsof the 2012 IEEE International Conference on Roboticsand Biomimetics，2012，12：890-895.

[11]彭惠，熊智.IMU安裝及標(biāo)度因數(shù)誤差動(dòng)態(tài)參數(shù)辨識(shí)方法[J].中國空間科學(xué)技術(shù)，2014（1）：42-49.

[12]楊玲，沈云中，樓立志.基于中位參數(shù)初值的等價(jià)權(quán)抗差估計(jì)方法[J].測繪學(xué)報(bào)，2011，40（1）：28-32.

[13]劉江，蔡伯根，唐濤，等．基于CKF的GNSS/INS列車組合定位魯棒濾波算法[J].交通運(yùn)輸工程學(xué)報(bào)，2010，10（5）：102-107．

[14]陳有榮，袁建平．一種改進(jìn)的INS/GPS組合導(dǎo)航魯棒H∞多重漸消容錯(cuò)濾波算法[J].宇航學(xué)報(bào)，2009，10（3）：930-936.

[15]鄧海峰，高井祥.簡化5級-GKF在SINS大失準(zhǔn)角初始對準(zhǔn)中的應(yīng)用[J].測繪科學(xué)技術(shù)學(xué)報(bào)，2014（5）：473-476．

[16]閆麗榮，高愛華，楊捷.諧振法測量損耗卡爾曼濾波數(shù)據(jù)處理[J].西安工業(yè)大學(xué)學(xué)報(bào)，2015（8）：603-608.

[17]楊曼，李仁忠，劉陽陽，等.基于改進(jìn)迭代匹配濾波的織物疵點(diǎn)檢測[J].西安工程大學(xué)學(xué)報(bào)，2017（3）：383-389.