基于預測風險場的智能汽車主動避撞運動規劃*

王安杰，鄭 玲，李以農，王 戡

（1.重慶大學汽車工程學院，機械傳動國家重點實驗室，重慶 400044；2.重慶車輛檢測研究院有限公司，重慶 401122；3.汽車主動安全測試技術重慶市工業和信息化重點實驗室，重慶 401122）

前言

自動駕駛技術已成為汽車行業的熱門研究課題，據調查顯示，90%左右的碰撞事故是人為因素造成的［1］。自動駕駛技術可以避免人為因素造成的交通事故，提高安全性和便利性。同時，為不因引入自動駕駛技術而造成新的安全隱患和事故，自動駕駛技術本身的安全性和可靠性至關重要；完備的運動規劃可保證自動駕駛的可靠性，運動規劃也是自動駕駛的關鍵技術之一。

運動規劃旨在考慮環境狀態變化的前提下，為車輛規劃出一條滿足初始狀態、目標狀態以及速度、加速度、曲率等約束的無碰撞路徑。Lee等［2］提出了一種預測占用圖的風險評估方法，結合加速度軌跡采樣篩選得到風險最小的安全軌跡。Kim等［3］提出了一種識別周圍環境潛在風險的方法，并借此找到風險最小的安全路徑。任玥等［4］引入虛擬力場，提出了基于模型預測理論的避障路徑規劃方法，表現出良好的魯棒性。朱冰等［5］提出了安全場引導的RRT*算法，減少了計算量，提高了收斂速度。肖浩等［6］借鑒人工勢場理論思想，提出了一種基于危險斥力場的主動避撞局部路徑規劃算法，具有運算量小、安全可靠的特點。修彩靖等［7］改進了傳統人工勢場法，建立了考慮道路約束和車輛約束的目標函數，既消除了抖動問題，又滿足了安全指標。Kim等［8］基于車輛目標車道概率的運動預測來定量評估一組候選路徑的碰撞風險，該風險評估算法可以估計與周圍車輛即將到來的碰撞風險。Hesse等［9］結合勢場法和彈性帶理論，并將車輛動力學引入到彈性帶中，提出了一種擴展的彈性帶路徑規劃方法。Houenou等［10］提出了一種基于恒定橫擺角速度、恒加速度模型和基于行為識別相結合的軌跡預測方法，使短期和長期軌跡預測都具有較好的精度。張一鳴等［11］結合駕駛意圖和物理運動模型完成了軌跡預測以及車輛運動規劃。Werling等［12］針對結構化道路，結合五次多項式和最優控制，提出了一種在Frenet坐標系下的直接軌跡生成方法。

以上避撞運動規劃的風險勢場主要為道路縱向和橫向上的風險評估，局限于當前狀態，未能準確描述障礙物風險隨時間的變化趨勢。且主要著重于前方障礙物研究，關于側方和后方障礙物對本車的碰撞風險關注不足。

針對高速緊急工況，本文中在考慮場景預測的前提下，提出了考慮時間因素的預測風險場，用于描述障礙物風險及其隨時間的變化趨勢，包括側方和后方障礙物。并據此建立基于動態規劃和二次規劃的直接軌跡生成運動規劃方法，以保證車輛在緊急工況下的安全性和穩定性。

運動規劃架構如圖1所示，融合場景預測信息在縱向、橫向以及時間3個維度下建立了用于風險評估的預測風險場，包含障礙物風險評估、道路環境風險評估和穩定性風險評估；并在各項約束下，使用動態規劃完成本車行為決策，得到一條分段線性的決策軌跡；以決策軌跡為參考軌跡，利用多項式曲線和二次規劃方法對軌跡進行優化處理，實現主動避撞運動規劃。

圖1 基于動態風險評估的運動規劃架構

1 坐標轉換

為便于在彎道下進行碰撞風險評估，須將Cartesian坐標系下的車輛坐標、航向角信息轉換到Frenet坐標系下。如圖2所示，P點代表車輛位置，A、B兩點分別表示道路參考線的起點和終點，且道路參考線的Cartesian坐標和Frenet坐標、車輛的Cartesian坐標均已知。表1為求解車輛位置P點Frenet坐標系下坐標和航向角的偽代碼。考慮實際行車工況，在道路半徑大于100 m、且車輛位置偏離參考線不超過10 m的情況下，取搜索精度ε為0.001 m，任意車輛可在6次迭代之內找到唯一投影坐標。

