基于勢能場虛擬力的智能網聯車輛運動規劃*

田洪清，丁 峰，鄭訊佳，黃荷葉，王建強

（清華大學，汽車安全與節能國家重點實驗室，北京 100084）

前言

智能網聯車輛運動規劃算法能提高車輛行駛安全性、優化行車軌跡和速度、節約能耗并改善行駛舒適性［1］。根據車輛當前狀態和周圍環境中其他車輛狀態信息，規劃出安全、高效、可行的參考軌跡［2］。運動規劃方法可分為圖搜索法、隨機采樣法、幾何曲線法、最優化法、仿生學習法和勢能場法。

眾多運動規劃算法各有其優勢和缺點。圖搜索法中A*算法以起始點為中心向外搜索，以通過路徑為代價值，以與終點直線距離作為啟發值，使搜索沿著距離最短方向進行［3］，該算法能規劃最優路徑，但其增量式計算方法導致計算復雜度高［4］；快速隨機搜索樹算法用隨機采樣方式生成一個擴展樹［5］，在擴展樹中搜索規劃路徑，速度較快，但規劃路徑為局部最優軌跡；幾何曲線法通過擬合逼近控制點來形成［6］，該方法平滑性好，能適應車輛動力學約束條件，但難以實時避障；最優控制法通過最優控制理論找到可行的最優控制量［7］，缺點在于難以準確描述最優軌跡的特征；人工勢能場法將環境信息抽象為引力場和斥力場函數，通過場函數來規劃從起始點到目標點的無碰撞路徑［8-9］，具有規劃速度快、動態避障效果好的優點。但傳統的人工勢能場方法忽略了動態障礙物的運動速度，存在軌跡振蕩等問題［10-11］。

針對現有運動規劃算法存在的問題，本文中提出基于勢能場虛擬力模型的車輛運動規劃方法，該方法通過獲取自車與周邊車輛的運動狀態，建立縱橫向勢能場虛擬力模型，提出了基于車輛運動狀態與勢能場虛擬力相互循環迭代的運動規劃方法。

1 勢能場虛擬力模型

智能網聯交通環境中，車輛能夠獲取自車與周邊車輛的位置和速度等運動狀態信息，周邊車輛因其相對于自車的運動狀態而形成勢能場，車輛行駛過程中產生的加減速和換道運動可視為因勢能場作用產生的虛擬力，稱之為勢能場虛擬力。車輛的縱向加減速和橫向換道等行為即可等效為在勢能場虛擬力作用下的運動。

本文中基于車輛相對位置、速度和駕駛意圖，采用人工勢能場方法生成縱向和橫向勢能場虛擬力，用于車輛縱向與橫向規劃。按其相互作用關系可分為引力與斥力；按其產生機理可分為位置虛擬力與速度虛擬力。在圖1（a）所示的道路中，相同車速情況下，勢能場虛擬力的大小和方向由車距決定；如圖1（b）所示，當車輛間距相同而速度不同時，虛擬力由兩車的速度差決定，說明勢能場虛擬力與車輛位置和速度均有關。因此，在本文中提出的勢能場虛擬力綜合考慮位置與速度的影響，分為位置虛擬力與速度虛擬力。

圖1 勢能場虛擬力模型框架

其中位置虛擬力表征由道路上障礙物和車輛等物體與自車之間的相對位置而生成的勢能場，對自車運動產生影響的位置虛擬作用力；速度虛擬力表征由道路上障礙物、車輛等物體與自車之間的相對運動速度而生成的勢能場，對自車運動產生影響的速度虛擬作用力。二者合成道路行駛環境中虛擬合力∑F，如式（1）所示。

勢能場虛擬力模型框架如圖2所示。

圖2 勢能場虛擬力模型框架

圖2框架表明，勢能場虛擬力按運動方向可劃分為縱向和橫向虛擬力。縱向方向的引力和斥力分別對應車輛的加速和減速運動，橫向方向的引力則對應車輛的換道或車道線保持運動。

建立勢能場虛擬力模型，在縱向運動控制方面，車距變小時產生縱向斥力，車距變大時產生縱向引力，從而保持安全高效的跟車距離。在橫向運動控制方面，當鄰車道路符合換道條件時生成換道引力，車輛進行橫向換道運動；當不符合換道條件時生成車道保持引力，使得其保持在自車車道內行駛。勢能場虛擬力兼顧虛擬力的距離反饋和速度反饋，提高運動規劃的穩定性，消除運動規劃過程中的振蕩。

1.1 縱向勢能場虛擬力模型

1.1.1 縱向人工勢能場

人工勢能場法是用于動態環境機器人導航的一種經典方法。文獻［12］中定義的人工勢能場如式（2）所示。

