基于平滑A*人工勢場法的機器人動態(tài)路徑規(guī)劃

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

摘要：針對動態(tài)環(huán)境下移動機器人的路徑規(guī)劃問題，提出了平滑A*人工勢場法的路徑規(guī)劃方法。首先采用平滑A*算法在靜態(tài)障礙物環(huán)境中進行全局路徑規(guī)劃；其次，在機器人遇到動態(tài)障礙物時采用A*人工勢場法進行局部動態(tài)路徑規(guī)劃，并以此調(diào)整全局路徑規(guī)劃結(jié)果；最后對路徑規(guī)劃結(jié)果進行平滑處理。將平滑A*人工勢場法應(yīng)用于機器人動態(tài)路徑規(guī)劃，并與D*算法進行對比。實驗結(jié)果表明，該算法能夠在動態(tài)環(huán)境下規(guī)劃出一條更為優(yōu)化的路徑，有效縮短了路徑長度，提高了規(guī)劃效率。

關(guān)鍵詞：A*算法；人工勢場法；路徑規(guī)劃；機器人

DOIDOI：10.11907/rjdk.172192

中圖分類號：TP301

文獻標識碼：A文章編號文章編號：16727800（2018）001000803

Abstract：A path planning method based on smoothed A* artificial potential field is proposed aiming the path planning problem of mobile robot in dynamic environment， In order to improve the planning efficiency， Firstly a smoothed A* algorithm was used to perform the global path planning in the static obstacle environment；Secondly the A* artificial potential field method was applied to perform the local dynamic path planning and adjusted then the global path planning results when the robot encounters a dynamic obstacle；Finally the path planning results were smoothed.The smoothed A* artificial potential field method is applied to the robot dynamic path planning and compared with the D*algorithm. Experimental results have shown that the proposed algorithm can be worked out a more optimized path in dynamic environment， effectively shorten the path length， improve the planning efficiency.

Key Words：A* algorithm； artificial potential field method； path planning； robot

0引言

路徑規(guī)劃是移動機器人控制領(lǐng)域的一個重要研究方向，它是指依據(jù)某些指標在一個障礙物環(huán)境下搜尋一條從起始點到目標點的最優(yōu)或近似最優(yōu)無碰撞路徑[12]。依據(jù)環(huán)境信息，大體上可將路徑規(guī)劃分為環(huán)境信息已知的全局路徑規(guī)劃和基于傳感器感知的局部路徑規(guī)劃[3]。其中，全局路徑規(guī)劃能處理障礙物信息已知情況下的路徑規(guī)劃，但當出現(xiàn)動態(tài)障礙物時，規(guī)劃效果不理想；局部路徑規(guī)劃能夠通過傳感器反饋的信息，規(guī)避環(huán)境中的動態(tài)障礙物。

A*算法是一種經(jīng)典的啟發(fā)式搜索算法，適用環(huán)境已知的全局路徑規(guī)劃，它具備最優(yōu)性、完備性和高效性[4]，利用A*算法可以得到較好的路徑規(guī)劃效果。但是由于A*算法的計算特點，規(guī)劃出的路徑點一般存在折線多、累計轉(zhuǎn)折角度大等問題。平滑A*[5]通過建立平滑模型，有效降低了路徑中的轉(zhuǎn)折點與轉(zhuǎn)折角度。D*算法[6]是基于A*算法提出的機器人動態(tài)路徑規(guī)劃算法，其思想是在向目標點移動過程中，若檢測到環(huán)境中的動態(tài)障礙物，則搜尋障礙物附近節(jié)點的變化情況，重新規(guī)劃路徑。人工勢場法[7]是一種比較成熟的局部路徑規(guī)劃算法，其由于具有計算速度快、適用于實時控制的優(yōu)點而得到廣泛應(yīng)用。但由于該方法依據(jù)的是局部環(huán)境信息，缺乏全局自我調(diào)節(jié)能力，易陷入局部最優(yōu)[8]。

本文提出平滑A*算法和改進人工勢場法結(jié)合的動態(tài)路徑規(guī)劃算法。首先，為提高算法搜索效率，采用文獻[5]中的平滑A*算法對障礙物環(huán)境進行全局路徑規(guī)劃，再引入改進的人工勢場法對動態(tài)障礙物進行避障。

1A*算法

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

A*算法是一種經(jīng)典的啟發(fā)式算法，在靜態(tài)環(huán)境中能高效地求出從起始點到目標點的路徑，其估價函數(shù)定義為：

