基于雙重A*算法的移動機器人動態(tài)環(huán)境路徑規(guī)劃

陳偉華，林 穎，文宗明，繆丹云

(華南理工大學(xué)廣州學(xué)院 機械工程學(xué)院，廣州 510800)

0 引言

移動機器人路徑規(guī)劃是導(dǎo)航中的關(guān)鍵技術(shù)之一，是機器人技術(shù)領(lǐng)域研究的重點和熱點。移動機器人路徑規(guī)劃是指在有障礙物的環(huán)境中為機器人規(guī)劃出一條從起始點到目標(biāo)點的最優(yōu)或者次優(yōu)安全無碰撞路徑[1-6]。路徑規(guī)劃問題通常存在環(huán)境信息量大、障礙物多等約束，其已被證明是非確定性多項式困難問題[7]。路徑規(guī)劃問題一直以來都是移動機器人研究的熱點和難點[8-9]。

A*算法在機器人全局路徑尋優(yōu)和規(guī)劃方面應(yīng)用非常廣泛。A*算法是一種經(jīng)典的啟發(fā)式搜索算法，并廣泛應(yīng)用于路徑搜索問題中。A*算法是一類適用于全局環(huán)境信息已知的路徑規(guī)劃方法，同時也適用于路徑的二次規(guī)劃[6]。但是在動態(tài)環(huán)境中，移動機器人運動過程會遇到障礙物，則其運動路徑需要重新規(guī)劃。重新規(guī)劃的算法很多，比如遺傳算法、蟻群算法、增強學(xué)習(xí)算法等，每個算法都有其優(yōu)缺點，其中實時性是算法的缺點之一。為解決環(huán)境信息局部變動情況下路徑規(guī)劃問題，并且減少路徑規(guī)劃時間，提高移動機器人運動的實時性，提出了基于雙重A*算法的路徑規(guī)劃方法，即采用A*算法分別進行全局路徑規(guī)劃和局部路徑規(guī)劃。首先在移動機器人運動前，在全局環(huán)境信息已知的條件下進行全局最優(yōu)路徑規(guī)劃；接著，機器人沿著全局規(guī)劃的路徑運動，當(dāng)在前進的路徑上出現(xiàn)障礙物，則再一次采用A*算法進行局部路徑規(guī)劃，局部區(qū)域比全局區(qū)域小很多，能減小路徑規(guī)劃時間，提高機器人運動的實時性，使移動機器人的運動路徑全局最優(yōu)和局部最優(yōu)兼并，并使機器人能在動態(tài)環(huán)境中安全無碰撞運動。

1 基于雙重A*算法的路徑規(guī)劃

1.1 A*算法原理

A*算法在人工智能中是一種典型的啟發(fā)式搜索算法(Heuristic Search Algorithm)，是一種靜態(tài)路徑規(guī)劃求解最短路徑最有效的方法，其基本公式如下[10]：

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

(1)

其中，f(n)為節(jié)點n的估價函數(shù)，g(n)為在狀態(tài)空間中從初始節(jié)點到n節(jié)點的實際代價，h(n)為從n節(jié)點到目標(biāo)節(jié)點最優(yōu)路徑的估計代價。

圖1 A*算法進行全局路徑規(guī)劃的路徑

估價函數(shù)的正確選取將直接關(guān)系到A*算法的成功與否，而函數(shù)的確定卻與實際情形有著密切的關(guān)系。所以選擇的估價函數(shù)好壞是很重要的，一個不恰當(dāng)?shù)膯l(fā)式函數(shù)反而會減慢A*算法，導(dǎo)致其產(chǎn)生出不好的路徑[10]。本文取兩節(jié)點間的曼哈頓距離作為估價值，即h(n)=abs(dx-sx)+abs(dy-sy)，(sx，sy)為當(dāng)前節(jié)點坐標(biāo)，(dx，dy)為目標(biāo)節(jié)點的坐標(biāo)。g(n)表示沿路徑從初始節(jié)點到當(dāng)前節(jié)點n的移動消耗成本，令水平和垂直移動的消耗成本為1，對角線方向的消耗成本為1.4。見圖1，已知全局環(huán)境的柵格地圖(27行×26列格子)，機器人或障礙物在柵格地圖中的位置用它們所占格子在柵格地圖中的行號和列號表示，即(行號，列號)表示位置坐標(biāo)(x，y)，則圖片的左上角的第一個格子位置為(1，1)，右下角的最后一個格子的位置為(27，26)。設(shè)機器人的初始點位置為(4，22)，目標(biāo)點位置為(18，2)，如圖1所示，采用A*算法進行全局路徑規(guī)劃，得到如圖1中所示的路徑。移動機器人沿著路徑從初始點運動到目標(biāo)點消耗的成本為26.8。

