孫 寧，閆夢如，倪 捷，葛如海，秦洪懋，唐翊銘，高行健

(1.江蘇大學(xué)汽車與交通工程學(xué)院，鎮(zhèn)江 212013； 2.北京航空航天大學(xué)，車路協(xié)同與安全控制重點(diǎn)實(shí)驗(yàn)室，北京 100191)

前言

隨著汽車自動駕駛技術(shù)的發(fā)展，車輛對環(huán)境感知的要求也越來越高。獲取實(shí)時、準(zhǔn)確的自身和周圍車輛位置信息(如經(jīng)度、緯度、高度等)是環(huán)境感知內(nèi)容的重要組成部分，也相應(yīng)面臨著更加嚴(yán)格的精度要求。如自動緊急制動系統(tǒng)，對前方車輛的位置誤差要求就在1m以內(nèi)。因此，如何提高車輛位置信息的定位精度[1]，是目前國內(nèi)外汽車領(lǐng)域?qū)W者面臨的一項(xiàng)新課題。高精度車輛定位方法的研究，對發(fā)展汽車自動駕駛技術(shù)具有十分重要的意義。

鑒于經(jīng)濟(jì)成本因素，目前單一應(yīng)用傳統(tǒng)GPS或毫米波雷達(dá)，都無法滿足上述車輛位置定位的高精度要求。而通信技術(shù)的發(fā)展則為高精度車輛定位提供了一種新的途徑。在車車通信、車路通信的支持下，車輛間可相互交換豐富的信息，同時，車輛自身獲取周圍環(huán)境信息的能力大大提升，從而可以計算獲得更加準(zhǔn)確的位置信息[2-3]。本文中在傳統(tǒng)GPS和毫米波雷達(dá)獲取初步車輛定位信息的基礎(chǔ)上，參照文獻(xiàn)[4]中的思路，提出一種基于GPS/雷達(dá)集成(GPS/radar integration，GRI)的多車協(xié)同定位方法，為獲取高精度的車輛位置信息提供一種新思路。

1 多車協(xié)同定位的總體框架設(shè)計

本文中提出的多車協(xié)同定位系統(tǒng)由GPS、毫米波雷達(dá)和車車通信3大模塊組成。其中GPS數(shù)據(jù)已經(jīng)通過計算轉(zhuǎn)換為局部區(qū)域的平面坐標(biāo)。車車通信模塊為LTE-V/DSRC。

系統(tǒng)中在基于GRI的多車協(xié)同定位過程中，對定位的估計分為兩階段。第一階段，進(jìn)行基于本車位置信息的粗估計，即通過GPS獲取含噪聲的車輛的絕對位置信息，經(jīng)卡爾曼濾波器處理，再通過雷達(dá)得到周圍車輛位置的粗估計；第二階段，將粗估計的結(jié)果通過車車通信進(jìn)行分享，進(jìn)行綜合多車位置信息的融合估計，得出更加準(zhǔn)確的本車和周圍車輛的絕對位置。協(xié)同定位的總體工作流程如圖1所示。

2 基于GPS與雷達(dá)的車輛位置信息估計

GPS模塊獲得的是帶有噪聲的車輛絕對位置信息，通過設(shè)計濾波器對該噪聲信號進(jìn)行處理，可實(shí)現(xiàn)基于本車GPS觀測位置信息粗估計。本文中采用擴(kuò)展卡爾曼濾波[5-6]對GPS信號進(jìn)行消噪處理。

2.1 基于GPS本車觀測定位信息估計

對于車輛而言，當(dāng)側(cè)偏角為零時，將t時刻的線速度用vt表示，角速度用ωt表示，則控制向量設(shè)正的ωt產(chǎn)生一個逆時針旋轉(zhuǎn)，正的vt產(chǎn)生一個向前移動。速度運(yùn)動模型的狀態(tài)向量為

式中：xt′，yt為直角坐標(biāo)系下的位置坐標(biāo)；θt為車輛航向角。

在無噪聲影響的理想情況下，設(shè)初始狀態(tài)向量為(x，y，θ)T，控制向量為(v，ω)T，在 Δt時間后的狀態(tài)向量為(x′，y′，θ′)T，則有

式中為該 Δt時間內(nèi)運(yùn)動的圓心。

經(jīng)簡化，在 Δt時間后的狀態(tài)向量(x′，y′，θ′)T可以表示為

實(shí)際上，獲取的車輛運(yùn)動信息含有噪聲，假設(shè)該噪聲來自控制量。首先，設(shè)真實(shí)運(yùn)動的線速度和角速度為和則有

式中：εv和εω分別為真實(shí)控制量與理想控制量的誤差。

