基于自適應卡爾曼濾波的GPS/INS位置組合導航

許根源，王直，王志強

（江蘇科技大學電子與信息學院，江蘇鎮江212003）

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

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

1 自適應Kalman濾波

1.1 Kalman濾波方程

設控制對象的離散模型為：

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

一步預測方程：

一步預測均方誤差方程：

狀態估值計算方程：

其中，K（k）為狀態估計器或Ka1man濾波增益矩陣是x（k）的一步最優預報估計是 關 于y（k）的 一 步 預 報 估 計 ，是 關 于y（k）的 一 步預報估計誤差。為了獲得x（k）最小狀態估計值，令狀態估計誤差的協方差最小，則有：

濾波增益方程：

上列式（2）～（6）構成了Ka1man濾波遞推公式。

1.2 自適應卡爾曼

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

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

v（k）的實測方差為：

可得到濾波嚴格收斂判據為[5]：

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

2 狀態、觀測方程的建立

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

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

INS/GPS組合模型的逼近線性離散時間系統如下：

式中，Xk和Zk分別為被估計的狀態向量和觀測向量：Φk,k-1和Hk為狀態轉移矩陣和觀測矩陣；Wk為導航系統的過程噪聲，Vk為導航系統的量測噪聲，過程噪聲和量測噪聲是相互獨立的，同時是帶時變均值和協方差的正態白噪聲序列。

其中，q(k)、Q(k)為導航系統的過程噪聲，Wk時變均值和協方差，r(k)、R(k)為導航系統的量測噪聲，Vk時變均值和協方差。

導航系統多為多傳感器組合導航系統，為了便于對導航系統做深入的研究，導航基準坐標系采用東、北、天坐標系，用此坐標系作為車輛導航定位[3]的基準坐標系。在分析研究INS及GPS組合導航系統的性能及誤差數據時，取15維狀態矩陣、9維觀測矩陣，并采用位置、姿態組合觀測[6-7]：

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

3 算例仿真

3.1 仿真流程圖

仿真流程圖如圖1所示。

圖1 仿真流程圖

3.2 仿真結果

3.2.1 普通Kalman濾波

根據仿真分析先利用飛行軌跡發生器產生一組狀態數據，對這組狀態數據進行Kalman濾波，如圖2是飛行軌跡發生器產生狀態數據得到的軌跡圖。

分析位置誤差需要先將位置誤差進行放大，放大比較可以將不明顯的位置誤差變為易于觀察。

圖2 飛行軌跡圖

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

圖3 位置誤差比較圖

圖4 速度誤差比較

3.2.2 自適應Kalman濾波

自適應Kalman濾波如圖5、6所示。

3.2.3 自適應與非自適應Kalman濾波比較

可以看出隨著時間的推移，自適應Kalman濾波[6]有較少的誤差，誤差的波動也比非自適應小。

圖5 位置誤差比較圖

圖6 速度誤差比較

圖7 自適應與非自適應位置誤差比較

根據以上的仿真數據分析，采用自適應Kalman濾波的導航系統相比于單純采用標準Kalman濾波的導航系統有更好的濾波效果。

4 結論

圖8 自適應與非自適應速度誤差比較

Kalman[16-17]濾波器的最優估計不僅可以應用于對系統狀態估計而且在智能控制算法中的可以利用Kalman濾波器的最優估計參數調整加快系統速度。Kalman濾波算法會有更多的擴展應用及繼續發展，能夠擴展的地方還有很多。標準Kalman濾波在實際使用中會存在一些局限性[7]，此時就應當進行相應的優化，在一些特定的控制系統中采用自適應Kalman濾波將會比較好的控制系統誤差。自適應Kalman濾波具有很高的工程價值。

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

[2]周衛東，喬相偉.QUKF算法及其在SINS初始對準中的應用[J].儀器儀表學報，2010（2）：264-269.

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

[4]錢華明，葛磊，彭宇.多漸消因子卡爾曼濾波及其SINS初始對準中的應用[J].中國慣性技術學報，2012，10（3）：287-291．

[5]許長輝，高井祥，王堅．GNSS/INS組合系統的抗差卡爾曼濾波[J].中國礦業大學學報，2010，39（5）：773-778.

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

[7]宋迎春.GPS動態導航定位的當前統計模型與自適應濾波[J].湖南人文科技學院學報，2010，10（5）：7-9.

[8]蘇宛新，黃春梅.自適應Kalman濾波在SINS初始對準中的應用[J].中國慣性技術學報，2010，18（1）：44-47．

[9]孫楓，唐李軍．基于CubatureKalmanfilter的INS/GPS組合導航濾波算法[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安裝及標度因數誤差動態參數辨識方法[J].中國空間科學技術，2014（1）：42-49.

[12]楊玲，沈云中，樓立志.基于中位參數初值的等價權抗差估計方法[J].測繪學報，2011，40（1）：28-32.

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

[14]陳有榮，袁建平．一種改進的INS/GPS組合導航魯棒H∞多重漸消容錯濾波算法[J].宇航學報，2009，10（3）：930-936.

[15]鄧海峰，高井祥.簡化5級-GKF在SINS大失準角初始對準中的應用[J].測繪科學技術學報，2014（5）：473-476．

[16]閆麗榮，高愛華，楊捷.諧振法測量損耗卡爾曼濾波數據處理[J].西安工業大學學報，2015（8）：603-608.

[17]楊曼，李仁忠，劉陽陽，等.基于改進迭代匹配濾波的織物疵點檢測[J].西安工程大學學報，2017（3）：383-389.