1．2 基于雙重A*算法的路徑規(guī)劃原理

圖2 基于雙重A*算法的路徑規(guī)劃流程圖

雙重A*算法指的是全局A*算法和局部A*算法相結(jié)合。移動機器人路徑規(guī)劃中，當(dāng)已知運動環(huán)境的柵格地圖、移動機器人運動的起點和目標(biāo)點位置，則可采用A*算法進行全局最優(yōu)的路徑規(guī)劃(見圖1)。但在運動過程中，移動機器人會碰到一些靜止或動態(tài)的障礙物，這時需要進行局部路徑規(guī)劃，局部路徑規(guī)劃中再一次采用A*算法。基于雙重A*算法的路徑規(guī)劃流程圖如圖2所示。

基于雙重A*算法的路徑規(guī)劃步驟如下：

(1)已知運動環(huán)境的柵格地圖，移動機器人運動起點位置，需要到達的目標(biāo)點位置，運動前先采用A*算法規(guī)劃出全局的運動路徑(見圖1)。

(2)移動機器人按第(1)步規(guī)劃出的全局運動路徑開始運動，當(dāng)在運動路徑中出現(xiàn)障礙物時，移動機器人則進行局部規(guī)劃：當(dāng)障礙物為靜止時則轉(zhuǎn)到第(3)步，當(dāng)障礙物為動態(tài)時則轉(zhuǎn)到第(4)步。

(3)當(dāng)移動機器人前面運動路徑上出現(xiàn)靜止障礙物時，則移動機器人在局部范圍內(nèi)采用A*算法進行路徑規(guī)劃。如圖3所示，靜止障礙物在地圖上占有四個格子，位置為(9，16)、(9，17)、(10，16)和(10，17)，局部規(guī)劃區(qū)域為圖中方框的范圍，局部路徑規(guī)劃的局部起點為移動機器人當(dāng)前點其位置為(8，18)，局部目標(biāo)點為越過障礙物沿著靠近全局目標(biāo)點方向的全局運動路徑上的一點其位置為(11，15)，在局部區(qū)域采用A*算法規(guī)劃出來的局部路徑為(8，18)-(8，17)-(8，16)-(8，15)-(9，15)-(10，15)-(11，15)。全局路徑與局部路徑混合則可以得到機器人運動的路徑。

圖3 障礙物為靜止物體的規(guī)劃路徑

(4)當(dāng)移動機器人前面運動路徑上出現(xiàn)動態(tài)障礙物時，根據(jù)動態(tài)障礙物運動方向進行不同的路徑規(guī)劃：①當(dāng)動態(tài)障礙物運動方向與移動機器人運動方向相同，則機器人慢速與障礙物保持一定距離進行跟隨運動。②當(dāng)動態(tài)障礙物運動方向相對移動機器人運動方向是側(cè)向穿越，則機器人減速運動到離障礙物一定安全距離的地方停止，等待障礙物穿越完成機器人再繼續(xù)向前運動。③當(dāng)動態(tài)障礙物(如圖4(1)中(19，12)，圖4(2)中(17，12)，圖4(3)中(16，12)，圖4(4)中(14，12)，圖4(5)中(12，12)，圖4(6)中(3，12)的位置)運動方向與移動機器人運動方向相反，則移動機器人實時進行局部路徑規(guī)劃，直至障礙物消失在移動機器人前進的局部路徑范圍內(nèi)或者障礙物不在機器人前進運動路徑上，如圖4運動順序圖所示。圖4中位置(15，12)為局部路徑規(guī)劃的局部起點，位置(17，11)為局部目標(biāo)點，局部路徑為(15，12)-(15，13)-(16，13)-(17，13)-(17，12)-(17，11)。

