摘　要　根據DR傳感器誤差的數學模型,將其誤差參數作為狀態變量,建立了基于‘當前’統計模型的GPS/DR組合定位的
卡爾曼濾波模型。仿真結果表明該算法在多種情況下均可使定位精度保持在較高范圍內,完全可以滿足普通車輛的定位
要求。

關鍵詞　全球定位系統　　航位推算　　卡爾曼濾波

     目前,對于陸用車輛定位來說,較常用的定位方式有利用全球定位系統( Gobal Position System,GPS)和航位推算(Dead Reckoning, DR)兩種。GPS定位具有全球地面連續覆蓋、定位精度高和抗干擾能力強等諸多優點,用戶只需要利用GPS接收機就可以方便的得到車輛的三維位置、速度和時間等信息。但當車輛行使在市區高層建筑物、隧道等特殊區域時,會造成GPS衛星信號的質量下降,甚至中斷;造成定位誤差增大和定位的不連續[ 1 ] 。DR 定位是一種完全自助式的定位方法,其原理主要是利用航向傳感器和距離傳感器實時檢測車輛的航向角和位移,從而推算出車輛的位置信息,具有短時間定位精度高、自主行強等特點[ 1 ] 。由DR的定位原理可知,DR的推算過程其實是一個信息累加的過程。隨著時間的推移,航向傳感器和位置傳感器的測量誤差會使定位誤差不斷累積加大。因此,需選用高精度的DR傳感器來提高測量精度。但是,這必然會提升定位系統的成本,而成本問題是目前影響定位系統普及和發展的一個主要問題。因此,選用低成本的DR傳感器,充分利用GPS和DR兩種定位方式優勢,相互補充校正的GPS/DR組合定位方式成為一種必然的選擇。

1　GPS /DR組合定位的卡爾曼濾波算法

利用GPS/DR組合定位時,在定位過程中不可避免的伴隨著各種隨機干擾,這些干擾勢必造成對定位精度的影響。因此,要對其進行濾波處理,以減小這些隨機干擾對定位精度的影響。卡爾曼濾波器是一種常用的方法。在應用卡爾曼濾波器時,需要建立系統的狀態方程和量測方程。

1. 1　狀態方程的建立

由于采用低成本的DR傳感器,因此測量誤差較大,為了能準確的估計和校正DR傳感器的測量誤差,需要確立其誤差模型,這里采用常用的DR傳感器:角速率陀螺儀和里程計。

角速率陀螺儀的測量輸出可表示為[ 2 ] :
ω測= ( KT +δK)ω真+ω0 +δω +w1 (1)
(1)式中: ω測:陀螺儀的實際輸出角速率; ω真:車輛的真實角速率; KT :陀螺儀的標稱刻度因子; δK :刻度因子誤差; ω0 :陀螺儀的零點標稱值; δω :陀螺儀的零點標稱誤差; w1 :陀螺儀的量測噪聲。w1可表示為的高斯白噪聲, δK和δω可用一階馬爾可夫過程描述[ 3 ] ,即δÛK = - τ1δK + w2δÛ ω = - τ2δω +w3 ,τ1 和τ2 分別為馬爾可夫過程的相關時間常數的倒數, w2 和w3 分別為和的高斯白噪聲。里程計的測量輸出可表示為[ 4, 5 ] :S測= (ST +δS ) N +w4 (2)

(2)式中: S測:定時間內車輛的行進距離; ST :里程計的標稱刻度因子; δS :刻度因子誤差; N : 定時間內里程計發出的脈沖總數; w4 :里程計的量測噪聲。

w4 可表示為的高斯白噪聲, δS 為一階馬爾可夫過程[ 4 ] ,即: δÛS = -τ3δS +w5 ,τ3 為馬爾可夫過程的相關時間常數的倒數, w5 為的高斯白噪聲。由以上的分析可以看出角速率陀螺儀的主要誤差參數為刻度因子誤差δK和零點標稱誤差δω、里程計的主要誤差參數為刻度因子誤差δS 。因此,在應用Kalman濾波時將其擴展為狀態變量進行在線估計。