其中，X是當前節(jié)點，f（x）是當前節(jié)點的總啟發(fā)代價，g（x）是初始點到當前點的實際路徑代價，h（x）是當前點到目標點的估計路徑代價。

h（x）的取值對算法效率具有決定性作用。本文采用當前點與目標點之間的歐幾里得距離作為估計代價，如下所示：

其中，xt為目標點橫坐標，yt為目標點縱坐標，xx為當前點橫坐標，yx為當前點縱坐標。

A*算法尋徑首先要創(chuàng)建兩張表：open list和close list。其中，open list用來保存拓展節(jié)點，close list用來保存障礙物和已使用過的節(jié)點。搜尋過程中，先從open list找出總啟發(fā)代價最小的點設(shè)為當前節(jié)點，將其放入close list，并對其進行擴展；將擴展后的節(jié)點更新到open list，再從open list選取總啟發(fā)代價最小的點設(shè)為當前節(jié)點；重復(fù)過程，直到找到目標點。

1.2平滑A*算法endprint

文獻[5]中提出，傳統(tǒng)A*算法規(guī)劃出的路徑存在冗余轉(zhuǎn)折點，為了避免不必要的轉(zhuǎn)折，對求得的路徑進行平滑處理。從當前點開始，依次連接之后的節(jié)點，判斷是否與障礙物有交點，如果無交點，舍棄中間節(jié)點，求出平滑路徑。具體過程如下：①取出傳統(tǒng)A*算法規(guī)劃出的路徑點，將起點設(shè)為pcurrent，下一節(jié)點依次設(shè)為pnext、pdnext；②執(zhí)行check（pcurrent，pdnext，barrier list）方法獲得返回值，如果為0則繼續(xù)執(zhí)行，否則跳轉(zhuǎn)至第4步；③pnext賦值給pcurrent，pdnext賦值給pnext，pdnext下一節(jié)點賦值給pdnext，跳轉(zhuǎn)至第5步；④路徑點中刪除pnext節(jié)點，將pdnext賦值給pnext，pdnext下一節(jié)點賦值給pdnext；⑤若pdnext為目標點則繼續(xù)執(zhí)行，否則跳轉(zhuǎn)至第2步；⑥輸出平滑后的路徑。

其中，barrier list是路徑規(guī)劃環(huán)境中的障礙物列表，函數(shù)check（pcurrent，pdnext，barrier list）用來檢測pcurrent和pdnext連線與障礙物邊線是否相交，如果不相交則返回1，否則返回0。

2人工勢場法

2.1傳統(tǒng)人工勢場法

人工勢場法在機器人運動空間構(gòu)造虛擬的力勢場，包括目標點產(chǎn)生的引力勢場和障礙物產(chǎn)生的斥力勢場。目標點引力與當前點到目標間距離成正比，障礙物產(chǎn)生斥力與當前點到障礙物距離成反比，通過引力和斥力的作用，尋找一個從初始點到目標點的安全無碰撞路徑。其引力勢場函數(shù)和斥力勢場函數(shù)分別定義為：

式中，X是當前位置，Xg是目標點位置，X0是障礙物位置，Katt是引力場常量，Krep是斥力場常量，ρ0是障礙物最大威脅距離。

根據(jù)引力勢場和斥力勢場的負梯度可以得到引力函數(shù)和斥力函數(shù)：

通過人工勢場法可以在較簡單的障礙物環(huán)境中規(guī)劃出一條從初始點到目標點的安全無碰撞路徑。當環(huán)境中障礙物變多，則可能存在人工勢場法的局部極小值點問題。局部極小值點是機器人運動環(huán)境中某些點由于受多個力的共同作用，造成了斥力和引力相互抵消，從而使合力為0。隨著障礙物增多，尋徑過程中陷入局部極小值點的機率隨之增大。

2.2A*人工勢場法

針對人工勢場法局部極小值點問題，提出A*人工勢場法。主要包括局部極小值檢測和A*人工勢場法避障。

本文人工勢場法局部極小值檢測采用一定間隔內(nèi)機器人行進距離和閾值進行比較的方法。該方法首先根據(jù)機器人移動步長和障礙物影響范圍大小設(shè)置一個閾值T，然后檢測當前節(jié)點前一個節(jié)點與后一個節(jié)點之間的距離并與T進行比較。若距離小于閾值T，可以認為機器人陷入了局部極小值點，反之則沒有陷入。

