張?zhí)m勇，陳輝煌，孟 坤

(1.哈爾濱工程大學(xué) 自動(dòng)化學(xué)院， 黑龍江 哈爾濱 150001) (2.東南大學(xué) 毫米波國(guó)家重點(diǎn)實(shí)驗(yàn)室， 江蘇 南京 210096)

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

針對(duì)卡爾曼濾波降低誤差的研究，國(guó)內(nèi)外學(xué)者取得了比較顯著的成果。比如，孫罡等[1]通過(guò)改良參數(shù)和穩(wěn)定模型的方法獲得靜態(tài)的精準(zhǔn)的GPS定位；SHI E[2]通過(guò)獲得大量數(shù)據(jù)，實(shí)現(xiàn)動(dòng)態(tài)的精準(zhǔn)的GPS定位；王虎等[3]根據(jù)不同的情況建立不同的卡爾曼濾波器，保證了動(dòng)態(tài)定位的動(dòng)態(tài)性和準(zhǔn)確性?，F(xiàn)有的卡爾曼濾波器要么無(wú)法適用于動(dòng)態(tài)定位，要么不具有普適性。

本文在前人研究基礎(chǔ)上，以農(nóng)用機(jī)動(dòng)車(chē)輛的GPS定位為研究對(duì)象。設(shè)計(jì)卡爾曼濾波器既能適用于動(dòng)態(tài)定位，又具有廣泛應(yīng)用性，該濾波器同樣適用DGPS?？柭鼮V波器通過(guò)建立狀態(tài)方程和量測(cè)方程描述系統(tǒng)的動(dòng)態(tài)過(guò)程。它具有實(shí)時(shí)數(shù)據(jù)處理，無(wú)需存儲(chǔ)數(shù)據(jù)以及最小無(wú)偏差性等優(yōu)點(diǎn)，是當(dāng)前應(yīng)用最廣的一種動(dòng)態(tài)數(shù)據(jù)處理方法[4]。

1 卡爾曼濾波器建模

卡爾曼濾波要求對(duì)應(yīng)用對(duì)象數(shù)學(xué)建模，而且模型的精確程度將影響濾波結(jié)果的優(yōu)劣。

本文針對(duì)農(nóng)墾聯(lián)合收割機(jī)建立相應(yīng)的數(shù)學(xué)模型。在農(nóng)用機(jī)械實(shí)際作業(yè)時(shí)，由于機(jī)械運(yùn)動(dòng)速度較低，相對(duì)于GPS接收機(jī)的采樣頻率而言，通過(guò)車(chē)載的定位系統(tǒng)和車(chē)輛的位移變化就能夠計(jì)算出車(chē)輛的實(shí)時(shí)位置。下面建立經(jīng)度方向的濾波遞推方程，緯度方向同理[5-6]。

設(shè)采樣時(shí)間為T(mén)，數(shù)學(xué)模型如下：

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

(1)

ZGPSE(k)=Xk+Vk

(2)

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

E{Wk}=0

(3)

Rww(k,j)=Qkδkj

(4)

E{Vk}=0

(5)

Rvv(k,j)=Rkδkj

(6)

式(1)中，Xk為k時(shí)刻車(chē)輛的狀態(tài)：Xk=[EkVk]，Ek為k時(shí)刻車(chē)輛的位置經(jīng)度，Vk為k時(shí)刻車(chē)輛的向東的速度分量；φk,k-1為狀態(tài)轉(zhuǎn)移矩陣，φk,k-1=[1a]，a為車(chē)輛位移相對(duì)于位置經(jīng)度的變化參數(shù)

(7)

2 卡爾曼濾波器設(shè)計(jì)

根據(jù)上述的數(shù)學(xué)模型建立卡爾曼濾波器模型。

(8)

(9)

(10)

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

(11)

(12)

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

(13)

本文利用卡爾曼濾波盡量準(zhǔn)確地估計(jì)出系統(tǒng)的真實(shí)狀態(tài)，其必要前提是濾波過(guò)程具有某種穩(wěn)定性，從而證明實(shí)際的濾波器有效[7]。由于模型可能存在錯(cuò)誤，僅根據(jù)建立的系統(tǒng)狀態(tài)模型是無(wú)法判斷實(shí)際的濾波狀態(tài)估計(jì)是否正確，因此只能根據(jù)系統(tǒng)的量測(cè)估計(jì)誤差來(lái)間接地檢驗(yàn)這種濾波的有效性[8]。為此，本文從實(shí)際的濾波效果出發(fā)定義這種特性。