式中：Up為基于位置的人工勢能；r為障礙物與自車之間的距離；rmax為勢能場作用的最遠距離；rmin為勢能場最大值作用距離；n為位置梯度指數，決定了勢能場變化梯度。

在跟車場景下勢能場分布如圖3所示。

圖3 跟車場景下人工勢能場分布

傳統人工勢能場依據車輛與靜態障礙物之間的相對位置建模。在動態環境中，為體現車輛速度在勢能場建模中的作用，采用式（3）來表示速度勢能場。

式中：Uv為基于速度的人工勢能；vx為自車速度；vt為目標車輛速度；kv為速度勢能系數；m為速度梯度指數。

1.1.2 縱向斥力模型

車輛在人工勢能場環境中的運動規劃與虛擬力相關［13］，勢能場虛擬力與人工勢能場之間的關系由式（4）描述［14］。縱向斥力又可分為縱向位置斥力和縱向速度斥力。

（1）縱向位置斥力

車輛在勢能場環境運動過程中受到目標引力和障礙物斥力作用。根據車輛位置得到基于勢能場的縱向位置斥力模型：

將式（5）所得到的縱向位置斥力Fp，通過式（6）所示的歸一化方法進行處理后可得到標準縱向位置斥力Fr。

式中：Fr為經歸一化處理的標準縱向位置斥力；kr為縱向位置斥力系數；Fpmax為斥力最大值；Fpmin為斥力最小值。

將縱向位置斥力Fp進行歸一化處理，并轉換到道路坐標系后，道路坐標系下的標準縱向位置斥力如式（7）所示。

式中：(xb，yb)為障礙車輛位置坐標；(x，y)為自車位置坐標；Frmax為標準縱向位置斥力最大值。

由式（7）可知，由勢能場虛擬力方法定義的縱向位置斥力模型具備有界性和可控性，即縱向位置斥力值在[0，Frmax]范圍，位置梯度指數對勢能場虛擬力的影響如圖4所示。

圖4 位置梯度指數對勢能場虛擬力的影響

圖4中：s1點為安全距離rmin；s2點為縱向位置斥力的最大作用距離rmax。在運動規劃中按障礙物特征與自車性能選取位置梯度指數n，當n=6.2 時，自車與前車距離較遠時曲線斜率小，斥力增長慢，能提高行駛舒適性；距離近時曲線斜率大，斥力增加較快，能避免發生碰撞。梯度系數起到兼顧自車行駛舒適性與安全性的作用。

（2）縱向速度斥力

針對傳統勢能場方法在運動規劃中的軌跡振蕩問題，本文中提出了速度勢能場概念，考慮車輛的運動速度特征，歸一化處理后得到標準速度斥力模型如式（8）所示。

式中：Fv為車輛的速度斥力；Δvmax為車輛之間的最大速度差，大于此速度差則速度斥力為最大值，Fvmax=kv；Δvmin為最小速度差，小于此速度則斥力為0；kv為速度斥力系數；m為速度梯度指數。

對勢能場虛擬力影響如圖5所示。

圖5 速度梯度指數對勢能場虛擬力的影響

圖中設定Δvmin= 2 m/s，Δvmax= 30 m/s，自車速度vx= 32 m/s 時，當速度vt∈[0，30 m/s]范圍內，車輛間速度差小則曲線斜率小，斥力增長慢，車輛行駛舒適性好；速度差大則曲線斜率大，斥力增長快，車輛緊急制動減速避撞。同時，由于速度虛擬力的阻尼作用，能消除因車間距快速變化而產生的軌跡振蕩問題。

1.1.3 縱向引力模型

當車間距大于理想跟車距離時，運動規劃采用引力模型，縱向引力模型可分為位置和速度引力模型。

（1）縱向位置引力

設定車輛理想跟車距離為目標點與自車的距離，如圖6 所示，目標點位置采用駕駛員主觀安全距離模型來測算［15］。

圖6 縱向位置引力與車距關系

圖中前車處于A位置，自車處于C位置，目標點為B位置。理想跟車距離為da，實際跟車距離為d，縱向位置引力模型如式（9）所示。

式中：Fa為位置引力；xa為目標點位置坐標；dmax為引力最大作用距離，此范圍外引力取Famax；dmin為最小作用距離，小于此距離則引力為0；ka為位置引力系數。

（2）縱向速度引力

與縱向斥力模型相類似，縱向引力模型中的速度引力為

式中：Fb為縱向速度引力；vb為前車速度；Fbmax為最大速度引力；kb為速度引力系數。

1.2 橫向勢能場虛擬力模型

針對車輛橫向勢能場建模難題，本文中設計了基于隱馬爾可夫模型（hidden Markov model，HMM）的換道意圖識別方法，建立橫向勢能場虛擬力模型。

1.2.1 橫向人工勢能場

橫向人工勢能場建模參數在車輛位置和速度基礎上，增加了橫向運動決策系數。建立的橫向人工勢能場模型如式（11）所示。

