基于改進A*算法的移動機器人路徑規劃

陸皖麟，雷景森，邵 炎

(66133部隊， 北京 100043)

路徑規劃技術是移動機器人研究的熱點問題，對它的研究為控制機器人自主行駛奠定基礎[1]。路徑規劃的定義可以表示為：設定起始點、目標點后，依據規劃算法搜尋從起點至目標點同時可以避障的一條最佳路線。依照機器人的環境情況將運動路徑列為兩類：一類是基于地圖信息已知的全局路徑規劃，此類路徑規劃環境中的信息包括障礙物和可行駛區域全部已知；另外一類是根據相機、慣性導航、里程計等獲取實時信息的局部路徑規劃，局部路徑規劃是處在障礙物信息未知的環境[2]。常用的全局路徑規劃算法有[3]：A*算法、Dijkstra算法、粒子群算法、遺傳算法等。A*算法最早由Nilsson提出來的啟發式算法，它的核心是對目標點進行不斷搜尋，從而取得機器人的避障路徑。通過對狀態空間中搜索點進行評價，取得最佳節點，然后依據此位置節點繼續進行搜尋，一直到找到目標點為止。該算法具有編程方法相對簡單，參數設置較少，搜索路徑效率高[4]的特點。

1 A*算法原理

A*算法是獲取最短路徑比較有效的搜索方法，也是典型的啟發式算法。A*算法一般用于靜態全局規劃，其核心表達式為

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

(1)

其中：f(n)表示從起點開始經過當前節點到目標節點的路徑長度；g(n)表示起點至當前節點n的路徑長度，此時g(n)已經是機器人從起始點到當前點的最短距離；h(n)是從狀態節點n到目標狀態的估計代價函數距離。由于g(n)是實際距離，為了在靜態地圖可以搜尋到最優解，選擇代價h(n)類型非常重要，f(n)的值取決于h(n)的大小。下面描述算法搜索節點過程，如圖1所示，綠色小格表示起始點位置，紅色小格表示目標點位置，藍色小格周圍的8個點表示可以設定為當前狀態空間的子節點候選點，設小格的一條邊長度為10，式(1)簡寫為F=G+H，G表示起始點至當前狀態的曼哈頓距離，H是當前狀態節點至目標節點估計代價函數值，這里代價函數H的計算使用的是歐式距離，起始點周圍8個格子F、G、H各數值見圖，由圖可知2號小格F最小，將其作為接下來要處理的點，然后檢查2號周圍的小格的G、H值，黑色方格是障礙物，在計算G、H值時忽略其不可通行，確定2號的待選小格是1號、3號。由于1號、3號是父節點(起始點)的8個相鄰節點、3號節點周圍沒有可選新的相鄰節點，故從起始點選擇待處理節點是1號而不是2號、3號。按照此思路，4號節點是1號節點的子節點。

圖1 A*算法搜索節點

在已構建的地圖中，設起始點坐標節點為S(Sx,Sy),當前節點C(Cx,Cy)，目標節點的坐標T(Tx,Ty)，若使用相對簡單的歐式距離估價函數值可表示為

(2)

2 A*算法流程

下面將以偽代碼的形式描述A*算法流程，首先設置各項參數，start：機器人路徑規劃的出發點；goal：機器人路徑規劃的目標點位置；g(n)：n節點到start點的實際移動距離；h(n)：n節點到達目標點的理論移動距離；openlist：搜尋節點的過程中保存未檢測的節點列表；closelist：已經被檢測的節點保存至該列表；comaFrom：保存子節點和父節點，最終生成的路徑在此列表中。使用偽代碼為：

把起點加入到openlist

do

{

搜尋openlist中f(n)值最小的節點，它表示當前節點

加入至closelist

對當前節點相鄰的所有候選節點

if(是障礙物點‖在closelist中)

{

繼續搜尋下一個相鄰候選節點

}

if(openlist不存在此節點)

{

將此候選節點加入openlist，更新父節點，計算它的f(n)、g(n)、h(n)值

}

if(候選節點已經在openlist內)

{

if(使用g(n)值作為評判標準，來檢測新的路徑，判斷是否有更低的g(n))

{

將它的父節點換為當前節點，計算當前節點的f(n)、h(n)的值

}}

使用comaFrom保存父節點

}while(目標節點在openlist里面，表示路徑規劃成功)

若openlist列表是空值，表示沒有可尋找的路徑。

最后把comaFrom保存的所有父節點從起點開始以此連接起來，該路線就是A*算法所求的路徑。

傳統A*算法路徑規劃程序結構如圖2所示。

3 A*算法改進

盡管傳統A*算法已經成為移動機器人導航中廣泛使用的路徑規劃算法，但是算法規劃出來的路徑存在拐點多，轉折次數多的問題，在實際環境中不利于機器人的行駛[5]。因此在研究A*算法的基礎上提出一種適宜機器人行駛改進的A*算法。

圖2 傳統A*算法路徑規劃程序框圖

為解決A*算法在實際路徑規劃中不能求最短路徑，Anthony Stentz提出基于插值方法的D*方法[6]，使路徑變得平滑，但采用插值的方法使計算量增大，實時性變差。遺傳算法、神經網絡算法等智能算法也用于路徑規劃，但對于靜態地圖而言智能算法花費時間較多。為優化A*算法的規劃路徑，減少路徑的長度，針對A*算法的3個方面加以改進：

1) 設定openlist訪問時間閾值