量級(jí)上與正常量測(cè)值相差很大或者誤差超越了正常狀態(tài)所允許的誤差范圍的，工程上稱(chēng)之為野值。必須先將這些野值剔除掉，否則會(huì)給數(shù)據(jù)處理帶來(lái)很大的誤差。下面討論野值的自動(dòng)剔除方法。

設(shè)對(duì)狀態(tài)Xk的測(cè)量值為Z1，Z2，…，Zk-1，…，預(yù)測(cè)值為Xk/k-1，預(yù)測(cè)殘差為dk，有：

dk=ZK-XK/K-1

(14)

其協(xié)方差矩陣為：

(15)

(16)

(17)

式(17)中，Pk/k-1為預(yù)測(cè)誤差協(xié)方差矩陣；RK為量測(cè)噪聲協(xié)方差矩陣。利用預(yù)測(cè)殘差的統(tǒng)計(jì)性質(zhì)可對(duì)ZK的每個(gè)分量進(jìn)行判別，判別式為：

(18)

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

此方法在應(yīng)用過(guò)程中最重要的特點(diǎn)就是快速。在實(shí)際應(yīng)用中可以用到穩(wěn)態(tài)濾波，將Pk/k和Pk/k-1取為固定值，并利用軟件程序自動(dòng)剔除野值點(diǎn)[10]。

圖1 卡爾曼濾波算法流程

在準(zhǔn)確知道數(shù)學(xué)模型基礎(chǔ)上，可以采用以上模型。然而實(shí)際情況很難準(zhǔn)確知道，因此濾波的效果會(huì)出現(xiàn)瑕疵。于是提出另一種卡爾曼濾波，能夠消除模型誤差以增強(qiáng)濾波的效果[11]。

對(duì)系統(tǒng)的非線性高斯特性進(jìn)行分析，即系統(tǒng)噪聲和測(cè)量噪聲為不相關(guān)的高斯白噪聲。但實(shí)際GPS測(cè)量中設(shè)備受到自身產(chǎn)生的熱噪聲以及環(huán)境的影響[12]。而基于空間的卡爾曼濾波要求線性系統(tǒng)，所以本文的自適應(yīng)卡爾曼濾波利用非線性函數(shù)一階泰勒級(jí)數(shù)展開(kāi)思想，通過(guò)求運(yùn)動(dòng)過(guò)程方程和運(yùn)動(dòng)觀測(cè)過(guò)程的偏導(dǎo)數(shù)觀測(cè)數(shù)據(jù)，實(shí)現(xiàn)非線性系統(tǒng)方程近似的濾波算法，滿(mǎn)足系統(tǒng)的線性要求，再利用卡爾曼濾波進(jìn)行處理。

3 自適應(yīng)卡爾曼濾波及仿真

針對(duì)數(shù)學(xué)建模中存在誤差，本部分主要討論消除誤差方法。結(jié)合上述提出的數(shù)學(xué)模型，由于車(chē)輛在機(jī)動(dòng)狀態(tài)下噪聲干擾可能是有色噪聲，而非理想的白噪聲；再者，勻速的數(shù)學(xué)模型在動(dòng)態(tài)GPS定位中未必能夠表達(dá)車(chē)輛的狀態(tài)等[13]?；诖?，本文引入改進(jìn)措施：通過(guò)引進(jìn)虛擬噪聲，把問(wèn)題歸結(jié)為帶未知變?cè)肼暯y(tǒng)計(jì)系統(tǒng)的卡爾曼濾波自適應(yīng)濾波問(wèn)題。

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

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)

稱(chēng)ηk-1和εk為虛擬噪聲，其均值和協(xié)方差陣是時(shí)變的，即

Eηk=μη(k)

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

Eεk=με(k)

(23)

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

(24)

按照文獻(xiàn)[4]中介紹的方法推導(dǎo)，得出自適應(yīng)卡爾曼濾波公式為

(25)

(26)

(27)

(28)

(29)

(30)

(31)

(32)

(33)

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

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

