拓展搜索鄰域的平滑A*算法機(jī)器人路徑規(guī)劃

任玉潔，付麗霞，張 勇，毛劍琳

(昆明理工大學(xué) 信息工程與自動(dòng)化學(xué)院，云南 昆明 650500)

路徑規(guī)劃已經(jīng)成為當(dāng)前機(jī)器人控制領(lǐng)域研究的熱點(diǎn)之一，它是指在一個(gè)障礙物環(huán)境下按照一定評判準(zhǔn)則搜尋一條從起始點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)或次優(yōu)安全無碰撞路徑[1-3]。目前主要的路徑規(guī)劃方法有遺傳算法[4]、蟻群算法[5]、人工勢場法[6]、Dijkstra算法[7]。但是這些算法都存在一些缺陷。其中，遺傳算法編碼長度變化范圍大，收斂性較差；蟻群算法搜索速度慢，且易陷入局部最優(yōu)；人工勢場法易陷入局部極小值，且在障礙物附近易發(fā)生振蕩；Dijkstra算法直接搜索全局而不考慮目標(biāo)點(diǎn)信息，搜尋時(shí)間長。

A*算法作為一種經(jīng)典啟發(fā)式搜索算法在20世紀(jì)60年代被提出，它具備最優(yōu)性、完備性和高效性[8]，在路徑規(guī)劃鄰域備受關(guān)注，利用A*算法可以得到較好路徑規(guī)劃效果。但是由于A*算法的計(jì)算特點(diǎn)，規(guī)劃出的路徑點(diǎn)一般存在折線多、累計(jì)轉(zhuǎn)折角度大、易舍棄部分點(diǎn)而生成次優(yōu)解等問題[9]。文獻(xiàn)[10]提出利用微分改進(jìn)A*算法，有效降低轉(zhuǎn)折次數(shù)，但是涉及大量計(jì)算。文獻(xiàn)[11]提出建立平滑A*模型，路徑點(diǎn)一定程度上得到平滑。文獻(xiàn)[12]提出以每個(gè)柵格邊線作為路徑點(diǎn)，使路徑點(diǎn)不局限于柵格中心點(diǎn)，提高了A*算法規(guī)劃結(jié)果的平滑性。

本文在基于柵格法構(gòu)建機(jī)器人路徑規(guī)劃環(huán)境模型基礎(chǔ)上，在傳統(tǒng)的A*算法上增加目標(biāo)點(diǎn)方向擴(kuò)展鄰域并對規(guī)劃結(jié)果進(jìn)行二次平滑處理。利用改進(jìn)后的A*算法規(guī)劃的路徑不僅簡化了路徑點(diǎn)數(shù)，路徑長度也有效降低。

1 問題描述

1.1 環(huán)境搭建

移動(dòng)機(jī)器人路徑規(guī)劃主要是解決環(huán)境建模和路徑搜索策略[13]。移動(dòng)機(jī)器人環(huán)境建模有柵格法、拓?fù)鋱D、幾何空間法等。其中，柵格法易創(chuàng)建和維護(hù)，本文采用柵格法建模[14-15]。假設(shè)機(jī)器人在固定區(qū)域內(nèi)移動(dòng)，柵格法將該區(qū)域縱橫方向分別劃分為大小相等的柵格。移動(dòng)機(jī)器人可以看作是二維平面中點(diǎn)狀移動(dòng)物，將障礙物映射成二維平面中不可通行的區(qū)域。在范圍內(nèi)分布任意有限障礙物，障礙物的位置不確定，障礙物形狀分布不確定，為了簡化研究，當(dāng)障礙物小于一個(gè)柵格時(shí)，將障礙物補(bǔ)全為一個(gè)柵格，其區(qū)域禁止通行。設(shè)機(jī)器人步長為1，構(gòu)建柵格環(huán)境。

1.2 A*算法

A*算法結(jié)合Dijkstra算法和BFS算法信息塊，采用估價(jià)函數(shù)來估計(jì)任意點(diǎn)到目標(biāo)點(diǎn)的代價(jià)，其估價(jià)函數(shù)定義為

f(X)=g(X)+h(X)

(1)

其中，f(x)是節(jié)點(diǎn)X的總啟發(fā)式代價(jià)；g(x)是起始點(diǎn)S到當(dāng)前點(diǎn)X的實(shí)際路徑長度；h(x)是當(dāng)前點(diǎn)X到目標(biāo)點(diǎn)T的估計(jì)代價(jià)。g(x)的計(jì)算如下

(2)

其中，li是節(jié)點(diǎn)l-1到節(jié)點(diǎn)l之間的實(shí)際路徑長度。

h(x)的取值對算法效率有關(guān)鍵作用。在實(shí)際環(huán)境中，可以選取兩點(diǎn)之間的歐幾里得距離作為估計(jì)代價(jià)，如下

