基于kalman濾波的智能小車GPS軌跡跟蹤①

(1.中國科學(xué)技術(shù)大學(xué)信息學(xué)院，安徽 合肥 230026 2.安徽財貿(mào)職業(yè)學(xué)院 云桂信息學(xué)院，安徽 合肥 230601)

0 引 言

全球定位系統(tǒng)(GPS)是一種全天候空間基準(zhǔn)的導(dǎo)航系統(tǒng)，可滿足位于全球任何地方精確三位導(dǎo)航與定位的新一代導(dǎo)航和定位系統(tǒng)，有著廣泛的應(yīng)用領(lǐng)域。智能小車GPS軌跡跟蹤是將GPS接收機(jī)安裝在運(yùn)動目標(biāo)(智能小車)上就可以進(jìn)行導(dǎo)航跟蹤和定位。通過GPS接收機(jī)實時接收在軌導(dǎo)航衛(wèi)星播發(fā)的信號，算出接受載體(智能小車)的位置和速度。對于民用領(lǐng)域GPS的時鐘信號認(rèn)為加入了高頻振蕩隨機(jī)干擾信號，該干擾信號可看作GPS定位的觀測噪聲，致使所有派生的衛(wèi)星信號均產(chǎn)生高頻的抖動[1]。為提高智能小車的GPS軌跡跟蹤精度，采用了卡爾曼濾波器對智能小車GPS軌跡跟蹤速度以及位置的觀測信號進(jìn)行最優(yōu)狀態(tài)估計。

1 卡爾曼濾波(The Kalman Filter Algorithm)

kalman濾波是一種利用線性系統(tǒng)狀態(tài)方程，是通過系統(tǒng)輸入輸出觀測數(shù)據(jù)，對系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計的算法[2～3]，由于觀測數(shù)據(jù)中包括系統(tǒng)中的噪聲和干擾的影響，其最優(yōu)估計可看作為濾波過程[4]，其線性隨機(jī)微分方程描述如下：

x(k)=Ax(k-1)+B(u(k)+w(k))Z(k)=

Hx(k)+v(k)

上式中，x(k)、z(k)、u(k)分別為k時刻的系統(tǒng)狀態(tài)、測量值和k時刻對系統(tǒng)的控制量[5]，A、B為系統(tǒng)參數(shù)，H為測量系統(tǒng)參數(shù)，對于多模型系統(tǒng)和測量系統(tǒng)，A、B、H均為矩陣[6]，w(k) 、v(k)分別為過程控制干擾噪聲和測量噪聲，對于線性隨機(jī)微分系統(tǒng)，測量和過程都是高斯白噪聲(White Gaussian Noise)，它們的covariance 分別為Q和R。采用系統(tǒng)的過程模型來預(yù)測下一狀態(tài)的系統(tǒng)?，F(xiàn)假設(shè)系統(tǒng)的狀態(tài)是k，根據(jù)系統(tǒng)的模型，可以基于系統(tǒng)的上一狀態(tài)而預(yù)測出現(xiàn)在的狀態(tài)[7]：

x(k|k-1)=Ax(k-1|k-1)+Bu(k)

(1)

(1)式中x(k|k-1)是利用上一狀態(tài)預(yù)測結(jié)果，x(k-1|k-1)是上一狀態(tài)最優(yōu)結(jié)果，u(k)為現(xiàn)在狀態(tài)的控制量，現(xiàn)在系統(tǒng)結(jié)果已經(jīng)更新了，但對應(yīng)于x(k|k-1)的covariance還沒更新，用p表示covariance：

p(k|k-1)=Ap(k-1|k-1)A′+Q

(2)

(2) 式中Q是系統(tǒng)過程的covariance，p(k|k-1)和p(k-1|k-1)分別是x(k|k-1)和x(k-1|k-1)對應(yīng)的covariance，A′表示A的轉(zhuǎn)置矩陣。式(1)，(2)是對系統(tǒng)的預(yù)測，有了當(dāng)前狀態(tài)的預(yù)測值，再收集當(dāng)前狀態(tài)的測量值，結(jié)合測量值和預(yù)測值，就可得到當(dāng)前狀態(tài)(k)的最優(yōu)化估算值x(k|k)：

x(k|k)=x(k|k-1)+Kg(k) (z(k)-Hx(k|k-1))

(3)

其中Kg為Kalman增益( Gain)：

Kg(k)=P(k|k-1)H，/ (HP(k|k-1)H，+R)

(4)