消除模型誤差的卡爾曼濾波與一般卡爾曼濾波[14]的最大不同在于：算法中需要計(jì)算動(dòng)態(tài)誤差的期望和方差，它們不再是常值矩陣。當(dāng)然，這種濾波方法也可用于時(shí)變?cè)肼暤臑V波問(wèn)題中。

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

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

圖3 自適應(yīng)濾波仿真曲線

圖2是一般卡爾曼濾波算法的仿真曲線。圖中虛線是根據(jù)數(shù)學(xué)模型計(jì)算的真實(shí)值，實(shí)線是由濾波算法得到的估計(jì)值。估計(jì)誤差等于真實(shí)值與估計(jì)值的差。仿真結(jié)果表明，在200s的仿真時(shí)間內(nèi)，位置誤差在[-2,2]之間?？柭鼮V波遞推算法能夠比較精確的確定模型的位置，有效的去除了隨機(jī)噪聲。

圖3是消除誤差的卡爾曼濾波算法仿真曲線?？柭鼮V波的推算結(jié)果與數(shù)學(xué)模型的準(zhǔn)確性有關(guān)，而數(shù)學(xué)模型的建立存在誤差。為了更進(jìn)一步提高估計(jì)值的精度，將誤差模型考慮在內(nèi)并進(jìn)行了仿真。為進(jìn)一步證明本文自適應(yīng)卡爾曼濾波的有效性，將本文方法和一般卡爾曼濾波以及改進(jìn)卡爾曼濾波仿真結(jié)果進(jìn)行對(duì)比。從圖3可見(jiàn)，200 s的自適應(yīng)濾波后誤差在[-1,1]之間，與一般濾波相比，誤差減小、濾波效果加強(qiáng)。

表1 卡爾曼濾波和自適應(yīng)卡爾曼濾波仿真誤差結(jié)果 %

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

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

4 結(jié)論

本文將GPS定位與卡爾曼濾波結(jié)合，建立車(chē)輛無(wú)規(guī)則運(yùn)動(dòng)數(shù)學(xué)模型。不僅降低了模型維數(shù)，在保證可觀性的同時(shí)，降低了算法的復(fù)雜度，提高實(shí)時(shí)性和降低了成本。而在算法方面，首先采用一般的濾波器算法和防止發(fā)散的方法仿真，其次針對(duì)卡爾曼濾波應(yīng)用的實(shí)際問(wèn)題，引入消除誤差模型的卡爾曼濾波，進(jìn)一步提高了定位精度，與以往采用的卡爾曼濾波相比，本文方法在去除誤差方面給出了更加有效，對(duì)于農(nóng)業(yè)車(chē)輛的定位已經(jīng)足夠。這樣不僅降低了車(chē)輛導(dǎo)航定位的成本，而且增強(qiáng)了系統(tǒng)的實(shí)用性。

[1] 孫罡,王昌明,張愛(ài)軍.GPS靜態(tài)單點(diǎn)定位的濾波算法比較[J].南京理工大學(xué)學(xué)報(bào)(自然科學(xué)版),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] 王虎,王解先,白貴霞,等.改進(jìn)的漸消卡爾曼濾波在GPS動(dòng)態(tài)定位中的應(yīng)用[J].同濟(jì)大學(xué)學(xué)報(bào)(自然科學(xué)版),2011,39(1):124-128.

[4] 胡振濤,胡玉梅,劉先省.量測(cè)提升卡爾曼濾波[J].電子學(xué)報(bào),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] 趙欣,王仕成,廖守億,等.基于抗差自適應(yīng)容積卡爾曼濾波的超緊耦合跟蹤方法[J].自動(dòng)化學(xué)報(bào),2014,40(11):2530-2540.

[8] 劉勝,張青春,張?zhí)m勇.聚類(lèi)算法的GPS靜態(tài)單點(diǎn)定位方法[J].哈爾濱工業(yè)大學(xué)學(xué)報(bào),2012,44(11):71-74.

[9] 王忠,游志勝,杜傳利,等.GPS/INS動(dòng)態(tài)卡爾曼濾波優(yōu)化算法[J].四川大學(xué)學(xué)報(bào)(工程科學(xué)版),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ǎo)航中的應(yīng)用[D].哈爾濱工程大學(xué),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] 張海,常艷紅,車(chē)歡.基于GPS/INS不同測(cè)量特性的自適應(yīng)卡爾曼濾波算法[J].中國(guó)慣性技術(shù)學(xué)報(bào),2010,18(6):696-701.

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