基于安全域的局部路徑規(guī)劃優(yōu)化算法

毛莞丁，鄒若冰，劉晨麗

（陜西汽車集團有限責任公司，西安 710043）

據(jù)我國道路交通事故統(tǒng)計，交通事故九成以上都與駕駛員自身原因有關[1]。自動駕駛車輛是由車輛的控制器、傳感器等發(fā)揮主要作用，代替駕駛員駕駛車輛，可以為車輛行駛安全以及交通安全提供良好的保障[2]。決策規(guī)劃是自動駕駛的關鍵部分之一，是自動駕駛車輛可以到達目標點的保障。它融合多傳感信息，根據(jù)駕駛需求進行行為決策，指導當前自動駕駛車輛做出行為動作，如正常行駛、跟車、超車、換道、停車等，同時依據(jù)局部環(huán)境信息、行為決策結果和車輛實時位姿信息，在滿足一定的運動學約束下，規(guī)劃局部空間和時間內車輛期望的運動軌跡。

當前研究人員提出了很多的路徑規(guī)劃算法，如人工勢場法[3-4]、快速隨機樹（Fast Random Tree，RRT）算法[5]、模糊算法[6-7]、蟻群算法[8]等，有些方法[9-10]是直接找到連接起點和終點的全局軌跡。另外一些方法[11-19]是先得到一系列有限的軌跡，從該軌跡集合中，通過最小化給定的代價函數(shù)選擇最優(yōu)軌跡。對于如何生成軌跡集合，需要選擇參數(shù)模型，如高階多項式。WERLING等[14]提出的基于Frenet框架的最優(yōu)軌跡生成（Optimal Trajectory Generation，OTG）算法，將軌跡規(guī)劃問題分解成橫向和縱向兩個一維空間的路徑規(guī)劃問題，求解軌跡集，并從橫向和縱向分別考慮代價函數(shù)，加權求和得到總的代價函數(shù)，通過代價函數(shù)最小化選取最優(yōu)軌跡。該方法具有較強的實用性，是目前普遍采用的局部路徑規(guī)劃算法之一。Apollo EM規(guī)劃器[15]與OTG算法相似，同樣采用降維思想，將軌跡規(guī)劃問題分解為橫向和縱向，再合成三維軌跡，根據(jù)代價函數(shù)篩選出最好的合成軌跡。周慧子等[16]針對自動駕駛中避障的動態(tài)路徑規(guī)劃問題，提出一種在已知車輛的初始位置、速度、方向和障礙物位置情況下，實時避開障礙物的動態(tài)規(guī)劃算法。梁廣民[17]提出了一種新的自動駕駛軌跡規(guī)劃方法，該方法可以為自動駕駛生成舒適安全的軌跡。郭蓬等[18]在使用Frenet坐標系理論的基礎上，提出了一種考慮到車道線曲率和障礙物模型信息的方法，然后通過最小化代價函數(shù)，選出局部最優(yōu)路徑。毛晨悅等[19]提出一種基于人工勢場法的無人機路徑規(guī)劃避障算法，提高無人機的避障性能。

在代價函數(shù)方面，OTG算法[14]的代價函數(shù)是從橫向和縱向分別考慮，主要包含舒適性、運行時長和偏離目標三個方面。Apollo EM規(guī)劃器[15]的代價函數(shù)（簡稱cost函數(shù)）包含到達目的偏差、橫向偏移、是否碰撞以及加速度。周慧子等[16]從安全、平滑和連續(xù)這3個方面來設計代價函數(shù)。梁廣民[17]根據(jù)每個候選軌跡與每個障礙物的碰撞概率和每條軌跡到目標位置的最短距離選擇最合適的軌跡。郭蓬等[18]算法的代價函數(shù)考慮了擬合軌跡到障礙物的危險程度、軌跡平順性、軌跡到當前參考線的偏離程度、擬合軌跡行駛方向的改變程度、無人駕駛汽車最小轉彎半徑。