圖2 車輛在Frenet坐標系下的投影

表1 Frenet坐標投影搜索算法

將車輛投影到Frenet坐標系并完成局部路徑規劃后，規劃的軌跡需要轉換到Cartesian坐標系下進行評估以及后續跟蹤［12］。式（1）為坐標、航向角和曲線從Frenet坐標到Cartesian坐標的轉換所需信息。

圖3為Frenet坐標系下的軌跡轉換。式（3）為坐標轉換關系式。式（4）和式（5）為Cartesian坐標下的航向角和曲率轉換關系式。

圖3 Frenet坐標系下的軌跡轉換

式中：Δθ=θx-θr，假設車輛軌跡一直沿參考線附近運動，則有|Δθ|＜ π/2，1-κrd＞0；κr和κ'r分別為參考線某一點在Cartesian坐標系下的曲率和曲率變化率。當參考線用參數曲線表示時，其κr和κ'r可用式（6）和式（7）表示。

結合式（2）～式（7）可得規劃軌跡上某一點在Cartesian坐標系下的坐標、航向角和曲率。

2 動態風險評估模型

2.1 車輛運動預測

在評估本車在動態環境下的風險時，須根據交通參與者的類型、位置、速度和加速度等信息預測其未來一段時間內的運動規律。場景預測包含動態物體和靜態物體的運動預測。研究表明，基于物理規律的運動模型可以有效預測物體運動軌跡，但僅限于短期的碰撞風險預測［13］；因此本文選擇短時間內精確度較高的CA模型作為車輛運動預測模型，其表達式為

假設預測時長為tn，將時間離散化后代入CA模型即可求得車輛位置序列的預測軌跡。同時，為在同一時刻判斷本車風險，給出相同時間序列下的本車軌跡點，即運動規劃得到的軌跡序列。

2.2 障礙車風險評估

障礙物風險模型如式（9）所示，將障礙車風險評估劃分為5個區域，如圖4所示。

圖4 障礙車風險評估示意圖

式中：sr和dr分別為同一時刻下本車與障礙車的縱向相對距離和橫向相對距離，均為絕對值；ssafe和dsafe分別為本車應與障礙車的縱橫向安全距離，其具體數值由相應場景決定，大于此值時風險值為0；scri和dcri分別為障礙車矩形膨脹框長寬的1 2，小于此值時風險值為1；sbuffer和dbuffer為單調遞減函數，描述從碰撞到安全區域風險逐漸降低的變化過程；m為障礙車數量。以縱向為例，將上述參數定義如下：

式中：θobs為障礙車航向角；Lobs和Wobs分別為障礙車的長和寬；egocri為本車矩形膨脹框長度的1 2；vr為相對速度；ksv為安全碰撞時間余量，物理意義與TTC相同；此處也可用安全車間時距來獲取安全距離；smin為縱向相對速度為0時的最小安全距離。

最后，將所有障礙物的風險進行整合。即在同一時刻，障礙車在Frenet坐標系下某一點處的風險用所有障礙物在該點處的風險最大值表示，如式（13）所示。

2.3 道路環境風險評估

在沒有其他障礙物時，道路對本車的影響因素主要考慮車道線以及道路左右邊界位置，道路邊界的危險程度是高于車道線的，而每條車道中心的位置應是沒有風險的，勢場值是最低的，車道線主要用于約束車輛行駛于車道中間。因此采用三角函數構建道路的風險模型［14］。道路風險模型僅與車輛的橫向坐標d有關，如圖5所示。

圖5 道路風險勢場

式中：lanew表示車道寬度；p為車道線處的風險值，用于約束本車的換道意圖，其值大小與車道線類型有關，比如，白色虛線處的風險值最小，雙黃線的風險值最大；dcl和dcr分別為最左側和左右側車道中心線處的橫向坐標值。

2.4 穩定性風險評估

本車過大的縱橫向加速度存在一定的穩定性風險，可能導致側滑或側翻事故；且橫向風險大于縱向風險，因此用橢圓描述縱橫向加速度穩定性安全區域。式（16）為穩定性風險模型。其中：μ為路面附著系數；g為重力加速度；aengine為本車的最大加速度；as和ad分別為縱橫向加速度；assafe和adsafe分別為安全橢圓長半軸和短半軸長度；aellipse為a到原點連線與安全橢圓交點處的加速度值。圖6為穩定性風險勢場。

