消除模型誤差的卡爾曼濾波在GPS定位中的應用研究

張蘭勇，陳輝煌，孟 坤

(1.哈爾濱工程大學 自動化學院， 黑龍江 哈爾濱 150001) (2.東南大學 毫米波國家重點實驗室， 江蘇 南京 210096)

如今車載GPS定位導航已經廣泛應用。隨著技術的提高和應用需求的不斷擴大，而后出現的差分GPS(DGPS)已經能夠很好的克服偽距測量中出現的誤差，如衛星時鐘誤差、星歷誤差等，使GPS定位精度提高到分米級別。但是，DGPS需要建立基站，成本高，存在基站和移動站通信半徑限制，而且在GPS測量誤差中仍然存在著由于接收設備自身產生的熱噪聲顫動和干擾的影響。

針對卡爾曼濾波降低誤差的研究，國內外學者取得了比較顯著的成果。比如，孫罡等[1]通過改良參數和穩定模型的方法獲得靜態的精準的GPS定位；SHI E[2]通過獲得大量數據，實現動態的精準的GPS定位；王虎等[3]根據不同的情況建立不同的卡爾曼濾波器，保證了動態定位的動態性和準確性?，F有的卡爾曼濾波器要么無法適用于動態定位，要么不具有普適性。

本文在前人研究基礎上，以農用機動車輛的GPS定位為研究對象。設計卡爾曼濾波器既能適用于動態定位，又具有廣泛應用性，該濾波器同樣適用DGPS?？柭鼮V波器通過建立狀態方程和量測方程描述系統的動態過程。它具有實時數據處理，無需存儲數據以及最小無偏差性等優點，是當前應用最廣的一種動態數據處理方法[4]。

1 卡爾曼濾波器建模

卡爾曼濾波要求對應用對象數學建模，而且模型的精確程度將影響濾波結果的優劣。

本文針對農墾聯合收割機建立相應的數學模型。在農用機械實際作業時，由于機械運動速度較低，相對于GPS接收機的采樣頻率而言，通過車載的定位系統和車輛的位移變化就能夠計算出車輛的實時位置。下面建立經度方向的濾波遞推方程，緯度方向同理[5-6]。

設采樣時間為T，數學模型如下：

Xk=φk,k-1Xk-1+Wk-1

(1)

ZGPSE(k)=Xk+Vk

(2)

其中WK和Vk是零均值的白噪聲隨機向量序列，即

E{Wk}=0

(3)

Rww(k,j)=Qkδkj

(4)

E{Vk}=0

(5)

Rvv(k,j)=Rkδkj

(6)

式(1)中，Xk為k時刻車輛的狀態：Xk=[EkVk]，Ek為k時刻車輛的位置經度，Vk為k時刻車輛的向東的速度分量；φk,k-1為狀態轉移矩陣，φk,k-1=[1a]，a為車輛位移相對于位置經度的變化參數

(7)

2 卡爾曼濾波器設計

根據上述的數學模型建立卡爾曼濾波器模型。

(8)

(9)

(10)

Kk=Pk/k-1[Pk/k-1+Rk]-1

(11)

(12)

Pk/k=[I-Kk]Pk/k-1

(13)

本文利用卡爾曼濾波盡量準確地估計出系統的真實狀態，其必要前提是濾波過程具有某種穩定性，從而證明實際的濾波器有效[7]。由于模型可能存在錯誤，僅根據建立的系統狀態模型是無法判斷實際的濾波狀態估計是否正確，因此只能根據系統的量測估計誤差來間接地檢驗這種濾波的有效性[8]。為此，本文從實際的濾波效果出發定義這種特性。

量級上與正常量測值相差很大或者誤差超越了正常狀態所允許的誤差范圍的，工程上稱之為野值。必須先將這些野值剔除掉，否則會給數據處理帶來很大的誤差。下面討論野值的自動剔除方法。

設對狀態Xk的測量值為Z1，Z2，…，Zk-1，…，預測值為Xk/k-1，預測殘差為dk，有：

dk=ZK-XK/K-1

(14)

其協方差矩陣為：

(15)

(16)

(17)

式(17)中，Pk/k-1為預測誤差協方差矩陣；RK為量測噪聲協方差矩陣。利用預測殘差的統計性質可對ZK的每個分量進行判別，判別式為：

(18)

