徐靖賢

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

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

1 車輛模型建模

1.1 使用笛卡爾坐標系

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

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

1.2 車輛模型建立

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

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

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

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

圖2 車輛模型示意圖

2 構建運動走廊

2.1 障礙物邊界預處理

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

圖3 障礙物合并示意圖

2.2 運動走廊生成

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

圖4 危險域劃分示意圖

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

圖5 運動走廊生成

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

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

3.1 貝塞爾曲線的性質

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

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

3.2 硬約束優(yōu)化

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

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

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

4 總結與展望

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