近些年的研究中，許多學者也對傳統(tǒng)的路徑規(guī)劃算法進行了改進和優(yōu)化。胡澮冕[20]針對RRT算法收斂速度慢、路徑不規(guī)則的問題，基于雙向RRT算法和概率搜索策略，提出了一種變概率策略下的雙向RRT搜索優(yōu)化算法。在仿真中進行了多場景重復試驗測試，仿真結果表明，改進后的算法在搜索速度和路徑長度上有明顯改善。王飛等[21]針對工作環(huán)境已知的情況，研究了移動機器人在復雜三維環(huán)境下路徑規(guī)劃的問題，提出一種改進的蟻群優(yōu)化算法。試驗結果表明，該算法有較高的全局搜索能力，收斂速度明顯加快，驗證了該算法的有效性和可行性。王云常等[22]為簡化無人機飛行路徑規(guī)劃算法并提高其避障效果，提出一種人工勢場法和A星算法相結合的路徑規(guī)劃算法，仿真試驗證明，該算法與人工勢場法和A星算法相比，提高了避障效果，縮短了搜索時間。

本文從代價函數(shù)的角度出發(fā)，對傳統(tǒng)OTG算法進行優(yōu)化。上述算法的代價函數(shù)包含大多數(shù)的評價方法（安全性、平滑性、連續(xù)性等），但是無法表征動態(tài)障礙物對車輛行駛安全的影響。例如，主控車輛和障礙車輛之間距離相等，兩者既可以是相向而行，也可以是同向而行，但兩者對主控車輛的影響不同，選取出的最優(yōu)軌跡也會不同。針對這種情況上述局部路徑規(guī)劃算法的代價函數(shù)會篩選出相同軌跡，而無法做出準確的評判。因此，傳統(tǒng)OTG算法的代價函數(shù)僅考慮了三方面的約束，且只針對靜態(tài)情況，對動態(tài)環(huán)境安全因素的考慮不足。為了解決上述問題，提出一種簡單可行的影響因子──安全域，反映了主控車輛與周圍動態(tài)環(huán)境的安全性。這個概念的提出將為局部路徑規(guī)劃的優(yōu)化提供一定的依據(jù)。

1 傳統(tǒng)OTG算法

傳統(tǒng)OTG算法在進行路徑規(guī)劃時分別從橫向和縱向考慮，最后進行融合。算法分為兩部分，第一部分是生成軌跡集合，第二部分是對軌跡集合進行評價，篩選出最優(yōu)軌跡作為自動駕駛汽車最終的期望運行軌跡。

1.1 軌跡的生成

WERLING等[14]研究的OTG算法采用橫縱向分別求解軌跡，再耦合的方式來實現(xiàn)。其中橫縱向軌跡分別采用五次多項式[23]來表示，公式如下：

式中：d(t)為橫向軌跡的五次多項式；αd0、αd1、αd2、αd3、αd4和αd5為橫向軌跡五次多項式的系數(shù)；t為時間，s；s(t)為縱向軌跡的五次多項式；αs0、αs1、αs2、αs3、αs4和αs5為縱向軌跡五次多項式的系數(shù)。

1.2 軌跡的評價

在求得橫縱向軌跡集合后，需要利用代價函數(shù)對其進行評價，選出最優(yōu)軌跡。傳統(tǒng)OTG算法的代價函數(shù)也分為橫向和縱向，橫向軌跡的代價函數(shù)為：

縱向軌跡的代價函數(shù)為：

式中：kj為Jerk[24]權重；Jt[d(t)]為關于d(t)的Jerk；Jt[s(t)]為關于s(t)的Jerk；kt為時長權重；T為軌跡的控制周期，s；kd為偏離道路中心線權重；d12為橫向目標位置與橫向軌跡末端位置之差的平方；s˙1為縱向目標速度，m/s ；sc˙為縱向軌跡末端速度，m/s。

將橫向和縱向的代價函數(shù)加權求和，得到總的代價函數(shù)為：

式中：klat為橫向代價函數(shù)的權重；klon為縱向代價函數(shù)的權重。

2 傳統(tǒng)OTG算法的優(yōu)化

本文提出了“安全域”這一新概念，客觀表征了車輛行駛過程中車、人、路等交通要素對行駛安全的影響。同時，也提出了量化安全域參數(shù)的方法并成功應用于傳統(tǒng)OTG算法的優(yōu)化。