式中：UH為橫向人工勢能；yt為目標車道中心位置；hi為橫向運動決策系數。

1.2.2 橫向換道意圖識別

HMM 方法從可觀察參數中確定該過程中的隱含參數。本文中將HMM 用于車輛行駛過程中的車道線保持或換道行為的橫向運動意圖識別。

車輛保持車道線或換道行駛的行為意圖取決于自車及周邊車輛的位置和速度等因素。如圖7 所示，當一定范圍內的車輛速度與距離滿足條件時，用HMM方法可以識別其橫向運動意圖。

將上述方法用于車輛橫向運動規劃，以自車道與鄰車道的車輛位置、速度、車間距等特征參數為觀測變量，將車輛橫向運動狀態等效為橫向勢能場，設為隱狀態，通過HMM 方法識別車輛橫向運動意圖。用HMM方法由觀測變量對隱變量進行預測，能準確地識別車輛橫向運動意圖，規劃車輛橫向運動，避免發生碰撞。

車輛行駛過程中按HMM 方法可將行駛狀態分為右 換 道、左換道、跟 馳 和自 由行駛[s1，s2，s3，s4]4 種狀態，以自車速度、前車速度、側前車輛速度、側后車輛速度、前方車距、側后方車距、側方車輛間距[vC，vB，vD，vA，da，dh，d]為觀測變量，建立基于HMM的有限狀態機模型如圖8所示。

圖8 基于HMM的有限狀態機

圖中，A∈[a11，a12，…，a44]為行駛狀態轉移概率矩陣，B∈[b1，b2，b3，b4]為觀測概率分布矩陣，車輛行駛狀態概率由式（12）描述。

式中：pn(i) 為當前時刻運動狀態的概率；pn+1(i)為下一時刻狀態概率；O(tn)為觀測變量矩陣；Pn+1=[pn+1(1)，pn+1(2)，pn+1(3)，pn+1(4)]為橫向運動狀態輸出概率矩陣。由式（12）可預測車輛橫向運動狀態，即車輛駕駛意圖［16］，用式（13）將其轉換為橫向運動決策系數。

1.2.3 橫向引力模型

（1）橫向位置引力

設定目標車道中心線為引力目標點，建立橫向引力模型：

式中：Fc為車輛橫向位置引力；[kd，kd，kc，kc]為位置勢能系數矩陣；[hl，hr，hf，he]為決策系數矩陣，取決于行駛狀態輸出矩陣Pn+1；yc為自車車道線位置；yd為目標車道線位置；dh為車道寬度。

由HMM模型得到車輛橫向運動狀態，該方法對車輛駕駛意圖進行識別，識別過程中考慮周邊車輛的位置和速度信息，橫向運動規劃時避免了換道碰撞風險。

（2）橫向速度引力

為抑制橫向運動中的振蕩，模型中引入橫向速度引力模型：

式中：Fd為橫向速度引力；vy為橫向運動速度；vdmax為橫向最大速度，大于此速度時引力為Fdmax；kd為橫向速度引力系數。

2 車輛運動規劃方法

基于勢能場虛擬力的車輛運動規劃用當前時刻自車和周邊車輛狀態，按勢能場虛擬力模型計算勢能場縱向虛擬力FL和橫向虛擬力FH，以虛擬力為輸入，計算得到下一時刻車輛的運動狀態為運動規劃輸出，并將該輸出反饋到輸入端，如圖9 所示，通過循環迭代方法進行車輛運動規劃。

圖9 基于勢能場虛擬力的車輛運動規劃框架

2.1 縱向運動規劃

由勢能場虛擬力模型框架可知，車輛在行駛過程中在縱向上受到位置斥力、位置引力、速度斥力和速度引力4種虛擬力的綜合作用。

式中FL為縱向虛擬力合力。

縱向運動規劃采用雙積分運動學模型。

式中：a為加速度；M為自車質量。

將系統進行離散化處理后建立車輛運動狀態空間方程［17］：

其中：

式中：X(k)為車輛當前時刻的運動狀態；X(k+ 1)為下一時刻運動狀態；u(k)為控制輸入；A為狀態矩陣；B為控制矩陣；Ts為采樣周期。

縱向運動規劃過程中，根據當前時刻自車與前車的位置和速度，計算縱向虛擬力FL，結合當前時刻車輛運動狀態X(k)，規劃下一時刻運動狀態X(k+ 1)。

2.2 橫向運動規劃

車輛橫向上受到位置引力和速度引力的綜合作用。

式中FH為橫向虛擬力合力。

橫向運動規劃采用雙積分運動學模型，如式（17）和式（18）所示。采用HMM 方法計算自車受到的橫向虛擬力FH，結合當前時刻運動狀態Y(k)，規劃下一時刻車輛的運動狀態Y(k+ 1)。