傳統A*算法在當前節點擴展子節點時，每次都要把所有候選節點擴展完畢，這種方法在復雜環境下可能存在一定的無效搜索，將會耗費一定的時間，因此，在傳統A*算法的基礎上，每次檢測openlist表中最先插入的節點在經過一定時間閾值N后，判斷是否得到擴展，若未擴展，則將此節點作為最高級優先擴展，在一定程度可以避免陷入局部最小值，解決無法規劃出全局路徑問題。

2) Floyd算法優化

Floyd算法借鑒動態規劃的思路搜尋最短路徑，是用來解決兩點之間的最優距離問題的算法[7]。使用Floyd算法和A*算法結合的方法以減少路徑長度，符合實際應用需求。Floyd算法原理如圖3所示。

圖3 Floyd算法原理

L(A,D)為A、D兩點間的距離，如圖3，A、D存在障礙物，可設L(A,D)=+∞，R(A,D)=A->D

B點是A點和D之間已規劃出的節點：

若

L(A,B)+L(B,D)

(3)

則

L(A,D)=L(A,B)+L(B,D)

(4)

R(A,D)=A->B->D

(5)

在B、D線段上插入點C：

若

L(A,C)+L(C,D)

(6)

則

L(A,D)=L(A,C)+L(C,D)

(7)

R(A,D)=A->C->D

(8)

去除C點，A到D的優化路徑表示為一段優化后的弧形路徑R(A,D)。采取Floyd算法優化A*算法規劃的路徑能夠刪除多余的點，更加適合移動機器人行駛。

3) 路徑光滑處理

A*算法規劃的路徑存在許多尖銳的拐點和折點，這種拐點不符合移動機器人的平穩運動。為了滿足移動機器人在規劃的路徑上能夠平穩移動，需要對規劃的路徑進行平滑處理[8]，用平滑的曲線來代替尖銳點。其原理框圖如圖4。

圖4 平滑處理原理框圖

(9)

過直線BC方程為

(10)

由圖可知

(11)

設圓弧半徑

|p1r|=|p2r|=R

(12)

由式可得

|p1B|=|p2B|=R·cot(δ/2)

(13)

設

(14)

由A，B點坐標和式得p1和p2點坐標:

p1·x=ε1·x1+(1-ε1)·x2

p1·y=ε1·y1+(1-ε1)·y2

p2·x=(1-ε2)·x2+ε2·x3

p2·y=(1-ε2)·y2+ε2·y3

(15)

過p1點和點p2分別做直線AB和BC的垂線，其交點為。則圓弧方程為：

(x-ry)2+(y-ry)2=R2

(16)

那么軌跡A->P1->P2->C的方程為:

(17)

通過以上方法可以在拐點、折點處，采取Floyd算法去除多余的點，縮短機器人行駛路徑，然后再平滑算法處理路徑，從而減少拐點的彎折程度，有利于機器人轉向行駛以及避障操作[9]。A*算法改進后的程序流程框圖如圖5所示。

圖5 改進A*算法路徑規劃程序流程框圖

4 仿真實驗與分析

為驗證改進后的A*算法效果，仿真使用MATLAB2016b進行編程。在MATLAB中采用GUI設計A*算法圖形化驗證界面，如圖6所示，在guide里面設置兩個按鈕，一個命名為A*算法路徑規劃，另外一個命名為生成障礙物，它們會自動生成.m文件，使用GUI可編輯文本設置起點坐標、目標點坐標，分別在A*算法路徑規劃和生成障礙物按鈕生成的.m文件導入相關的代碼，修改對應Function函數的參數，生成圖7的GUI界面，點擊生成障礙物按鈕，會出現長和寬為32×32，且周圍有一定障礙物的地圖，地圖上一個柵格單位長為1。

圖6 仿真界面GUI設計

圖7 A*算法驗證界面

構建仿真環境地圖，地圖中障礙物隨機設置，同時在GUI編輯文本中隨機設置起點坐標為(3,3)，目標點隨機設置為(29,22)。仿真功能就是實現從起點(3,3)至(29,22)尋找一條最優路徑。實驗一采用傳統A*算法進行路徑規劃，路徑仿真結果見圖8(a)；實驗二僅設定openlist訪問時間閾值，進行路徑規劃，路徑仿真結果見圖8(b)；實驗三在實驗二基礎上增加Floyd和平滑處理算法，路徑仿真結果見圖8(c)。

圖8 仿真路徑規劃效果

分析對比實驗結果可知改進后的算法路徑總長度縮短了，拐點和折點數量有所降低，提高了路徑平滑度，更加適合移動機器人的運動。統計實驗數據如表1：改進后規劃時間略有增加，這是因為增加了Floyd算法和平滑算法。但規劃時間相差不是很大，在可以接受范圍內，這是由于對算法做了改進，在開啟列表(openlist)增加了一個閾值N，提高了尋找子節點的效率。

表1 仿真實驗數據

5 結論

1) 基于靜態環境下A*算法，為了提高路徑規劃的實時性并且減少搜索節點數量，提出了在A*算法的openlist加入判斷閾值N，若搜索節點的次數超過N，判斷最初加入的節點是否擴展，未擴展設它為最高級擴展節點，該方法能夠縮短規劃時間。

2) 針對傳統A*算法搜索路徑存在的拐點多、路徑彎折程度大等問題利用Floyd算法進行優化，然后進行路徑平滑處理：用圓弧曲線代替折點使其適合機器人運動。

3) 改進后的A*算法與傳統A*算法的MATLAB仿真結果對比分析可知，兩者規劃時間差別不大，但改進后的路徑更加平滑，路徑長度更短，更利于機器人的行駛。