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

毛莞丁，鄒若冰，劉晨麗

（陜西汽車集團(tuán)有限責(zé)任公司，西安 710043）

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

當(dāng)前研究人員提出了很多的路徑規(guī)劃算法，如人工勢場法[3-4]、快速隨機(jī)樹（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ù)，加權(quán)求和得到總的代價函數(shù)，通過代價函數(shù)最小化選取最優(yōu)軌跡。該方法具有較強的實用性，是目前普遍采用的局部路徑規(guī)劃算法之一。Apollo EM規(guī)劃器[15]與OTG算法相似，同樣采用降維思想，將軌跡規(guī)劃問題分解為橫向和縱向，再合成三維軌跡，根據(jù)代價函數(shù)篩選出最好的合成軌跡。周慧子等[16]針對自動駕駛中避障的動態(tài)路徑規(guī)劃問題，提出一種在已知車輛的初始位置、速度、方向和障礙物位置情況下，實時避開障礙物的動態(tài)規(guī)劃算法。梁廣民[17]提出了一種新的自動駕駛軌跡規(guī)劃方法，該方法可以為自動駕駛生成舒適安全的軌跡。郭蓬等[18]在使用Frenet坐標(biāo)系理論的基礎(chǔ)上，提出了一種考慮到車道線曲率和障礙物模型信息的方法，然后通過最小化代價函數(shù)，選出局部最優(yōu)路徑。毛晨悅等[19]提出一種基于人工勢場法的無人機(jī)路徑規(guī)劃避障算法，提高無人機(jī)的避障性能。

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

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

本文從代價函數(shù)的角度出發(fā)，對傳統(tǒng)OTG算法進(jìn)行優(yōu)化。上述算法的代價函數(shù)包含大多數(shù)的評價方法（安全性、平滑性、連續(xù)性等），但是無法表征動態(tài)障礙物對車輛行駛安全的影響。例如，主控車輛和障礙車輛之間距離相等，兩者既可以是相向而行，也可以是同向而行，但兩者對主控車輛的影響不同，選取出的最優(yōu)軌跡也會不同。針對這種情況上述局部路徑規(guī)劃算法的代價函數(shù)會篩選出相同軌跡，而無法做出準(zhǔn)確的評判。因此，傳統(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算法在進(jìn)行路徑規(guī)劃時分別從橫向和縱向考慮，最后進(jìn)行融合。算法分為兩部分，第一部分是生成軌跡集合，第二部分是對軌跡集合進(jìn)行評價，篩選出最優(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ù)對其進(jìn)行評價，選出最優(yōu)軌跡。傳統(tǒng)OTG算法的代價函數(shù)也分為橫向和縱向，橫向軌跡的代價函數(shù)為：

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

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

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

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

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

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

2.1 安全域

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

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

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

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

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

圖1 車輛的安全域示意圖

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

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

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

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

2.2 算法優(yōu)化

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

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

圖2 t= 0 s

圖3 t=(T/2) s

圖4 t= T s

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

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

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

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

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

3 算法驗證

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

3.1 仿真測試

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

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

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

圖6 仿真結(jié)果展示界面

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

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

圖7 場景一

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

圖8 場景二

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

圖9 場景三

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

3.2 實車測試

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

圖10 試驗平臺

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

圖11 實車試驗結(jié)果

5 總結(jié)與展望

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

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

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