基于AStar算法的避障路徑局部規劃方法開發

劉志軍,蘇 亮,吳少雄

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

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

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

本文基于AStar與路徑平滑算法,結合實際應用場景,重新設計局部路徑規劃流程,實現用于避障的局部路徑規劃,最后利用Carla[3]仿真環境進行模擬與驗證。

1 AStar算法的介紹

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

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

(1)

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

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

(2)

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

(3)

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

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

AStar算法會創建OpenList[2]和CloseList[2]2個表對節點信息進行管理。其中OpenList用來保存可能被搜索到的節點,并根據代價函數計算的結果進行排序;CloseList用來保存已經被舍棄的節點,算法不會對這部分節點進行搜索。當搜索完成后,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內;P0為起始點坐標;P1、P2為控制點坐標。

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

圖1 路徑平滑算法

3 AStar算法在繞障中的應用流程

智能車繞障場景是局部路徑規劃的一種典型應用,如果要使用AStar算法進行規劃則需要進行2次甚至多次規劃。讓其可以根據實時環境不斷地調整規劃路徑,根據此需求設計如圖2所示的規劃流程。

圖2 AStar局部路徑規劃流程

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

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

4.1 Carla仿真平臺簡介

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

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

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

圖3 仿真環境與自動駕駛程序的交互

4.2 場景搭建

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

4.3 AStar算法可行性仿真

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

4.3.1 生成局部地圖

如圖5所示,當前方障礙物侵入行駛路徑造成無法通行,程序將柵格化激光雷達點云信息,并以當前位置作為原點生成局部地圖。

4.3.2 駛離全局規劃路徑

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

4.3.3 多次路徑規劃

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

圖7 多次規劃

4.3.4 返回全局規劃

如圖8所示,經過多次路徑規劃,車輛已經完全繞開障礙物。當滿足駕駛全局規劃的條件時,算法會生成一條返回全局規劃的過渡路徑。

圖8 返回全局規劃

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

圖9 軌跡回放

5 結束語

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