僅在εv和εω的作用下，車輛的真實(shí)運(yùn)動和理想運(yùn)動之間的誤差有兩個自由度，只能描述圓周運(yùn)動，這與車輛真實(shí)運(yùn)動的情況并不相符。因此須引入一個誤差參數(shù)，以使理想運(yùn)動與運(yùn)動誤差疊加后的真實(shí)狀態(tài)空間與建立的模型中所有可能的狀態(tài)組合的狀態(tài)空間等價。為此，引入虛擬控制量γ＝0，且有

此時，新的控制向量為(v，ω，γ)T，其中γ≡0，εγ為虛擬控制量誤差，這樣的引入只是為使得對運(yùn)動誤差的建模變得完整，使：

式中εθ表示車輛航向角的噪聲。

此外，假設(shè)控制向量中各誤差均服從正態(tài)分布：

因此，車輛的真實(shí)運(yùn)動模型表示為

因?yàn)檐囕v運(yùn)動模型為非線性模型，故應(yīng)用擴(kuò)展卡爾曼濾波對車輛狀態(tài)進(jìn)行估計。

基于擴(kuò)展卡爾曼濾波的位置估計具體步驟如下[7]。

將t時刻真實(shí)狀態(tài)向量記為V t＝(xt，yt，θt)T，濾波器對狀態(tài)的預(yù)測(無觀測信息)記為獲取觀測信息后修正的狀態(tài)估計記為

控制向量記為其中γ≡0，觀測向量記為預(yù)測狀態(tài)協(xié)方差矩陣記為，獲得觀測信息后估計協(xié)方差矩陣記為Σt，記狀態(tài)預(yù)測誤差為，記狀態(tài)估計誤差為有

觀測向量由GPS采集的經(jīng)緯度與航向角獲得，控制向量中的速度由GPS采集，橫擺角速度根據(jù)車輛轉(zhuǎn)向盤轉(zhuǎn)角和速度轉(zhuǎn)換得到。

通過狀態(tài)轉(zhuǎn)移方程預(yù)測t時刻狀態(tài)：

對狀態(tài)轉(zhuǎn)移方程求μt-1的1階偏導(dǎo)的雅可比矩陣：

狀態(tài)轉(zhuǎn)移方程求u t的1階偏導(dǎo)的雅可比矩陣：

t時刻預(yù)測狀態(tài)協(xié)方差矩陣更新為

其中Q為控制噪聲協(xié)方差矩陣：

記t時刻控制誤差為有

狀態(tài)觀測方程：

對狀態(tài)觀測方程求的雅可比矩陣：

計算卡爾曼增益：

其中R為觀測噪聲協(xié)方差矩陣：

記t時刻GPS觀測誤差為

根據(jù)觀測信息修正對狀態(tài)的估計：

更新t時刻估計狀態(tài)協(xié)方差矩陣為

式中I為與μt同維數(shù)的單位矩陣。μt和Σt即為t時刻車輛狀態(tài)估計的輸出結(jié)果。

2.2 基于雷達(dá)的目標(biāo)絕對位置計算

采用毫米波雷達(dá)對周圍車輛進(jìn)行觀測。毫米波雷達(dá)的特點(diǎn)為對目標(biāo)的縱向(y向)距離測量精度較高，而橫向(x向)距離測量精度較低[8-9]。記t時刻雷達(dá)對第j個目標(biāo)相對位置的測量誤差假設(shè)

將GPS本車粗估計絕對位置與雷達(dá)探測目標(biāo)相對位置進(jìn)行疊加，得到雷達(dá)第j個目標(biāo)的絕對位置：

由于自身粗估計誤差與雷達(dá)探測相對位置誤差均服從多維正態(tài)分布，由正態(tài)分布性質(zhì)可得的協(xié)方差矩陣為

上述和即為基于雷達(dá)觀測的目標(biāo)絕對位置信息。

3 多車定位信息融合

3.1 基于車車通信的本車定位估計

如上所述，對GPS定位信息進(jìn)行濾波后的本車位置估計結(jié)果，通過車車通信發(fā)送給周圍車輛。同時，車輛通過毫米波雷達(dá)可探知周圍車輛相對于本車的位置。由于周圍車輛狀態(tài)中的航向角θ項(xiàng)在相對測距中不起作用，故記t時刻周圍車輛i通信發(fā)送的本車位置估計為狀態(tài)估計協(xié)方差矩陣為

記t時刻本車?yán)走_(dá)探測到車輛i的相對位置為換算到直角坐標(biāo)系為其中為i車與本車的距離為i車與本車連線和水平x軸的夾角如圖 2所示。

圖2 雷達(dá)探測參數(shù)示意

則根據(jù)車輛i與本車相對定位得到的本車位置估計為