圖6 穩定性風險勢場

2.5 預測風險場

將周圍車輛風險模型、道路環境風險模型和穩定性風險模型合并為預測風險場（PRF）模型。式（17）為預測風險場的計算方法。

表2為三車道示例場景，假設本車不做任何避撞操作，沿當前車道勻速行駛。t=0 s時刻下障礙物風險勢場如圖7所示，圖中藍點為當前時刻本車位置，本車與前車、后車的勢場值相等。0.5 s之后的風險勢場如圖8（c）所示，由于前車有4 m/s2的減速度，此時本車與前車的勢場值最大，對本車的威脅最高，符合駕駛經驗。

圖7 障礙車風險勢場

圖8 預測風險場

表2 三車道示例場景

3 運動規劃

3.1 行為決策

使用動態規劃（dynamic programming，DP）進行車輛的行為決策［15］。行為決策就是根據車輛周圍的動態環境信息決策出本車的駕駛行為和目標點位置［16］。本文以時間作為階段變量k的劃分依據，相鄰兩個階段的時間間隔為dt，將車輛的行為決策問題劃分為相互聯系的n個階段，當前時刻t0作為最后一個階段，末時刻tn作為第一階段；以Frenet坐標系下的縱向坐標和橫向坐標作為狀態變量x（s，d）和決策變量dec（s，d）。如圖9所示，行為決策主要包含允許狀態集合與允許決策集合、指標函數和動態規劃求解。

圖9 基于動態規劃的行為決策框架

為滿足路面附著要求以及本車加速性能限制，縱橫向加速度須滿足式（18）。

式中：tk為階段k對應的未來時刻；x0和v0分別為初始位置和速度；alimit和xlimit分別為加速度極限和在tk時間內能到達的極限位置。

依據式（19）可得車輛每個階段在道路中所能到達的位置，即允許狀態集合，如圖10所示。且在正常行車時不會出現倒車情況，所以Smin一定大于當前時刻縱向坐標。同理，將tk替換為dt，用限速值代替初速度，以當前狀態作為初始位置，即可得到某一狀態下的允許決策集合D。

圖10 階段k的允許狀態集合示意圖

得到允許狀態和決策集合后，須確定指標函數以衡量階段的決策效果，如式（20）所示。階段指標函數ck(xk，deck)表示在第k階段處于xk狀態下執行決策deck后的效果，f（xxk）為最優指標函數，是求解最優決策序列的重要依據；Tk為狀態轉移方程，用于獲取執行決策后的車輛位置狀態。

階段指標函數定義如下：

階段指標函數的第1項為建立的預測風險場函數，vk和ak分別為初始時刻到當前時刻的平均速度和平均加速度；參考速度vref與交通規則、道路曲率有關；g為速度保持函數，本車速度高于vref時的代價比低于vref時大。

動態規劃搜索過程就是利用式（20）遞推關系式的一個逆序遞推計算求解過程。即從最后一個階段開始，依次計算由當前階段到最后一個階段最優指標函數值最小的狀態序列，最終獲得離散的三維軌跡序列。

3.2 軌跡規劃

由于分段線性的DP軌跡無法滿足速度連續性要求，因此，使用多項式曲線對軌跡進行優化處理和速度規劃。如圖11所示，主要包含構造二次規劃（quadratic programming，QP）模型和軌跡檢測，并通過迭代得到一條滿足速度、加速度、曲率約束的無碰撞軌跡。

圖11 基于二次規劃的軌跡規劃框架

二次規劃模型主要根據DP軌跡、平滑指標、車輛動力學約束、交通規則，依據多項式曲線構造，縱橫向軌跡用式（22）的多項式曲線表示，目標函數和約束條件如式（23）所示。

式中：s0、vs0、as0分別為初始時刻縱向位置、速度和加速度；d0、vd0、ad0分別為初始時刻橫向位置、速度和加速度；sn、dn為末時刻的縱橫向坐標，由行為決策的目標點位置決定；vmax為限速值，用縱向速度近似代替。目標函數第1項表示在對應時刻多項式曲線和DP軌跡之間距離；第2項是穩定性指標，也可衡量軌跡的平滑度。最后兩項為縱橫向加速度約束，為滿足線性約束要求，做了放大處理。

軌跡檢測是為確定優化軌跡滿足加速度約束、曲率約束以及無碰撞要求。加速度須滿足式（18）。曲率約束如式（24）所示，曲率須將軌跡轉換回Cartesian坐標系計算，κmax為允許的最大曲率，由前輪最大轉角決定。

