孫 寧,閆夢如,倪 捷,葛如海,秦洪懋,唐翊銘,高行健
(1.江蘇大學(xué)汽車與交通工程學(xué)院,鎮(zhèn)江 212013; 2.北京航空航天大學(xué),車路協(xié)同與安全控制重點實驗室,北京 100191)
隨著汽車自動駕駛技術(shù)的發(fā)展,車輛對環(huán)境感知的要求也越來越高。獲取實時、準(zhǔn)確的自身和周圍車輛位置信息(如經(jīng)度、緯度、高度等)是環(huán)境感知內(nèi)容的重要組成部分,也相應(yīng)面臨著更加嚴格的精度要求。如自動緊急制動系統(tǒng),對前方車輛的位置誤差要求就在1m以內(nèi)。因此,如何提高車輛位置信息的定位精度[1],是目前國內(nèi)外汽車領(lǐng)域?qū)W者面臨的一項新課題。高精度車輛定位方法的研究,對發(fā)展汽車自動駕駛技術(shù)具有十分重要的意義。
鑒于經(jīng)濟成本因素,目前單一應(yīng)用傳統(tǒng)GPS或毫米波雷達,都無法滿足上述車輛位置定位的高精度要求。而通信技術(shù)的發(fā)展則為高精度車輛定位提供了一種新的途徑。在車車通信、車路通信的支持下,車輛間可相互交換豐富的信息,同時,車輛自身獲取周圍環(huán)境信息的能力大大提升,從而可以計算獲得更加準(zhǔn)確的位置信息[2-3]。本文中在傳統(tǒng)GPS和毫米波雷達獲取初步車輛定位信息的基礎(chǔ)上,參照文獻[4]中的思路,提出一種基于GPS/雷達集成(GPS/radar integration,GRI)的多車協(xié)同定位方法,為獲取高精度的車輛位置信息提供一種新思路。
本文中提出的多車協(xié)同定位系統(tǒng)由GPS、毫米波雷達和車車通信3大模塊組成。其中GPS數(shù)據(jù)已經(jīng)通過計算轉(zhuǎn)換為局部區(qū)域的平面坐標(biāo)。車車通信模塊為LTE-V/DSRC。
系統(tǒng)中在基于GRI的多車協(xié)同定位過程中,對定位的估計分為兩階段。第一階段,進行基于本車位置信息的粗估計,即通過GPS獲取含噪聲的車輛的絕對位置信息,經(jīng)卡爾曼濾波器處理,再通過雷達得到周圍車輛位置的粗估計;第二階段,將粗估計的結(jié)果通過車車通信進行分享,進行綜合多車位置信息的融合估計,得出更加準(zhǔn)確的本車和周圍車輛的絕對位置。協(xié)同定位的總體工作流程如圖1所示。
GPS模塊獲得的是帶有噪聲的車輛絕對位置信息,通過設(shè)計濾波器對該噪聲信號進行處理,可實現(xiàn)基于本車GPS觀測位置信息粗估計。本文中采用擴展卡爾曼濾波[5-6]對GPS信號進行消噪處理。
對于車輛而言,當(dāng)側(cè)偏角為零時,將t時刻的線速度用vt表示,角速度用ωt表示,則控制向量設(shè)正的ωt產(chǎn)生一個逆時針旋轉(zhuǎn),正的vt產(chǎ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)運動的圓心。
經(jīng)簡化,在 Δt時間后的狀態(tài)向量(x′,y′,θ′)T可以表示為
實際上,獲取的車輛運動信息含有噪聲,假設(shè)該噪聲來自控制量。首先,設(shè)真實運動的線速度和角速度為和則有
式中:εv和εω分別為真實控制量與理想控制量的誤差。
僅在εv和εω的作用下,車輛的真實運動和理想運動之間的誤差有兩個自由度,只能描述圓周運動,這與車輛真實運動的情況并不相符。因此須引入一個誤差參數(shù),以使理想運動與運動誤差疊加后的真實狀態(tài)空間與建立的模型中所有可能的狀態(tài)組合的狀態(tài)空間等價。為此,引入虛擬控制量γ=0,且有
此時,新的控制向量為(v,ω,γ)T,其中γ≡0,εγ為虛擬控制量誤差,這樣的引入只是為使得對運動誤差的建模變得完整,使:
式中εθ表示車輛航向角的噪聲。
此外,假設(shè)控制向量中各誤差均服從正態(tài)分布:
因此,車輛的真實運動模型表示為
因為車輛運動模型為非線性模型,故應(yīng)用擴展卡爾曼濾波對車輛狀態(tài)進行估計。
基于擴展卡爾曼濾波的位置估計具體步驟如下[7]。
將t時刻真實狀態(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é)果。
采用毫米波雷達對周圍車輛進行觀測。毫米波雷達的特點為對目標(biāo)的縱向(y向)距離測量精度較高,而橫向(x向)距離測量精度較低[8-9]。記t時刻雷達對第j個目標(biāo)相對位置的測量誤差假設(shè)
將GPS本車粗估計絕對位置與雷達探測目標(biāo)相對位置進行疊加,得到雷達第j個目標(biāo)的絕對位置:
由于自身粗估計誤差與雷達探測相對位置誤差均服從多維正態(tài)分布,由正態(tài)分布性質(zhì)可得的協(xié)方差矩陣為
上述和即為基于雷達觀測的目標(biāo)絕對位置信息。
如上所述,對GPS定位信息進行濾波后的本車位置估計結(jié)果,通過車車通信發(fā)送給周圍車輛。同時,車輛通過毫米波雷達可探知周圍車輛相對于本車的位置。由于周圍車輛狀態(tài)中的航向角θ項在相對測距中不起作用,故記t時刻周圍車輛i通信發(fā)送的本車位置估計為狀態(tài)估計協(xié)方差矩陣為
記t時刻本車雷達探測到車輛i的相對位置為換算到直角坐標(biāo)系為其中為i車與本車的距離為i車與本車連線和水平x軸的夾角如圖 2所示。
圖2 雷達探測參數(shù)示意
則根據(jù)車輛i與本車相對定位得到的本車位置估計為
假設(shè)t時刻雷達對相對位置的測量誤差向量為在x向和y向上均服從正態(tài)分布且互不相關(guān),即為雷達相對測距的協(xié)方差矩陣,則根據(jù)車輛i與本車相對定位得到的本車位置估計的協(xié)方差矩陣為
?和即為本車與車輛i相對定位得到的本車位置估計的輸出結(jié)果。
如果本車在時刻t有附近的Nt輛車的絕對位置信息及其與本車的相對位置信息,則可通過上節(jié)的方法得到Nt個通過相對定位得到的本車位置估計,加上本車自車通過擴展卡爾曼濾波得到的位置估計,共有Nt+1個對本車的位置估計結(jié)果。文中基于數(shù)據(jù)融合理論[1,10]對這Nt+1個估計結(jié)果進行綜合。
記t時刻車輛i的絕對位置觀測誤差為控制誤差為本車對車輛i的雷達觀測誤差為本文假設(shè):
則有
式中:δpq為克羅內(nèi)克函數(shù)另外,假設(shè)不同車輛的初始狀態(tài)估計誤差不相關(guān):
記本車與車輛i相對定位得到的本車位置估計誤差為基于上述條件,不難證明:
基于上述推論,基于局部估計誤差不相關(guān)假設(shè)下的最優(yōu)融合估計對所有本車位置估計進行融合:
式中:Nt為t時刻本車附近獲知絕對位置與雷達相對位置的車輛數(shù)。
融合后的估計協(xié)方差矩陣為
μ′t和Σ′t即為多車信息融合得到的本車位置估計。
為驗證基于GRI的多車協(xié)同定位算法的有效性,通過MATLAB中建立了多車運動場景,并進行對上述各部分算法的仿真。仿真時間為30s,時間步長0.1s。
本文中多車運動仿真場景分為直線運動場景與換道運動場景兩種。
(1)直線運動場景
該場景中有2個車道,車道寬3.75m,每車道5輛車,均沿各自車道的中心線直線行駛。左側(cè)車道為快車道,5輛車初始車速均為 50km/h,車間距80m。右側(cè)車道為慢車道,5輛車初始車速均為30km/h,車距50m,如圖3所示。仿真開始后t在0~10s時間段內(nèi),兩車道的頭車均按照如圖4所示的加速度曲線運動。t>10s后,兩頭車保持勻速直線運動。其他8輛車根據(jù)線性跟馳模型對前車的速度波動產(chǎn)生響應(yīng):
式中:xr(t)和xf(t)為t時刻后車和前車沿車道方向到原點的距離,參數(shù)取λ=0.3,T=1s。
圖3 直線運動場景示意圖
(2)換道運動場景
該場景中有2個車道,車道寬3.75m,每車道各有5輛車,初始時均沿各自車道的中心線直線行駛。兩車道上車輛的初始車速均為30km/h,4號與5號車之間、6號與7號車之間前后車間距100m,其他車輛前后車間距50m,如圖5所示。
開始仿真后t在3~8s時間段內(nèi),2號車和9號
圖4 頭車加速度曲線(部分)
圖5 換道運動場景示意圖
車進行換道操作,其他車輛保持勻速直線運動。換道操作的轉(zhuǎn)向角模型如圖6所示。又根據(jù)本文中車輛運動仿真模型,得到的換道位置曲線如圖7所示。
圖6 車輛換道轉(zhuǎn)向角模型
圖7 換道運動橫向位置曲線
本文中的仿真試驗對雷達、GPS與車車通信等各模塊的功能參數(shù)設(shè)置如下。
雷達:可探測到以本車位置為圓心,半徑100m范圍內(nèi)的周圍車輛與本車的相對位置,要求周圍車輛與本車之間無遮擋。一般的毫米波雷達對縱向位置探測精度比橫向位置探測精度高,誤差均滿足正態(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更新一次。
在兩個場景下,對基于GRI的多車協(xié)同定位算法進行仿真,結(jié)果如圖8、圖9和表1所示。其中圖8和圖9分別為直行場景和換道場景下協(xié)同定位算法x向和y向的均方根誤差隨時間的變化曲線,而表1則為按通信正常和通信失效兩個階段分別計算的兩種場景平均的x向、y向和總的均方根誤差。皆為除頭車(5#車和10#車)外其余8輛車的平均結(jié)果。
圖8 直線場景協(xié)同定位算法的均方根誤差的時間歷程
由表1可見,協(xié)同定位的誤差水平總體保持在0.6m以下。這說明本文中的方法科學(xué)有效,能高精度地對車輛進行定位。從表中還可看出,在通信失效情況下定位誤差較大,這體現(xiàn)了車車通信在基于GRI的多車協(xié)同定位中的重要性。
圖9 換道場景協(xié)同定位算法的均方根誤差的時間歷程
表1 本車定位誤差統(tǒng)計
本文中提出了基于GRI的多車協(xié)同定位的方法,并通過MATLAB軟件進行了模擬仿真,通過大量重復(fù)試驗取均方根值分析各種定位算法的誤差,發(fā)現(xiàn)最優(yōu)融合的協(xié)同定位算法在通信理想條件下誤差極小,即便短時間通信失效,平均誤差水平也控制在0.6m以內(nèi),這表明該方法合理有效,能高精度地對周圍行駛車輛進行定位。但是由于本文中速度運動模型有一定的理想假設(shè),實際的車輛轉(zhuǎn)向過程中側(cè)偏角廣泛存在,與車輛真實運動的描述有一定偏差,對該因素的忽略可能導(dǎo)致本車位置的濾波跟蹤效果減弱。
參考文獻
[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].交通運輸工程學(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.