式(18)中，Rk(i,i)表示對角線上的第i個元素；dk(i)表示dK的第i個分量，C為常量，可取3或4。如果上式成立，判別Zk(i)為正確的測量；反之，判別Zk(i)為野值，其中Zk(i)為Zk的第i個分量。在遞推濾波過程中，野值進行自動剔除。其基本思想是：當判別Zk(i)為野值時，在遞推濾波求得增益矩陣Kk后，在軟件中自動把Kk的第i行元素全部沖為零，計算濾波值Xk/k和濾波誤差協方差矩陣Pk/k的公式不變[9]。

此方法在應用過程中最重要的特點就是快速。在實際應用中可以用到穩態濾波，將Pk/k和Pk/k-1取為固定值，并利用軟件程序自動剔除野值點[10]。

圖1 卡爾曼濾波算法流程

在準確知道數學模型基礎上，可以采用以上模型。然而實際情況很難準確知道，因此濾波的效果會出現瑕疵。于是提出另一種卡爾曼濾波，能夠消除模型誤差以增強濾波的效果[11]。

對系統的非線性高斯特性進行分析，即系統噪聲和測量噪聲為不相關的高斯白噪聲。但實際GPS測量中設備受到自身產生的熱噪聲以及環境的影響[12]。而基于空間的卡爾曼濾波要求線性系統，所以本文的自適應卡爾曼濾波利用非線性函數一階泰勒級數展開思想，通過求運動過程方程和運動觀測過程的偏導數觀測數據，實現非線性系統方程近似的濾波算法，滿足系統的線性要求，再利用卡爾曼濾波進行處理。

3 自適應卡爾曼濾波及仿真

針對數學建模中存在誤差，本部分主要討論消除誤差方法。結合上述提出的數學模型，由于車輛在機動狀態下噪聲干擾可能是有色噪聲，而非理想的白噪聲；再者，勻速的數學模型在動態GPS定位中未必能夠表達車輛的狀態等[13]?；诖耍疚囊敫倪M措施：通過引進虛擬噪聲，把問題歸結為帶未知變噪聲統計系統的卡爾曼濾波自適應濾波問題。

假定第一節的數學模型式(1)、式(2)含有未知的模型誤差Δφk,k-1和ΔHk，即真實系統的數學模型為

Xk=(φk,k-1+Δφk,k-1)Xk-1+Wk

(19)

ZGPSE(k)=(I+ΔHk)Xk+Vk

(20)

令

ηk-1=Δφk,k-1Xk-1+Wk-1

(21)

εk=ΔHkXk+Vk

(22)

稱ηk-1和εk為虛擬噪聲，其均值和協方差陣是時變的，即

Eηk=μη(k)

cov(ηk,ηj)=Q(k)δkj

Eεk=με(k)

(23)

cov(εk,εj)=R(k)δkj

(24)

按照文獻[4]中介紹的方法推導，得出自適應卡爾曼濾波公式為

(25)

(26)

(27)

(28)

(29)

(30)

(31)

(32)

(33)

式中，dk=(1-b)/(1-bk+1)，0

對于模型中的誤差Δφk,k-1Xk-1，一般考慮是不穩定的加速度模型。仿真中將一定范圍內隨機的加速度值加入到模型中，為了不增加狀態的維數模型，將這一部分算為虛擬誤差，由于虛擬誤差是時變的，事先不可知，因此采用上述的自適應卡爾曼濾波算法將噪聲的協方差和期望實時算出。

消除模型誤差的卡爾曼濾波與一般卡爾曼濾波[14]的最大不同在于：算法中需要計算動態誤差的期望和方差，它們不再是常值矩陣。當然，這種濾波方法也可用于時變噪聲的濾波問題中。

基于以上的研究，本文使用軟件建模仿真。包括一般卡爾曼濾波算法和消除誤差的自適應卡爾曼濾波算法。仿真結果如圖2，圖3所示。

圖2 一般卡爾曼濾波仿真曲線

圖3 自適應濾波仿真曲線

圖2是一般卡爾曼濾波算法的仿真曲線。圖中虛線是根據數學模型計算的真實值，實線是由濾波算法得到的估計值。估計誤差等于真實值與估計值的差。仿真結果表明，在200s的仿真時間內，位置誤差在[-2,2]之間。卡爾曼濾波遞推算法能夠比較精確的確定模型的位置，有效的去除了隨機噪聲。

圖3是消除誤差的卡爾曼濾波算法仿真曲線。卡爾曼濾波的推算結果與數學模型的準確性有關，而數學模型的建立存在誤差。為了更進一步提高估計值的精度，將誤差模型考慮在內并進行了仿真。為進一步證明本文自適應卡爾曼濾波的有效性，將本文方法和一般卡爾曼濾波以及改進卡爾曼濾波仿真結果進行對比。從圖3可見，200 s的自適應濾波后誤差在[-1,1]之間，與一般濾波相比，誤差減小、濾波效果加強。

