譙小康,屈小媚

(西南民族大學(xué)計算機系統(tǒng)國家民委重點實驗室,四川成都 610041)

1 引言

隨著城市化進程的不斷推進,車輛基數(shù)的高速增長導(dǎo)致交通問題日益突出.車輛定位技術(shù)作為智能交通系統(tǒng)的核心技術(shù),受到國內(nèi)外學(xué)者的高度重視[1].基于車聯(lián)網(wǎng)的定位技術(shù)主要包括差分定位[2](differential positioning,DP)技術(shù)與協(xié)同定位[3](cooperative positioning,CP)技術(shù).由于多路徑效應(yīng)[4]的存在,在城市環(huán)境下DP技術(shù)無法實現(xiàn)定位精度提升,文[5]中的實驗結(jié)果也驗證了這一結(jié)論.圖1展示了一種GPS(global positioning system)衛(wèi)星信號受到多路徑效應(yīng)導(dǎo)致反射、阻隔的場景.協(xié)同定位是近幾年來國內(nèi)外研究熱點[6–8].基于車聯(lián)網(wǎng)的協(xié)同定位是指車輛通過車路協(xié)同系統(tǒng)(vehicle to everything,V2X)進行信息交換融合定位,其通信方式主要包括車輛與車輛(vehicle to vehicle,V2V)、車輛與路邊節(jié)點(vehicle to infrastructure,V2I)以及車輛與行人(vehicle to pedestrian,V2P),融合的信息包括到達角度(angle of arrival,AOA)、到達時間(time of arrival,TOA)、接收信號強度(received signal strength,RSS)、載波頻率偏移(carrier frequency offset,CFO),或這些測量的混合[9–11].協(xié)同定位并不局限于車輛應(yīng)用,也已被應(yīng)用于其他具有或不具有移動性的傳感器網(wǎng)絡(luò)[12–14].

圖1 多路徑效應(yīng)場景Fig.1 Multipath effect scenario

為處理GPS多路徑問題,基于車聯(lián)網(wǎng)V2X通信的協(xié)同定位可有效增強GPS的定位精度[15–17].文[17]中,待估車輛利用V2X來獲取輔助車輛以及路邊節(jié)點的位置信息,采用3個濾波器結(jié)構(gòu),選取最優(yōu)效果作為主濾波器,一定程度上改進了車輛定位精度.然而,該方法路邊節(jié)點位置信息丟失時定位性能一般,且采用3個濾波器造成計算資源浪費.另一方面,依賴于V2I通信的協(xié)同定位需要沿路安裝基礎(chǔ)設(shè)施,其成本過高不易實現(xiàn).最近,文[18]提出利用V2P測得車輛到行人的距離信息,從而改進GPS的定位精度.然而,由于行人是非協(xié)作地被動定位,在行人數(shù)量較多的情況下,難以對V2P觀測進行準確的數(shù)據(jù)關(guān)聯(lián).

因此,本文考慮在基于V2V的車載自組織網(wǎng)絡(luò)中,利用GPS、航位推算DR(dead reckoning)以及車輛之間的TOA測量進行分布式協(xié)同定位.DR是一種獨立定位技術(shù),它依靠傳感器來獲取車輛的位移和航向等信息,成本低且易于實現(xiàn),其定位誤差隨著時間推移不斷累積[19]與GPS有很強的互補關(guān)系.在V2V場景下對于每個車輛而言,既是待估車輛,同時又是其他車輛的輔助車輛.由于TOA是非線性測量,一些方法用擴展卡爾曼濾波(extended Kalman filter,EKF)來預(yù)處理TOA觀測[20],以獲得動態(tài)定位.然而,由于EKF基于非線性系統(tǒng)的一階泰勒近似來傳遞觀測的均值和協(xié)方差,在一階近似誤差較大的情況下,其定位性能不能取得很好的效果.