2.1 安全域

影響自動駕駛車輛的行駛安全性因素有很多，主要包括主控車輛、其他交通參與者以及道路等。交通參與者主要有機動車、非機動車、行人、甚至路上出現(xiàn)的動物以及路障等，這些交通參與者可以是運動的，也可以是靜止的。道路主要包含道路的車道數(shù)、寬度、形狀等。

對于車輛行駛安全性而言，道路上的交通參與者都有可能對其產生潛在的風險。為了評估自動駕駛主控車輛周圍環(huán)境對行駛安全性的影響，本文提出了安全域的概念，用來表征主控車輛行駛過程中周圍車、人、路各因素對行駛安全性的影響程度。它是一個包含時間和空間兩個維度的變量（車輛行駛過程中各個參與者要素的動態(tài)變化）。安全域是根據(jù)交通參與者的自身特性在其周圍形成一個安全范圍，兩個交通參與者安全域的重疊面積越大，說明兩者碰撞的概率越高，行駛安全性越低，反之，重疊面積越小，碰撞概率越低，行駛安全性越高。

安全域的范圍是由交通參與者的類型、位置、質量、長寬、運動狀態(tài)決定的。王建強等[25]提出了一個等效質量的概念，把物體的類型、質量及其運動狀態(tài)這些因素統(tǒng)一表達。定義物體i的等效質量如式（6）所示：

式中：mi為物體i的質量，kg；Type為物體的類型；δi(T ype)m為 關于物體i的類型的函數(shù)；νi為物體i的速度，m/s；αk，βk均為待定常數(shù)；k為速度多項式的項數(shù)。

本文用矩形表示交通參與者的安全域，如圖1所示。圖1中紅色矩形框表示車輛，黑色矩形框表示車輛的安全域，紅色箭頭表示車輛的行駛方向。A(x0,y0)是車輛的中心坐標，A'(x,y)是車輛安全域的中心坐標。

圖1 車輛的安全域示意圖

圖1中，a是車輛中心到安全域末端的距離，b是車輛中心到安全域前端的距離，c是安全域的寬，d是安全域的長，分別為：

式中：l為車輛的長，m；w為車輛的寬，m；ηa（ Type）、ηb（ Type）和ηc（ Type）為關于Type的函數(shù)。

由式（7）～（10）可以求出車輛安全域的長d和寬c，同時可以得到安全域的中心坐標：

式中：yaw為車輛的航向角。

2.2 算法優(yōu)化

基于安全域的定義，可以應用于局部路徑規(guī)劃，以便尋找一條安全性更高的最優(yōu)軌跡。傳統(tǒng)OTG算法的代價函數(shù)對安全性的考慮不夠全面，并沒有考慮到規(guī)劃出軌跡后，在軌跡的控制周期T內主控車輛與障礙物碰撞的可能性。把安全域考慮進去，彌補這個缺陷，提高軌跡的安全性。在傳統(tǒng)OTG算法的基礎上，考慮交通參與者的安全域，對評價軌跡的代價函數(shù)進行改進，新添加一項，以達到優(yōu)化傳統(tǒng)OTG算法的目的。

圖2～4是同一場景下不同時刻的安全域重疊部分示意圖，其中，圖2是t=0 s，圖3是t=(T/2) s，圖4是t=Ts。圖中淺灰色的線表示車道線；紅色線是規(guī)劃出的軌跡；藍色矩形是主控車輛，綠色矩形是障礙物車輛；藍色箭頭是主控車輛的行駛方向，綠色箭頭是障礙物車輛的行駛方向，長度表示速度的大小；藍色框是主控車輛的安全域，綠色框是障礙物車輛的安全域；黑色部分為剔除超過道路邊界的安全域重疊部分。

圖2 t= 0 s

圖3 t=(T/2) s

圖4 t= T s

Coverlap是在軌跡的控制周期T內，隨著時間變化，軌跡上每一時刻主控車輛和所有障礙物的安全域重疊部分OverlapArea的面積累加和，如式（13）所示。面積累加和越小，說明兩輛車的碰撞可能性越小，行駛安全性越高。反之，面積累加和越大，說明兩輛車的碰撞可能性越大，行駛安全性越低。

