郭 建,楊 朋,曾志豪,黃穎駒,王家淳

(廣州城市理工學(xué)院機(jī)械工程學(xué)院,廣州 510800)

0 引言

移動(dòng)機(jī)器人(AGV)路徑規(guī)劃可以幫助機(jī)器人更好的服務(wù)于智能制造、智慧工廠等高自動(dòng)化場(chǎng)景下的物流運(yùn)輸,具有廣泛的應(yīng)用前景[1]。

路徑規(guī)劃可分為基于先驗(yàn)信息的全局路徑規(guī)劃和基于傳感器信息的局部路徑規(guī)劃[2],其中全局路徑規(guī)劃有:RRT算法[3]、A*算法[4]、遺傳算法(GA)[5]、Dijkstra算法[6]。局部路徑規(guī)劃則包含:人工勢(shì)場(chǎng)法(APF)[7]、D*算法[8]、DWA算法[9]、蟻群算法和強(qiáng)化學(xué)習(xí)法等。

Dijkstra算法在搜索最優(yōu)路徑時(shí),需要對(duì)地圖環(huán)境進(jìn)行幾何劃分,從起點(diǎn)開始向周圍所有節(jié)點(diǎn)搜索,直至搜索到目標(biāo)點(diǎn)為止,所尋路徑一定是最優(yōu)的,但在搜索路徑時(shí)需要擴(kuò)展許多無用的節(jié)點(diǎn)。且路徑軌跡存在折線,不夠平滑。馬新國(guó)等[9]提出一種融合改RRT和Dijkstra算法,利用改進(jìn)的RRT算法獲得可行路徑并進(jìn)行柵格化,在柵格化后的可行路徑上采用Dijkstra算法搜索最短路徑。提高機(jī)器人動(dòng)態(tài)路徑規(guī)劃效率。但忽視機(jī)器人對(duì)軌跡光滑程度的要求。鞏慧等[10]采用基于幾何拓?fù)鋵W(xué)方法對(duì)Dijkstra算法規(guī)劃的路徑進(jìn)行平滑處理,有效減少了機(jī)器人轉(zhuǎn)彎次數(shù),提高軌跡平滑度,但僅在靜態(tài)障礙物的條件下進(jìn)行實(shí)驗(yàn)驗(yàn)證。局部路徑規(guī)劃中傳統(tǒng)DWA算法前瞻性不足[11-12],遇到“C”型障礙物時(shí)無法避障,也非最優(yōu)路徑。卞永明等[13]提出一種改進(jìn)DWA算法,增加計(jì)算仿真軌跡與關(guān)鍵航跡點(diǎn)的最小距離的評(píng)價(jià)函數(shù),實(shí)現(xiàn)了安全性提升和路徑優(yōu)化。王彬等[14]在DWA算法中改進(jìn)障礙物距離評(píng)價(jià)子函數(shù),提出了動(dòng)態(tài)窗口法的自適應(yīng)環(huán)境改進(jìn)策略,使路徑更加平滑,但舍棄了可用速度的選擇,導(dǎo)致效率降低。

因此本文針對(duì)Dijkstra算法路徑轉(zhuǎn)折多,全局規(guī)劃效率低,DWA算法無法進(jìn)行全局規(guī)劃問題,本文提出一種融合改進(jìn)的Dijkstra算法和改進(jìn)的DWA算法,解決動(dòng)、靜態(tài)障礙物的場(chǎng)景下移動(dòng)機(jī)器人的路徑規(guī)劃問題。仿真實(shí)驗(yàn)以及移動(dòng)機(jī)器人樣機(jī)實(shí)驗(yàn)結(jié)果表明,本文研究的融合算法能夠提高移動(dòng)機(jī)器人全局規(guī)劃效率以及避障能力。

1 移動(dòng)機(jī)器人設(shè)計(jì)與建模

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

移動(dòng)機(jī)器人底盤采用差分驅(qū)動(dòng)結(jié)構(gòu),由2個(gè)主動(dòng)輪和4個(gè)從動(dòng)輪組成,如圖1所示。

圖1 差分驅(qū)動(dòng)移動(dòng)機(jī)器人

圖1中,OX1Y1為全局坐標(biāo)系,PXRYR為移動(dòng)機(jī)器人坐標(biāo)系,θ為機(jī)器人相對(duì)于全局坐標(biāo)系偏轉(zhuǎn)角。設(shè)時(shí)間間隔Δt,移動(dòng)機(jī)器人做勻速直線運(yùn)動(dòng),t+1時(shí)刻,由機(jī)器人的線速度和角速度v、ω,可得:

