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

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

0 引言

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

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

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

1 移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型

圖1為研究的四輪差速驅(qū)動(dòng)移動(dòng)機(jī)器人簡(jiǎn)化模型[25-26]。在低速狀態(tài)下,該模型假定移動(dòng)機(jī)器人懸架和輪子都是剛性的,輪子做純滾動(dòng),忽略Z軸平移。對(duì)該移動(dòng)機(jī)器人分別建立全局坐標(biāo)系(XGOYG)和局部坐標(biāo)系(XOY)。移動(dòng)機(jī)器人局部坐標(biāo)系依據(jù)全局坐標(biāo)系進(jìn)行確定。附著于移動(dòng)機(jī)器人自身的局部坐標(biāo)系隨著移動(dòng)機(jī)器人的運(yùn)動(dòng)而運(yùn)動(dòng)。局部坐標(biāo)系以移動(dòng)機(jī)器人底盤質(zhì)心為坐標(biāo)系原點(diǎn),以移動(dòng)機(jī)器人前進(jìn)方向?yàn)閄軸,沿著X軸逆時(shí)針?biāo)叫D(zhuǎn)90°建立Y軸。以移動(dòng)機(jī)器人的前進(jìn)方向(即X軸)和全局坐標(biāo)系X之間的夾角θ為移動(dòng)機(jī)器人的方向角。該夾角為[-180°, 180°],順時(shí)針?lè)较驗(yàn)樨?fù),逆時(shí)針?lè)较驗(yàn)檎?。這樣,該移動(dòng)機(jī)器人下一時(shí)刻狀態(tài)Xt+1在1個(gè)輸入控制量Ut的驅(qū)動(dòng)下,可由當(dāng)前時(shí)刻狀態(tài)Xt推導(dǎo)得出,如式(1)所示。

圖1 移動(dòng)機(jī)器人簡(jiǎn)化模型

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

(1)

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

2 改進(jìn)A*算法

為解決傳統(tǒng)A*算法規(guī)劃的路線轉(zhuǎn)折點(diǎn)較多的問(wèn)題,對(duì)傳統(tǒng)A*算法進(jìn)行改進(jìn)。改進(jìn)之處在于由原來(lái)的8個(gè)搜索方向改為符合運(yùn)動(dòng)學(xué)約束的6個(gè)搜索方向,并對(duì)路徑進(jìn)行平滑處理,如圖2所示。

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

改進(jìn)算法流程圖如圖3所示,具體步驟如下：

圖3 改進(jìn)算法流程圖

(Ⅰ)劃分柵格地圖。

(Ⅱ)維護(hù)2個(gè)列表(open_list 和close_list)。open_list中存放待搜索的節(jié)點(diǎn),close_list中存放搜索過(guò)的節(jié)點(diǎn)。

(Ⅲ)改進(jìn)搜索策略。對(duì)于部分差速移動(dòng)機(jī)器人來(lái)說(shuō),移動(dòng)機(jī)器人不能直接進(jìn)行左右方向的平移運(yùn)動(dòng),故將傳統(tǒng)A*向周圍8個(gè)鄰近節(jié)點(diǎn)搜索方式改進(jìn)成考慮運(yùn)動(dòng)學(xué)約束的6個(gè)搜索方式,即左前、直行、右前、左后、右后和倒退方向。

(Ⅳ)按照改進(jìn)A*進(jìn)行啟發(fā)式搜索,其中每個(gè)節(jié)點(diǎn)的評(píng)價(jià)函數(shù)可以表示為

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

(2)

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

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

(Ⅵ)將去除節(jié)點(diǎn)的前后2個(gè)節(jié)點(diǎn)進(jìn)行3次樣條插值。

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

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

圖4 3次樣條插值

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

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

表1 路徑規(guī)劃結(jié)果對(duì)比

3 局部路徑規(guī)劃

TEB局部路徑規(guī)劃由一系列的位姿組成,用符號(hào)Q表示,如式(3)所示。每個(gè)點(diǎn)的位姿由Xi=(xi,yi,θi)表達(dá),描述了移動(dòng)機(jī)器人每個(gè)位置點(diǎn)的坐標(biāo)xi,yi,以及朝向角θi。

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

(3)

其中：xi為位姿點(diǎn)的橫坐標(biāo),m;yi為位姿點(diǎn)的縱坐標(biāo),m;θi為位姿點(diǎn)的朝向角,rad。

TEB算法將2個(gè)位姿點(diǎn)之間設(shè)置時(shí)間間隔ΔTi,表示從一個(gè)構(gòu)型到另外一個(gè)構(gòu)型所需的時(shí)間。并將所有相鄰位姿點(diǎn)時(shí)間間隔的集合用τ表示：

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

(4)

其中：ΔTi為相鄰位姿點(diǎn)時(shí)間間隔,s。

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

TEB=(Q,τ)。

(5)

TEB在一些情況下會(huì)對(duì)移動(dòng)機(jī)器人本身進(jìn)行限制,具體有如下限制：

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

(6)

(7)

(8)

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

(9)

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

(Ⅱ)最快路徑約束fk。

fk=(∑ΔTi)2。

(10)

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

fpath=f(dmin,rpmax);

(11)

fobs=f(-dmin,romin),

(12)

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

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

圖6 TEB求解模型

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

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

圖7 算法融合框架

(13)

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

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

(14)

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

5 仿真驗(yàn)證

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

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

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

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

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

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

6 實(shí)物試驗(yàn)

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

圖9 試驗(yàn)移動(dòng)機(jī)器人

圖10 ROS節(jié)點(diǎn)圖

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

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

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

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

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

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

表5 路徑規(guī)劃試驗(yàn)結(jié)果

7 結(jié)論

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

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

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