無跡卡爾曼濾波(unscented Kalman filter,UKF)[21]是另一種廣泛應(yīng)用的非線性濾波方法.UKF 的基本原理是無跡變換(unscented transform,UT).首先生成一組sigma點,然后將系統(tǒng)非線性函數(shù)應(yīng)用于每個sigma點,得到變換后的向量.最后,變換向量的樣本均值和樣本協(xié)方差給出真實均值和協(xié)方差的估計.理論上,如果系統(tǒng)非線性函數(shù)完全確定,即不含未知變量,UKF能達到非線性系統(tǒng)的二階近似精度.

然而,在本文的V2V協(xié)同定位場景中,TOA測量函數(shù)既是待估車輛位置的非線性函數(shù),也是輔助車輛位置的非線性函數(shù).輔助車輛的真實位置是未知的,因此非線性TOA測量函數(shù)包含未知的隨機參數(shù),轉(zhuǎn)換sigma點的步驟不可直接實現(xiàn).克服這一困難的傳統(tǒng)UKF方法是在轉(zhuǎn)換sigma點過程中直接使用GPS觀測.注意到GPS觀測存在觀測誤差,這將導(dǎo)致相應(yīng)的無跡變換中的誤差增大,特別是在GPS定位誤差較大的情況下.

文[22]提出了一種改進的UKF算法,通過將隨機參數(shù)擴維到狀態(tài)向量來提升算法性能.理論分析證明,在非線性系統(tǒng)中噪聲概率密度函數(shù)是正態(tài)分布的情況下,通過改進的UT變換得到的近似均值和協(xié)方差具有三階近似精度.因此,本文將輔助車輛的位置信息作為隨機參數(shù)擴維到狀態(tài)向量,建立擴維后的運動模型.在相應(yīng)的改進無跡變換中,由于狀態(tài)向量的維數(shù)增加,使得sigma點的數(shù)目增大.此外,每個改進的sigma點都包含待估車輛狀態(tài)和輔助車輛的位置,因此在轉(zhuǎn)換sigma點的步驟中,非線性TOA測量函數(shù)不再含有未知參數(shù).雖然改進的UKF方法具有更高的計算復(fù)雜度,但與傳統(tǒng)UKF方法相比,其估計性能明顯提高,如Monte Carlo仿真所示.

論文的其余結(jié)構(gòu)安排如下,第2節(jié)描述了車聯(lián)網(wǎng)的分布式協(xié)同定位模型.第3節(jié)介紹了傳統(tǒng)UKF與改進UKF下的協(xié)同定位算法.第4節(jié)提供了車輛直線行駛下和環(huán)線行駛下的Monte Carlo數(shù)值模擬結(jié)果,第5節(jié)給出了本文的結(jié)論.

2 車聯(lián)網(wǎng)協(xié)同定位模型

考慮某段道路上的車載自組織網(wǎng)絡(luò)由n個車輛節(jié)點組成,每個車輛都可通過車載自組織網(wǎng)絡(luò)與輔助車輛進行V2V實時通信.一個典型的協(xié)同定位場景如圖2所示.考慮各車輛利用GPS,DR以及車輛之間的TOA測量進行協(xié)同定位.文獻[23]指出基于TOA的二維目標定位需要至少3個輔助平臺,因此本文假設(shè)車輛的數(shù)量n≥4.假設(shè)各車輛的真實位置分布在一個二維平面上,車輛i(i=1,···,n)在k時刻的狀態(tài)向量記為,其中符號“′”表示轉(zhuǎn)置,和分別表示車輛位置與速度矢量,為其加速度.

圖2 車輛協(xié)同定位場景Fig.2 Vehicle co-positioning scenario

本文所涉及的公式符號較多,為方便閱讀給出了如表1所示的部分符號說明.

表1 符號說明Table 1 Symbol description

實際應(yīng)用中,每個車輛的真實位置信息均未知,但可通過GPS衛(wèi)星導(dǎo)航系統(tǒng)得到位置信息的一個實時觀測,其觀測模型為