(1)

式中:xt、yt、θt為t時(shí)刻下的位姿,θt為運(yùn)動(dòng)方向和水平方向的夾角。

1.2 移動(dòng)機(jī)器人硬件

基于4 GB的64位Ununtu16.04操作系統(tǒng),通過局域網(wǎng)實(shí)現(xiàn)上位機(jī)系統(tǒng)和機(jī)器人樣機(jī)的通訊,實(shí)現(xiàn)遠(yuǎn)程操控移動(dòng)機(jī)器人進(jìn)行自主避障和導(dǎo)航功能。移動(dòng)機(jī)器人樣機(jī)如圖2所示。

圖2 移動(dòng)機(jī)器人樣機(jī)

采用興頌LE-50621-V1激光雷達(dá)掃描和獲取環(huán)境信息。使用步科FD124S-CA-000行走輪提供動(dòng)力,外殼為3D打印材料ABS,移動(dòng)機(jī)器人參數(shù)如表1所示。

表1 移動(dòng)機(jī)器人參數(shù)

移動(dòng)機(jī)器人運(yùn)動(dòng)參數(shù)如表2所示。

表2 機(jī)器人自身參數(shù)

2 Dijkstra算法改進(jìn)及仿真

2.1 Dijkstra算法

Dijkstra算法屬于廣度優(yōu)先搜索算法解決賦權(quán)有向圖或者無向圖的單源最短路徑問題,算法最終可以得到一個(gè)最短路徑樹,如圖3所示。

圖3 Dijkstra算法路徑導(dǎo)航

圖3中綠色為起點(diǎn),黃色為終點(diǎn),黑色物塊表示障礙物,淺灰色表示搜索路徑時(shí)經(jīng)過的節(jié)點(diǎn),深灰色區(qū)域表示搜索到最優(yōu)的軌跡。

2.2 改進(jìn)的Dijkstra算法

傳統(tǒng)Dijkstra算法以指定頻率運(yùn)行,生成從當(dāng)前位置到目標(biāo)位置的一條安全路線。雖能完成避障,但在機(jī)器人繞過障礙物后,以當(dāng)前位置重新規(guī)劃到目標(biāo)點(diǎn)的路線,這種方式忽略了初始路徑的全局最優(yōu)的特點(diǎn)。

改進(jìn)路徑規(guī)劃始終保留以初始位置為起點(diǎn)的全局路徑規(guī)劃,并以局部?jī)?yōu)化的方式代替重新生成全局路線的原方案。步驟為:

步驟1:根據(jù)靜態(tài)全局代價(jià)地圖——不包含實(shí)時(shí)障礙物信息,生成初始路線;

步驟2:以指定頻率,根據(jù)動(dòng)態(tài)的局部代價(jià)地圖優(yōu)化初始路線。當(dāng)存在與障礙物碰撞的子路徑段,使用Dijkstra算法重新計(jì)算的安全路徑替換,得到優(yōu)化后路線,即只對(duì)部分路徑進(jìn)行重新規(guī)劃,其余采用初始路徑;

步驟3:對(duì)于局部代價(jià)地圖內(nèi)存在多個(gè)障礙物的情況,將多障礙物視為同一障礙物,一并繞開,以縮短小車?yán)@障用時(shí)、提高效率。改進(jìn)前后的路徑規(guī)劃效果對(duì)比如圖4所示。

圖4 傳統(tǒng)路徑規(guī)劃與改進(jìn)路徑規(guī)劃效果示意圖

2.3 路徑平滑

由于障礙物和局部軌跡優(yōu)化,全局路徑存在轉(zhuǎn)折、不平滑的情況,不利于移動(dòng)機(jī)器人平穩(wěn)運(yùn)動(dòng),故需對(duì)路徑進(jìn)行平滑優(yōu)化處理。

