基于概率統計的車輛運動軌跡預測方法

張金旺，章永進，徐友春

(1.軍事交通學院 研究生管理大隊，天津 300161； 2.軍事交通學院 軍用車輛系，天津 300161)

基于概率統計的車輛運動軌跡預測方法

張金旺1，章永進2，徐友春2

(1.軍事交通學院 研究生管理大隊，天津 300161； 2.軍事交通學院 軍用車輛系，天津 300161)

針對自主駕駛車輛預測障礙物車輛運行軌跡的問題，以高速公路上的車輛為研究對象，對其運動進行簡化建模，采集大量數據作為訓練樣本對模型進行訓練，并分別采用統計距離和馬爾科夫鏈對其橫向運動和縱向運動進行預測。仿真實驗表明，該模型能夠有效預測障礙物車輛3 s時間內的軌跡，可為自主駕駛車輛碰撞預警和軌跡規劃提供可靠依據。

馬爾科夫預測；軌跡預測；自主駕駛車輛；統計距離；高速公路

自動駕駛領域的安全問題是人們一直關注的重要問題之一。對于駕駛環境中的靜態障礙物，自主駕駛車輛可安全地沿規劃層規劃的軌跡行駛，但對于動態障礙物，僅靠實時感知來進行規劃存在較高的安全隱患。基于這樣的考慮，自主駕駛車輛在運行時應該對其周圍的運動目標進行有效預測，獲取未來時刻的運動軌跡，這樣可以大大提高自主駕駛車輛行駛的安全性。

現有的軌跡預測方法根據建模的抽象程度可以分為3類：基于物理學模型的軌跡預測方法、基于運動模式的軌跡預測方法和基于交互感知的軌跡預測方法[1]。Zhang R等[2]運用卡爾曼濾波(Kalman filter)和基于恒定車速的物理學模型對障礙物車輛進行軌跡預測。Schreier M等[3]將障礙物車輛運動模式通過貝葉斯網絡進行分類，然后運用Monte-Carlo方法進行仿真，計算出碰撞概率。Lefèvre S等[4]引入了一個名為“預期機動”的中間變量，對道路交叉口車輛的聯合運動進行建模，運用狀態因子來解釋車輛間相互影響，并將車輛之間的因果依賴性表達為環境的函數。陳思靜等[5]利用車輛軌跡的歷史數據構建狀態轉移矩陣，提出了基于馬爾科夫鏈的車輛軌跡預測方法。

自主駕駛車輛在高速公路環境運行時，對障礙物車輛軌跡預測的實時性要求比較高，為平衡對障礙物車輛預測的準確性與實時性要求，采用基于運動模式的軌跡預測方法對障礙物車輛軌跡進行預測，為自主駕駛車輛碰撞預警和軌跡規劃提供依據。

1 總體思路和假設條件

自主駕駛車輛在高速公路上行駛時，障礙物車輛軌跡預測將為其軌跡規劃提供重要的參考，是自動駕駛中的關鍵問題。基于物理學模型的軌跡預測方法在特定運行狀態下可以獲取較好的預測精度，但在真實駕駛環境中，車輛運動存在隨機性，在相同初始條件下，兩次實驗到達位置可能并不相同，這樣的不確定主要來自于感知層誤差、駕駛狀態、地面摩擦系數等因素，這些誤差很難通過建立準確的模型來解決。為了提高預測精度，獲取障礙物更全面的運動狀態信息，本文采用概率思想解決本問題，運用統計距離和馬爾科夫鏈分別對障礙物車輛將要行駛的車道和縱向位置進行預測，進而表示出障礙物在不同時刻運動狀態的概率分布。

為了方便描述，做幾點假設：

(1)在高速公路上行駛時，道路曲率較小，可假設障礙物車輛在直道上行駛；

(2)將自主駕駛車輛和障礙物車輛簡化為質點，其運動分解為相互獨立的橫向運動和縱向運動，并建立坐標系，以主車GPS位置作為坐標原點、前進方向為x軸正方向、車體右側為y軸正方向，建立平面直角坐標系；

(3)障礙物車輛運動可分解為自主駕駛車輛坐標系下的縱向運動和橫向運動，障礙物車輛在高速公路直線行駛時，一直與自主駕駛車輛同向行駛，行駛航向基本保持一致；

(4)假設障礙物車輛按照交通法規正常行駛，沒有應急狀況。在高速公路行駛時，很少出現應急狀態。應激狀態在統計上屬于小概率事件，在本文中不考慮。

2 運動模型

在高速公路上行駛的車輛運動分解為沿車體前進方向的縱向運動和與其方向垂直的橫向運動。在車輛換道過程中，換道長度約100 m，與道路寬度相差兩個量級，航向差別很小，在假設中縱向失速可以忽略。在高速公路上行駛時，車輛之間會保持相應的安全距離，很少出現距離很近的換道行為。因此，將橫向運動作為定性分析障礙物車輛運動模式的依據，并在運動模式預測的基礎上將縱向位置作為是否發生碰撞的依據。