圖4 障礙物為運動物體的規(guī)劃路徑

1.3 局部規(guī)劃區(qū)域的確定

當(dāng)移動機器人運動過程中遇到障礙物時需要進行局部路徑規(guī)劃，局部規(guī)劃區(qū)域的確定如下：當(dāng)移動機器人運動的下一步有障礙物時，記下移動機器人當(dāng)前點的位置，檢測障礙物占有前進運動路徑的格子數(shù)n(如圖3中n=2)，設(shè)s=n+6，如果s為偶數(shù)，則s=n+7。①當(dāng)下一步的障礙物所占格子(如圖5中黑色格子)在機器人當(dāng)前點(如圖5a中第8行第4列格子)所占格子的正上方，則局部規(guī)劃區(qū)域格子數(shù)為(s+1)*s，如圖5a的第2行到第9行的方框所示。②當(dāng)下一步的障礙物所占格子在機器人當(dāng)前點(如圖5b中第3行第4列的格子)所占格子的正下方，則局部規(guī)劃區(qū)域格子數(shù)為(s+1)*s，如圖5b的第2行到第9行的方框所示。③當(dāng)下一步的障礙物所占格子在機器人當(dāng)前點(如圖5c中第6行第9列的格子)所占格子的左方，則局部規(guī)劃區(qū)域格子數(shù)為s*(s+1)，如圖5c的第2行到第10行的方框所示。④當(dāng)下一步的障礙物所占格子在機器人當(dāng)前點(如圖5d中第6行第2列的格子)所占格子的右方，則局部規(guī)劃區(qū)域格子數(shù)為s*(s+1)，如圖5d的第3行到第9行的方框所示。圖5中除了表示機器人當(dāng)前點的另一個灰色格子為局部規(guī)劃區(qū)域內(nèi)的目標(biāo)點位置，它也是全局路徑中的一點。

圖5 局部規(guī)劃區(qū)域

2 仿真分析

為了驗證本文所提出的方法可行，對機器人在動態(tài)環(huán)境中運動過程遇到的幾種情況(靜止的障礙物，同向運動的障礙物，側(cè)向穿過的障礙物，反向運動的障礙物)進行仿真。仿真軟件為Matlab2013b，仿真的環(huán)境為(27×0.5)m×(26×0.5)m的長方形柵格地圖，每小格的尺寸為0.5 m ×0.5 m，一共27×26個格子，如圖6所示。設(shè)機器人的初始位置為(4，22)，目標(biāo)點為(26，13)，坐標(biāo)以格子位置的行號和列號表示，如初始位置(4，22)為在第4行第22列格子的位置，本文下面的坐標(biāo)表示一樣。當(dāng)沒有障礙物時，機器人從起點到達目標(biāo)點，使用A*算法得到如圖7所示的運動路徑。當(dāng)在運動過程中分別遇到四個障礙物：①靜止的障礙物((9，16)、(9，17)、(10，16)和(10，17)四個格子表示)；②同向運動的障礙物(圖8(1)中(16，13)所示)；③側(cè)向穿過的障礙物(圖8(2)中(19，12)所示)；④反向運動的障礙物(圖8(3)中(18，13)所示)。使用本文提出的方法則機器人的運動路徑順序圖如圖8所示。

圖6 仿真環(huán)境的柵格地圖

圖7 沒障礙物情況的運動路徑

圖8 機器人的運動路徑順序圖

