徐靖賢

（長(zhǎng)安大學(xué) 汽車學(xué)院，陜西 西安 710064）

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

1 車輛模型建模

1.1 使用笛卡爾坐標(biāo)系

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

圖1 笛卡爾坐標(biāo)系和SL坐標(biāo)系

1.2 車輛模型建立

軌跡規(guī)劃任務(wù)中的約束通常包括運(yùn)動(dòng)學(xué)約束、兩點(diǎn)邊界約束和防撞約束。車輛運(yùn)動(dòng)學(xué)模型可表示為

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

表1 車輛運(yùn)動(dòng)學(xué)模型相關(guān)參數(shù)及含義

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

圖2 車輛模型示意圖

2 構(gòu)建運(yùn)動(dòng)走廊

2.1 障礙物邊界預(yù)處理

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

圖3 障礙物合并示意圖

2.2 運(yùn)動(dòng)走廊生成

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

圖4 危險(xiǎn)域劃分示意圖

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

圖5 運(yùn)動(dòng)走廊生成

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

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

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

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

從起點(diǎn)到終點(diǎn)每段軌跡可以表示為

3.2 硬約束優(yōu)化

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

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

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

4 總結(jié)與展望

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