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

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

0 引言

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2 仿真分析

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

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

圖7 沒障礙物情況的運(yùn)動路徑

圖8 機(jī)器人的運(yùn)動路徑順序圖

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

3 結(jié)論

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

[參考文獻(xiàn)]

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

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

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

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

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

[6] 秦玉鑫，王紅旗，杜翠杰. 基于雙層A*算法的移動機(jī)器人路徑規(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] 曲道奎，杜振軍，徐殿國，等.移動機(jī)器人路徑規(guī)劃方法研究[J].機(jī)器人，2008，30(2)：97-101，106.

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

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

(編輯李秀敏)