自動駕駛車輛避障軌跡規劃

徐靖賢

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

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

1 車輛模型建模

1.1 使用笛卡爾坐標系

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

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

1.2 車輛模型建立

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

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

表1 車輛運動學模型相關參數及含義

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

圖2 車輛模型示意圖

2 構建運動走廊

2.1 障礙物邊界預處理

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

圖3 障礙物合并示意圖

2.2 運動走廊生成

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

圖4 危險域劃分示意圖

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

圖5 運動走廊生成

3 硬約束軌跡優化

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

3.1 貝塞爾曲線的性質

貝塞爾曲線是由若干控制點按照一定權重組合而成的曲線，convex hull性質保證曲線始終被約束在控制點組成的凸包內，hodograph性質則保證曲線的各階導數仍是貝塞爾曲線，可以確保規劃的軌跡、速度、加速度、加速度連續，后續的優化問題構造都是基于這些性質的，其表現形式如下：

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

3.2 硬約束優化

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

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

圖6 運動走廊貝塞爾曲線優化軌跡

4 總結與展望

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