另一方面,各車輛自帶的慣性導(dǎo)航系統(tǒng)可用于DR航位推算.基于DR和GPS信息融合的車輛定位可以描述為如下的線性系統(tǒng)

其中:矩陣F,G為狀態(tài)方程參數(shù),觀測矩陣

向量ωk?1是零均值高斯白噪聲,其協(xié)方差矩陣記為.該線性系統(tǒng)可直接應(yīng)用著名的Kalman濾波算法進行迭代預(yù)測.

本文的目的在于,在車輛GPS觀測和DR動態(tài)運動模型的基礎(chǔ)上,結(jié)合車載自組織網(wǎng)絡(luò)中的TOA觀測和V2V 信息傳輸,實現(xiàn)各車輛動態(tài)定位精度的提升.在下文中,對待估車輛l的狀態(tài)記為,其輔助車輛i(i≠l)的位置信息為.

待估車輛l與輔助車輛i之間的TOA觀測數(shù)學(xué)模型可以描述為

其觀測誤差向量的協(xié)方差矩陣記為Qt.

本文考慮對車載自組織網(wǎng)絡(luò)中的每個待估車輛l,利用測距雷達獲取其輔助車輛的TOA 觀測,利用V2V信息傳輸獲取輔助車輛的位置信息,并結(jié)合自身的DR動態(tài)運動模型,來設(shè)計一種改進的無跡卡爾曼濾波器.該濾波器的創(chuàng)新點在于充分考慮了輔助車輛位置的不確定性,從而提高濾波性能.

各車輛節(jié)點均利用本文提出的改進無跡卡爾曼濾波器進行分布式計算,得到自己及其輔助車輛的位置估計,然后各車輛將計算得到的估計值通過V2V通信進行信息傳輸,其信息傳輸方式如圖3所示.

圖3給出a,b,c3輛車在協(xié)同定位過程中的信息流向,圖中實線表示k ?1時刻的信息流向,3輛車兩兩互相傳輸信息,i=a,b,c,其中為k ?1 時刻車輛i的位置估計,為k ?1時刻車輛i的位置估計的協(xié)方差矩陣.

圖3 車輛協(xié)同定位信息流向Fig.3 Information flow of co-positioning

每輛車將收到的信息通過濾波器進行數(shù)據(jù)融合得到k時刻的信息,然后通過如虛線所示信息流向?qū)⑾乱豢痰臓顟B(tài)向量以及協(xié)方差傳輸給鄰近車輛,從而實現(xiàn)分布式協(xié)同定位.

3 協(xié)同定位算法

3.1 UKF協(xié)同定位算法

在車聯(lián)網(wǎng)協(xié)同定位模型中,TOA觀測向量tk相對于狀態(tài)向量Xk以及輔助車輛的真實位置均是非線性的.對于傳統(tǒng)的UKF方法而言,可以利用輔助車輛的GPS觀測值作為輔助車輛位置的估計,因此待估車輛的定位問題可以描述為如下的離散時間非線性系統(tǒng):

對于傳統(tǒng)的無跡卡爾曼濾波先初始化狀態(tài)向量和協(xié)方差,然后生成sigma點并加權(quán)求和預(yù)測狀態(tài)向量的均值和先驗協(xié)方差.根據(jù)得到的先驗協(xié)方差生成新的sigma點,并加權(quán)求和預(yù)測觀測向量的均值和協(xié)方差以及狀態(tài)向量均值與觀測向量均值之間的協(xié)方差.值得注意的是在對觀測向量進行預(yù)測時沒有輔助車輛的真實位置,這里使用輔助車輛GPS位置代替真實位置進行預(yù)測估計.最后,使用標準卡爾曼濾波進行狀態(tài)估計的預(yù)測更新.

3.2 改進UKF分布式協(xié)同定位算法

注意到觀測方程(4)中,TOA測量函數(shù)既是待估車輛位置的非線性函數(shù),也是輔助車輛位置的非線性函數(shù).上一小節(jié)中的傳統(tǒng)UKF方法使用GPS觀測來代替輔助車量的真實位置.在本小節(jié)中,將提出一種改進的無跡卡爾曼濾波方法.通過分析輔助車輛的真實位置與待估車輛真實位置都不可獲得,為了處理這種情況,將輔助車輛的位置擴維到狀態(tài)向量上,如下所示:

在第k ?1步,經(jīng)過車聯(lián)網(wǎng)V2V信息傳輸,即圖3中實線部分的信息傳輸,擴維后系統(tǒng)(10)的初始狀態(tài)更新為

基于擴維后的運動模型,其simga點的數(shù)量增加到2(2(n ?1)+6)+1=4n+9,生成的sigma點具體如下:

參數(shù)λ應(yīng)滿足2n+4+λ>0,相應(yīng)的加權(quán)系數(shù)為

通過DR動態(tài)運動模型對各sigma點進行預(yù)測,并預(yù)測狀態(tài)向量的均值

相應(yīng)的先驗協(xié)方差為

根據(jù)得到均值及協(xié)方差,生成新的sigma點

對于改進的無跡卡爾曼濾波而言,每個sigma點都應(yīng)該使用系統(tǒng)的非線性測量函數(shù)進行變換,因此轉(zhuǎn)換后的sigma點計算如下:

為直觀的理解整個算法的流程,給出如下算法1所示的偽代碼,其中:N表示采樣次數(shù),n代表車輛總數(shù).

最后,車聯(lián)網(wǎng)中的車輛再進行如圖3虛線部分所示的兩兩數(shù)據(jù)傳輸,各車輛節(jié)點根據(jù)接收到的信息再進行下一步的分布式定位.

本文通過將TOA觀測函數(shù)中輔助車輛位置擴維到狀態(tài)向量來改進UKF中UT變換的近似精度.傳統(tǒng)的UKF濾波在UT變換過程中直接使用帶噪聲的GPS觀測,并忽略其觀測誤差,這在理論上無法保證UT變換后的近似精度.下面的引理在理論上保證了本文的改進UKF方法能達到非線性系統(tǒng)的三階近似精度[22].

引理1對于具有對稱概率密度函數(shù)的隨機向量,如果非線性函數(shù)包含未知隨機參數(shù),則通過改進的無跡變換(23)–(24)得到的t的近似均值和協(xié)方差具有三階近似精度.

4 仿真實驗與結(jié)果分析

在數(shù)值模擬實驗中,假設(shè)車載自組織網(wǎng)絡(luò)中的車輛總數(shù)n=4,時間步長δt=0.2 s,采樣點數(shù)為100.車輛機動參數(shù)設(shè)定為:假設(shè)車載GPS定位誤差方差為,里程計和陀螺儀測量誤差方差分別為和,加速度誤差方差為,所有車輛都有V2V通信和TOA設(shè)備模塊,TOA測量誤差為零均值高斯白噪聲,其方差為(2s/c)2,c為信號的傳播速度.車輛定位精度由均方根誤差來比較,其定義為RMSE,其中L=1000為Monte Carlo仿真次數(shù).

4.1 仿真實驗1

為驗證本文提出的改進UKF的GPS/DR/TOA分布式協(xié)同定位方法的性能,通過數(shù)值模擬實驗比較了GPS定位、GPS/DR定位、UKF的GPS/DR/TOA協(xié)同定位以及改進UKF的GPS/DR/TOA協(xié)同定位4種方法在以速度vx=vy=15 m/s勻速直線行駛下位置估計的RMSE.車輛的直線行駛軌跡如圖4所示.

圖4 直線行駛下的車輛軌跡Fig.4 Trajectory of vehicles with straight-line travel

各車輛在4種方法下的RMSE展示在圖5–8中.從圖5–8的結(jié)果可以看出單獨GPS的定位效果是最差的,定位精度只有4.9862 m.基于KF的GPS/DR定位能夠很大程度上提高定位精度,定位精度能夠達到3.3977 m.UKF的GPS/DR/TOA定位能夠進一步的提高定位精度,定位精度能夠達到2.3723 m.改進UKF的GPS/DR/TOA定位能夠更進一步的提高定位精度,定位精度能夠達到1.9743 m左右.

