融合改進A*和TEB算法的移動機器人路徑規(guī)劃

郭志軍,尹亞昆,李亦軒,于秀濤,張 鵬

(1.河南科技大學 車輛與交通工程學院,河南 洛陽 471003;2.河南凱瑞車輛檢測認證中心有限公司,河南 焦作 454000;3.黃河交通學院 汽車工程學院,河南 焦作 454000)

0 引言

移動機器人路徑規(guī)劃是實現(xiàn)移動機器人自主導航的重要環(huán)節(jié)[1]。日益復雜的應用場景使得移動機器人路徑規(guī)劃功能遇到了越來越多的挑戰(zhàn)[2-3]。主要包括如何滿足各種靜、動態(tài)復雜場景要求,如何兼顧局部和全局路徑規(guī)劃要求,如何提升路徑規(guī)劃的實時性等[4-5]。這些問題受到重視。

為了解決移動機器人在復雜環(huán)境中路徑規(guī)劃問題,國內(nèi)外學者對此進行了大量研究,主要提出了全局和局部兩大類規(guī)劃算法。全局路徑規(guī)劃算法主要有傳統(tǒng)的Dijsra[6]、A*[7-9]、D*[10]、快速搜索樹(rapid-exploration random tree,RRT)[11,12]、蟻群算法[13]、粒子群算法[14]、遺傳算法[15]等。局部路徑規(guī)劃算法主要有動態(tài)窗口法(dynamic window approach,DWA)[16-18]、人工勢場法[19,20]和定時彈性帶(time elastic band,TEB)算法[21]等。其中,全局路徑規(guī)劃算法中的Dijstra和A*算法較適用于柵格地圖條件下的路徑規(guī)劃。A*算法由于具有啟發(fā)式搜索特點,搜索效率較Dijstra算法有一定優(yōu)勢,但存在搜索路線轉折點較多或路線不夠平順的問題。

針對該問題,研究了傳統(tǒng)A*算法的改進方法[22-24]。鑒于TEB算法在局部路徑規(guī)劃具有時間最優(yōu)的性質,能對局部動態(tài)障礙物有效避障。嘗試融合改進A*和TEB兩種算法。新的算法不僅改善了移動機器人全局規(guī)劃的性能,并且能夠對環(huán)境中的動態(tài)障礙物進行規(guī)避。通過仿真和實物試驗研究了所提融合算法的規(guī)律和優(yōu)勢。

1 移動機器人運動學模型

圖1為研究的四輪差速驅動移動機器人簡化模型[25-26]。在低速狀態(tài)下,該模型假定移動機器人懸架和輪子都是剛性的,輪子做純滾動,忽略Z軸平移。對該移動機器人分別建立全局坐標系(XGOYG)和局部坐標系(XOY)。移動機器人局部坐標系依據(jù)全局坐標系進行確定。附著于移動機器人自身的局部坐標系隨著移動機器人的運動而運動。局部坐標系以移動機器人底盤質心為坐標系原點,以移動機器人前進方向為X軸,沿著X軸逆時針水平旋轉90°建立Y軸。以移動機器人的前進方向(即X軸)和全局坐標系X之間的夾角θ為移動機器人的方向角。該夾角為[-180°, 180°],順時針方向為負,逆時針方向為正。這樣,該移動機器人下一時刻狀態(tài)Xt+1在1個輸入控制量Ut的驅動下,可由當前時刻狀態(tài)Xt推導得出,如式(1)所示。

圖1 移動機器人簡化模型

Xt+1=F(Xt,Ut)+Vt,

(1)

其中：Ut為t時刻系統(tǒng)的輸入;Vt為系統(tǒng)的隨機噪聲和模型本身的不確定性,采用正態(tài)分布,均值為0,方差根據(jù)定位精度確定。

2 改進A*算法

為解決傳統(tǒng)A*算法規(guī)劃的路線轉折點較多的問題,對傳統(tǒng)A*算法進行改進。改進之處在于由原來的8個搜索方向改為符合運動學約束的6個搜索方向,并對路徑進行平滑處理,如圖2所示。

(a) 傳統(tǒng)A*算法搜索策略 (b) 改進A*算法搜索策略圖2 改進A*算法和傳統(tǒng)A*算法搜索策略對比

改進算法流程圖如圖3所示,具體步驟如下：

圖3 改進算法流程圖

(Ⅰ)劃分柵格地圖。

(Ⅱ)維護2個列表(open_list 和close_list)。open_list中存放待搜索的節(jié)點,close_list中存放搜索過的節(jié)點。