假設(shè)t時刻雷達(dá)對相對位置的測量誤差向量為在x向和y向上均服從正態(tài)分布且互不相關(guān)，即為雷達(dá)相對測距的協(xié)方差矩陣，則根據(jù)車輛i與本車相對定位得到的本車位置估計的協(xié)方差矩陣為

?和即為本車與車輛i相對定位得到的本車位置估計的輸出結(jié)果。

3.2 本車定位信息最優(yōu)融合估計

如果本車在時刻t有附近的Nt輛車的絕對位置信息及其與本車的相對位置信息，則可通過上節(jié)的方法得到Nt個通過相對定位得到的本車位置估計，加上本車自車通過擴(kuò)展卡爾曼濾波得到的位置估計，共有Nt+1個對本車的位置估計結(jié)果。文中基于數(shù)據(jù)融合理論[1，10]對這Nt+1個估計結(jié)果進(jìn)行綜合。

記t時刻車輛i的絕對位置觀測誤差為控制誤差為本車對車輛i的雷達(dá)觀測誤差為本文假設(shè)：

則有

式中：δpq為克羅內(nèi)克函數(shù)另外，假設(shè)不同車輛的初始狀態(tài)估計誤差不相關(guān)：

記本車與車輛i相對定位得到的本車位置估計誤差為基于上述條件，不難證明：

基于上述推論，基于局部估計誤差不相關(guān)假設(shè)下的最優(yōu)融合估計對所有本車位置估計進(jìn)行融合：

式中：Nt為t時刻本車附近獲知絕對位置與雷達(dá)相對位置的車輛數(shù)。

融合后的估計協(xié)方差矩陣為

μ′t和Σ′t即為多車信息融合得到的本車位置估計。

4 仿真試驗(yàn)與測試

4.1 仿真參數(shù)設(shè)置

為驗(yàn)證基于GRI的多車協(xié)同定位算法的有效性，通過MATLAB中建立了多車運(yùn)動場景，并進(jìn)行對上述各部分算法的仿真。仿真時間為30s，時間步長0.1s。

本文中多車運(yùn)動仿真場景分為直線運(yùn)動場景與換道運(yùn)動場景兩種。

(1)直線運(yùn)動場景

該場景中有2個車道，車道寬3.75m，每車道5輛車，均沿各自車道的中心線直線行駛。左側(cè)車道為快車道，5輛車初始車速均為 50km/h，車間距80m。右側(cè)車道為慢車道，5輛車初始車速均為30km/h，車距50m，如圖3所示。仿真開始后t在0～10s時間段內(nèi)，兩車道的頭車均按照如圖4所示的加速度曲線運(yùn)動。t＞10s后，兩頭車保持勻速直線運(yùn)動。其他8輛車根據(jù)線性跟馳模型對前車的速度波動產(chǎn)生響應(yīng)：

式中：xr(t)和xf(t)為t時刻后車和前車沿車道方向到原點(diǎn)的距離，參數(shù)取λ＝0.3，T＝1s。

圖3 直線運(yùn)動場景示意圖

(2)換道運(yùn)動場景

該場景中有2個車道，車道寬3.75m，每車道各有5輛車，初始時均沿各自車道的中心線直線行駛。兩車道上車輛的初始車速均為30km/h，4號與5號車之間、6號與7號車之間前后車間距100m，其他車輛前后車間距50m，如圖5所示。

開始仿真后t在3～8s時間段內(nèi)，2號車和9號

圖4 頭車加速度曲線(部分)

圖5 換道運(yùn)動場景示意圖

車進(jìn)行換道操作，其他車輛保持勻速直線運(yùn)動。換道操作的轉(zhuǎn)向角模型如圖6所示。又根據(jù)本文中車輛運(yùn)動仿真模型，得到的換道位置曲線如圖7所示。

圖6 車輛換道轉(zhuǎn)向角模型

圖7 換道運(yùn)動橫向位置曲線

本文中的仿真試驗(yàn)對雷達(dá)、GPS與車車通信等各模塊的功能參數(shù)設(shè)置如下。

雷達(dá)：可探測到以本車位置為圓心，半徑100m范圍內(nèi)的周圍車輛與本車的相對位置，要求周圍車輛與本車之間無遮擋。一般的毫米波雷達(dá)對縱向位置探測精度比橫向位置探測精度高，誤差均滿足正態(tài)分布，標(biāo)準(zhǔn)差σrx＝0.2m，σry＝0.1m。 每隔0.1s更新一次。

GPS：可提供本車的絕對位置和速度信息，橫向、縱向和航向角誤差均滿足正態(tài)分布，標(biāo)準(zhǔn)差σzx＝σzy＝3.33m，σzθ＝π/180，σzv＝1m/s。 每隔0.1s更新一次。