圖5 直線行駛下車輛1位置估計的RMSEFig.5 RMSE of location estimation for vehicle 1 with straight-line travel

圖6 直線行駛下車輛2位置估計的RMSEFig.6 RMSE of location estimation for vehicle 2 with straight-line travel

圖7 直線行駛下車輛3位置估計的RMSEFig.7 RMSE of location estimation for vehicle 3 with straight-line travel

圖8 直線行駛下車輛4位置估計的RMSEFig.8 RMSE of position estimation for vehicle 4 with straight-line travel

4.2 仿真實驗2

為了進一步驗證比較改進UKF的GPS/DR/TOA分布式協(xié)同定位方法的性能,再次比較了4種方法在環(huán)線行駛下位置估計的RMSE,環(huán)線行駛階段可以分為4個階段,其中4輛車行駛的第1階段k=1,···,25速度分別為

圖9 環(huán)線行駛下的車輛軌跡Fig.9 Trajectory of vehicles with ring-line travel

各車輛在4種方法下的RMSE展示在圖10–13中.從圖10–13的結(jié)果可以看出,單獨的GPS定位效果是最差的,定位精度只有5.043 m.基于KF的GPS/DR 定位能夠很大程度上提高定位精度,定位精度能夠達到3.5962 m.UKF的GPS/DR/TOA定位能夠進一步的提高定位精度,定位精度能夠達到2.9228 m.改進UKF的GPS/DR/TOA定位能夠更進一步的提高定位精度,定位精度能夠達到2.3468 m.

圖10 環(huán)線行駛下車輛1位置估計的RMSEFig.10 RMSE of location estimation for vehicle 1 with ring-line travel

圖11 環(huán)線行駛下車輛2位置估計的RMSEFig.11 RMSE of location estimation for vehicle 2 with ring-line travel

圖12 環(huán)線行駛下車輛3位置估計的RMSEFig.12 RMSE of location estimation for vehicle 3 with ring-line travel

圖13 環(huán)線行駛下車輛4位置估計的RMSEFig.13 RMSE of location estimation for vehicle 4 with ring-line travel

式(31)定義性能提升率參數(shù)μ,作為評價定位性能相對改善程度的指標:

其中:RMSEA表示改進UKF 的協(xié)同定位精度,RMSEB表示其它方法的定位精度.若μ為正說明改進協(xié)同定位相較其它方法定位性能得到改善,值越大說明改善的程度越高,反之則說明性能惡化.改進的UKF協(xié)同定位方法與獨立的GPS定位和GPS/DR以及傳統(tǒng)的UKF協(xié)同定位性能比較如表2所示,結(jié)果表明在勻速直線行駛的情況下,改進的UKF協(xié)同定位方法與獨立的GPS定位和GPS/DR以及傳統(tǒng)的UKF協(xié)同定位性能相比較,性能分別提升了60%,42%和17%,在環(huán)線行駛的情況下,性能提升率分別為53%,35%和20%.

表2 改進UKF協(xié)同定位的性能提升率Table 2 Performance improvement rate of improved UKF co-positioning

5 總結(jié)

本文針對車輛定位場景中GPS信號容易受到多路徑效應(yīng)的影響,在道路上修建路邊節(jié)點成本過高等實際問題,提出利用車載自組織網(wǎng)絡(luò)中的V2V通信進行車輛分布式協(xié)同定位的問題.在車輛之間的TOA觀測模型中,存在輔助車輛真實位置未知的問題,本文提出的改進UKF協(xié)同定位方法通過將輔助車輛位置擴維到狀態(tài)向量,使得輔助車輛位置信息能夠跟隨待估車輛狀態(tài)信息同步更新以提高定位性能.Monte Carlo仿真結(jié)果表明,改進的UKF分布式協(xié)同定位系統(tǒng)顯著增強了車輛定位的精度.