GPS信號阻塞情況下的EKF算法改進

肖晶晶,陸明泉

(清華大學(xué)電子工程系,北京100084)

GPS衛(wèi)星定位解算是根據(jù)偽距、偽距增量等測量值，計算接收機的位置P、速度V和時間T等信息的過程。目前GPS實時定位解算中最為常用的兩種方法為迭代最小二乘算法(ILS)和擴展卡爾曼濾波(EKF)。為了準(zhǔn)確計算接收機的三維位置以及時間未知數(shù)的值，解算過程需要至少4顆衛(wèi)星的測量信息。但是當(dāng)GPS信號出現(xiàn)遮擋時，接收機只能接收到3顆衛(wèi)星的測量信息，解算方程就不夠4個，ILS不再適用。參考文獻[1]引入EKF，利用隨著時間推移的多組數(shù)據(jù)進行實時定位解算，但定位精度也很難滿足用戶的需求。

為了解決上述問題，本文提出了一種改進的EKF算法。利用在垂直地面方向上的位置變化緩慢這一運動特性，建立了改進EKF算法的系統(tǒng)模型，并通過理論分析得到了濾波器參數(shù)，最后利用真實的衛(wèi)星數(shù)據(jù)進行驗證。需要特別指出的是，由于本文提出的改進EKF算法利用的是在垂直地面方向上的位置變化緩慢的特征，故而該算法的適用場合為車載等地面用戶的定位解算，不適合在垂直地面方向上高速運動的情形。

1 定位解算的系統(tǒng)模型

衛(wèi)星定位解算的系統(tǒng)模型包括狀態(tài)模型和觀測模型兩部分。令向量yt、xt分別表示系統(tǒng)模型的測量值和系統(tǒng)狀態(tài)參量:

式(1)中，系統(tǒng)的測量值yt包括偽距ρit和偽距增量ρ˙it；式(2)中，系統(tǒng)狀態(tài)參量xt包括接收機在地心地固(ECEF)坐標(biāo)系下的位置(Xt,Yt,Zt)和速度(X˙t,Y˙t,Z˙t)以及光速c乘以時偏δt和光速c乘以時漂δ˙t8個參量。其中i=1,2,…，n,表示第i顆衛(wèi)星(n為有效衛(wèi)星數(shù)量)，t為采樣時刻。

1.1 測量模型

系統(tǒng)的測量模型描述了系統(tǒng)測量值與系統(tǒng)狀態(tài)參量之間的關(guān)系。偽距與系統(tǒng)狀態(tài)參量的關(guān)系可表示為:

式(3)中(Xit,Yit,Zit)為ECEF坐標(biāo)系下的衛(wèi)星位置，vρit為偽距的測量噪聲。為了簡化計算，一般偽距測量噪聲vρit采用白噪聲表示，且各通道之間的vρit相互獨立。

偽距增量描述了衛(wèi)星與接收機的徑向速度，它與狀態(tài)參量的關(guān)系可表示為:

式(4)中(X˙it,Y˙it,Z˙it)為ECEF坐標(biāo)系下的衛(wèi)星速度，vρ˙itit為偽距增量的測量噪聲。

綜上所述，完整的測量方程為:

其中vt=[vρit,vρ˙it]T,i=1,2,…,n。在實際應(yīng)用中，考慮到偽距和偽距增量的測量誤差分別為米級、分米級，所以Vt的協(xié)方差矩陣R設(shè)為:

由于式(3)是非線性的，故而系統(tǒng)測量模型是非線性模型。擴展卡爾曼濾波(EKF)使用了低階Taylor級數(shù)逼近非線性系統(tǒng)，在定位解算模型中主要體現(xiàn)在測量方程被近似線性化，即式(5)被寫為:

式(7)中，Ht=?h/?x表示偽距測量值ρit和偽距增量ρ˙it對系統(tǒng)狀態(tài)參量xt的偏導(dǎo)數(shù)矩陣。一般計算Ht矩陣(Jacobian矩陣)需在每個采樣時刻得到一步預(yù)測值xt+1|t后重新計算，計算EKF算法公式中的Ht是對定位解算問題線性化的關(guān)鍵步驟。

1.2 狀態(tài)模型

系統(tǒng)的狀態(tài)模型描述了系統(tǒng)狀態(tài)參量的時間更新過程。由參考文獻[3-4]可知，更新過程的表達(dá)式為:

式(8)中的系數(shù)矩陣A、B為:

式(9)中的T為采樣時間間隔。

式(8)中的wt表示系統(tǒng)狀態(tài)轉(zhuǎn)移的噪聲模型:

由參考文獻[4]可以得到(wδ1,wδ2)的協(xié)方差矩陣為:

式(12)中，

式(13)中的參數(shù)為:h0=9.4×10-20,h-1=1.8×10-19,h-2=3.8×10-21

綜上所述，系統(tǒng)的狀態(tài)模型的一步轉(zhuǎn)移噪聲wt協(xié)方差矩陣為:

2 改進的EKF

