基于AStar算法的避障路徑局部規(guī)劃方法開發(fā)

劉志軍,蘇 亮,吳少雄

(廈門金龍聯(lián)合汽車工業(yè)有限公司,福建 廈門 361023)

汽車自動駕駛技術(shù)的路徑規(guī)劃可分為全局規(guī)劃和局部規(guī)劃[1]。全局規(guī)劃是指在一定的地圖范圍內(nèi)規(guī)劃出起點到終點的行駛路徑,全局規(guī)劃是一次性完成的;局部規(guī)劃是指對全局規(guī)劃結(jié)合實際環(huán)境信息進行規(guī)劃路徑的細化,當(dāng)全局規(guī)劃的行駛區(qū)域內(nèi)出現(xiàn)障礙物時就需要局部規(guī)劃來解決。而避障規(guī)劃則是局部規(guī)劃中非常重要的一環(huán),能夠大大提高自動駕駛的智能化程度。

一次性的全局規(guī)劃通常先利用AStar[2]進行路徑點的搜索,再對結(jié)果進行平滑處理得到適合行駛的路徑。而局部規(guī)劃往往會因環(huán)境的變化,需要二次甚至多次規(guī)劃才能得到合適的行駛路徑。

本文基于AStar與路徑平滑算法,結(jié)合實際應(yīng)用場景,重新設(shè)計局部路徑規(guī)劃流程,實現(xiàn)用于避障的局部路徑規(guī)劃,最后利用Carla[3]仿真環(huán)境進行模擬與驗證。

1 AStar算法的介紹

AStar算法是在Dijkstra算法基礎(chǔ)上改進而來的一種啟發(fā)式的路徑搜索算法。而Dijkstra[4]算法為基于廣度優(yōu)先空間搜索算法,從起始點周圍的節(jié)點開始迭代擴展搜索至終點,從而得到起始點到終點的規(guī)劃點信息。這種算法對算力要求高,且執(zhí)行時間長。AStar算法通過代價函數(shù)(式(1))[5]來確定迭代搜索的方向,避免盲目搜索導(dǎo)致的算力消耗,算力要求相對較低,且執(zhí)行時間較短。

f(a)=g(a)+h(a)

(1)

式中：f(a)為節(jié)點a的代價;g(a)起始節(jié)點到節(jié)點a的代價;h(a)為節(jié)點a到目標節(jié)點的代價。

h(a)的計算可以是歐幾里得距離h1(a),也可以是曼哈頓距離h2(a)[6]：

(2)

h2(a)=|xa-xgoal|+|ya-ygoal|

(3)

式中：xa、ya分別為節(jié)點a的x坐標和y坐標;xgoal、ygoal分別為目標節(jié)點的x坐標和y坐標。

歐幾里得距離h1(a)計算的是節(jié)點a到目標節(jié)點的直線距,需要進行平方與開根號計算,會占用較多的浮點算力;曼哈頓距離h2(a)僅計算節(jié)點a得到目標節(jié)點橫向與縱向的距離和,只需要用到加法計算,可以提高計算效率,優(yōu)先推薦使用。

AStar算法會創(chuàng)建OpenList[2]和CloseList[2]2個表對節(jié)點信息進行管理。其中OpenList用來保存可能被搜索到的節(jié)點,并根據(jù)代價函數(shù)計算的結(jié)果進行排序;CloseList用來保存已經(jīng)被舍棄的節(jié)點,算法不會對這部分節(jié)點進行搜索。當(dāng)搜索完成后,OpenList將被清空。

2 路徑平滑處理方法的介紹

本文方案的第一步是進行路徑的平滑處理,利用AStar算法最終得到離散路徑點信息,為了避免路徑抖動帶來的控制算法振蕩問題,需要使用貝塞爾曲線[7]算法對離散的路徑點進行平滑處理。貝塞爾曲線有一階、二階和高階之分,一階貝塞爾曲線為一條直線,不滿足使用要求;高階貝塞爾曲線則會消耗大量算力,因而選擇式(4)所示的二階貝塞爾曲線進行路徑平滑處理。

B(t)=(1-t)2P0+2t(1-t)P1+t2P2,t∈[0,1]

(4)

式中：B(t)為t時刻下的坐標,這里限定t在1 s內(nèi);P0為起始點坐標;P1、P2為控制點坐標。

圖1是使用二階貝塞爾平滑前后的路徑對比圖。從圖中可看出,采用平滑算法后的路徑曲線更加貼合實際車輛行駛的軌跡,也更能讓控制算法執(zhí)行。

圖1 路徑平滑算法

3 AStar算法在繞障中的應(yīng)用流程

智能車繞障場景是局部路徑規(guī)劃的一種典型應(yīng)用,如果要使用AStar算法進行規(guī)劃則需要進行2次甚至多次規(guī)劃。讓其可以根據(jù)實時環(huán)境不斷地調(diào)整規(guī)劃路徑,根據(jù)此需求設(shè)計如圖2所示的規(guī)劃流程。

圖2 AStar局部路徑規(guī)劃流程