檢測到移動機器人陷入局部極小值點時，對局部極小值點用A*算法進行規(guī)劃從而將其避開，脫離局部極小值點后回到人工勢場法。傳統(tǒng)A*算法路徑點為柵格中點，對障礙物的判斷為檢測拓展節(jié)點是否為障礙物點。但此時路徑點不再局限于柵格中點，在尋徑過程中，連接當前節(jié)點與拓展節(jié)點，通過判斷連線與障礙物邊線是否有交點判斷是否與障礙物相交。圖1是傳統(tǒng)人工勢場法的路徑規(guī)劃結(jié)果，圖2是A*人工勢場法的路徑規(guī)劃結(jié)果。

3平滑A*人工勢場法

將平滑A*算法與A*人工勢場法結(jié)合。首先用柵格法[9]進行建模，將機器人移動區(qū)域劃分為相同的小柵格。靜態(tài)障礙物標記為黑色，可通行區(qū)域標記為白色。由于障礙物形狀不確定，為了簡化操作，當障礙物不足一個柵格時，將其補全至一個柵格；然后采用平滑A*算法對障柵格法處理后的障礙物環(huán)境進行全局路徑規(guī)劃，當遇到動態(tài)障礙物時，將全局規(guī)劃路徑點中障礙物的前一節(jié)點設(shè)為初始節(jié)點，障礙物后一節(jié)點設(shè)為目標點，采用A*人工勢場法進行動態(tài)路徑規(guī)劃；再對A*人工勢場法規(guī)劃的路徑進行平滑處理，消除冗余節(jié)點，最后輸出路徑。具體流程如圖3所示。

4實驗研究

為了驗證動態(tài)規(guī)劃性能，在Matlab2014上進行仿真，設(shè)人工勢場法初始參數(shù)引力場常量Katt=100，斥力場常量Krep=3，斥力影響范圍ρ0=3，步長l=0.5；圖中矩形網(wǎng)格為動態(tài)障礙，沿y軸負方向作速度為1柵格/s的勻速運動。圖4為在20×20柵格環(huán)境下采用平滑A*算法預(yù)規(guī)劃路徑圖，圖5為未碰撞時的路徑圖，圖6是動態(tài)障礙物碰撞時D*算法規(guī)避路線圖，圖7是動態(tài)障礙物碰撞時平滑A*人工勢場法算法規(guī)避路線圖。表1給出了D*算法和平滑A*人工勢場法路徑規(guī)劃參數(shù)對比。

實驗結(jié)果表明，當傳感器檢測到動態(tài)障礙物時，采用A*人工勢場法能夠有效避障并快速回到預(yù)規(guī)劃路徑，與D*算法相比，路徑長度減少了7.001%，累計轉(zhuǎn)折次數(shù)減少了23.077%，累計轉(zhuǎn)折角度減少了37.973%。

5結(jié)語

本文將平滑A*算法與改進人工勢場法相結(jié)合，首先用平滑A*算法進行全局路徑規(guī)劃，再用A*人工勢場法引導(dǎo)機器人進行動態(tài)避障，最后對求得的路徑點進行平滑處理。仿真實驗結(jié)果表明，本文算法在動態(tài)環(huán)境中能較好地滿足移動機器人的路徑規(guī)劃要求。

參考文獻：

[1]朱大奇，顏明重.移動機器人路徑規(guī)劃技術(shù)綜述[J].控制與決策，2010，25（7）：961967.

[2]ZAMIRIAN M， KAMYAD A V， FARAHI M H.A novel algorithm for solving optimal path planning problems based on parametrization method and fuzzy aggregation[J].Physics Letters A，2009，373（38）：34393449.

[3]楊世良.動態(tài)環(huán)境下機器人混合型路徑規(guī)劃算法[D].鄭州：鄭州大學(xué)，2011.

[4]李沖，張安，畢文豪.單邊矩形擴展A*算法[J].機器人，2017，39（1）：4656.

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

[6]FERGUSON D， STENTZ A. The field D* algorithm for improved path planning and replanning in uniform and nonuniform cost environments[R].Technical Report CMUTRRI0519，2005.

[7]KHATIB O. Realtime obstacle avoidance for manipulators and mobile robots[M]. Sage Publications， Inc.1986.

[8]張殿富，劉福.基于人工勢場法的路徑規(guī)劃方法研究及展望[J].計算機工程與科學(xué)，2013，35（6）：8895.

[9]王娟娟，曹凱.基于柵格法的機器人路徑規(guī)劃[J].農(nóng)業(yè)裝備與車輛工程，2009（4）：1417.

（責(zé)任編輯：黃健）endprint