基于坐標變換的交互多模型車載定位研究*

焦雨琪,牛 蕊,夏 志

(南京郵電大學 通信與信息工程學院，江蘇 南京 210003)

0 引言

隨著汽車的使用量增多以及道路越來越復雜，人們對于車輛定位導航精度的要求也越來越高[1]。如今二維平面的車輛導航可以滿足基本的定位需求，但是三維平面內的定位精度還不夠，尤其是高度定位，在遇到復雜的三維場景時，經常不能判斷車輛在哪一層，從而出現導航信息匹配實際路線錯誤的現象[2]。為了提高車輛的定位精度，考慮對GPS數據進行濾波處理[3]，在現實條件下，實際的系統總是非線性的，此時對比標準卡爾曼濾波，無跡卡爾曼濾波更能表現出其優越性。為了解決車輛運動狀態突然變化時定位效果不佳的現象，采用交互多模型算法，使其具有自適應濾波功能。坐標變換算法將建立平面和斜坡兩個不同的坐標系，通過兩個坐標系X、Y、Z三軸之間的聯系求出平面坐標系中車輛的高度數據。

1 無跡卡爾曼濾波

卡爾曼濾波是一種動態估計算法，通過狀態方程和觀測方程實現“觀測-更新”這一動態傳遞過程[4]。

考慮如下的非線性系統：

Xk=f(Xk-1,k-1)+Γ(Xk-1,k-1)Wk-1

(1)

Zk=h(Xk,k)+Vk

(2)

其中，Xk是系統的狀態向量；f(·)是非線性的過程函數；Zk是觀測向量；h(·)是觀測的非線性函數；Wk-1、Vk是零均值白噪聲序列。

無跡卡爾曼濾波首先對該系統狀態向量進行U變換，再對變換后的狀態向量進行濾波處理[5]。過程如下：

(1)計算Sigma點：

(3)

其中 ，n為狀態向量維數，Pk-1為誤差協方差矩陣。

(2)將Sigma點經過f(·)和h(·)非線性變換后再求和可以得到狀態更新量和觀測更新量Xk,k-1和Zk,k-1。

需要注意，U變換只是選取少量確定的Sigma點，不是隨機地從給定分布中進行采樣。

2 交互多模型

在現實生活中運動目標往往是機動的，單憑一種運動模型遠遠不能描述車輛的運動狀態[6]。在目標跟蹤的過程中采用多個運動模型，各個模型之間的轉移服從馬爾可夫鏈。

車輛的運動模型分為兩大類：CV模型和CA模型。CV模型描述的是速度不變的直線運動，此時只需要考慮車輛的位置和速度。CA模型描述的是車輛加速度不變的直線運動，此時需要考慮車輛的位置、速度和加速度[7]。

CV模型的狀態向量可為:

CA模型的狀態向量可為:

其中，XX、XY、XZ分別為X軸、Y軸、Z軸的位置信息，VX、VY、VZ分別為X軸、Y軸、Z軸的速度信息，AX、AY、AZ分別為X軸、Y軸、Z軸的加速度信息。

觀測向量為：

其中，Sx表示X軸方向的位置信息，Sy表示Y軸方向的位置信息，Sz表示Z軸方向的位置信息，ΔS表示測量時間內車輛位移的路程信息。以上信息可由GPS信息和里程計信息獲得。一般來說，觀測向量維數必須小于或等于狀態向量的維數。

3 交互多模型卡爾曼濾波

交互多模型使用不同的模型對應不同的運動狀態，首先輸入交互，在各個模型切換時通過轉移概率來控制，每個模型并行濾波，再融合各個模型的概率輸出最終的狀態估計[8]。

3.1 輸入交互

假設模型i到模型j的轉移概率為Pij，每個濾波器的模型概率為μj(k-1)。

模型i到j的混合概率為：

(4)

其中歸一化常數：

(5)

模型j的混合狀態估計：

(6)

模型j的混合協方差估計：

(7)

將混合狀態估計作為濾波輸入的初始狀態。

3.2 并行濾波

量測更新得到k時刻的狀態值和協方差矩陣：

(8)

(9)

(10)

其中，Z(k)為觀測量。

3.3 更新概率模型

模型j的似然函數：

(11)

其中：

vj=Z(k)-Zj(k/k-1)

(12)

(13)

更新后模型j的概率為：

(14)

其中，c為歸一化常數：

(15)

3.4 輸出交互

最終k時刻的總狀態估計：

(16)

k時刻總的協方差估計：

(17)

從以上步驟可以看出濾波的結果是多個模型加權后的模型，不同時刻選取概率最大的模型來描述此時的運動狀態。

4 坐標變換