如圖2所示,正常行駛情況的路徑由全局規(guī)劃負責(zé),當(dāng)遇到障礙物并且滿足避障條件時,以車輛當(dāng)前坐標為原點,建立新的Odometry[8]坐標系。整個繞障過程在這個新的坐標系中完成,車輛完成繞障后,駛回全局規(guī)劃的路線上完成整個局部路徑規(guī)劃。路徑規(guī)劃前需要通過激光雷達獲取車輛前方的點云信息,并且生成costmap[9]。可行駛區(qū)域需要根據(jù)實際道路標線進行規(guī)劃,案例中規(guī)劃的可行駛區(qū)域為單向雙車道,相比全局規(guī)劃,受限制的規(guī)劃區(qū)域算法能夠更快速完成求解。由于車輛前方有障礙物遮擋,激光雷達無法探測障礙物后面的環(huán)境信息,只能預(yù)估目標距離,并在參考線上找到對應(yīng)的參考點作為目標點。在AStar規(guī)劃的結(jié)果上重新選取偏離參考線最遠的點作為新的目標點,將當(dāng)前起始位置到目標點的路徑通過貝塞爾曲線進行平滑處理,處理后的曲線再與原有參考線進行拼接。當(dāng)車輛運動到目標點時觸發(fā)路徑重規(guī)劃,如果原始參考線上沒有障礙物,則通過貝塞爾曲線重新規(guī)劃一條返回原始參考線的路徑。如果當(dāng)前參考線上仍然存在無法通過的障礙物,則通過AStar重新規(guī)劃繞障路徑。

4 AStar繞障算法的可行性仿真分析

4.1 Carla仿真平臺簡介

Carla是一款支持自動駕駛系統(tǒng)開發(fā)、測試的開源仿真平臺,本文采用該平臺進行AStar繞障算法的可行性仿真分析。

要進行AStar繞障算法的仿真分析,首先需要進行相應(yīng)的場景搭建,然后根據(jù)圖2介紹的AStar局部規(guī)劃流程編寫自動駕駛程序,再通過自動駕駛程序進行車輛控制的仿真分析。

Carla平臺可以模擬多種環(huán)境下的城市道路、行人、車輛等原始要素,并將這些原始要素組合成開發(fā)者需要的仿真場景。同時Carla也支持多種傳感器(GNSS、攝像頭、激光雷達等),通過自帶的RosBridge[10]功能實現(xiàn)Carla與ROS Topic[11]或者其他第三方自動駕駛程序的交互,完成閉環(huán)仿真,如圖3所示。

圖3 仿真環(huán)境與自動駕駛程序的交互

4.2 場景搭建

在Carla中構(gòu)建一個用于模擬避障路徑規(guī)劃的場景,如圖4所示。圖中①為受控制的自主車輛(ego car),②為障礙車輛,線段③圍成的區(qū)域為規(guī)劃的可行駛區(qū)域,線段④為全局規(guī)劃得到的參考線。

4.3 AStar算法可行性仿真

自動駕駛程序可行性仿真分析包括地圖生成、駛離全局規(guī)劃、多次路徑規(guī)劃、返回全局規(guī)劃。根據(jù)圖4所搭建的場景運行自動駕駛程序,對繞障算法流程進行可行性驗證。

4.3.1 生成局部地圖

如圖5所示,當(dāng)前方障礙物侵入行駛路徑造成無法通行,程序?qū)鸥窕す饫走_點云信息,并以當(dāng)前位置作為原點生成局部地圖。

4.3.2 駛離全局規(guī)劃路徑

如圖6所示,使用4.3.1節(jié)生成的局部地圖信息,通過AStar算法得到一條能夠避開前方障礙物的局部路徑。

4.3.3 多次路徑規(guī)劃

由于激光雷達無法探測出前方障礙物的長度,當(dāng)車輛通過4.3.2節(jié)的路徑繞開障礙物后,激光雷達可以探測到車輛的長度信息。如圖7所示,當(dāng)前一次規(guī)劃要走完時,仍然無法繞回原有路徑,算法會繼續(xù)規(guī)劃,直到車輛完全繞開障礙物。

圖7 多次規(guī)劃

4.3.4 返回全局規(guī)劃

如圖8所示,經(jīng)過多次路徑規(guī)劃,車輛已經(jīng)完全繞開障礙物。當(dāng)滿足駕駛?cè)忠?guī)劃的條件時,算法會生成一條返回全局規(guī)劃的過渡路徑。

圖8 返回全局規(guī)劃

圖6～圖9為整個繞障算法的執(zhí)行過程,均為自動駕駛程序視角。圖6中車輛在全局規(guī)劃的參考線上發(fā)現(xiàn)障礙物,規(guī)劃出一條駛離全局規(guī)劃的避障路徑;圖7為避障路徑行駛結(jié)束后,自動駕駛程序發(fā)現(xiàn)原全局規(guī)劃路徑上仍然有障礙物而繼續(xù)規(guī)劃避障路徑;圖8為自動駕駛程序規(guī)劃返回全局規(guī)劃路徑的過渡路徑;最后在圖9的回放中,可以看到整個避障過程的行駛路徑。

圖9 軌跡回放

5 結(jié)束語

目前,該避障算法已經(jīng)在仿真環(huán)境中完成多場景的驗證測試,并在物流車與觀光車等園區(qū)專用車輛上進行應(yīng)用測試,成熟后將在開放道路的載客車輛上應(yīng)用。