(3)

其中，xt表示目標(biāo)點(diǎn)的橫坐標(biāo)；yt表示目標(biāo)點(diǎn)的縱坐標(biāo)；xx表示當(dāng)前點(diǎn)的橫坐標(biāo)；yx表示當(dāng)前點(diǎn)的縱坐標(biāo)。

A*算法在尋徑過程中總是搜尋具有最小總啟發(fā)代價(jià)的點(diǎn)作為當(dāng)前節(jié)點(diǎn)。再開始運(yùn)行時(shí)首先要?jiǎng)?chuàng)建兩張表：open list和close list。其中，open list用來保存已經(jīng)生成但是未使用的節(jié)點(diǎn)，close list用來保存障礙物和已經(jīng)使用的節(jié)點(diǎn)。搜尋過程中，先從open list找出總啟發(fā)代價(jià)最小的點(diǎn)設(shè)為當(dāng)前節(jié)點(diǎn)，將其放入close list，并對它進(jìn)行擴(kuò)展，將擴(kuò)展后的節(jié)點(diǎn)更新到open list，再從open list選取總啟發(fā)代價(jià)最小的點(diǎn)設(shè)為當(dāng)前節(jié)點(diǎn)，重復(fù)過程，直到找到目標(biāo)點(diǎn)。

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

傳統(tǒng)的A*算法存在冗余點(diǎn)多、轉(zhuǎn)折點(diǎn)多等問題，本文提出改進(jìn)的A*算法，在傳統(tǒng)的A*算法中引入目標(biāo)點(diǎn)方向搜索鄰域，并對得到的路徑點(diǎn)進(jìn)行二次平滑處理。

2.1 可搜索鄰域

傳統(tǒng)A*算法在柵格地圖上進(jìn)行路徑規(guī)劃，每個(gè)節(jié)點(diǎn)為柵格中心點(diǎn)，從當(dāng)前點(diǎn)的8個(gè)方向上拓展，運(yùn)動(dòng)方向也是π/4的整數(shù)倍，如圖1所示。

圖1 A*算法搜索鄰域

這樣求出的路徑可能不是最優(yōu)，且轉(zhuǎn)折點(diǎn)較多，為解決這個(gè)問題，引入角度計(jì)算模塊，每個(gè)節(jié)點(diǎn)鄰域除了上述8個(gè)，再加入當(dāng)前節(jié)點(diǎn)與目標(biāo)點(diǎn)角度方向步長為1的拓展節(jié)點(diǎn)；如果當(dāng)前節(jié)點(diǎn)與目標(biāo)點(diǎn)距離<1時(shí)，將目標(biāo)點(diǎn)方向拓展節(jié)點(diǎn)為目標(biāo)點(diǎn)。計(jì)算當(dāng)前節(jié)點(diǎn)X軸方向與目標(biāo)點(diǎn)方向夾角

angle_att=arcsin(del_Yi/ri)

(4)

其中，angle_att為當(dāng)前節(jié)點(diǎn)與目標(biāo)點(diǎn)連線與X軸方向的夾角；del_Yi為當(dāng)前點(diǎn)與目標(biāo)點(diǎn)Y軸的差值；ri為當(dāng)前點(diǎn)與目標(biāo)點(diǎn)之間的距離。加入目標(biāo)點(diǎn)方向搜索鄰域后，改進(jìn)A*算法搜索鄰域?yàn)閳D2所示。

圖2 改進(jìn)后A*算法搜索鄰域

改進(jìn)后的A*算法路徑點(diǎn)不再局限于柵格中點(diǎn)，且角度不再限制為π/4的整數(shù)倍，在尋徑過程中，連接當(dāng)前節(jié)點(diǎn)與拓展節(jié)點(diǎn)，判斷連線與障礙物邊線是否有交點(diǎn)，如果不與障礙物相交，將拓展節(jié)點(diǎn)加入open list。

2.2 二次平滑處理

上述方法規(guī)劃出的路徑依然存在冗余轉(zhuǎn)折點(diǎn)，為獲得更好路徑，避免不必要的轉(zhuǎn)折，對求得的路徑進(jìn)行二次平滑處理。從當(dāng)前點(diǎn)開始，依次連接之后節(jié)點(diǎn)，檢測這些點(diǎn)連線的狀態(tài)。判斷是否與障礙物有交點(diǎn)，舍棄中間節(jié)點(diǎn)，進(jìn)行兩次平滑處理。具體平滑處理過程如下：

第1步將上述改進(jìn)A*算法處理后的路徑點(diǎn)取出，從起點(diǎn)開始依次分析。將起點(diǎn)設(shè)為PCurrent；PCurrent下一節(jié)點(diǎn)設(shè)為PNext；PNext下一節(jié)點(diǎn)設(shè)為PDNext；