式中：nobs為障礙物的數(shù)量；Car （A′,a,b,c）為主控車輛的信息；Obs （A′,a,b,c）為障礙物的信息；Overlap ()為計算主控車輛與障礙物重疊面積的函數(shù)。

因此，總的代價函數(shù)變?yōu)椋?/p>

式中：koverlap為安全域重疊部分的面積累加和的權重。

顯然，上述的軌跡優(yōu)化代價函數(shù)中并沒有包含關于障礙物規(guī)避的相關懲罰，并且代價函數(shù)中也沒有包含最大速度、最大加速度和最大曲率等控制約束限制，也就是說當前的優(yōu)化軌跡集合并沒有考慮障礙物規(guī)避和控制約束。因此，在完成所有備選軌跡的損失計算后進行一次軌跡檢查，過濾不符合控制約束條件限制的、可能會碰撞障礙物的軌跡等，檢查內容包括：檢查縱向上的速度是否超過設定的最大限速；檢查縱向上的加速度是否超過設定的最大加速度；檢查軌跡的曲率是否超過最大曲率；檢查軌跡是否會引起碰撞。在軌跡檢查后，得到一組有效軌跡集合，然后選取代價函數(shù)最小的軌跡，就是最優(yōu)軌跡。

3 算法驗證

為了驗證本文優(yōu)化后算法的有效性和可行性，對算法進行仿真測試和實車測試。

3.1 仿真測試

自主搭建了Matlab仿真平臺，如圖5所示，主要包括3個部分：（1）道路模型，利用三次樣條擬合道路，獲取道路信息，提供簡易地圖。（2）交通參與者模型，包含交通參與者的類型、速度、加速度、加塞運動等，提供交通流。（3）算法仿真結果展示，開發(fā)以主控車輛為中心的自適應繪圖工具，提供可視化界面，如圖6所示。根據(jù)當前主控車輛視角，自適應動態(tài)生成以主控車輛為中心的車輛周圍環(huán)境，包括主控車輛和其他參與者的位姿與運動狀態(tài)、軌跡等。

圖5 Matlab仿真平臺的基本框架

圖6是算法仿真結果的可視化界面。其中，右上角的Currtime是仿真的當前時間，T是軌跡的控制周期，S是最優(yōu)軌跡末端的橫坐標，D是最優(yōu)軌跡末端的縱坐標，V是主控車輛要到達的速度，Decision表示行為決策結果。淺灰色的虛線表示道路車道線，深藍色的實線表示道路中心參考線，從上到下分別是1車道、2車道和3車道。有數(shù)字的黃色框是表示路程的路牌。黑色矩形框表示主控車輛，黑色矩形框外面的亮藍色矩形框表示主控車輛的防碰撞圈，其他顏色的矩形框表示除主控車輛以外的交通參與者。亮藍色的線表示規(guī)劃出的所有軌跡，黑色線表示有效軌跡，紅色線表示選擇的最優(yōu)軌跡。

圖6 仿真結果展示界面

在Matlab仿真平臺上，采用3個不同的測試場景對其進行試驗，把傳統(tǒng)OTG算法和優(yōu)化后的OTG算法進行對比。在試驗中用到了兩種類型的車，分別是主控車輛（大車）和障礙物車輛（小車）。當Type為大車時，ηa=03.、ηb=08.和ηc=05.；當Type為小車時，ηb=02.、ηb=07.和ηc=04.。圖7～9展示了在3個場景中采用不同的代價函數(shù)得到不同的最優(yōu)軌跡。同一個場景中規(guī)劃出的所有軌跡都一樣，只是最優(yōu)軌跡略有不同。圖7～9中（a）表示的是傳統(tǒng)OTG算法的仿真結果，（b）表示的是優(yōu)化后OTG算法的仿真結果。

由圖7可以看出，亮藍色車輛有側方插入的趨勢，（a）中選擇的是回到道路中心線的軌跡，增加了主控車輛與亮藍色車輛碰撞的概率。而（b）中最優(yōu)軌跡選擇偏向1車道的軌跡，考慮到側方有插入趨勢的亮藍色車輛和前方距離較近的綠色車輛，降低了主控車輛與它們的碰撞概率。