車輛在平地上是二維空間，到了坡面，對于全局坐標來說還是三維，但是對于坡面坐標系來說就是二維，高度為零[9]。坐標變換是對空間實體的位置描述，是從一種坐標系變換到另一種坐標系，通過兩個坐標系之間的對應關系來實現。任何維的旋轉可以表述為向量與合適尺寸的方陣的乘積。最終一個旋轉等價于在另一個不同坐標系下對點位置的重新表述[10]，如圖1所示。

圖1 坐標系S和坐標系S1示意圖

如圖1坐標系S、S1所示，φ為方向角，θ為仰角，取坐標系S為全局直角坐標系，r=xex+yey+zez為S1坐標系上某個動點坐標值。

令rd1=xd1ex+yd1ey+zd1ez為平面M1上的已知點，該平面法向矢量設為n1=cosθsinφex-cosθcosφey+sinθez，其中ex、ey、ez為S坐標系的坐標軸單位矢量。則M1平面上與S坐標原點最小距離為n1·rd1，該點稱為ro1點，平面方程為(r-rd1)·n1。

取坐標系S1的原點為ro1=(n1·rd1)n1，坐標軸Z1取為平面法向量n1，X1軸和Y1軸在M1平面上。用S1表示的動點rs1與S表示的r的關系為r=ro1+rs1。

取X1軸為M1平面上的方位方向，Y1軸為仰角方向，則ez1=n1,ex1=(cosφ,sinφ,0)，又因為ey1·ex1=0，ey1·ez1=0,故得:

ey1=(-sinθsinφ,sinθcosφ,cosθ)

(18)

(19)

當φ=0°時，

(20)

顯然，z1=0即為S1坐標系的M1平面方程。由于x、y可由GPS得到，z1又為0，自然可以求出高度值z[11]。當高度位置信息求出之后，可以利用GIS定位到確定的一條車道上，避免出現車輛定位錯誤的現象[12]。

5 仿真實驗及結果分析

仿真實驗時，車輛在三維空間內做S型運動，方向角45°，坡度45°，初始位置位于(100,500,0)坐標處，初始速度為(0,10,0)，經過直線勻速運動250濾波點之后，開始上坡進行變速S型運動。采樣周期為0.1 s[13]。圖2～圖4為仿真結果圖。

圖2 基于坐標變換的IMMUKF車輛運動軌跡

圖3 基于IMMUKF-3D算法車輛運動Z軸方向定位誤差

圖4 基于坐標變換的IMMUKF車輛運動Z軸方向定位誤差

表1和表2列出了4次仿真實驗中高度誤差和總誤差數據。

表1 車載三維S型運動高度定位誤差 (m)

表2 車載三維S型運動定位總誤差 (m)

此實驗由IMMUKF-3D算法和基于坐標變換的IMMUKF算法分別模擬三維空間內車輛上坡運動。由仿真實驗結果圖2可以看出車輛在斜坡上做S型拐彎運動，初始位于(100,500,0)坐標處，初始速度為(0,10,0)，經過直線勻速運動250濾波點之后，開始上坡。采用交互多模型融合無跡卡爾曼濾波，在車輛運動狀態出現突然加速或突然減速時能夠及時調整模型概率，使車輛運動速度能夠很好地追蹤上標準速度。由圖3、圖4可以看出，基于IMMUKF-3D算法車輛運動Z軸方向定位誤差與基于坐標變換的IMMUKF車輛運動Z軸方向定位誤差相比，明顯誤差較大。圖4由于前250濾波點時還在平地，坐標還未變換，因此無高度誤差。通過表1和表2進一步分析仿真實驗結果可以更加直觀地發現，基于坐標變換的IMMUKF算法在高度定位精度和總體定位精度方面都有明顯的提高。其能夠區分車輛是在橋上還是橋下，減少由于高度定位誤差較大而將車輛導航地圖匹配出錯的情況[14]。

從仿真結果可以發現基于坐標變換的IMMUKF算法在三維空間中可以有效減少高度定位誤差，在車輛上坡的情況下可以減少車道匹配出錯的情況，在一定程度上有效地提高了車輛的定位精度。

6 結束語

由于GPS提供的高度信息相比水平信息具有更高的誤差，利用之前的IMMUKF-3D算法并不能完全解決高度定位精度不夠的問題，導致車輛導航錯誤[15]。利用坐標變換的原理，將三維空間轉化為坡面二維平面，通過三維坐標系的X軸、Y軸、Z軸與二維平面的X1軸、Y1軸之間的關系求出車輛高度信息。通過仿真實驗結果可以直觀發現，基于坐標變換的IMMUKF算法在高度定位精度和總體定位精度方面都有明顯的提高。