第2步執(zhí)行Check(PCurrent,PDNext,barrier list)方法獲得返回值，如果為0繼續(xù)執(zhí)行，否則跳轉(zhuǎn)至第4步；

第3步PNext賦值給PCurrent，PDNext賦值給PNext，PDNext下一節(jié)點(diǎn)賦值給PDNext，跳轉(zhuǎn)至第5步；

第4步路徑點(diǎn)中刪除PNext節(jié)點(diǎn)，將PDNext賦值給PNext，PDNext下一節(jié)點(diǎn)賦值給PDNext；

第5步若PDNext為目標(biāo)點(diǎn)繼續(xù)執(zhí)行，否則跳轉(zhuǎn)至第2步；

第6步輸出平滑后的路徑。

其中，函數(shù)Check(PCurrent,PDNext,barrier list)是檢測PCurrent和PDNext連線與障礙物邊線是否相交，如果不相交返回1，否則返回0。barrier list為障礙物列表。

根據(jù)上述步驟，對圖3中的實(shí)線路徑進(jìn)行平滑處理。到C點(diǎn)時(shí)，由于C、E連線與障礙物相交所以將D設(shè)為PCurrent；處理后刪除E、F節(jié)點(diǎn)，得到虛線路徑。但是虛線路徑中C、G連線與障礙物不相交，存在冗余轉(zhuǎn)折點(diǎn)D。對虛線路徑再次進(jìn)行平滑處理，二次平滑處理后得到點(diǎn)線路徑，有效消除冗余節(jié)點(diǎn)。

圖3 路徑對比

2.3 算法流程

改進(jìn)的A*算法首先通過拓展鄰域A*算法進(jìn)行路徑規(guī)劃，再對結(jié)果進(jìn)行二次平滑處理，具體流程圖如圖4所示。

第1步初始化參數(shù)，創(chuàng)建open list和close list，將障礙物加入close list；

第2步起點(diǎn)設(shè)為當(dāng)前節(jié)點(diǎn)；

第3步當(dāng)前節(jié)點(diǎn)不是目標(biāo)點(diǎn)，繼續(xù)執(zhí)行，否則跳轉(zhuǎn)至第8步；

第4步求出當(dāng)前節(jié)點(diǎn)8個(gè)基礎(chǔ)拓展節(jié)點(diǎn)和目標(biāo)點(diǎn)方向拓展節(jié)點(diǎn)加入open list；

第5步如果open list不為空，繼續(xù)執(zhí)行，否則標(biāo)記沒有路徑，跳轉(zhuǎn)至第8步；

第6步取出open list中總啟發(fā)代價(jià)最小值點(diǎn)設(shè)為當(dāng)前節(jié)點(diǎn)，并將當(dāng)前節(jié)點(diǎn)從open list移動(dòng)到close list，記錄其父節(jié)點(diǎn)；

第7步根據(jù)父節(jié)點(diǎn)遞推得到路徑點(diǎn)，并對其二次平滑處理；

第8步輸出結(jié)果：規(guī)劃后的路徑或沒有路徑。

圖4 算法流程圖

3 實(shí)驗(yàn)研究

為了驗(yàn)證加入目標(biāo)點(diǎn)方向鄰域再二次平滑處理的A*算法優(yōu)越性，在Matlab 2014上對不同環(huán)境下移動(dòng)機(jī)器人路徑規(guī)劃進(jìn)行仿真，并給出相應(yīng)結(jié)果。將改進(jìn)后A*算法規(guī)劃的路徑分別與平滑A*算法、多步長蟻群算法規(guī)劃的路徑對比。圖5和圖6為在10×10相同環(huán)境下文獻(xiàn)[11]中平滑A*算法與本文改進(jìn)A*算法求得路徑圖；圖7和圖8是在文獻(xiàn)[16]相同障礙物環(huán)境下多步長蟻群算法與本文改進(jìn)后A*算法給出對應(yīng)環(huán)境下的路徑規(guī)劃結(jié)果。

圖5 平滑A*算法

圖6 改進(jìn)后A*算法

圖7 多步長蟻群算法

圖8 改進(jìn)后A*算法

在表1和表2中分別給出平滑A*算法、多步長蟻群算法同改進(jìn)后的A*算法在路徑長度、轉(zhuǎn)折次數(shù)以及轉(zhuǎn)折角度方面的仿真結(jié)果。

表1 平滑A*算法與改進(jìn)后A*算法參數(shù)對比

表2 多步長蟻群算法與改進(jìn)后A*算法參數(shù)對比