圖7 場景一

由圖8可以看出，主控車輛前方有一輛綠色車輛，（a）中仍然沿著道路中心線直行，增加了主控車輛與它碰撞的概率。而（b）中最優(yōu)軌跡選擇避開綠色車輛、偏向1車道的軌跡，大大降低了車輛碰撞的可能性。

圖8 場景二

由圖9可以看出，主控車輛右側有一輛正在側方插入的綠色車輛，（a）中選擇回到道路中心線的軌跡，會使主控車輛與綠色車輛碰撞的概率大大增加。而（b）中最優(yōu)軌跡選擇偏向1車道的軌跡，大大降低了主控車輛與亮藍色車輛的碰撞概率。

圖9 場景三

通過比較圖7～9中的（a）和（b），表明對OTG算法的代價函數(shù)進行改進，規(guī)劃出的最優(yōu)路徑與傳統(tǒng)OTG相比，考慮的因素更多，使行駛更加安全，更加擬人化。這就證明通過考慮交通參與者的安全域對代價函數(shù)進行改進這一方法是有效的，達到了優(yōu)化傳統(tǒng)OTG算法的目的。

3.2 實車測試

為驗證本文局部路徑規(guī)劃算法，使用陜汽長頭牽引車作為試驗平臺，試驗車搭載一套P2高精度MEMS組合導航系統(tǒng)，1個8線激光雷達為ibeo 傳感器，1個前向毫米波雷達為杭州智波科技有限公司推出的一款77 GHz中長距離毫米波雷達 ZB710LA01-A，1個側向毫米波雷達為智波科技 79 GHz 高分辨率雷達和1個內嵌深度學習算法的單目前視相機。圖10為試驗車，（a）是試驗車的正面，（b）是試驗車的側面。圖11為實車試驗結果，實車試驗時車道為單向單車道，在試驗時由于場地限制允許軌跡跨越對向車道。圖11中（a）為車輛避障時的行駛軌跡，（b）為車輛的方向盤轉角，（c）為車輛的行駛速度。

圖10 試驗平臺

由圖11可以看出，本文的局部路徑規(guī)劃算法規(guī)劃出的路徑能夠成功避開前方動態(tài)障礙物，并且在避障過程中，車輛的方向盤轉角整體變化平緩且連續(xù)，沒有發(fā)生突變和振蕩情況。通過實車試驗驗證了算法具有實時性和良好的穩(wěn)定性。

圖11 實車試驗結果

5 總結與展望

（1）首次提出了安全域的概念，將車輛行駛過程中車、人、路之間各因素對行駛安全性的影響程度統(tǒng)一用安全域表示。安全域的大小表示交通參與者周圍的安全范圍，兩個安全域的重疊面積大小表示行駛安全性的高低，重疊面積越大，行駛安全性越低，重疊面積越小，行駛安全性越高。

（2）利用安全域對傳統(tǒng)OTG算法的代價函數(shù)進行改進，以達到優(yōu)化算法的目的。通過Matlab仿真平臺對其進行驗證，結果顯示優(yōu)化后的OTG算法比傳統(tǒng)OTG算法更安全，表明優(yōu)化是有效的。同時，在實車試驗中驗證了優(yōu)化算法的實時性和穩(wěn)定性。

（3）未來展望主要有以下3點：①對OTG算法的優(yōu)化只采用了改進代價函數(shù)的方法，后續(xù)可以利用多種優(yōu)化方式（調整權重、在預測層加入機器學習算法等），以期達到更好的效果。②僅把安全域應用于路徑規(guī)劃算法，以后的研究可以把安全域應用于更多領域，如行為決策，給行為決策提供新的思路和方法，使安全域發(fā)揮更大的作用。③安全域僅考慮了車、人、路之間的互相影響，但忽略了駕駛人在車輛行駛時造成的影響。駕駛人是行駛過程中非常重要的一環(huán)，后續(xù)研究也應考慮駕駛人對行駛安全性的影響，盡可能地與安全域結合，得到一種更全面的表征行駛安全性的指標，為自動駕駛決策規(guī)劃提供更可靠的依據(jù)。