自動駕駛車輛避障軌跡規(guī)劃

徐靖賢

（長安大學 汽車學院，陜西 西安 710064）

隨著人工智能不斷發(fā)展，作為無人駕駛重要模塊的軌跡規(guī)劃已經(jīng)逐漸成為研究的熱點。軌跡規(guī)劃大致分為全局規(guī)劃和局部規(guī)劃，其中局部規(guī)劃的主要作用是如何考慮自車與周圍障礙物位置關(guān)系找出避免碰撞的路徑。對于換道等避障場景，無人駕駛車輛需要從當前車道變更到相鄰車道或偏置行駛，輸出一條安全無碰撞的軌跡同時兼顧時效性、舒適性，常利用圓弧、正弦函數(shù)生成換道軌跡，但是這些方法無法滿足車輛實際行駛過程中的換道避障要求。劉亞龍利用BP神經(jīng)網(wǎng)絡特性進行自學習，通過車輛實際換道軌跡數(shù)據(jù)建立換道軌跡模型[1]。陳偉為應對環(huán)境信息的變化采用了滾動窗口的局部規(guī)劃方法，在局部規(guī)劃過程中采用啟發(fā)式算法進行換道關(guān)鍵路徑點處理，再利用過渡圓弧平滑生成變道軌跡[2]。李瑋等針對換道軌跡曲率不連續(xù)現(xiàn)象引入B樣條來進行重規(guī)劃，從而重新建立自由變道模型[3]。CHEN J等利用八叉樹數(shù)據(jù)結(jié)構(gòu)中的有效操作在線生成有大型重疊三位網(wǎng)格組成的自由空間飛行走廊，然后基于生成滿足高階動力學約束的平滑軌跡[4]。DANIEL M等基于優(yōu)化將路徑規(guī)劃問題構(gòu)造成典型二次規(guī)劃問題，能夠?qū)崟r生成三維最優(yōu)路徑并且滿足輸入約束從而較好解決了緊密約束環(huán)境下的軌跡生成問題[5]。DING W C等使用統(tǒng)一的方法來描述環(huán)境語義信息，建立一種新穎的無碰撞立方體組成的動態(tài)約束即時空語義走廊結(jié)構(gòu)，并且使用分段貝塞爾曲線參數(shù)化來保證生成軌跡的安全無碰撞且符合動力學要求[6]。本文從車輛避障出發(fā)，考慮車輛形狀與周圍障礙物位置關(guān)系提出一種考慮車輛形狀的障礙物邊界處理方法，并基于運動走廊采用貝塞爾曲線進行軌跡優(yōu)化從而確保生成軌跡的安全性和可行性。

1 車輛模型建模

1.1 使用笛卡爾坐標系

SL坐標系也叫做Frenet Frame，如圖1所示以道路中心線為參考，S表示道路中心線的方向，L表示與道路中心線垂直的方向。在結(jié)構(gòu)化道路上行駛的時候，SL坐標系能夠降低求解維度簡化問題，道路邊界限制更容易轉(zhuǎn)化為線性約束，比XY坐標系更加貼合實際需求，在目前的軌跡規(guī)劃中得到了廣泛的運用。但是由于SL坐標系不能保證唯一性，位置點可能會在曲線上產(chǎn)生多個投影，所以在投影時需要額外的限制和處理。此外SL坐標系下無法對自我車輛的真實運動學進行建模，無法確保軌跡的連續(xù)性[7]，這些缺陷很容易導致違反軌跡曲率限制，當?shù)缆非瘦^大時這一情況會更加明顯，在避障場景下選用笛卡爾坐標系則可以避免這些問題[7]。

圖1 笛卡爾坐標系和SL坐標系

1.2 車輛模型建立

軌跡規(guī)劃任務中的約束通常包括運動學約束、兩點邊界約束和防撞約束。車輛運動學模型可表示為

(x,y)表示車輛后軸中心位置，其他參數(shù)含義如表1所示。

表1 車輛運動學模型相關(guān)參數(shù)及含義