(Ⅲ)改進搜索策略。對于部分差速移動機器人來說,移動機器人不能直接進行左右方向的平移運動,故將傳統(tǒng)A*向周圍8個鄰近節(jié)點搜索方式改進成考慮運動學約束的6個搜索方式,即左前、直行、右前、左后、右后和倒退方向。

(Ⅳ)按照改進A*進行啟發(fā)式搜索,其中每個節(jié)點的評價函數(shù)可以表示為

f(n)=g(n)+h(n),

(2)

其中：f(n)為規(guī)劃點n的評價函數(shù);g(n)為定點n到起始位置的代價值;h(n)為定點n到目標位置的啟發(fā)式代價值。對于每一個規(guī)劃點n,改進A*算法都要評價其代價函數(shù)f(n),選取最小的f(n)對應的路徑點作為可行的路徑點。

(Ⅴ)判斷每個節(jié)點的曲率是否發(fā)生突變。若突變,將當前節(jié)點去除;若不突變,則保留。

(Ⅵ)將去除節(jié)點的前后2個節(jié)點進行3次樣條插值。

(Ⅶ)規(guī)劃結束,輸出全局路徑。

與用直線或者圓弧去擬合機器人路徑相比,3次樣條插值法在路徑平滑上具有很大優(yōu)勢。將相鄰2個節(jié)點之間的變化用1個3次多項式來描述,就可解決規(guī)劃路線轉折點較多的問題,使得規(guī)劃的軌跡更加平滑。3次樣條插值方式如圖4所示。

圖4 3次樣條插值

圖5為改進A*算法和傳統(tǒng)A*算法仿真結果對比。由圖5可知：改進A*算法轉折點明顯減少,規(guī)劃的路徑更為平滑。參數(shù)如表1所示。改進A*算法的轉折次數(shù)相對減少了67%,安全距離增加了88%,路徑長度減少了18%,搜索時間縮短了21%。改進A*算法在全局路徑規(guī)劃的效果大大改善,增加了全局規(guī)劃的最優(yōu)性。但是仍然無法對環(huán)境中的動態(tài)障礙物進行避障。為此,在Move_base框架下,結合局部路徑規(guī)劃算法實現(xiàn)動態(tài)避障的功能。

(a) 傳統(tǒng)A*算法

表1 路徑規(guī)劃結果對比

3 局部路徑規(guī)劃

TEB局部路徑規(guī)劃由一系列的位姿組成,用符號Q表示,如式(3)所示。每個點的位姿由Xi=(xi,yi,θi)表達,描述了移動機器人每個位置點的坐標xi,yi,以及朝向角θi。

Q={Xi}i=0,…,n,n∈N,

(3)

其中：xi為位姿點的橫坐標,m;yi為位姿點的縱坐標,m;θi為位姿點的朝向角,rad。

TEB算法將2個位姿點之間設置時間間隔ΔTi,表示從一個構型到另外一個構型所需的時間。并將所有相鄰位姿點時間間隔的集合用τ表示：

τ={ΔTi}i=0,…,n-1,

(4)

其中：ΔTi為相鄰位姿點時間間隔,s。

最終,TEB被定義為2個序列的元組,如式(5)所示。

TEB=(Q,τ)。

(5)

TEB在一些情況下會對移動機器人本身進行限制,具體有如下限制：

(Ⅰ)速度約束fvel。限制移動機器人最大線速度和最大角速度。速度限制以損失的形式實現(xiàn),用fvel表示。速度Vxi、Vyi和角速度根據(jù)向后差分可得。

(6)

(7)

(8)

fvel=f(vxi,vyi,ωi),

(9)

其中：Vxi為位姿點的橫向速度,m/s;Vyi為位姿點的縱向速度,m/s;ωi為位姿點的角速度,rad/s。

(Ⅱ)最快路徑約束fk。

fk=(∑ΔTi)2。

(10)

(Ⅲ)通過點約束fpath和障礙物約束fobs。TEB算法在規(guī)劃的時候既要跟隨全局規(guī)劃路徑點,又要避免碰撞障礙物。全局路徑規(guī)劃點對TEB規(guī)劃點有吸引的作用。約束以懲罰函數(shù)的形式實現(xiàn)。

fpath=f(dmin,rpmax);

(11)

fobs=f(-dmin,romin),

(12)

其中：dmin為TEB局部規(guī)劃點和障礙物之間的最小距離,m;rpmax為TEB局部規(guī)劃點和全局規(guī)劃點的最大距離,m;romin為TEB局部規(guī)劃點與全局路徑點之間的最小距離,m。

TEB約束的求解方式采用圖優(yōu)化求解,求解模型如圖6所示。將每一個狀態(tài)和時間間隔作為頂點,各種約束作為邊建立超圖進行多目標優(yōu)化,使用g2o框架進行求解。