為了使Kalman濾波器能不斷地運(yùn)行下去，直至系統(tǒng)過程結(jié)束，就要不斷更新k狀態(tài)下x(k|k)的covariance:

p(k|k)=(I-Kg(k)H)P(k|k-1)

(5)

(5)式中I為1的矩陣，當(dāng)系統(tǒng)進(jìn)入k+1狀態(tài)時，P(k|k)就是式子(2)的P(k-1|k-1)。這樣，算法就可以自回歸的運(yùn)算下去。根據(jù)公式(1)、(2)、(3)、(4)、(5)，通過matlab軟件編寫程序?qū)崿F(xiàn)正弦信號rk=sin(2*pi*0.1/100.*(0∶N-1))在干擾信號noise=(rand(1,N)-0.5)*R^0.5下的濾波效果如圖1、圖2所示。對比圖1、圖2可見經(jīng)過kalman濾波后波形更加真實。

圖1 濾波前

圖2 濾波后

2 智能小車GPS導(dǎo)航定位

2.1 智能車導(dǎo)航原理如圖3所示：

已知目標(biāo)經(jīng)緯度(x2,y2)和無人車經(jīng)緯度(x1,y1)，兩點(diǎn)間距離至N極夾角為

而車輛通過獲取磁傳感器數(shù)據(jù)，也會有一個與N極的夾角β，即車頭與N極夾角。通過計算二者角度差θ=β-α，從而計算出控制轉(zhuǎn)向舵機(jī)應(yīng)該輸出的角度。

圖3 導(dǎo)航原理圖

這其中應(yīng)該有四種可能。如圖3-1，當(dāng)θ>π時，車頭在N極左側(cè)，目標(biāo)位置在N極右側(cè)，車輪應(yīng)該右轉(zhuǎn)|π-θ|。

當(dāng)0<θπ時，車頭和目標(biāo)位置都在N極右側(cè)，車頭與N極夾角更大，這時車輪應(yīng)該左轉(zhuǎn)θ。

當(dāng)-π<θ0時，車頭和目標(biāo)位置都在N極右側(cè)，目標(biāo)位置與N極夾角更大，這時車輪應(yīng)該右轉(zhuǎn)|θ|。

如圖3-2，當(dāng)θ-π時，車頭在N極右側(cè)，目標(biāo)位置在N極左側(cè)，這時車輪應(yīng)該左轉(zhuǎn)π-|θ|。程序流程圖如下：

2.2 GPS是一種利用時間測距的定位系統(tǒng)

其定位的基本原理如圖4所示:

圖4 衛(wèi)星與物體幾何關(guān)系

如圖4所示，設(shè)物體所在點(diǎn)P到衛(wèi)星Si的距離為dpi，如衛(wèi)星時鐘與接收機(jī)時鐘完全同步，則有如下方程：

dpi=C(tpr-tsv)-CτA

(6)

式中C為光速，tpr為地面接收機(jī)同步觀測的時刻，tsv為衛(wèi)星Si同步發(fā)射的時刻，τA為傳播過程中的附加延時。因為在傳播過程中衛(wèi)星時鐘和接收機(jī)在實際應(yīng)用中不能達(dá)到完全同步，所以，實測距離并非實際距離，稱為偽距[8]，記為ρpi，并且有:

ρpi=C(tpri-tsvi)

(7)

式中:tpri、tsvi分別為含鐘差的地面物體接收機(jī)的觀測時刻和含鐘差的衛(wèi)星Si的發(fā)射時刻， 其中：

tpri=tpr+δtpri

(8)

tsvi=tsv+δtsvi

(9)

式中:δtprit、δtsvi-分別是地面接收機(jī)的鐘差和衛(wèi)星si發(fā)射時刻的鐘差，

則有：

ρpi=C(tpr-tsv)+C(δtpri-δtsvi)=

dpi+CτA+C(δtpri-δtsvi)=dpi+dpi

(10)

式中:δdpi為測距誤差。

另一方面，接收機(jī)可通過衛(wèi)星星歷計算出Si點(diǎn)的坐標(biāo)(Xsi，Ysi，Zsi)，這里，設(shè)用戶位置p點(diǎn)的坐標(biāo)為(Xp,Yp,Zp)，則有:

(11)

將式(11 )代入式(10)

+δdpi

(12)

其中，Xp，Yp，Zp和δdpi為未知數(shù)，因此，若同時觀測到4顆不同衛(wèi)星到P點(diǎn)的偽距值ρpi(i=1，2，3，4)，根據(jù)式( 2-7 )導(dǎo)出的四個方程聯(lián)立成方程組，即可求出各未知數(shù)，從而求出P點(diǎn)的坐標(biāo)(Xp，Yp，Zp)。

