焦雨琪,牛 蕊,夏 志

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

0 引言

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

1 無(wú)跡卡爾曼濾波

卡爾曼濾波是一種動(dòng)態(tài)估計(jì)算法，通過(guò)狀態(tài)方程和觀(guān)測(cè)方程實(shí)現(xiàn)“觀(guān)測(cè)-更新”這一動(dòng)態(tài)傳遞過(guò)程[4]。

考慮如下的非線(xiàn)性系統(tǒng)：

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

(1)

Zk=h(Xk,k)+Vk

(2)

其中，Xk是系統(tǒng)的狀態(tài)向量；f(·)是非線(xiàn)性的過(guò)程函數(shù)；Zk是觀(guān)測(cè)向量；h(·)是觀(guān)測(cè)的非線(xiàn)性函數(shù)；Wk-1、Vk是零均值白噪聲序列。

無(wú)跡卡爾曼濾波首先對(duì)該系統(tǒng)狀態(tài)向量進(jìn)行U變換，再對(duì)變換后的狀態(tài)向量進(jìn)行濾波處理[5]。過(guò)程如下：

(1)計(jì)算Sigma點(diǎn)：

(3)

其中 ，n為狀態(tài)向量維數(shù)，Pk-1為誤差協(xié)方差矩陣。

(2)將Sigma點(diǎn)經(jīng)過(guò)f(·)和h(·)非線(xiàn)性變換后再求和可以得到狀態(tài)更新量和觀(guān)測(cè)更新量Xk,k-1和Zk,k-1。

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

2 交互多模型

在現(xiàn)實(shí)生活中運(yùn)動(dòng)目標(biāo)往往是機(jī)動(dòng)的，單憑一種運(yùn)動(dòng)模型遠(yuǎn)遠(yuǎn)不能描述車(chē)輛的運(yùn)動(dòng)狀態(tài)[6]。在目標(biāo)跟蹤的過(guò)程中采用多個(gè)運(yùn)動(dòng)模型，各個(gè)模型之間的轉(zhuǎn)移服從馬爾可夫鏈。

車(chē)輛的運(yùn)動(dòng)模型分為兩大類(lèi)：CV模型和CA模型。CV模型描述的是速度不變的直線(xiàn)運(yùn)動(dòng)，此時(shí)只需要考慮車(chē)輛的位置和速度。CA模型描述的是車(chē)輛加速度不變的直線(xiàn)運(yùn)動(dòng)，此時(shí)需要考慮車(chē)輛的位置、速度和加速度[7]。

CV模型的狀態(tài)向量可為:

CA模型的狀態(tài)向量可為:

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

觀(guān)測(cè)向量為：

其中，Sx表示X軸方向的位置信息，Sy表示Y軸方向的位置信息，Sz表示Z軸方向的位置信息，ΔS表示測(cè)量時(shí)間內(nèi)車(chē)輛位移的路程信息。以上信息可由GPS信息和里程計(jì)信息獲得。一般來(lái)說(shuō)，觀(guān)測(cè)向量維數(shù)必須小于或等于狀態(tài)向量的維數(shù)。

3 交互多模型卡爾曼濾波

交互多模型使用不同的模型對(duì)應(yīng)不同的運(yùn)動(dòng)狀態(tài)，首先輸入交互，在各個(gè)模型切換時(shí)通過(guò)轉(zhuǎn)移概率來(lái)控制，每個(gè)模型并行濾波，再融合各個(gè)模型的概率輸出最終的狀態(tài)估計(jì)[8]。

3.1 輸入交互

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

模型i到j(luò)的混合概率為：

(4)

其中歸一化常數(shù)：

(5)

模型j的混合狀態(tài)估計(jì)：

(6)

模型j的混合協(xié)方差估計(jì)：

(7)

將混合狀態(tài)估計(jì)作為濾波輸入的初始狀態(tài)。

3.2 并行濾波

量測(cè)更新得到k時(shí)刻的狀態(tài)值和協(xié)方差矩陣：

(8)

(9)

(10)

其中，Z(k)為觀(guān)測(cè)量。

3.3 更新概率模型

模型j的似然函數(shù)：

(11)

其中：

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

(12)

(13)

更新后模型j的概率為：

(14)

其中，c為歸一化常數(shù)：

(15)

3.4 輸出交互

最終k時(shí)刻的總狀態(tài)估計(jì)：

(16)

k時(shí)刻總的協(xié)方差估計(jì)：

(17)

從以上步驟可以看出濾波的結(jié)果是多個(gè)模型加權(quán)后的模型，不同時(shí)刻選取概率最大的模型來(lái)描述此時(shí)的運(yùn)動(dòng)狀態(tài)。

4 坐標(biāo)變換

車(chē)輛在平地上是二維空間，到了坡面，對(duì)于全局坐標(biāo)來(lái)說(shuō)還是三維，但是對(duì)于坡面坐標(biāo)系來(lái)說(shuō)就是二維，高度為零[9]。坐標(biāo)變換是對(duì)空間實(shí)體的位置描述，是從一種坐標(biāo)系變換到另一種坐標(biāo)系，通過(guò)兩個(gè)坐標(biāo)系之間的對(duì)應(yīng)關(guān)系來(lái)實(shí)現(xiàn)。任何維的旋轉(zhuǎn)可以表述為向量與合適尺寸的方陣的乘積。最終一個(gè)旋轉(zhuǎn)等價(jià)于在另一個(gè)不同坐標(biāo)系下對(duì)點(diǎn)位置的重新表述[10]，如圖1所示。

圖1 坐標(biāo)系S和坐標(biāo)系S1示意圖

如圖1坐標(biāo)系S、S1所示，φ為方向角，θ為仰角，取坐標(biāo)系S為全局直角坐標(biāo)系，r=xex+yey+zez為S1坐標(biāo)系上某個(gè)動(dòng)點(diǎn)坐標(biāo)值。

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

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

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

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

(18)

(19)

當(dāng)φ=0°時(shí)，

(20)

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

5 仿真實(shí)驗(yàn)及結(jié)果分析

仿真實(shí)驗(yàn)時(shí)，車(chē)輛在三維空間內(nèi)做S型運(yùn)動(dòng)，方向角45°，坡度45°，初始位置位于(100,500,0)坐標(biāo)處，初始速度為(0,10,0)，經(jīng)過(guò)直線(xiàn)勻速運(yùn)動(dòng)250濾波點(diǎn)之后，開(kāi)始上坡進(jìn)行變速S型運(yùn)動(dòng)。采樣周期為0.1 s[13]。圖2～圖4為仿真結(jié)果圖。

圖2 基于坐標(biāo)變換的IMMUKF車(chē)輛運(yùn)動(dòng)軌跡

圖3 基于IMMUKF-3D算法車(chē)輛運(yùn)動(dòng)Z軸方向定位誤差

圖4 基于坐標(biāo)變換的IMMUKF車(chē)輛運(yùn)動(dòng)Z軸方向定位誤差

表1和表2列出了4次仿真實(shí)驗(yàn)中高度誤差和總誤差數(shù)據(jù)。

表1 車(chē)載三維S型運(yùn)動(dòng)高度定位誤差 (m)

表2 車(chē)載三維S型運(yùn)動(dòng)定位總誤差 (m)

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

從仿真結(jié)果可以發(fā)現(xiàn)基于坐標(biāo)變換的IMMUKF算法在三維空間中可以有效減少高度定位誤差，在車(chē)輛上坡的情況下可以減少車(chē)道匹配出錯(cuò)的情況，在一定程度上有效地提高了車(chē)輛的定位精度。

6 結(jié)束語(yǔ)

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