自車形狀一般表示為矩形，如圖2所示，導致防撞約束在計算中不可微分，判斷矩形外觀與障礙物是否有交集計算量很大，如果只是考慮后軸中心點，則很難考慮到車輛外形特征，因此，考慮將車輛用兩個圓形來代替，這種方法既可以減少計算量又可以保證冗余[7]。

圖2 車輛模型示意圖

2 構(gòu)建運動走廊

2.1 障礙物邊界預處理

軌跡規(guī)劃需要考慮自車與周圍障礙物的位置關(guān)系，即除考慮自車車輛位置與形狀，還需要對環(huán)境建模。Apollo EM planner 利用撒點的方式基于動態(tài)規(guī)劃的路徑步驟提供一條粗略的路徑信息，缺點在于需要逐個對障礙物進行碰撞檢測復雜度較高（EM planner）。如果能提前將障礙物提前合并處理，雖然降低了解空間，但是也相應避免了碰撞檢測，因此，本文基于運動走廊提出一種障礙物合并方法，當存在多個障礙物時求出所有需要關(guān)注障礙物的邊界信息并進行排列，將位置有重疊的障礙物進行合并，如圖3所示障礙物1由兩個重疊的障礙物進行合并，分別取障礙物邊界最值作為合并組合的邊界，依此類推合并完成后可以得到互不重疊的障礙物組合，每個障礙物組合的起點和終點處分別產(chǎn)生通道節(jié)點，三個障礙物組合共生成6個通道節(jié)點以及對應的障礙物，這樣就得到了需要關(guān)注的障礙物的邊界組合和潛在行駛通道，從而降低周圍環(huán)境的復雜度便于構(gòu)建運動走廊。

圖3 障礙物合并示意圖

2.2 運動走廊生成

軌跡規(guī)劃不僅要對車輛進行建模，環(huán)境同樣需要建模處理，上文將車輛簡化為兩個圓盤的模型，如果將車輛收縮半徑對應地進行障礙物膨化，那么車輛的軌跡規(guī)劃可以視為兩個質(zhì)點的避障軌跡。如圖4所示，車輛轉(zhuǎn)向過程中P1為距離前車最近的點即最容易發(fā)生角碰的危險點，由此畫圓可以得到車輛避障過程中的危險域，半徑為圖2中車輛模型示意圖中圓盤的半徑，基于得到的障礙物邊界，軌跡規(guī)劃即可進行傳統(tǒng)的全局搜索得到接近全局最優(yōu)的路徑粗解。全局路徑規(guī)劃的研究目前比較成熟，經(jīng)典的柵格法、A*等算法有著廣泛的運用，其中Hybrid A*算法作為A*算法的變種引入了角度維度考慮了車輛運動學特性，得到的路徑能夠較好的滿足避障場景下對于車輛對于轉(zhuǎn)向約束的要求，本文使用該算法得到初始軌跡點，并在此基礎上進行擴展得到可行駛通道，Hybrid A*算法研究比較成熟且本文研究重點為運動走廊生成以及軌跡優(yōu)化故不展開介紹。

圖4 危險域劃分示意圖

全局路徑規(guī)劃得到初始軌跡點，擴展得到期望的運動走廊，即按照軌跡點依次向外擴張矩形直至接觸到障礙物邊界，多個矩形的聯(lián)通區(qū)域就構(gòu)成了安全行駛走廊。具體的做法是從初始軌跡點開始進行膨脹，即依次向周圍四個方向擴展，每次擴展一定的距離并直到在膨脹方向上矩形框觸及障礙物邊界，接下來第二個軌跡點開始膨脹，直到所有全局規(guī)劃得到的軌跡點膨脹完畢。為得到盡量稀疏的運動走廊，已經(jīng)被膨脹區(qū)域覆蓋的軌跡點不再進行擴展，如圖5所示的膨脹區(qū)域即擴展矩形的組合就是所求的可行駛區(qū)域，這樣就保證可行駛區(qū)域內(nèi)沒有障礙物存在，根據(jù)可行駛區(qū)域進行軌跡優(yōu)化也就不用考慮障礙物避障，從而能夠快速有效優(yōu)化出安全無碰撞符合車輛運動的軌跡。