根據(jù)第1節(jié)中建立的系統(tǒng)模型，系統(tǒng)測量值yt為2參數(shù)，系統(tǒng)狀態(tài)參量xt為8參數(shù)，為了得到準(zhǔn)確解，故而需要至少4顆有效衛(wèi)星的測量值。為了能夠滿足用戶需求的定位精度，本文提出利用一般車輛行人等用戶在垂直地面方向上的位置變化緩慢這一運動特性，增加如下方程:

式(15)中，Dt表示接收機在NED(以用戶為中心，三個坐標(biāo)軸N、E和D分別指向北、東和下)坐標(biāo)系下D軸方向上的位置，D˙t表示在D軸上的速度，Dt+1|t、D˙t+1|t分別表示Dt和D˙t的一步預(yù)測值。對于在垂直地面方向上位置變化緩慢的用戶，該方程均成立。

利用式(15)，本文改進了擴展卡爾曼濾波(EKF)的時間更新過程。下面分兩部分進行說明。

令用戶在NED坐標(biāo)下的八狀態(tài)參量為:

式(17)中，R1為從用戶在ECEF坐標(biāo)系下的狀態(tài)參量xt到用戶在NED坐標(biāo)系下的狀態(tài)參量mt的轉(zhuǎn)換矩陣，利用坐標(biāo)系旋轉(zhuǎn)的原理可以求得[2]。A2為用戶在NED坐標(biāo)系下的狀態(tài)參量mt的一步轉(zhuǎn)移矩陣。它們的表達(dá)式如下:

根據(jù)參考文獻[2]可知，式(18)中，

式(20)中λ為用戶的經(jīng)度，φ為用戶的緯度。

易知R1為單位正交陣,故而用戶從NED坐標(biāo)系轉(zhuǎn)換到ECEF坐標(biāo)系時，轉(zhuǎn)換矩陣即為R1T。

為了與普通的EKF進行對比,改進EKF使用的狀態(tài)參量x?t的自協(xié)方差矩陣Pt和系統(tǒng)的噪聲協(xié)方差矩陣Qt仍為在ECEF坐標(biāo)系下的初始值。那么狀態(tài)參量的協(xié)方差矩陣Pt的時間更新過程如下:

式(21)中的系數(shù)矩陣為:

綜上所述，并結(jié)合參考文獻[5]，得到改進的EKF算法的計算過程如下:

(1)初始化:

(2)時間更新:

(3)測量更新:

根據(jù)理論分析可以得出，改進的EKF相對于普通的EKF來說，增加了一項先驗信息，故而使得只有3顆有效衛(wèi)星時的定位解算有了4個解算方程，因此可以相對準(zhǔn)確地解算出4個未知數(shù)的值；而在有效衛(wèi)星數(shù)不少于4顆時，增加的方程(15)仍然滿足，故而也不會影響定位精度。

3 算法仿真和分析

在本文的算法驗證實驗中，使用了Leika GPS1200測量型接收機在車載動態(tài)下采集的GPS信號的偽距和偽距增量的測量值，其中偽距測量值經(jīng)過偽距增量測量值的平滑。算法的驗證是在PC機上使用Matlab完成的。

在算法驗證中，設(shè)置采樣時間間隔為T=1 s。接收機的運動狀態(tài)為從靜止?fàn)顟B(tài)變化到在地面上高速運動最后又逐步靜止。測試數(shù)據(jù)長度為2 500 s。參考軌跡是接收機使用動態(tài)實時差分(RTK)方法測定的，方差精確至毫米級。分別在有效衛(wèi)星不少于4顆和只有3顆的情況下，比較了幾種定位解算算法的定位結(jié)果。

3.1 有效衛(wèi)星數(shù)不少于4顆時的定位結(jié)果

以有效衛(wèi)星數(shù)為4顆為例，分別使用ILS、EKF和改進的EKF算法進行定位，得到以ECEF坐標(biāo)系下XYZ三維坐標(biāo)表示的定位結(jié)果，如圖1所示。

由圖1可知，接收機在前400 s時基本保持靜止，然后開始運動至1 600 s，最后逐步靜止至測試結(jié)束。三種解算方法都可以較好地進行定位解算。為了更好地分析定位結(jié)果，畫出圖1中的XYZ三維坐標(biāo)上的定位誤差的均方根，如圖2所示。

為了更好地比較ILS、普通EKF與改進EKF算法，計算圖2中三種算法在XYZ三個坐標(biāo)方向上的定位結(jié)果的均方根誤差的平均值，如表1所示。

表1 三種定位結(jié)果的誤差均方根的平均值比較

從圖2和表1可以更清晰地看出，在有效衛(wèi)星不少于4顆時，三種算法在以ECEF坐標(biāo)系表示的XYZ方向上的定位誤差的均方根均不超過20 m，平均定位誤差不超過10 m，定位解算精度都很好。其中改進的EKF與普通EKF定位精度相當(dāng)，而ILS的定位均方根誤差抖動很小。這是因為，每當(dāng)有新的測量數(shù)據(jù)時，ILS算法都會經(jīng)過多次迭代計算直至結(jié)果收斂，而EKF與改進的EKF算法均只是利用新息進行一次計算，計算量要遠(yuǎn)遠(yuǎn)小于ILS，故而定位結(jié)果會稍有抖動。