將車輛的運動看成東向和北向的二維運動,車輛的模型選“當前”統計模型, 取系統的狀態變
量為[ 6 ] :
X = [ e　ve 　ae 　n　vn 　an 　δK　δω　δS ]T
(3)
(3)式中e , v e , a e分別表示東向的位置、速度和加速度分量; n , v n , a n分別表示北向的位置、速度和加速度分量。系統的離散化狀態方程為:
X ( k + 1) =Φ ( k + 1) X ( k) +U ( k) +W ( k)
(4)
Φ ( k + 1) = diag[Φe ( k + 1) , 　Φn ( k + 1) ,
Φδ ( k + 1) ] (5)
Φe ( k + 1) =
1 T 1 /τe (τe T + e-τeT - 1)
0 1 1 /τe (1 - e-τeT )
0 0 e-τeT
(6)
Φδ ( k + 1) = diag[ e-τ1T 　e-τ2T 　e-τ3T ] (7)
將(4)式中的τe換成τn可得Φn ( k + 1) ,其中T為采樣周期, τe和τn分別為東向和北向的機動時間常數的倒數。
利用“當前”統計模型的自適應濾波算法[ 7 ] ,可將(2)式簡化為: X ( k + 1) = Φ1 ( k + 1) X ( k) +W ( k) 。
其中:
Φ1 ( k + 1) = diag[Φ1e ( k + 1) , 　Φ1n ( k + 1) ,
Φ1δ ( k + 1) ] (8)
Φ1δ ( k + 1) =Φδ ( k + 1) (9)
Φ1e ( k + 1) =Φ1n ( k + 1) =
1 T T2 /2
0 1 T
0 0 1
　(10)

1. 2　量測方程的建立

取GPS接收機的東向和北向位置信息eG 和nG ,角速率陀螺的輸出ω測,里程計在一個采樣周期T內的輸出S測為觀測量,即觀測向量:
Z = [ eG , 　nG 　ω測　S測]T (11)
可取KT = ST = 1,ω0 = 0 ,可得:
Z =
eG
nG
ω測
S測
=
e
n
(1 +δk)
9
9t arc tan
ve
vn
+δω
(1 +δS ) T v2
e + v2
n
+
v1
v2
ω1
ω4
(12)
v1 和v2 是GPS接收機東向和北向的位置測量噪聲,分別為(0,σ6
2 )和( 0,σ7
2 )的高斯白噪聲。對( 10)
式進行離散化后可得:
Z ( k) =
e(k)
n (k)
1 +δK(k + 1)
vn (k) ae (k) - ve (k) an (k)
ve
2 (k) + vn
2 (k)
+δω(k)
1 +δS (k + 1) T ve
2 (k) + vn
2 (k)
+
[ v1 ( k) v2 ( k) v3 ( k) v4 ( k) ]T (13)
即: 　　Z ( k) = h [X ( k) ] +V ( k) (14)
對(14)式進行離散化處理,再一步預測^ X ( k,
k - 1) 處展開,忽略高次項可得:
Z ( k) = h [ ^ X ( k, k - 1) ] +
5h [ ^ X ( k, k - 1) ]
5[ ^ X ( k, k - 1) ]
×
[X ( k) - ^ X ( k, k - 1) ] +V ( k) (15)
令量測矩陣H ( k) =
5h [ ^ X ( k, k - 1) ]
5[ ^ X ( k, k - 1) ]
=
1 0 0 0 0 0 0 0 0
0 0 0 1 0 0 0 0 0
0 H31 H32 0 H34 H35 H36 1 0
0 H41 0 0 H44 0 0 0 H48
(16)(16)式中: 　　H31 = [ 1 +δ^K ( b) ] ×
[ ^ae ( b) ^ve ( b) - 2^ve ( b) ^vn ( b) ^ae ( b) - ^an ( b) ^vn ( b) ]
^ve
2 ( b) + ^vn
2 ( b)
(17)
H32 = [ 1 +δ^K ( b) ]
^vn ( b)
^ve
2 ( b) + ^vn
2 ( b)
(18)
H34 = [1 +δ^K ( b) ] ×
[ ^an ( b) ^ve ( b) - 2^ve ( b) ^vn ( b) ^ae ( b) - ^ae ( b) ^vn ( b) ]
^ve
2 ( b) + ^vn
2 ( b)
(19)
H35 = - [1 +δ^K ( b) ]
^ve ( b)
^ve
2 ( b) + ^vn
2 ( b)
(20)
H36 =
^vn ( b) ^ae ( b) - ^ve ( b) ^an ( b)
^ve
2 ( b) + ^vn
2 ( b)
(21)
H41 = [1 +δ^ S ( b) ]
Tve ( b)
^ve
2 ( b) + ^vn
2 ( b)
(22)
H44 = [1 +δ^ S ( b) ]
Tvn ( b)
^ve
2 ( b) + ^vn
2 ( b)
(23)
H48 = T ^ve
2 ( b) + ^vn
2 ( b) (24)
上述各式中b = ( k, k - 1) 表示k - 1時刻到k時刻的一步預測。

