基于預(yù)測風(fēng)險場的智能汽車主動避撞運(yùn)動規(guī)劃*

王安杰，鄭 玲，李以農(nóng)，王 戡

（1.重慶大學(xué)汽車工程學(xué)院，機(jī)械傳動國家重點(diǎn)實(shí)驗(yàn)室，重慶 400044；2.重慶車輛檢測研究院有限公司，重慶 401122；3.汽車主動安全測試技術(shù)重慶市工業(yè)和信息化重點(diǎn)實(shí)驗(yàn)室，重慶 401122）

前言

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

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

以上避撞運(yùn)動規(guī)劃的風(fēng)險勢場主要為道路縱向和橫向上的風(fēng)險評估，局限于當(dāng)前狀態(tài)，未能準(zhǔn)確描述障礙物風(fēng)險隨時間的變化趨勢。且主要著重于前方障礙物研究，關(guān)于側(cè)方和后方障礙物對本車的碰撞風(fēng)險關(guān)注不足。

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

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

圖1 基于動態(tài)風(fēng)險評估的運(yùn)動規(guī)劃架構(gòu)

1 坐標(biāo)轉(zhuǎn)換

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

圖2 車輛在Frenet坐標(biāo)系下的投影

表1 Frenet坐標(biāo)投影搜索算法

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

圖3為Frenet坐標(biāo)系下的軌跡轉(zhuǎn)換。式（3）為坐標(biāo)轉(zhuǎn)換關(guān)系式。式（4）和式（5）為Cartesian坐標(biāo)下的航向角和曲率轉(zhuǎn)換關(guān)系式。

圖3 Frenet坐標(biāo)系下的軌跡轉(zhuǎn)換

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

結(jié)合式（2）～式（7）可得規(guī)劃軌跡上某一點(diǎn)在Cartesian坐標(biāo)系下的坐標(biāo)、航向角和曲率。

2 動態(tài)風(fēng)險評估模型

2.1 車輛運(yùn)動預(yù)測

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

假設(shè)預(yù)測時長為tn，將時間離散化后代入CA模型即可求得車輛位置序列的預(yù)測軌跡。同時，為在同一時刻判斷本車風(fēng)險，給出相同時間序列下的本車軌跡點(diǎn)，即運(yùn)動規(guī)劃得到的軌跡序列。

2.2 障礙車風(fēng)險評估

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

圖4 障礙車風(fēng)險評估示意圖

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

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

最后，將所有障礙物的風(fēng)險進(jìn)行整合。即在同一時刻，障礙車在Frenet坐標(biāo)系下某一點(diǎn)處的風(fēng)險用所有障礙物在該點(diǎn)處的風(fēng)險最大值表示，如式（13）所示。

2.3 道路環(huán)境風(fēng)險評估

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

圖5 道路風(fēng)險勢場

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

2.4 穩(wěn)定性風(fēng)險評估

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

圖6 穩(wěn)定性風(fēng)險勢場

2.5 預(yù)測風(fēng)險場

將周圍車輛風(fēng)險模型、道路環(huán)境風(fēng)險模型和穩(wěn)定性風(fēng)險模型合并為預(yù)測風(fēng)險場（PRF）模型。式（17）為預(yù)測風(fēng)險場的計算方法。

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

圖7 障礙車風(fēng)險勢場

圖8 預(yù)測風(fēng)險場

表2 三車道示例場景

3 運(yùn)動規(guī)劃

3.1 行為決策

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

圖9 基于動態(tài)規(guī)劃的行為決策框架

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

式中：tk為階段k對應(yīng)的未來時刻；x0和v0分別為初始位置和速度；alimit和xlimit分別為加速度極限和在tk時間內(nèi)能到達(dá)的極限位置。

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

圖10 階段k的允許狀態(tài)集合示意圖

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

階段指標(biāo)函數(shù)定義如下：

階段指標(biāo)函數(shù)的第1項(xiàng)為建立的預(yù)測風(fēng)險場函數(shù)，vk和ak分別為初始時刻到當(dāng)前時刻的平均速度和平均加速度；參考速度vref與交通規(guī)則、道路曲率有關(guān)；g為速度保持函數(shù)，本車速度高于vref時的代價比低于vref時大。

動態(tài)規(guī)劃搜索過程就是利用式（20）遞推關(guān)系式的一個逆序遞推計算求解過程。即從最后一個階段開始，依次計算由當(dāng)前階段到最后一個階段最優(yōu)指標(biāo)函數(shù)值最小的狀態(tài)序列，最終獲得離散的三維軌跡序列。

3.2 軌跡規(guī)劃

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

圖11 基于二次規(guī)劃的軌跡規(guī)劃框架

二次規(guī)劃模型主要根據(jù)DP軌跡、平滑指標(biāo)、車輛動力學(xué)約束、交通規(guī)則，依據(jù)多項(xiàng)式曲線構(gòu)造，縱橫向軌跡用式（22）的多項(xiàng)式曲線表示，目標(biāo)函數(shù)和約束條件如式（23）所示。

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

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

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

圖12 碰撞檢測示意圖

4 仿真分析

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

4.1 縱向避撞工況

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

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

圖14 縱向避撞工況

4.2 橫向避撞工況

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

圖15 橫向避撞場景

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

圖16 橫向避撞工況

5 結(jié)論

針對自動駕駛汽車側(cè)方和后方車輛對本車的碰撞風(fēng)險，在Frenet坐標(biāo)系下，提出了融合障礙物運(yùn)動預(yù)測的預(yù)測風(fēng)險場，依據(jù)此風(fēng)險評估方法設(shè)計了主動避撞運(yùn)動規(guī)劃方法。

仿真結(jié)果表明：所構(gòu)建的預(yù)測風(fēng)險場能全面反映本車行駛的碰撞風(fēng)險和穩(wěn)定性風(fēng)險，且能準(zhǔn)確識別側(cè)方和后方車輛在不同相對位置和相對速度下的碰撞風(fēng)險差異以及碰撞風(fēng)險隨時間的變化趨勢；運(yùn)動規(guī)劃方法依據(jù)預(yù)測風(fēng)險場能準(zhǔn)確、直接地決策出風(fēng)險最小的安全軌跡和目標(biāo)點(diǎn)位置，并規(guī)劃出滿足各項(xiàng)約束的主動避撞軌跡，保障車輛運(yùn)行的安全性和穩(wěn)定性。

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