圖5 運動走廊生成

3 硬約束軌跡優(yōu)化

為了滿足無人駕駛車輛運動特性，軌跡規(guī)劃要求規(guī)劃出的軌跡要盡量平滑，為得到最優(yōu)的平滑軌跡，研究者通常將實際問題轉(zhuǎn)化為帶約束優(yōu)化問題，即在可行軌跡中找到最優(yōu)的特定軌跡，對得到的軌跡粗解進行優(yōu)化。軌跡優(yōu)化有利于路徑平滑但是不利于避障，基于避免碰撞檢測的思想，本文采用添加硬約束的方法進行軌跡優(yōu)化[4]，也就是將軌跡平滑優(yōu)化問題轉(zhuǎn)化為二次優(yōu)化問題然后添加硬約束進行求解，最后得到滿足要求的軌跡。上文根據(jù)障礙物邊界進行了全局路徑規(guī)劃，得到粗解后對初始軌跡點進行膨脹得到可行駛區(qū)域，在該區(qū)域內(nèi)利用貝塞爾曲線的性質(zhì)進行硬約束優(yōu)化，就能在保證得到安全的平滑優(yōu)化軌跡。

3.1 貝塞爾曲線的性質(zhì)

貝塞爾曲線是由若干控制點按照一定權(quán)重組合而成的曲線，convex hull性質(zhì)保證曲線始終被約束在控制點組成的凸包內(nèi)，hodograph性質(zhì)則保證曲線的各階導數(shù)仍是貝塞爾曲線，可以確保規(guī)劃的軌跡、速度、加速度、加速度連續(xù)，后續(xù)的優(yōu)化問題構(gòu)造都是基于這些性質(zhì)的，其表現(xiàn)形式如下：

從起點到終點每段軌跡可以表示為

3.2 硬約束優(yōu)化

軌跡優(yōu)化問題實際上是最優(yōu)控制問題，而Minimum Snap算法是經(jīng)典的軌跡優(yōu)化算法，主要思想是將軌跡規(guī)劃問題構(gòu)造成典型二次規(guī)劃問題，從而施加約束進行求解，Minimum Snap算法的代價函數(shù)自然是snap值的平方，本文使用每段貝塞爾曲線的三階導平方再求和，即期望過程中jerk最小，k=3，那么總的目標函數(shù)可以被寫為

基于貝塞爾曲線的凸包性質(zhì)，軌跡必然約束在凸包內(nèi)，只要將凸包限制在運動走廊內(nèi)，優(yōu)化的軌跡就會在允許邊界之內(nèi)，以此可以得到安全性與運動學不等式約束，兩段軌跡之間的連續(xù)性則帶來等式約束，最終將上述代價函數(shù)和約束條件整合起來，構(gòu)造成一個標準的二次優(yōu)化問題來進行求解。圖6為ros環(huán)境下進行的仿真試驗，規(guī)劃軌跡能夠較好滿足復雜環(huán)境下避障要求。

圖6 運動走廊貝塞爾曲線優(yōu)化軌跡

4 總結(jié)與展望

基于笛卡爾坐標系建立考慮車輛形狀的自車模型，提出一種新的障礙物邊界處理方法，根據(jù)自車與周圍環(huán)境的位置關(guān)系排除危險行駛區(qū)域建立運動走廊，并利用貝塞爾曲線性質(zhì)進行軌跡優(yōu)化，通過仿真驗證有效地避免與障礙物的碰撞，最大限度保證規(guī)劃軌跡的安全性和舒適性。不過笛卡爾坐標系在周圍環(huán)境復雜時計算需求量較大，如何在SL坐標系下建立滿足軌跡曲率要求的運動走廊需要進一步探索，另外障礙物合并會減少解空間，如何在保證安全性的同時減少求解空間損耗仍需研究。