3.2 有效衛(wèi)星數(shù)只有3顆時的定位結(jié)果

在有效衛(wèi)星數(shù)只有3顆時，ILS無法進行定位。分別使用普通EKF和改進的EKF算法進行定位，得到在ECEF坐標(biāo)系下的XYZ三維坐標(biāo)上的定位結(jié)果,如圖3所示。

為了更好地分析定位結(jié)果，畫出圖3中的XYZ三維坐標(biāo)上的定位誤差的均方根，如圖4所示。

為了更好地比較普通EKF算法與改進EKF算法，計算圖3中兩種算法XYZ三維坐標(biāo)上的定位結(jié)果的均方根誤差的平均值，如表2所示。

表2 EKF與改進EKF的定位結(jié)果的均方根誤差的平均值比較

由圖3、圖4和表2可知，在只有3顆有效衛(wèi)星的情況下，普通EKF的定位結(jié)果會出現(xiàn)明顯的偏移，而本文提出的改進的EKF算法的定位結(jié)果與參考軌跡擬合得很好，各個方向上的平均定位誤差均不超過10 m，定位精度很好。這說明本文提出的改進EKF算法在只有3顆有效衛(wèi)星時，極大地提高了EKF的定位精度。與理論結(jié)果吻合。

3.3 有效衛(wèi)星為3顆時改進EKF與有效衛(wèi)星為4顆時普通EKF的定位結(jié)果

為了進一步分析改進EKF算法的性能，將改進EKF算法在只有3顆有效衛(wèi)星時的定位誤差與普通EKF算法在有4顆有效衛(wèi)星時的定位誤差進行比較，如圖5所示。

計算圖5中的兩種情況下XYZ三維坐標(biāo)上的平均誤差的均方根，如表3所示。

由圖5和表3可知，在只有3顆有效衛(wèi)星時，本文提出的改進EKF算法所增加的先驗信息與增加一顆衛(wèi)星的測量信息的效果基本一致。故而說明在出現(xiàn)GPS信號阻塞的情況下，只有3顆有效衛(wèi)星時，此改進EKF算法的定位解算精度非常好。

表3 4星時普通EKF和3星時改進EKF的定位的平均誤差均方根比較

綜上所述，利用實測的衛(wèi)星數(shù)據(jù)進行驗證，結(jié)果表明:在3顆有效衛(wèi)星的情況下，在靜止?fàn)顟B(tài)和車載狀態(tài)下，改進的EKF算法的定位精度均明顯優(yōu)于普通的EKF，且與4顆有效衛(wèi)星時EKF的定位精度相當(dāng)；在不少于4顆有效衛(wèi)星的情況下，在靜止?fàn)顟B(tài)和車載狀態(tài)下，改進EKF算法的定位精度與普通EKF相當(dāng)。由此說明，本文提出增加的先驗信息在已有足夠測量信息時，并不會影響定位精度，而只有在3顆有效衛(wèi)星時，測量信息對于定位精度的改進與一顆有效衛(wèi)星的測量信息效果基本一致，從而大大提高了EKF算法的魯棒性。

本文在GPS信號出現(xiàn)阻塞，有效衛(wèi)星數(shù)降為只有3顆時，改進EKF算法以提高GPS定位解算精度，并使用真實衛(wèi)星數(shù)據(jù)對改進的EKF算法進行了驗證。實驗結(jié)果表明，在有效衛(wèi)星不少于4顆時，本文提出的改進EKF的定位精度與EKF相當(dāng)。在GPS信號阻塞，有效衛(wèi)星數(shù)只有3顆時，改進EKF的定位精度明顯優(yōu)于普通EKF，且與4顆有效衛(wèi)星時EKF的定位精度相當(dāng)。受本文算法改進中所引入的先驗信息的限制，本文提出的改進EKF算法適合車載等地面運用，不適合在垂直地面方向上高速運動的情形。

參考文獻

[1]COOPER S,DURRANT W H F.A kalman filter model for GPS Navigation of Land Vehicles,IEEE Conference on Intelligent Robots and System(IROS’94),Munich,Germany,1994(11):12-16.

[2]MISRA P,ENGE P著.全球定位系統(tǒng)——信號、測量與性能(第二版).羅鳴,曹沖,肖雄兵,等譯.北京:電子工業(yè)出版社，2008:103-106.

[3]AGOGINO A,ALAG S,GOEBEL K.Intelligent sensor validation and sensor fusion for reliability and safety enhancement in vehicle control,MOU132,UCB-ITSPRR-95-40,California PATH Ressarch Report,1995.

[4]KEE C,PARKINSON B W.Wide area differential GPS.Navigation:Journal of The Institute of Navigation.1991,38(2).

[5]曹潔,文如泉.非線性濾波算法在機動目標(biāo)跟蹤中的研究[J].信息化縱橫，2009,28(18):61-64.