2.1 橫向運動模型

在高速公路環境中定義兩種運動模式：直道行駛和換道行駛。自主駕駛車輛正常行駛時，需要對障礙物車輛將要處于的車道進行預測。首先，獲取本車GPS信息，完成地圖定位；然后進行坐標系轉化，定義自主駕駛車輛的位置向量G(e)為

G(e)=[0，0]T

(1)

自主駕駛車輛感知層獲取的目標車輛位置向量G(t)為

G(t)=[x，y]T

(2)

式中x、y分別為目標車輛在車體坐標系下的縱坐標和橫坐標。

在高精度地圖中可以獲取目標車輛所在車道中心線的GPS點，轉化到車體坐標系后，其相應表達式為

y(x)=c2x2+c1x+c0

(3)

式中c0、c1和c2為方程參數。

障礙物車輛運動模式可由其距離所在車道兩側邊界長度來判斷，定義Y(p)=[dl，dr]。dl和dr分別代表障礙物車輛當前位置距離車道左邊界和右邊界的距離。車道中心線上的點則定義為Y(l)=[dl，dr]，此時dl、dr都等于車道寬度的一半，這樣就可以得出統計距離的平方：

D2=(Y(l)-Y(p))T·(P(l)-P(p))-1·

(Y(l)-Y(p))

(4)

式中：P(l)和P(p)分別為Y(l)和Y(p)協方差矩陣；D2為卡方分布。

為了避免由于感知誤差所引起的誤判，選取最近的N組數據，賦給相應的權重ωi，進行加權計算，并且越新的數據權值越大：

(5)

在實際應用中設定閾值η，當Dk小于或者等于η時，表示車輛將保持當前車道行駛，記為0；當Dk大于η時，表明障礙物車輛將換道到臨近車道，然后對dl進行判斷，當dl>dr時，車輛將換道至右側車輛，記為1；當dl

(6)

(7)

式中：Yl為位置區間((l-1)Δy，lΔy)；Δy=Ylane/L為區間長度。

2.2 縱向運動模型

a(t+τ1)=λ1(v*-v(t))+ξ1

(8)

式中：τ1為駕駛員的反應時間；v*和v(t)分別為車輛的期望行駛速度和當前行駛速度；λ1為敏感系數；ξ1為隨機變量。

車輛的期望行駛速度依賴于交通規則、道路條件和車輛條件。對于行駛在公路環境下的車輛，為了簡化，將交通規則和道路條件視為不變量，假設v*僅依賴于車輛類型，同時忽略駕駛員的反應時間，那么在自由行駛模式下，任意時刻車輛的加速度a是關于當前速度v的函數，記為a=ξ1(v)，考慮到駕駛員操作的不確定性，所以ξ1(v)是一個隨機函數。

設障礙物車輛的縱向位置和速度分別為x和v，定義(x,v)表示車輛的運動狀態，其縱向運動滿足如下方程:

(9)

式中a為系統輸入，是關于v的隨機函數。

(10)

式中Xi為位置區間(xi-1，xi)，xi=x0+iΔx，Δx為區間長度，x0為x的最小可能值。

綜上所述，障礙物的橫向軌跡和縱向軌跡分別用不同離散時刻對其橫向位置和縱向位置的離散概率分布描述。參考概率碰撞檢測[7]中建模障礙物軌跡的方法，以t=0為起始時刻，設軌跡預測的時間長度為T，綜合考慮側向和縱向位置分布，將障礙物的概率軌跡定義為離散時刻障礙物在二維道路平面上位置的概率分布:

(11)

以車體為坐標系的模型示意圖如圖1所示，圖中自主駕駛車輛兩側的障礙物車輛前方二維平面被離散化為大小相同的矩形區域，每個區域的灰度值表示一定時間后該區域的概率，顏色越深表示概率越高。

圖1 車輛軌跡預測

3 障礙物車輛軌跡預測

在實際情況中影響障礙物車輛的橫向運動狀態和縱向運動狀態的關鍵變量很多，建立較為準確的模型難度較大，因此從統計學角度統計不同運動狀態下關鍵變量經驗分布的方法，并運用其分布對障礙物車輛運動狀態進行預測。

橫向運動的預測方法在2.1節已基本描述清楚，縱向運動較為復雜，重點介紹運動模式預測結束后的縱向運動的預測。