車車通信：可接收到以本車位置為圓心，半徑100m范圍內(nèi)的周圍車輛發(fā)送的自身位置信息。為測試在通信不穩(wěn)定的情況下基于GRI的多車協(xié)同定位算法的表現(xiàn)，設(shè)在t＝20～25s時間段內(nèi)車車通信失效，所有車輛無法獲得周圍車輛的絕對位置信息。在其他時間內(nèi)，每隔0.1s更新一次車輛發(fā)送和接收的通信信息。

另外，假設(shè)橫擺角速度測量誤差滿足正態(tài)分布，標(biāo)準(zhǔn)差σzω＝π/180s-1。每隔0.1s更新一次。

4.2 仿真結(jié)果分析

在兩個場景下，對基于GRI的多車協(xié)同定位算法進(jìn)行仿真，結(jié)果如圖8、圖9和表1所示。其中圖8和圖9分別為直行場景和換道場景下協(xié)同定位算法x向和y向的均方根誤差隨時間的變化曲線，而表1則為按通信正常和通信失效兩個階段分別計算的兩種場景平均的x向、y向和總的均方根誤差。皆為除頭車(5#車和10#車)外其余8輛車的平均結(jié)果。

圖8 直線場景協(xié)同定位算法的均方根誤差的時間歷程

由表1可見，協(xié)同定位的誤差水平總體保持在0.6m以下。這說明本文中的方法科學(xué)有效，能高精度地對車輛進(jìn)行定位。從表中還可看出，在通信失效情況下定位誤差較大，這體現(xiàn)了車車通信在基于GRI的多車協(xié)同定位中的重要性。

圖9 換道場景協(xié)同定位算法的均方根誤差的時間歷程

表1 本車定位誤差統(tǒng)計

5 結(jié)論

本文中提出了基于GRI的多車協(xié)同定位的方法，并通過MATLAB軟件進(jìn)行了模擬仿真，通過大量重復(fù)試驗(yàn)取均方根值分析各種定位算法的誤差，發(fā)現(xiàn)最優(yōu)融合的協(xié)同定位算法在通信理想條件下誤差極小，即便短時間通信失效，平均誤差水平也控制在0.6m以內(nèi)，這表明該方法合理有效，能高精度地對周圍行駛車輛進(jìn)行定位。但是由于本文中速度運(yùn)動模型有一定的理想假設(shè)，實(shí)際的車輛轉(zhuǎn)向過程中側(cè)偏角廣泛存在，與車輛真實(shí)運(yùn)動的描述有一定偏差，對該因素的忽略可能導(dǎo)致本車位置的濾波跟蹤效果減弱。

參考文獻(xiàn)

[1]QURESHIK N，ABDULLAH A H.Localization-based system challenges in vehicular Ad Hoc networks：Survey[J].Smart Computing Review，2014，4(6)：515-528.

[2]ALAM N，DEMPSTER A.Cooperative positioning for vehicular networks：Facts and future[J].IEEE Transactions on Intelligent Transportation Systems，2013，14(4).

[3]EFATMANESHNIK M，ALAM N，KEALY A，et al.A fast multidimensional scaling filter for vehicular cooperative positioning[J].Journal of Navigation，2012，65(2)：223-243.

[4]SHERER T P，MARTIN SM，et al.Radar probabilistic data association filter with GPSaiding for target selection and relative position determination[C].Proceedings of the ION 2017 Pacific PNT Meeting， Honolulu， Hawaii，2017：419-428.

[5]LIU K，LIM H B.Positioning accuracy improvement via distributed location estimate in cooperative vehicular networks[C].Conference Record-IEEE Conference on Intelligent Transportation Systems，2012：1549-1554.

[6]GOLESTAN K， SATTAR F， KARRAY F， et al.Localization in vehicular ad hoc networks using data fusion and V2V communication[J].Computer Communications，2015，71：61-72.

[7]劉江，蔡伯根，王云鵬.基于GNSS/DSRC融合的協(xié)同車輛定位方法[J].交通運(yùn)輸工程學(xué)報，2014(4)：116-126.

[8]AKCAN H，EVRENDILEK C.GPS-free directional localization via dual wireless radios[J].Computer Communications，2012，35(9)：1151-1163.

[9]YAN T，ZHANG W，WANG G.A grid-based on-road localization system in VANET with linear error propagation[J].IEEE Transactions on Wireless Communications，2014，13(2)：861-875.

[10]BHAWIYUGA A，NGUYEN H H，JEONG H Y.An accurate vehicle positioning algorithm based on vehicle-to-vehicle(V2V)communications[C].IEEE International Conference on Vehicular Electronics and Safety，IEEE，2012：273-278.