1. 3　自適應Ka lman濾波算法

Kalman濾波方程為:
X ( k, k - 1) =Φ1 ( k) X
∧
( k)
X
∧
( k) = X ( k, k - 1) + K ( k) { Z ( k) -
h [X ( k, k - 1) ] }
K ( k) = P ( k, k - 1) HT ( k) [H ( k) P ( k, k - 1)
HT ( k) + R ( k) ]- 1
P ( k) = I - K ( k) H ( k) P ( k, k - 1)
P ( k, k - 1) =Φ ( k) P ( k - 1)ΦT ( k) +Q ( k - 1)
(25)
(25)式中: R ( k) 為系統噪聲的協方差矩陣:
R ( k) = diag[σ6
2 , σ7
2 , σ1
2 , σ4
2 ],
　　Q ( k) 為系統噪聲的協方差矩陣,為了計算方便,在Q ( k) 計算時,用Φ1 ( k) 代替Φ ( k) ,可得:
Q ( k) = E [W ( k)WT ( k) ] =
diag[ 2τeσe
2Qe ( k) , 2τnσn
2Qn ( k) , Qδ ( k) ]。
其中: σe
2和σn
2分別為東向和北向機動加速度的方差。
Qδ ( k) = diag[σ1
2 (1 - e- 2τ1T ) , σ2
2 (1 - e- 2τ2T ) ,
σ3
2 (1 - e- 2τ3T ) ],
Qe ( k) = Qn ( k) =
T5 /20 T4 /8 T3 /6
T4 /8 T3 /3 T2 /2
T3 /6 T2 /2 T
。
2　仿真分析

利用Monte Carlo方法[ 8 ]在MATLAB環境下對上述算法進行仿真,驗證定位精度,選均方根誤差(RMSE)為仿真精度指標,仿真分別在以下兩種情況下進行:

(1)假設車輛勻速運動,車輛的初始狀態為:

X (0) = [ 0, 15, 0, 0, 15, 0, 0, 0, 0 ]T
　　采樣周期T = 0. 1 s,采樣點數N = 2 000,車輛運動過程歷時200 s,Monte Carlo仿真次數為50次,其余仿真參數如下:
τ1
=τ2 =τ3 = 0. 01 s- 1 , τe =τn = 1 s- 1 ,
σ1 =
0. 4°
s
,
σ2 =σ5 = 0. 001, σ3 = 0. 1, σ4 = 1 m / s, σ6 =
σ7 =15 m,
^ X (0) = [10, 　16, 　0. 2, 　10, 　16, 　0. 2, 　0,
　0, 　0 ]T ,
P (0) = diag[ 100, 　1, 　0. 04, 　100, 　1, 　0. 04,
　0, 　0, 　0 ]。