在縱向位置預測上，將時間軸離散化為等間隔的區間，在每個時間區間內假設障礙物的加速度為常量，從而可以迭代計算各離散時刻障礙物的運動狀態。根據式(8)，障礙物加速度a僅依賴于當前時刻的運動狀態，統計出不同速度段的加速度a的分布(如圖2所示)。高速公路上行駛速度較慢時，車輛基本處于加速狀態；隨著速度的提高，車輛速度趨于平穩，加速度大都集中在0附近，這也符合日常行車習慣。

圖2 加速度在不同速度階段概率分布

馬爾科夫鏈是一種特殊的離散時間隨機過程，由于具有“無記憶性”，即下一時刻系統狀態的概率分布僅依賴于當前時刻狀態，與之前的狀態無關，已被廣泛應用于車輛運動建模[7-8]。基于此，定義障礙物的縱向運動狀態為系統狀態，采用馬爾科夫鏈描述障礙物的縱向運動過程，由障礙物加速度的隨機性引導障礙物車輛向不同的運動狀態轉移。

對障礙物的運動狀態變量做離散化處理，將縱向位置離散化為I個區間X1，X2，…，XI的基礎上，進一步將速度離散化為等長度的J個區間V1，V2，…，VJ。定義系統狀態由縱向位置狀態和速度狀態構成，每個離散狀態Sm(m∈{1，2，…，M}，M=I·J)對應于一個位置區間xi(i∈{1，2，…，I})和一個速度區間Vj(j∈{1，2，…，J})：

Sm=(xi，Vj)，m=(i-1)J+j

(12)

式中Vj為速度區間(vj-1,vj),vj=v0+jΔv，Δv為區間長度，v0為障礙物速度的最小可能值。

根據障礙物在t=rΔt時刻的位置和速度，將此時障礙物處于狀態的概率表示為

(13)

(14)

進一步，定義轉移概率矩陣Φlon=[φnm]M×M，其中φnm為第n行第m列的元素，則障礙物縱向運動的狀態預測方程可表示為

Plon(rΔt)=Φlon·Plon((r-1)Δt)

(15)

(16)

假設在t=(r-1)Δt時刻障礙物處于Sm狀態，位置xstart和速度vend分別計算為

(17)

障礙物以加速度ak運動時間T后的位置xend和速度vend為

(18)

設xend∈Xi，vend∈Vj，則對n= 1，2，…，M，轉移概率矩陣Φlon中的元素φnm為

(19)

式中：

(20)

當進行縱向軌跡預測時，設在t=0時刻障礙物的位置為xinit、速度為vinit。首先，根據xinit和vinit初始化縱向運動狀態概率向量Plon(0)；然后，設r=1，迭代執行如下過程：根據Plon((r-1)Δt)計算Plon(T)，直至得到Plon(2T)、Plon(3T)。

針對Plon((r-1)Δt)每一個可能的運動狀態，根據運動狀態確定加速度條件分布，根據式(17)—(19)，計算轉移概率矩陣，根據計算障礙物在t=r時刻的縱向位置的離散分布：

根據以上軌跡預測算法，障礙物加速度的條件概率分布決定了縱向運動的轉移概率矩陣，進而決定該方向上的概率軌跡。因此，基于線下歷史軌跡數據，通過離線訓練得到不同運動狀態下的加速度條件分布，將其保存為計算參數。當進行在線軌跡預測時，根據運動狀態選擇相應的加速度條件分布，迭代計算各個離散時刻其運動狀態的概率分布。

4 實驗驗證

為檢驗本文設計的基于概率統計的軌跡預測方法的性能，設計了基于卡爾曼濾波[9]的預測算法和基于概率統計的軌跡預測方法。其中，卡爾曼濾波是一種應用普遍的回歸預測方法。實驗運行在主頻為2.6GHz的Intel core i5-4200U計算機上，內存為4GHz，操作系統為Windows 10，實驗平臺為Matlab R2012b。實驗數據為軍事交通學院智能車實驗平臺JJUV6采集的包含車輛運行時的經度、緯度、航向、速度和加速度等信息的實驗數據。實驗數據總量為京津高速時長為10 h記錄數據。運動模型訓練結束后，再進行10次數據采集，隨機抽取3次數據進行仿真(如圖3所示)。

圖3 預測障礙車輛所達區域

仿真結果表明，在完成的37次換道過程中，閾值設定在2.5時，檢測準確率達100%，閾值設定在1.8時，因駕駛員習慣和技術問題，車輛不可能完全保證在車道中央，換道準確率下降到83.7%。閾值過大時換道預測延遲過大，雖然本文對換道過程中的橫向運動不進行精確預測，只進行定性預測，但當閾值過大時換道延時會較高，預測結果將會對車輛規劃產生影響。

在縱向預測時，車道內橫向按正態分布進行預測，將剔除次點后的預測概率點轉化到高精度地圖中，任取100個位置點進行預測，分別預測1 s、2 s、3 s的位置點，預測結果見表1。

表1 預測結果統計