2.3 車輛平面運動規劃

綜合車輛縱向與橫向運動規劃，建立基于勢能場虛擬力的車輛平面運動規劃模型：

式中：ξ(k)為狀態量；U(k)為控制量；At為車輛運動狀態矩陣；Bt為車輛運動控制矩陣；Ct為車輛運動狀態輸出矩陣；η(k+ 1)為下一時刻運動狀態輸出。

3 仿真驗證

為驗證該方法可行性，本文中通過仿真分析，并與傳統勢能場規劃方法以及highD［18］真實自然駕駛數據集運動軌跡進行對比。

車輛運動規劃仿真環境如圖10 所示，自車前方存在一慢速車輛B，為避免發生碰撞，通過構建勢能場虛擬力模型進行運動規劃。

圖10 基于勢能場虛擬力的運動規劃仿真環境

3.1 振蕩抑制驗證

跟車狀態下，按傳統勢能場模型和本文中所述的勢能場虛擬力模型分別對車輛的運動狀態進行規劃，仿真工況如表1所示，仿真結果的車間距和車速對比如圖11所示。由圖可見，傳統勢能場方法會產生振蕩，而采用勢能場虛擬力規劃方法，仿真數據經過小幅振蕩后，車間距穩定于60 m，速度穩定于20 m/s。

表1 跟車運動規劃仿真參數

圖11 振蕩抑制仿真對比

3.2 跟車行駛仿真

跟車狀態下的仿真參數及數值如表1 所示，通過仿真實驗獲取車間距、自車速度和加速度結果如圖12 所示。由圖可見，運動速度和加速度平滑，當車速增加時，車輛減速度逐漸增加直到達到飽和值，車間距逐漸減小，并保持在安全范圍內。

圖12 跟車行駛工況

為驗證該規劃方法的實用性，將運動規劃數據與德國亞琛大學采集的車輛highD 數據集［18］進行對比，在相同條件下進行運動仿真，結果如圖13 所示。由圖可見，仿真數據與實車數據的行車間距、車速和加速度有相同的變化趨勢，該運動規劃結果與實車運動基本一致，行車間距比實車略大，加速度值相對實車數據較小，車輛行駛安全性與平順性更優。

3.3 換道行駛仿真

在換道行駛工況下，構建縱橫向勢能場虛擬力模型并進行運動規劃，仿真參數及數值如表2 所示。

圖13 跟車運動對比

表2 車輛換道運動規劃仿真參數

換道狀態下車輛的運動軌跡、速度和加速度如圖14 所示。由圖可見，基于勢能場虛擬力的仿真運動規劃軌跡平滑，符合車輛動力學特性，能避免與前車發生碰撞，滿足安全性要求。

表3 列出了6 種車速條件下車輛運動規劃仿真實驗數據，當車速增加時，行車軌跡曲率、橫向加速度和轉角增加，車間距變小，但在安全范圍之內。

圖14 車輛換道行駛工況圖

表3 運動規劃仿真實驗數據

為驗證該方法的實用性，將換道仿真數據與highD中的實車運動軌跡進行對比，在相同條件下進行運動仿真，結果如圖15 所示。由圖可見，仿真數據與實車數據的運動軌跡、軌跡曲率、橫向運動速度、車輛轉角具有相同的變化趨勢，該規劃方法與實車運動基本一致，且仿真軌跡更平滑，曲率小，車輛橫向運動速度低，規劃安全性更優。

圖15 highD實車換道運動對比圖

4 結論

（1）建立了智能網聯交通環境條件下的勢能場虛擬力模型框架，提出了位置和速度虛擬力的概念，反映了封閉交通系統中車輛間相互作用關系，闡明了車輛運動規劃的內在驅動機理。

（2）提出了基于車輛位置、速度和駕駛意圖的橫縱向勢能場虛擬力模型，采用歸一化方法將車輛行駛安全與效率融合為勢能場虛擬力模型。揭示了車輛狀態、勢能、虛擬力、運動之間的相互作用關系。

（3）根據勢能場虛擬力模型框架，建立了基于車輛位置與速度的雙反饋閉環運動規劃系統，運用車輛狀態空間方程，采用勢能場虛擬力與車輛狀態循環迭代的方法進行運動規劃，為車輛在結構化道路條件下運用勢能場理論進行自動駕駛運動規劃提供了新的研究思路。

（4）仿真實驗結果表明，采用該方法規劃的運動軌跡、速度等指標與實車運動數據基本一致，表明實際道路中車輛駕駛操控機制與勢能場虛擬力運動規劃機理基本一致；通過仿真對比實驗，表明該方法能夠克服傳統勢能場方法在結構化道路環境條件下進行運動規劃過程中存在的速度振蕩問題，具有較好的實用性。