路徑優(yōu)化可采用兩種優(yōu)化方案,一種方案為直接刪除兩個(gè)路徑點(diǎn)間無障礙物碰撞的路徑點(diǎn);但剔除點(diǎn)過多時(shí),將導(dǎo)致小車對(duì)既定軌跡的貼合性降低。第二種方案為采用平滑過渡法對(duì)路徑點(diǎn)進(jìn)行替換。本文采用第二種方法。對(duì)于某條在兩個(gè)路徑點(diǎn)之間無障礙物碰撞的子路徑,起始點(diǎn)為N0,終點(diǎn)為Nsn,過渡點(diǎn)為Nsi(0

(2)

對(duì)上述傳統(tǒng)規(guī)劃所得全局路徑進(jìn)行路徑拉直優(yōu)化處理后,得到平滑路徑如圖5所示。

圖5 路徑優(yōu)化效果圖

2.4 算法仿真驗(yàn)證

全局路徑規(guī)劃采用相同的起點(diǎn)與終點(diǎn),記錄下移動(dòng)機(jī)器人在算法改進(jìn)前后下,行進(jìn)中的實(shí)際姿態(tài)角變化情況,如圖6所示,可以對(duì)比出算法規(guī)劃的安全路徑在優(yōu)化前后的平滑性。

圖6 機(jī)器人實(shí)際姿態(tài)角變化圖

從圖6可以發(fā)現(xiàn),在執(zhí)行路徑優(yōu)化后,得到的全局路徑中的姿態(tài)角震蕩次數(shù)由29次減少至7次,有顯著地降低。優(yōu)化前姿態(tài)角的標(biāo)準(zhǔn)偏差為4.33°。優(yōu)化后姿態(tài)角的標(biāo)準(zhǔn)偏差為0.26°。

通過對(duì)比優(yōu)化前后的數(shù)據(jù),可以發(fā)現(xiàn)機(jī)器人姿態(tài)角震蕩次數(shù)顯著降低(7<29),以及小車前進(jìn)過程中的姿態(tài)角標(biāo)準(zhǔn)差(0.26°<4.33°)?？勺C明通過算法優(yōu)化后,機(jī)器人左右擺動(dòng)的情況得到明顯改善,且在機(jī)器人實(shí)際行進(jìn)過程中,其機(jī)身姿態(tài)更為平穩(wěn),工作路徑得到有效縮短。

多組數(shù)據(jù)對(duì)比:再次隨機(jī)選取10組坐標(biāo),如表3所示,通過多次隨機(jī)實(shí)驗(yàn),避免數(shù)據(jù)的偶然性。分別采用傳統(tǒng)算法與優(yōu)化算法進(jìn)行路徑規(guī)劃,對(duì)比其優(yōu)化前后的路程長(zhǎng)度及偏航角震蕩次數(shù)如表4所示。

表3 隨機(jī)選取坐標(biāo)組

表4 路程及震蕩次數(shù)對(duì)比

通過對(duì)比算法優(yōu)化前后的多組數(shù)據(jù),計(jì)算通過路徑平滑優(yōu)化后,機(jī)器人的平均路程縮短比例為0.65%,平均偏航角震蕩次數(shù)減少了67.70%,證明路徑規(guī)劃算法改進(jìn)后,路徑長(zhǎng)度縮短,且更為平滑。

3 動(dòng)態(tài)窗口法改進(jìn)及融合仿真

3.1 動(dòng)態(tài)窗口法

動(dòng)態(tài)窗口算法(DWA)是一種常用的局部避障規(guī)劃算法。移動(dòng)機(jī)器人在擁有完整環(huán)境信息的全局地圖下進(jìn)行導(dǎo)航時(shí),面對(duì)突然出現(xiàn)的障礙物,全局路徑規(guī)劃無法處理該問題,因此需要采用實(shí)時(shí)局部避障,DWA算法避開在速度搜索空間中可能與障礙物發(fā)生碰撞的路徑,對(duì)預(yù)測(cè)時(shí)間內(nèi)的移動(dòng)機(jī)器人速度空間采樣后,預(yù)測(cè)軌跡,并利用DWA算法中的評(píng)價(jià)函數(shù)選出最優(yōu)軌跡。其評(píng)價(jià)函數(shù)為:

E(v,ω)={σ(αhead(v,ω)+βdist(v,ω)+γvel(v,ω))}

(3)

式中:head(v,ω)為當(dāng)前速度下,評(píng)估目標(biāo)方向與模擬的軌跡末端偏差;σ為平滑函數(shù),α、β、γ為加權(quán)系數(shù),vel(v,ω)為機(jī)器人當(dāng)前運(yùn)動(dòng)速度大小的評(píng)價(jià)。

3.2 改進(jìn)DWA算法

傳統(tǒng)DWA算法常常因?yàn)樗俣扰c偏差角變化的固定參數(shù)使得路徑安全性降低,其速度評(píng)價(jià)函數(shù)的權(quán)重總是小于其他評(píng)價(jià)子函數(shù)的權(quán)重,即算法中速度無法保證快速平穩(wěn)到達(dá)終點(diǎn),就會(huì)導(dǎo)致該軌跡不是最優(yōu)解。除此之外,如果目標(biāo)點(diǎn)突然出現(xiàn)障礙物,在機(jī)器人即將抵達(dá)目標(biāo)點(diǎn)時(shí),會(huì)出現(xiàn)運(yùn)動(dòng)十分緩慢甚至卡頓現(xiàn)象的情況。因此需要?jiǎng)討B(tài)改變權(quán)重來解決此類問題。本文依靠障礙物的遠(yuǎn)近動(dòng)態(tài)改變速度評(píng)價(jià)權(quán)重大小,離障礙物越遠(yuǎn),越平緩接近終點(diǎn),離障礙物越近,能夠快速避開障礙物。改進(jìn)評(píng)價(jià)函數(shù)如式(4)所示。