仿真實驗表明，本文所采用的馬爾科夫預測方法能夠有效預測障礙物車輛所達位置，并且能夠較為全面地反映障礙物車輛的可達位置的概率信息。在與卡爾曼濾波算法對比(如圖4所示)中表明，馬爾科夫預測具有較小的預測偏差。分析兩種算法的原理可知，在相同的行駛速度和時間下，卡爾曼濾波算法對于加速度的分布認識不夠，不能有效地對特定速度的加速度進行有效選取、更改，而馬爾科夫預測算法能夠根據經驗數據的加速度進行有效預測，從而在預測障礙物車輛時有較小偏差。

圖4 縱向預測偏差對比

自主駕駛車輛在高速公路上行駛時，程序運行時效性是很重要的指標，將兩種算法的運行時效性進行對比，卡爾曼濾波算法在預測障礙物車輛3 s后運動狀態的時間為0.054 s，基于概率統計的軌跡預測方法的相應運行時間見表2。

表2 基于概率統計的軌跡預測時效 s

當設置迭代時間間隔為0.1 s，也就是預測障礙物車輛3 s內的概率軌跡的迭代次數為30次時，預測運行時間為0.094 s，完全能滿足自主駕駛車輛規劃時間低于200 ms的要求。隨著迭代間隔的增大，程序運行時間有所降低，但在迭代間隔達到0.3 s后，程序運行時間與卡爾曼濾波算法基本相同。迭代間隔的增長也就意味著在預測過程中假設障礙物車輛恒速運行的時間增長，對速度變化的敏感度性降低，進而影響到對障礙物車輛的預測精度。

5 結 語

本文將障礙物車輛運動分解為橫向運動和縱向運動，并根據高速公路環境特點，采用統計距離來預測橫向運動的換道行為，運用馬爾科夫鏈預測縱向運動上的可達位置。通過訓練模型在仿真實驗中的實驗數據表明，此方法可實現高速公路上障礙物車輛3 s時間內的預測，并可為自主駕駛車輛在進行軌跡規劃時提供可靠信息參考。但還存在所采用的訓練數據樣本不夠豐富、轉移概率矩陣過大、橫向預測不夠等問題，是下一步需要繼續改進和優化的方向。

[2] ZHANG R, CAO L, BAO S, et al. A method for connected vehicle trajectory prediction and collision warning algorithm based on V2V communication[J]. International Journal of Crashworthiness, 2017, 22(1): 15-25.

[3] SCHREIER M, WILLERT V, ADAMY J. Bayesian, maneuver-based, long-term trajectory prediction and criticality assessment for driver assistance systems[C]//Intelligent Transportation Systems (ITSC), 2014 IEEE 17th International Conference on. IEEE, 2014: 334-341.

[5] 陳思靜,張可.VANETs中的車輛移動規律性及軌跡預測研究[J].計算機工程與應用,2016,52(18):139-143.

[6] 李力.現代交通流理論與應用:卷1[M].北京:清華大學出版社, 2011:71-72.

[7] ALTHOFF M, STURSBERG O, BUSS M. Model-based prob-abilistic collision detection in autonomous driving[J]. IEEE Transactions on Intelligent Transportation Systems, 2009,10(2):299-310.

[8] KRUMM J A. Markov model for driver turn prediction[C]//SAE 2008 World Congress. Detroit, MI，2008:1-25.

[9] DENG H B, ZHANG L, WU Y, et al. Research on track estimation based on Kalman filtering algorithm[J]. Transducer and Microsystem Technologies, 2012, 31(5):4-7.

(編輯： 張峰)

Prediction Method of Vehicle Trajectory Based on Probability Statistics

ZHANG Jinwang1, ZHANG Yongjin2, XU Youchun2

(1.Postgraduate Training Brigade, Military Transportation University, Tianjin 300161, China; 2.Military Vehicle Department, Military Transportation University, Tianjin 300161, China)

Considering the problem of autonomous vehicles predicting obstacle vehicle trajectory, the paper firstly takes vehicles on expressway as study object, and simplifies the movement and models on it. Then, it trains the model by collecting large amounts of data, and predicts its lateral and longitudinal movement with statistical distance and Markov chain respectively. The simulation experiment shows that this model can predict the trajectory of obstacle vehicle within 3s, which can provide reliable basis for collision warning and trajectory planning for autonomous vehicles.

Markov prediction; trajectory prediction; autonomous vehicles; statistical distance; expressway

2017-03-14；

2017-05-08. 基金項目：國家自然科學基金重大項目(91220301). 作者簡介： 張金旺(1988—)，男，碩士研究生； 徐友春(1972—)，男，博士，教授，博士研究生導師.

10.16807/j.cnki.12-1372/e.2017.08.010

TP273

A

1674-2192(2017)08- 0041- 06

● 車輛工程 Vehicle Engineering