從圖8可知，當(dāng)移動機器人遇到第一個障礙物(靜止的障礙物)時，機器人將進行局部路徑規(guī)劃，局部規(guī)劃區(qū)域如圖5c所示，為9×10個格子范圍；當(dāng)遇到第二個障礙物(同向運動的障礙物)時，機器人將進行跟隨運動；當(dāng)遇到第三個障礙物(側(cè)向穿過的障礙物)時，機器人等待障礙物穿越完成再繼續(xù)運動；當(dāng)遇到第四個障礙物(反向運動的障礙物)時，機器人將進行局部路徑規(guī)劃，局部規(guī)劃區(qū)域如圖5b所示，為8×7個格子范圍。從仿真結(jié)果可知：①移動機器人在動態(tài)環(huán)境中能安全避障。②移動機器人運動前進行了一次A*算法全局路徑規(guī)劃和運動過程中進行了兩次A*算法局部路徑規(guī)劃，局部規(guī)劃區(qū)域分別為9×10個格子范圍和8×7個格子范圍，它們與全局規(guī)劃區(qū)域(27×26個格子范圍)相比，局部規(guī)劃區(qū)域小很多，則可以說明局部規(guī)劃時間比全局規(guī)劃時間小。③局部規(guī)劃時間減小可以提高移動機器人運動的實時性。

3 結(jié)論

基于雙重A*算法的移動機器人路徑規(guī)劃方法能讓移動機器人在動態(tài)環(huán)境中安全無碰撞運動，并且在環(huán)境信息局部變動情況下重新規(guī)劃運動路徑的時間減小，提高機器人運動的實時性。本文提出的方法是采用A*算法分別進行全局路徑規(guī)劃和局部路徑規(guī)劃。全局路徑規(guī)劃為機器人運動前根據(jù)已知起點位置，目標(biāo)點位置和全局運動環(huán)境的柵格地圖，采用A*算法進行全局路徑最優(yōu)規(guī)劃。局部路徑規(guī)劃為移動機器人在運動過程中遇到障礙物時，首先根據(jù)障礙物的運動特點，確定局部規(guī)劃區(qū)域，局部規(guī)劃的起點位置和目標(biāo)點位置，然后采用A*算法在局部規(guī)劃區(qū)域進行局部最優(yōu)路徑規(guī)劃。全局路徑與局部路徑混合則為移動機器人的運動路徑，該路徑能讓移動機器人在動態(tài)環(huán)境中安全無碰撞并且全局最優(yōu)和局部最優(yōu)兼并。并且，由于局部路徑規(guī)劃范圍比全局路徑規(guī)劃范圍小，則局部路徑規(guī)劃時間小，能減小總體的路徑規(guī)劃時間，提高機器人運動的實時性。

[參考文獻]

[1] 陸新華，張桂林．室內(nèi)服務(wù)機器人導(dǎo)航方法研究[J]．機器人，2003，25(1)：80-87．

[2] 黃玉清，梁靚．機器人導(dǎo)航系統(tǒng)中的路徑規(guī)劃算法[J]．微計算機信息，2006，22(7)：259-261.

[3] 黃健生．移動機器人的路徑規(guī)劃研究[D]．杭州：浙江大學(xué)，2008．

[4] 王殿君. 基于改進A*算法的室內(nèi)移動機器人路徑規(guī)劃[J]. 清華大學(xué)學(xué)報(自然科學(xué)版)，2012，52(8)：1085-1089.

[5] 王紅衛(wèi)，馬勇，謝勇，等. 基于平滑A*算法的移動機器人路徑規(guī)劃[J]. 同濟大學(xué)學(xué)報(自然科學(xué)版)，2010， 38(11): 1647-1650.

[6] 秦玉鑫，王紅旗，杜翠杰. 基于雙層A*算法的移動機器人路徑規(guī)劃[J]. 制造業(yè)自動化，2014，36(12)：21-25.

[7] HUNAG Biao, Kadali R. Dynamic modeling, predictive control and performance monitoring (a data-driven subspace approach) [M]. London: Springer, 2008.

[8] 曲道奎，杜振軍，徐殿國，等.移動機器人路徑規(guī)劃方法研究[J].機器人，2008，30(2)：97-101，106.

[9] 鄔再新, 李艷宏, 劉濤, 等. 動態(tài)環(huán)境中多移動機器人路徑規(guī)劃的一種新方法[J]. 組合機床與自動化加工技術(shù)，2008(3)：25-27.

[10]王殿君，魏洪興，任福君. 移動機器人自主定位技術(shù)[M]. 北京：機械工業(yè)出版社，2013.

(編輯李秀敏)