E(v,ω)={σ(αhead(v,ω)+βdist(v,ω)+εDvel(v,ω))}

(4)

式中:

(5)

根據(jù)障礙物遠(yuǎn)近動(dòng)態(tài)調(diào)節(jié)vel(v,ω)的權(quán)重值εD,在仿真狀態(tài)下,設(shè)定障礙物半徑R=0.5 m,根據(jù)障礙物距離設(shè)定a=0.4,b=0.1,則εD范圍為(0.1～0.4)。

基于MATLAB對(duì)比相同地圖環(huán)境下改進(jìn)前后的DWA算法仿真,起點(diǎn)為(0,0),如圖7所示。

(a) 傳統(tǒng)DWA算法 (b) 改進(jìn)后DWA算法

結(jié)合圖7和表5可得,針對(duì)局部路徑規(guī)劃,相對(duì)于傳統(tǒng)DWA算法,改進(jìn)的DWA算法運(yùn)行路程縮短26.23%,路徑轉(zhuǎn)折次數(shù)降低了67%,運(yùn)行時(shí)間減少22.36%。

表5 算法仿真對(duì)比

結(jié)合圖8和表6可得,相較于傳統(tǒng)的DWA算法,35×35 m柵格地圖環(huán)境內(nèi),改進(jìn)后的DWA算法運(yùn)行路程縮小9.68%,路徑轉(zhuǎn)折次數(shù)降低了33%,運(yùn)行時(shí)間縮短3.88%。改進(jìn)的DWA算法節(jié)約用時(shí),提高了避障效率,并且使得軌跡轉(zhuǎn)折次數(shù)降低,機(jī)器人運(yùn)動(dòng)更加穩(wěn)定。

表6 算法仿真對(duì)比

(a) 傳統(tǒng)DWA算法 (b) 改進(jìn)后DWA算法

3.3 融合優(yōu)化算法

在全局規(guī)劃方面,利用改進(jìn)的Dijkstra算法調(diào)整評(píng)價(jià)函數(shù),減少冗余節(jié)點(diǎn)、縮短時(shí)間,提高搜索效率。在局部規(guī)劃方面,利用改進(jìn)的DWA算法,依據(jù)移動(dòng)機(jī)器人距離障礙物的遠(yuǎn)近繞開障礙物,融合算法路徑規(guī)劃流程,如圖9所示。

圖9 融合算法路徑規(guī)劃流程圖

分別記錄融合算法對(duì)移動(dòng)機(jī)器人面對(duì)靜態(tài)障礙物以及動(dòng)態(tài)障礙物時(shí)軌跡規(guī)劃。其中,靜態(tài)障礙物場(chǎng)景下,移動(dòng)機(jī)器人軌跡如圖10所示。

圖10 融合算法靜態(tài)避障路徑仿真

圖10中,黑色曲線為實(shí)時(shí)的路徑規(guī)劃,黑色直線為起點(diǎn)到終點(diǎn)的直線距離?；疑€為障礙物。在初始全局路徑規(guī)劃下,隨著障礙物的增多,路線也隨之變化。動(dòng)態(tài)障礙物場(chǎng)景下,移動(dòng)機(jī)器人路徑規(guī)劃如圖11所示。

(a) 避障初始路線 (b) 避障行駛路線

圖11中序號(hào)①～③均為靜態(tài)障礙物,機(jī)器人模型從起點(diǎn)出發(fā),駛向終點(diǎn),靠近動(dòng)態(tài)障礙物時(shí),重新規(guī)劃路徑,繞過動(dòng)態(tài)障礙物。實(shí)驗(yàn)仿真表明,無論是靜態(tài)還是動(dòng)態(tài)障礙物,機(jī)器人依靠融合算法可以平滑且安全抵達(dá)目的地。

4 樣機(jī)實(shí)驗(yàn)及數(shù)據(jù)分析