圖6 TEB求解模型

4 融合算法實現(xiàn)

融合算法基于Move_base框架實現(xiàn),如圖7所示。所有節(jié)點利用機器人操作系統(tǒng)(robot operating system,ROS)節(jié)點進行通信。里程計信息、傳感器數(shù)據(jù)和地圖信息分別以Odom msgs、Sensor msgs和Map msgs的話題傳入Move_base導航框架。改進A*全局規(guī)劃器將全局參考路徑以Refer routing msgs話題的形式傳遞給局部路徑規(guī)劃器。全局規(guī)劃和局部規(guī)劃部分均使用插件的方式進行實現(xiàn)。局部路徑規(guī)劃將全局路徑規(guī)劃納入評價函數(shù)來達到跟蹤全局路徑的效果。評價函數(shù)參數(shù)包含軌跡與障礙物之間的距離、軌跡曲率與全局路徑的跟隨程度,具體可以用式(13)表示。

圖7 算法融合框架

(13)

其中：di為第n個節(jié)點與障礙物之間的距離,m;ci為第n個節(jié)點的軌跡曲率,m-1;ui為軌跡與全局路徑的跟隨程度,1代表完全跟隨,0代表完全不跟隨;Ki、Li、Ri分別為3個評價函數(shù)影響因子的權重。從多條路徑中選出一條評價函數(shù)評分最高的路徑作為局部避障軌跡。

Tra=max{tra1,tra2, … ,tran},

(14)

其中：tra1,tra2,…,tran為局部路徑規(guī)劃規(guī)劃出來的多條軌跡對應的評分。Tra作為得最高評分對應的軌跡。

5 仿真驗證

為檢驗本文所提算法的性能,在CPU Inter(R) Core(TM) i5-9300,主頻2.5 GHz,4核8線程計算機上對所提算法進行仿真驗證。改進A*算法規(guī)劃最大轉向角為80°,移動分辨率為1 cm,地圖尺寸為50 cm×50 cm。為了驗證融合算法的有效性,設置多組仿真對比試驗,分別搭建多組仿真環(huán)境。簡單環(huán)境搭建如圖8a所示。靜態(tài)障礙物環(huán)境搭建如圖8c所示。動態(tài)障礙物環(huán)境搭建如圖8e所示。簡單環(huán)境仿真結果如圖8b所示。靜態(tài)障礙物環(huán)境搭建如圖8d所示。動態(tài)障礙物環(huán)境搭建如圖8f所示。

表2為簡單環(huán)境下改進A*算法和傳統(tǒng)A*算法仿真結果對比。由表2可以看出：簡單環(huán)境中,改進A*算法在轉折次數(shù)上減少了3次。同時,由于改進了搜索方向,減少多余節(jié)點的搜索,改進A*算法相比于傳統(tǒng)A*算法降低了近63%的搜索時間。表3為靜態(tài)障礙物環(huán)境下改進A*算法和傳統(tǒng)A*算法仿真結果對比。由表3可以看出：靜態(tài)障礙物環(huán)境中,由于環(huán)境復雜度增加,傳統(tǒng)A*算法和改進A*算法搜索時間、轉折次數(shù)、路徑長度上都有增加,但改進A*算法相較于傳統(tǒng)A*算法在搜索時間上仍有53%的提升,在轉折次數(shù)上減少了4次。由于在安全距離上有所增加,在路徑長度上會犧牲一定的代價。表4為動態(tài)障礙物環(huán)境下3種算法仿真結果對比。由表4可以看出：對于動態(tài)障礙物環(huán)境,單獨的改進A*算法和傳統(tǒng)A*算法均不能有效避開隨機出現(xiàn)的動態(tài)障礙物。由于改進A*算法融合了TEB算法,此融合算法具備了避開障礙物的能力。同時保證了更大的安全距離,在安全距離上相比于傳統(tǒng)A*算法提升了2.7倍,相比于改進A*算法提升了近1倍。由于要避開動態(tài)障礙物,故在轉折次數(shù)和路徑長度上有所犧牲。由圖8d可以看出：傳統(tǒng)A*算法和改進A*算法都能避開靜態(tài)障礙物。由圖8f可以看出：傳統(tǒng)A*算法和改進A*算法均不能避開動態(tài)障礙物。改進A*算法融合TEB算法之后對動態(tài)障礙物能夠有效避讓。動態(tài)避障仿真結果如圖8g和圖8h所示。

(a) 簡單環(huán)境仿真場景 (b) 簡單環(huán)境仿真結果 (c) 靜態(tài)障礙物仿真場景 (d) 靜態(tài)障礙物仿真結果