表1 卡爾曼濾波和自適應卡爾曼濾波仿真誤差結果 %

將仿真結果進行統計得表1，可以看到各誤差段所占時間比。一般卡爾曼濾波在微小誤差段0～±0.2 m所占比較低，而本文的自適應卡爾曼濾波的微小誤差段所占比例極大增加，改進卡爾曼濾波在一定程度上縮小了誤差，但效果明顯不如自適應卡爾曼濾波。同時，一般卡爾曼濾波誤差較大的所占時間較多，而自適應卡爾曼濾波基本消除了大誤差，誤差僅為一般卡爾曼濾波器誤差的34%。

經過200 s仿真證明上述選用的自適應濾波與GPS定位組合，能夠除去噪聲干擾并解決模型參數漂移引起的誤差，將定位精度進一步提高。運用單點GPS無法消除偽距誤差，所以仿真結果中定位精度不理想，若能夠與DGPS聯合使用，效果將更加明顯。在實際應用中收割機的運動速度緩慢，完全能夠達到實時性的要求。

4 結論

本文將GPS定位與卡爾曼濾波結合，建立車輛無規則運動數學模型。不僅降低了模型維數，在保證可觀性的同時，降低了算法的復雜度，提高實時性和降低了成本。而在算法方面，首先采用一般的濾波器算法和防止發散的方法仿真，其次針對卡爾曼濾波應用的實際問題，引入消除誤差模型的卡爾曼濾波，進一步提高了定位精度，與以往采用的卡爾曼濾波相比，本文方法在去除誤差方面給出了更加有效，對于農業車輛的定位已經足夠。這樣不僅降低了車輛導航定位的成本，而且增強了系統的實用性。

[1] 孫罡,王昌明,張愛軍.GPS靜態單點定位的濾波算法比較[J].南京理工大學學報(自然科學版),2011,35(1):80-85.

[2] SHI E.An improved real-time adaptive Kalman filter for low-cost integrated GPS/INS navigation[C]// International Conference on Measurement,Information and Control.IEEE,2012:1093-1098.

[3] 王虎,王解先,白貴霞,等.改進的漸消卡爾曼濾波在GPS動態定位中的應用[J].同濟大學學報(自然科學版),2011,39(1):124-128.

[4] 胡振濤,胡玉梅,劉先省.量測提升卡爾曼濾波[J].電子學報,2016,44(5):1149-1155.

[5] LIU Y,GUO S,HOU R.Research on Kalman Filter in Error Processing of GPS[J].Electronic Science & Technology,2011.

[6] FAKHARIAN A,GUSTAFSSON T.Adaptive Kalman filtering based navigation[C]// IEEE International Conference on Networking,Sensing and Control.2015:181-185.

[7] 趙欣,王仕成,廖守億,等.基于抗差自適應容積卡爾曼濾波的超緊耦合跟蹤方法[J].自動化學報,2014,40(11):2530-2540.

[8] 劉勝,張青春,張蘭勇.聚類算法的GPS靜態單點定位方法[J].哈爾濱工業大學學報,2012,44(11):71-74.

[9] 王忠,游志勝,杜傳利,等.GPS/INS動態卡爾曼濾波優化算法[J].四川大學學報(工程科學版),2006,38(4):141-144.

[10] ZHOU Z,LI Y,LIU J,et al.Equality Constrained Robust Measurement Fusion for Adaptive Kalman-Filter-Based Heterogeneous Multi-Sensor Navigation[J].IEEE Transactions on Aerospace &Electronic Systems,2013,49(4):2146-2157.

[11] CARVALHO J P D,ASSAD E D,PINTO H S.Kalman filter and correction of the temperatures estimated by PRECIS model[J].Atmospheric Research,2011,102(1):218-226.

[12] 葛磊.容積卡爾曼濾波算法研究及其在導航中的應用[D].哈爾濱工程大學,2013.

[13] BARRIOS C,MOTAI Y.Improving Estimation of Vehicle’s Trajectory Using the Latest Global Positioning System With Kalman Filtering[J].IEEE Transactions on Instrumentation & Measurement,2011,60(12):3747-3755.

[14] 張海,常艷紅,車歡.基于GPS/INS不同測量特性的自適應卡爾曼濾波算法[J].中國慣性技術學報,2010,18(6):696-701.

[15] 胡輝，彭雄明，楊德進，等.新的自適應漸消擴展卡爾曼濾波在GPS定位中的應用[J].火力與指揮控制，2016(3):177-182.