采用移動(dòng)機(jī)器人樣機(jī)在真實(shí)環(huán)境下,驗(yàn)證融合算法的有效性。如圖12所示。

圖12 移動(dòng)機(jī)器人樣機(jī)實(shí)驗(yàn)

圖12中①～⑦為靜態(tài)障礙物,移動(dòng)機(jī)器人從起點(diǎn)出發(fā)首先規(guī)劃全局路徑,當(dāng)受到全局路徑存在障礙物時(shí),及時(shí)規(guī)劃局部避障路線,依次繞過障礙物抵達(dá)終點(diǎn)。面對(duì)真實(shí)的環(huán)境,能夠快速規(guī)劃出有效的避障路徑,驗(yàn)證了融合算法的有效性。

記錄該過程中,移動(dòng)機(jī)器人的線速度、角速度以及方位角,如圖13～圖15所示。

圖13 移動(dòng)機(jī)器人線速度

圖15 移動(dòng)機(jī)器人方位角

移動(dòng)機(jī)器人線速度速度平緩上升至遇到障礙物,線速度由0.45 m/s平緩降低并不斷調(diào)整。角速度的變化波動(dòng)明顯,證明融合算法實(shí)時(shí)調(diào)整避障路線,保證移動(dòng)機(jī)器人路徑安全。移動(dòng)機(jī)器人方位角變化平緩,不存在突變,證明融合算法規(guī)劃的路線變化平穩(wěn),符合移動(dòng)機(jī)器人應(yīng)用要求。

測(cè)得移動(dòng)機(jī)器人在起始點(diǎn)A和終點(diǎn)B的重復(fù)定位精度,重復(fù)16次實(shí)驗(yàn),以第一次機(jī)器人坐標(biāo)為參考,計(jì)算在A、B點(diǎn)機(jī)器人重復(fù)定位精度,如圖16和圖17所示。

圖16 A點(diǎn)重復(fù)定位精度

從圖16和圖17可知,移動(dòng)機(jī)器人在A、B點(diǎn)的X方向平均誤差≤30 mm,在A、B點(diǎn)的Y方向平均誤差≤30 mm。證明該融合算法規(guī)劃的移動(dòng)機(jī)器人運(yùn)動(dòng)具有良好定位精度。

綜上所述,改進(jìn)Dijkstra算法和改進(jìn)DWA算法的融合很好地規(guī)劃移動(dòng)機(jī)器人全局路徑與局部路徑,具有實(shí)時(shí)避障能力,保證機(jī)器人移動(dòng)過程中的效率和精確度。

5 結(jié)論

本文提出一種基于傳統(tǒng)Dijkstra算法和DWA算法進(jìn)行改進(jìn)和融合的導(dǎo)航算法。滿足移動(dòng)機(jī)器人運(yùn)全局路徑規(guī)劃和動(dòng)態(tài)避障。

(1)全局路徑規(guī)劃基于Dijkstra算法實(shí)現(xiàn)了平滑優(yōu)化的路徑規(guī)劃以及動(dòng)態(tài)避障功能。在實(shí)驗(yàn)中,經(jīng)過優(yōu)化后路徑中采用的姿態(tài)角的標(biāo)準(zhǔn)偏差從4.33°降到0.26°,震蕩次數(shù)從29次降到7次;多次實(shí)驗(yàn),路程平均縮短0.65%,震蕩次數(shù)平均減少67.70%,故路徑得到了明顯的平滑優(yōu)化改進(jìn)。

(2)通過對(duì)DWA算法中評(píng)價(jià)函數(shù)調(diào)整動(dòng)態(tài)加權(quán)系數(shù),保證移動(dòng)機(jī)器人在局部路徑規(guī)劃中更快避開障礙物并且到達(dá)最終目標(biāo)點(diǎn)。仿真實(shí)驗(yàn)結(jié)果表明,減少避障路程和時(shí)間,提高避障規(guī)劃效率。

(3)Dijkstra算法和DWA算法的融合保證在全局規(guī)劃路徑中尋找最優(yōu)局部路徑,實(shí)驗(yàn)數(shù)據(jù)表明,移動(dòng)機(jī)器人能夠快速進(jìn)行路徑規(guī)劃,具有實(shí)時(shí)避障的功能。機(jī)器人運(yùn)動(dòng)軌跡光滑,過程平穩(wěn)且較少抖動(dòng)。且移動(dòng)機(jī)器人重復(fù)定位精度≤30 mm,滿足實(shí)際應(yīng)用需求。