表2 簡單環(huán)境下改進A*算法和傳統(tǒng)A*算法仿真結果對比

表3 靜態(tài)障礙物環(huán)境下改進A*算法和傳統(tǒng)A*算法仿真結果對比

表4 動態(tài)障礙物環(huán)境下3種算法仿真結果對比

6 實物試驗

為了更一步驗證融合算法在靜態(tài)環(huán)境以及動態(tài)環(huán)境中的避障可行性。借助AUTOLABOR科研底盤進行試驗,該底盤實物即試驗移動機器人,如圖9所示。底盤搭載單線激光雷達、16線激光雷達以及慣性導航系統(tǒng)。工控機CPU為AMD Ryzen3 2 200G,核心頻率3.5 GHz,加速頻率3.7 GHz,4核8線程。工控機安裝Ubuntu18.04系統(tǒng),在此之上配置了Melodic版本ROS。所有傳感器節(jié)點信息由ROS進行連接,節(jié)點構建如圖10所示。

圖9 試驗移動機器人

圖10 ROS節(jié)點圖

試驗過程中,通過在環(huán)境中分別設置靜態(tài)和動態(tài)障礙物,驗證算法的有效性。首先將移動機器人放置在靜態(tài)障礙物的環(huán)境中進行試驗,來驗證融合算法在靜態(tài)障礙物環(huán)境中避障的有效性。然后將移動機器人移回到起點,將實驗室中行走的人物作為動態(tài)障礙物繼續(xù)進行試驗,驗證融合算法在動態(tài)障礙物環(huán)境中避障的有效性。

移動機器人構建環(huán)境地圖后,在規(guī)劃的路線上放置2個靜態(tài)障礙物,如圖11a所示。Rviz上位機顯示規(guī)劃結果如圖11b所示,移動機器人合理規(guī)劃出避開2個障礙物的路線,并按照規(guī)劃路線進行行駛。移動機器人規(guī)劃出的局部路徑不僅對障礙物做出了避讓,并在之后的規(guī)劃以及行駛中跟隨全局路徑。移動機器人實際移動軌跡如圖11c所示。

為了驗證融合算法在動態(tài)環(huán)境中的有效性,將行走的人作為試驗動態(tài)障礙物,如圖12a所示。移動障礙物行駛到如圖12b所示的A點時,移動機器人檢測到移動障礙物,如圖12c中A1點所示。移動機器人不再按照原來在靜態(tài)障礙物環(huán)境中規(guī)劃的路徑進行行駛,而繞過障礙物向右行駛。改進A*算法融合TEB算法規(guī)劃的路徑,如圖12d所示。試驗結果顯示移動機器人能夠有效規(guī)劃出避開動態(tài)障礙物的軌跡,驗證了融合算法對動態(tài)障礙物避障的有效性。路徑規(guī)劃部分參數(shù)如表5所示。

由表5所示的規(guī)劃參數(shù)可以看出：融合算法在靜態(tài)環(huán)境和動態(tài)環(huán)境都能夠有效避障。規(guī)劃的路線不僅轉折次數(shù)較少,而且較為平滑。充分驗證了相比于傳統(tǒng)A*算法,此融合算法在實際環(huán)境中不僅在全局規(guī)劃更優(yōu),而且能對局部動態(tài)障礙物進行有效避障。

(a) 添加靜態(tài)障礙物 (b) 規(guī)劃結果 (c) 靜態(tài)障礙物試驗實際效果

(a) 添加動態(tài)障礙物 (b) 動態(tài)障礙物試驗 (c) 上位機顯示 (d) 規(guī)劃結果

表5 路徑規(guī)劃試驗結果

7 結論

(1)在全局規(guī)劃上考慮了移動機器人的運動約束,并對傳統(tǒng)A*算法進行改進,有效減少了轉折點個數(shù)。使用3次樣條插值可使規(guī)劃的軌跡更加平順。使得改進A*算法相比于傳統(tǒng)A*算法在全局規(guī)劃方面具有更優(yōu)的性能。

(2)利用Move_base導航框架將改進A*算法和TEB算法進行充分融合,結合兩者的優(yōu)勢,既保證了全局規(guī)劃的最優(yōu)性,又能保證局部動態(tài)避障的有效性。

(3)使用融合算法的移動機器人在靜態(tài)以及動態(tài)環(huán)境中都能按照預期完成規(guī)劃任務。此算法是建立在環(huán)境部分已知的情況下進行的,下一步將研究移動機器人在完全未知的環(huán)境中自主導航的算法,提升移動機器人的路徑規(guī)劃水平。