與平滑A*算法相比，改進(jìn)后的A*算法路徑長度減少3.965%，累計(jì)轉(zhuǎn)折次數(shù)減少20%，累計(jì)轉(zhuǎn)折角度減少19.565%。與多步長蟻群算法相比，改進(jìn)后的A*算法路徑長度降低7.887%，累計(jì)轉(zhuǎn)折次數(shù)相同，累計(jì)轉(zhuǎn)折角度降低17.636%。其中，多步長蟻群算法在柵格中(10.5,6.5)點(diǎn)之后路徑規(guī)劃選擇了向下規(guī)劃的次優(yōu)路徑，增加了路徑長度，從而減小了轉(zhuǎn)折次數(shù)，累計(jì)轉(zhuǎn)折次數(shù)與本文改進(jìn)后A*算法相同。由實(shí)驗(yàn)結(jié)果可見，改進(jìn)后的A*算法規(guī)劃出的路徑性能參數(shù)優(yōu)于平滑A*算法、多步長蟻群算法。

4 結(jié)束語

傳統(tǒng)A*算法規(guī)劃出的路徑不能很好滿足實(shí)際需求，為獲得較優(yōu)路徑，拓展目標(biāo)點(diǎn)方向搜索鄰域，將對障礙物的判斷改為連接當(dāng)前點(diǎn)與拓展節(jié)點(diǎn)，判斷連線與障礙物邊線是否有交點(diǎn)，最后對求得的路徑點(diǎn)進(jìn)行二次平滑處理。通過仿真實(shí)驗(yàn)，結(jié)果表明，在不同的柵格環(huán)境下，本文改進(jìn)后的A*算法有效降低移動(dòng)機(jī)器人路徑規(guī)劃的路徑長度、轉(zhuǎn)折次數(shù)、轉(zhuǎn)折角度，能夠較好地滿足復(fù)雜環(huán)境下移動(dòng)機(jī)器人路徑規(guī)劃要求。

參考文獻(xiàn)

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

[2] Zhu Q, Hu J, Cai W, et al. A new robot navigation algorithm for dynamic unknown environments based on dynamic path re-computation and an improved scout ant algorithm [J]. Applied Soft Computing, 2011, 11(8):4667-4676.

[3] 張晨,游曉明. 基于柵格模型機(jī)器人路徑規(guī)劃的量子蟻群算法[J]. 電子科技,2016, 29(7):1-3.

[4] Quan-Min B U, Wang Z J, Tong X. An improved genetic algorithm for searching for pollution sources[J]. Water Science and Engineering,2013, 6(4):392-401.

[5] Mohamad M M, Dunnigan M W, Taylor N K. Ant colony robot motion planning[C]. Belgrade:The International Conference on Computer as A Tool,IEEE, 2006.

[6] Sheng J, He G, Guo W, et al. An improved artificial potential field algorithm for virtual human path planning[C].Changchun:Digital Techniques and Systems, International Conference on E-Learning and Games, Edutainment,2010.

[7] 張福浩,劉紀(jì)平,李青元.基于Dijkstra算法的一種最短路徑優(yōu)化算法[J].遙感信息, 2004(2):38-41.

[8] 李沖,張安,畢文豪.單邊矩形擴(kuò)展A*算法[J]. 機(jī)器人, 2017, 39(1):46-56.

[9] 秦玉鑫,王紅旗,杜翠杰. 基于雙層A*算法的移動(dòng)機(jī)器人路徑規(guī)劃[J].制造業(yè)自動(dòng)化, 2014(24):21-25.

[10] Trovato, Karen I Dorst. Differential A*[J]. IEEE Transactions on Knowledge & Data Engineering,2002,14(6):1218-1229.

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

[12] 辛煜,梁華為,杜明博,等.一種可搜索無限個(gè)鄰域的改進(jìn)A*算法[J].機(jī)器人,2014, 36(5):627-633.

[13] 張浩,孫新柱.增強(qiáng)D* Lite在自主移動(dòng)機(jī)器人安全路徑規(guī)劃中應(yīng)用[J].河北建筑科技學(xué)院學(xué)報(bào):自然科學(xué)版, 2014, 31(2):89-92.

[14] 楊杰,賀利樂,李榮麗,等.基于改進(jìn)勢場柵格法的移動(dòng)機(jī)器人路徑規(guī)劃[J].煤礦機(jī)械, 2012,33(8):80-82.

[15] 秦玉鑫,張高峰,王裕清.針對復(fù)雜環(huán)境的模塊化柵格地圖構(gòu)建算法[J].控制工程,2016, 23(10):1627-1633.

[16] 曾明如,徐小勇,羅浩,等.多步長蟻群算法的機(jī)器人路徑規(guī)劃研究[J].小型微型計(jì)算機(jī)系統(tǒng),2016, 37(2):366-369.