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

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

前言

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

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

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

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

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

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

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

2.1 基于GPS本車觀測定位信息估計(jì)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

記t時(shí)刻控制誤差為有

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

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

計(jì)算卡爾曼增益：

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

記t時(shí)刻GPS觀測誤差為

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

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

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

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

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

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

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

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

3 多車定位信息融合

3.1 基于車車通信的本車定位估計(jì)

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

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

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

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

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

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

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

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

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

則有

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

GPS：可提供本車的絕對(duì)位置和速度信息，橫向、縱向和航向角誤差均滿足正態(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時(shí)間段內(nèi)車車通信失效，所有車輛無法獲得周圍車輛的絕對(duì)位置信息。在其他時(shí)間內(nèi)，每隔0.1s更新一次車輛發(fā)送和接收的通信信息。

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

4.2 仿真結(jié)果分析

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

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

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

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

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

5 結(jié)論

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