仿真曲線如圖1和圖2所示,表1為位置精度指標比較表。單獨定位時,在僅僅歷時100 s后,由于誤差累積效應,位置誤差就已經達到了幾乎400 m,從表1中也可以很清楚的看到GPS/DR組合定位的定位精度比GPS單獨定位和DR單獨定位的定位精度高,分析原因主要是一方面由于對DR傳感器的誤差參數進行了在線估計,實現了誤差的動態補償; 另一方面由于GPS/DR組合定位充分利用了GPS和DR兩者的定位信息,兩者相互補償和校正,從而有效的抑制了DR誤差累積效應,使得定位精度得到了提高。

(2) 為了說明算法的可靠性,假設k時刻GPS信號被遮擋,有DR單獨工作,此時可以用下式來代替(10)中的e ( k) 和n ( k) 繼續濾波:
e ( k) = ^e ( k - 1) + S測( k) sin[ arc tan
^ve ( k - 1)
^vn ( k - 1)
+
Tω測( k) ],
n ( k) = ^n ( k - 1) + S測( k) cos[ arctan
^ve ( k - 1)
^vn ( k - 1)
+
Tω測( k) ]。
GPS中斷時位置誤差曲線假設在200 ～500 采樣點中GPS衛星信號消失,即GPS衛星信號中斷30 s,假設車輛勻速運動,車輛的初始狀態為:
X (0) = [ 0, 15, 0, 0, 15, 0, 0, 0, 0 ]T。
其余仿真參數同第一種情況, 仿真曲線如如圖3所示。

由圖4可以看出濾波精度總體保持在10 m以內,尤其在GPS衛星信號中斷的30 s過程中,也達到了較高的位置定位精度,始終保持在10 m以內,完全彌補了GPS信號中斷造成不能定位的缺陷,有效的提高了定位系統的工作可靠性。

3　結語

采用GPS/DR組合定位方式,將DR傳感器的測量誤差參數擴展為狀態變量,利用基于‘當前’統計模型的GPS/DR組合Kalman濾波算法對誤差參數進行在線估計,有效的抑制了DR誤差累積效應,在車輛作勻速和GPS信號中斷的情況下,對算法進行了計算機仿真,從仿真結果看均獲得了較高的定位精度,可以滿足普通車輛高精度和高可靠性的要求,但在車輛作較為復雜的運動時,該算法還需作進一步的改進和研究。


參考文獻

1　常　青,鄭平方,柳重堪,等. 車載GPS/DR組合導航系統數據融合算法研究. 通信學報, 2000; 21 (2) : 42—48
2　高　鵬,富　立,范躍祖. 車輛組合導航定位系統中測量元件的參數修正. 北京航空航天大學學報, 2003; 29 (8) , 731
3　Kim J inwon, Lee Janggyu, Jee Gyuin, et al. Compensation of gyro2scope errors and GPS/DR integration. Position Location and Naviga2tion Symposium, 1996; IEEE 1996; 22—26 Ap ril 1996, 464—470
4　徐愛功. 自動車輛定位導航系統中傳感器的誤差分析. 測繪工程. 1997; 6 (1) , 38—39
5　徐愛功. 用GPS觀測值對推斷數據進行非線性平滑的模型建立.解放軍測繪學院學報, 1997; 14 (2) : 87—88
6　寇艷紅, 張其善, 李先亮. 車載GPS/DR組合導航系統的數據融合算法. 北京航空航天大學學報報, 2003; 29 (3) : 264—268
7　周宏仁,敬忠良,王培德. 機動目標跟蹤. 北京: 國防工業出版社, 1991
8　何　友, 修建娟, 張晶薇, 等. 雷達數據處理及應用. 北京: 電子工業出版社, 2006