某一時刻的碰撞檢測如圖12所示。在sdz坐標系中，用虛線矩形對車輛做膨脹處理，以描述車輛的碰撞安全裕度。本車膨脹矩形不變，障礙車的膨脹矩形隨時間逐漸變大。通過判斷本車4個頂點是否在障礙車膨脹框內來判斷碰撞；以E1點為例，分別計算向量O1O2、O2O3、O3O4、O4O1與向量O1E1、O2E1、O3E1、O4E1的向量積，若所得結果中有z值大于0，則E1點在外部。若本車4個頂點都在外部則無碰撞；否則，該時刻極有可能碰撞。

圖12 碰撞檢測示意圖

4 仿真分析

仿真場景使用三車道、四障礙車的直道場景，針對高速場景下縱向和橫向障礙車快速靠近本車的危險工況，對所提出的運動規劃方法進行仿真驗證。

4.1 縱向避撞工況

縱向避撞工況場景如圖13所示。參考速度為25 m/s。本車在中間車道，中間車道前方和后方都有車輛快速接近本車，且前車有制動，左右車道各有一輛車勻速行駛，與本車同速。

若本車不采取避撞措施，2 s后必然發生碰撞。基于預測風險場的運動規劃結果如圖14（a）所示，由d-t視圖可知，決策軌跡橫向為換道避撞的形式，且識別出右車道車輛相對距離較遠，風險較小，選擇向右車道換道；由s-t視圖可知，由于本車當前速度低于參考速度，決策軌跡在換道過程中縱向速度表現為先減速再加速。優化軌跡速度和加速度如圖14（b）和圖14（c）所示。在縱向避撞工況中，側向加速度未超過穩定性極限。橫向上與決策軌跡一致，且軌跡合加速度和曲率均在約束范圍內；在多項式曲線作用下，且安全無碰撞時縱向上表現為輕微加速。該運動規劃方法能識別后方及側方車輛的風險，并一次性滿足速度、加速度和曲率等約束，直接規劃出一條安全無碰撞軌跡。

圖14 縱向避撞工況

4.2 橫向避撞工況

橫向避撞工況場景如圖15所示。本車速度為25 m/s，已達到參考速度；4輛障礙車與本車同速左右車道各有2輛障礙車，且本車左側有一輛障礙車以1 m/s的相對速度靠近本車。

圖15 橫向避撞場景

基于預測風險場的運動規劃結果如圖16（a）所示。由d-t視圖可知，決策軌跡橫向上表現為向右輕微避讓后返回中間車道；由s-t視圖可知，決策軌跡目標點的縱向坐標小于本車勻速到達的位置，因此縱向上表現為減速，這是因為高于參考速度時的代價比低于參考速度時大，且加速能力有限。優化軌跡速度和加速度如圖16（b）和圖16（c）所示。側向加速度很小，不存在側滑風險，橫向上與決策軌跡基本一致，在加速度約束下，以非常小的速度向右輕微避讓后返回中間車道；在多項式曲線作用下，縱向上表現為以較大的減速度制動避讓障礙車。綜上所述，根據預測風險場能準確識別出周圍車輛潛在風險隨時間的變化趨勢，實際完成緊急工況下主動避撞的運動規劃，且規劃結果符合實際駕駛操作，保障了本車在危險工況下的安全性和穩定性。

圖16 橫向避撞工況

5 結論

針對自動駕駛汽車側方和后方車輛對本車的碰撞風險，在Frenet坐標系下，提出了融合障礙物運動預測的預測風險場，依據此風險評估方法設計了主動避撞運動規劃方法。

仿真結果表明：所構建的預測風險場能全面反映本車行駛的碰撞風險和穩定性風險，且能準確識別側方和后方車輛在不同相對位置和相對速度下的碰撞風險差異以及碰撞風險隨時間的變化趨勢；運動規劃方法依據預測風險場能準確、直接地決策出風險最小的安全軌跡和目標點位置，并規劃出滿足各項約束的主動避撞軌跡，保障車輛運行的安全性和穩定性。

本文中運動預測使用的是短時間精度較高的恒加速度模型，隨著預測時間增加精度降低；由于車輛正常行駛時處于緊急工況的時間很少，因此下一步工作將考慮提高運動預測精度，將運動規劃擴展到非緊急工況下。