3 狀態(tài)方程建立

為了簡化模型，假定智能小車GPS沿直線方向運(yùn)動，設(shè)采樣時間為T0， GPS在采樣時刻處時的真實值用kT0表示，用y(k)表示T0時刻處的GPS觀測值，則測量模型描述如下：

y(k)=s(k)+v(k)

(13)

s(k+1)=s(k)+s(k)T0+0.5T02a(k)

a(k)=u(k)+w(k)

觀測方程為：

即系統(tǒng)的狀態(tài)空間方程為

x(k+1)=Φx(k)+Bu(k)+Гw(k)

y(k)=Hx(k)+v(k)

式中

4 仿真結(jié)果

設(shè)定智能小車在二維水平上運(yùn)動，初始位置設(shè)置為(100m，200m)，水平運(yùn)動速度為2m/s，垂直速度為2m/s，GPS接收機(jī)的掃描周期為T=1s，觀測噪聲的均值為0，方差為100。過程噪聲delta_w取值越小，目標(biāo)越接近勻速直線運(yùn)動，反之，則越接近于曲線運(yùn)動[10]，在matlab 軟件環(huán)境下仿真結(jié)果如下圖5、圖6所：

圖5 跟蹤軌跡圖

當(dāng)過程噪聲選取為：delta_w=1e-4，Q= delta_w *diag([0.5,1,0.5,1])，得到跟蹤軌跡如圖5所示；當(dāng)過程噪聲選取為：delta_w=0.1;，Q= delta_w *diag([0.5,1,0.5,1])，得到跟蹤軌跡如圖6所示：

當(dāng)過程噪聲選取為：delta_w=1e-4;Q= delta_w *diag([0.5,1,0.5,1])時得到跟蹤誤差圖，如圖7所示；當(dāng)過程噪聲選取為：delta_w=0.1;Q= delta_w *diag([0.5,1,0.5,1]) 時得到跟蹤誤差圖，如圖8所示。

圖5、圖6展示了真實軌跡、濾波軌跡與觀測軌跡的對比圖，紅色為濾波軌跡，黑色為真實軌跡，藍(lán)色為GPS 接收機(jī)的觀測軌跡，可以看出 GPS 接收機(jī)接收的信號跳躍性比較大，觀測軌跡振蕩比較明顯，說明測量噪聲影響比較大，不能真正的定位，無法真正的反映真實軌跡，且出現(xiàn)漂移現(xiàn)象。對比圖5、圖6的3條軌跡發(fā)現(xiàn)kalman 濾波后的濾波軌跡比較接近目標(biāo)的真實運(yùn)動軌跡，能夠完成GPS導(dǎo)航的要求。由圖7、圖8中可以看出位移的觀測噪聲最大接近25m，噪聲相對也是比較大，這僅限于仿真，實際傳感器誤差不可能這么大。但經(jīng)過Kalman濾波之后，位置誤差值降低到8m以下，可以看出，Kalman濾波雖然不能完全消除智能小車GPS導(dǎo)航中的干擾噪聲，但它完全可以最大限度地降低噪聲的影響。

圖6 跟蹤軌跡圖

圖7 跟蹤誤差圖(delta_w=1e-4;)

圖8 跟蹤誤差圖(delta_w=0.1)

5 結(jié) 語

針對基于kalman濾波的智能小車GPS軌跡跟蹤進(jìn)行了研究，分析了kalman濾波、GPS導(dǎo)航定位原理以及Matlab模型的建立，并對其進(jìn)行了仿真研究，得到以下結(jié)論：

1)為了降低智能小車GPS導(dǎo)航定位系統(tǒng)中的干擾信號，采用了離散型Kalman濾波對GPS信號進(jìn)行了處理，并通過對當(dāng)前時刻狀態(tài)結(jié)合歷史時刻狀態(tài)進(jìn)行了下一時刻的最優(yōu)估計。

2)由于文中GPS的噪聲信號屬于高斯白噪聲且服從正態(tài)分布，為了避免在長久意義上走曲線易發(fā)生耦合，文中只采用了直線軌跡跟蹤，后續(xù)研究將對曲線運(yùn)動的軌跡進(jìn)行優(yōu)化。

3)由仿真結(jié)果可得Kalman濾波盡管不能完全消除噪聲，但已能最大程度的抑制噪聲，也極大地提升了智能小車GPS軌跡跟蹤的穩(wěn)定性。