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

柴紅杰李建軍姚 明

(江蘇大學汽車與交通工程學院，江蘇鎮江 212001)

當今社會移動機器人在各行各業都扮演著重要角色，但是路徑規劃仍然是移動機器人應用中面臨的一個重要難題。它的目的是在有障礙物的環境中為移動機器人從起始位置到目標位置規劃出一條最優或次優安全無碰撞可行路徑并且得到的路徑要滿足一定的約束條件[1-2]。機器人路徑規劃的常用方法主要有蟻群算法[3]，粒子群優化算法[4]，柵格法[5]等。其中，由于柵格法具有結構簡單，易于實現，對傳感器容錯性強等優點，被廣泛應用于機器人路徑規劃中。基于柵格地圖的A*算法適用于環境信息已知的一類路徑規劃方法。已有多種改進的A*算法被提出，顧辰[6]在對機器人進行路徑規劃過程中，把擴展結點進行優先分級，避免穿過障礙物頂點，與障礙物有一定的安全距離，但路徑依然存在較多轉折點和安全問題；辛煜等[7]通過A*算法搜索離散的8 個鄰域擴展到無限個，增加了搜索方向，提高了路徑平滑性的性能指標，但計算量增大，致使搜索效率明顯降低；陳諾男[8]等提出更改障礙搜索矩陣的尺寸來獲得不同的安全間距，以保證不同機器人在不同地圖環境下的安全性，但未考慮機器人本體尺寸不利于實際環境規劃；李沖等[9]提出了一種單邊矩形擴展A*算法，采用受迫擴展規則，單條公共邊取代2 條相鄰冗余邊，簡化了終止條件，但在復雜環境搜索時效率明顯降低。卜新蘋等[10]提出改進的三階Bezier 曲線方法，雖然能實現路徑平滑，但是算法計算較繁瑣，實現效率低。

綜上所述，如果考慮到機器人的本體尺寸，基本的A*算法在搜索路徑時很有可能撞到障礙物，對機器人和障礙物來說是不安全的，而且不能得到次優或最優路徑。因此提出了一種在障礙物邊緣放置虛擬障礙進行緩沖和改進啟發搜索函數的算法，并在靜態和動態障礙物環境下進行仿真，最后利用動態切點法對路徑進行二次平滑處理，提高了算法的實時性和機器人的穩定性。

1 A*算法的機器人路徑規劃

1.1 環境場景的建立

環境模型的建立是對機器人進行控制的基礎，為實現路徑規劃算法，移動機器人看作二維平面移動的質點[11]，將障礙物信息以及機器人運動軌跡記錄在柵格上，黑色塊表示障礙物單元柵格，白色塊表示可通行區域單元柵格。機器人運動空間為，將柵格化的環境信息映射到直角坐標系XOY中，且Xmax和Ymax分別為X、Y軸上的最大值。假設機器人的移動步長為σ，對X、Y軸分別以σ進行劃分如圖1 所示。

圖1 柵格化的移動機器人環境

圖1 中，行列的柵格數分別為m＝采用“柵格-存儲”的映射法[12]將格子信息存儲在第m行、第n列，記為C(m，n)。定義從原點開始第一個柵格C(1，1)的序號i為1，C(2，1)的序號為2，…，在m×n的柵格中，坐標(x，y) 與柵格序號的映射關系為:

式中:mod，int 分別為求余、取整運算。

1.2 機器人路徑規劃的A*算法

A*算法是典型的啟發式搜索算法，其搜索區域為四方向或八方向。具體搜索如圖2 所示。

圖2 格柵場地及機器人運動方向

代價函數的核心表達式為:

式中:f(n)表示從起始節點Nstart到目標節點Ggoal總的代價消耗。g(n)表示從起始節點到當前節點Current 的實際消耗，h(n)為當前節點Current 到目標節點Ggoal的估計代價消耗值，f(n)的大小取決于h(n)的大小，h(n)越接近實際距離，消耗的總代價越小。因此分析A*的關鍵所在就是h(n)。假設真實路徑代價值用Ch(n，goal)表示，擴展搜索范圍用EX 表示，h(n)與Ch(n，goal)的不同對A*搜索算法的影響如表1 所示。

表1 h(n)對A*算法的影響

全局路徑規劃可分為3 個部分，首先是機器人環境模型的建立，其次調用A*算法進行路徑搜索，最后路徑輸出，如圖3 所示。

圖3 路徑規劃流程圖

2 改進的A*路徑規劃算法

2.1 虛擬障礙物的設置

A*算法把機器人運動的整個空間分成2 部分:自由空間和障礙物空間。沒有考慮到機器人的實體尺寸、轉彎半徑、定位與距離誤差等因素的影響。針對上述問題，提出了把整個運動區域進行安全等級劃分，在障礙物周圍放置虛擬障礙物形成緩沖區。

為保障機器人能安全快速的通過障礙物區域到達目標點，可把整個運動區域進行安全等級劃分。機器人在路徑規劃時選擇安全等級較高的優先進行規劃，遠離障礙物，減少干擾，提高安全性。機器人當前位置的代價值與距離最近障礙物距離D成反比。規定代價的范圍為[0 255]，代價值255 用MCV(Max Cost Value)表示。最近障礙物距離D 規定0 至BR(Buffer radius)，BR 一般取機器人半徑的2 倍。緩沖區代價等級R計算公式用如式(3)所示，R值越小安全等級越高。

式中:Rd(Robot radius)表示機器人半徑。

經過放置虛擬障礙物處理的地圖如圖4 所示。

圖4 虛擬障礙物地圖處理

圖4 中粗黑色部分代表障礙物所占據的空間，淺黑色(灰色)部分為虛擬放置障礙物膨脹部分，白色為自由無障礙區。其中R值越小代表障礙等級越低，在機器人路徑規劃中經優先規劃R較小的區域。

2.2 啟發函數的改進

分析表1 可知，A*算法的搜索性能受h(n)的影響，當的大小越接近真實代價消耗的總代價越小，h(n)搜索效率越高。實際中很難估計h(n)大小，一般有曼哈頓距離(Manhattan)、切比雪夫距離和歐氏距離3 種方法。歐氏距離相對實際距離偏短，Manhattan 距離相對于實際距離偏大。所以可以取Manhattan 距離和歐氏距離的中間值作為h(n)的估計值。中間值h(n)的估計值如圖5(a)、5(b)、5(c)所示。

圖5 中，線段d1和d2分別代表當前節點N點到目標節點G點的橫向距離與縱向距離，d1和d2之和為Manhattan 距離。斜線段為歐氏距離，虛線段為Manhattan 距離和歐氏距離的中間值，即改進的啟發函數h*(n)。改進后的h*(n)的推導如下所示:

圖5 改進啟發函數示意圖

(1)當d1＝d2時；

(2)當d1＞d2時；

(3)同理當d1＜d2時；

綜上h*(n)表達式如(7)所示。

式中:d1與d2的計算公式如(8)所示:其中d1與d2是在二維平面內的距離。N(xn，yn)為當前節點坐標，G(xg，yg)為目標點坐標。

改進后的算法流程圖如圖6 所示。

圖6 改進后A*算法

3 仿真結果和分析

3.1 靜態障礙物環境下

靜態障礙物環境也就是機器人所處的空間障礙物是固定不變的，不會突然出現移動的障礙物等情況。實驗環境:Intel(r)Core(TM)I5-6500 CPU。編譯工具MATLAB 2017a。分別對A*算法和改進A*算法進行仿真，地圖尺寸為30 m ×30 m，設起點為Start，目標點為Goal 進行仿真計算，結果如圖7 所示。

圖7(a)為A*算法沒有進行虛擬障礙物放置處理和未進行h*(n)改進的路徑規劃，可以很明顯地看出機器人貼近障礙物附近運行且轉彎幅度過大，生成的路徑不平滑且可靠性低[13]，這會導致機器人的碰撞幾率增大，燃料成本增加。

改進A*啟發算法轉折角度明顯減小，機器人遠離障礙物有效地降低了機器人與障礙物的碰撞幾率，提高了路徑的可靠性，如圖7(b)所示。

圖7 基本A*算和改進A*算仿真結果

圖8、圖9 為基本算法與改進算法運行時間和路徑規劃長度對比圖。

圖8 規劃時間對比圖

圖9 路徑長度對比圖

由圖9 可以看出在20 次仿真實驗中，基本A*算法路徑相對較短，但是最小轉折角度太過于小，很容易使機器人不能轉彎，也會導致電機磨損加重降低壽命。改進A*算法路徑長度比基本算法路徑長9.8%(路徑長度變長是由于規劃路徑遠離障礙物和機器人轉彎半徑增大的結果)，運行時間縮短16.3%，最小轉折角度也得到了很大的改善。累計轉折角度減少10%～20%。仿真結果證明改進A*啟發算法符合機器人最優路徑規劃需求，能有效地解決實體機器人通過狹窄區域或通道的路徑規劃問題，提高了機器人的穩定性。

表2 實驗結果平均值

3.2 動態障礙物環境下

在動態環境下，除了存在靜止的障礙物外，當突然有動態障礙物出現在移動機器人的工作環境中時，機器人就可能與其發生碰撞，這就需要機器人避開障礙物進行規劃。圖10 是在復雜動態環境中進行規劃的結果。起始點Start，目標點Goal，黑色、淺色方框分別為靜態和動態障礙物。由仿真結果可知改進算法可以有效地避開靜態和動態障礙物，提高機器人控制的可靠性。

圖10 動態路徑規劃結果

3.3 路徑平滑

改進的A*算法規劃的路徑雖然得到明顯提高但頻繁轉向會使機器人發生抖動，而平滑的路徑更便于移動機器人的控制。采用切線交點畫圓法進行路徑平滑。定義了Smooth 函數如圖11 所示:(1)取中間某一節點為中間節點，連接中間節點前后兩接點，若不與障礙物相交則去除中間節點，否則保持原路徑[14]，原理如圖12(a)所示；(2)若遇到轉折點則做切線交點畫圓法，如12(b)所示。

機器人起始位置N1(x1，y1)，終點位置Nn(xn，yn)。從起始位置依次對轉折點Ni(xi，yi)(i＝1，2，…，n-1)進行平滑處理，具體步驟如下:

(1)取中間節點連接前后相鄰節點。若不與障礙物相交則去除中間節點，否則保持原路徑。

(2)比較Ni-1Ni和NiNi+1。選擇較短的邊作為切點，切線與∠Ni-1NiNi+1角平分線交于Oi。

圖11 Smooth 流程

圖12 路徑平滑示意圖

(3)判斷圓弧是否與長邊Ni-1Ni有交點A。若有，轉至步驟(4)，若無轉至(5)。

(4)用圓弧PA代替邊Ni-1NiNi+1，并轉至步驟(6)。

(5)無交點就繼續在較短的邊取下一臨近節點，同理(2)。

(6)判斷是否遍歷所有節點。若是，返回(1)，否則結束。

路徑平滑后的結果如圖13、圖14 所示。

圖13 靜態路徑平滑結果

由圖13 可以看出平滑后路徑去除了冗余的節點，轉折點得到圓弧過渡，減少了機器人的轉彎頻率。可以看出未改進算法路徑會與障礙物邊緣發生摩擦，如圖14 中的②、③所示。平滑后的路徑可以實時避開突然出現的障礙物，更接近真實環境，如圖14 中的①所示，因此改進A*算法提高了機器人的穩定性、安全性和實時跟蹤效率，同時減少了電機磨損程度和燃料成本。

圖14 動態路徑平滑結果

4 結論

(1)針對移動機器人路徑規劃問題，從路徑的安全性、機器人的穩定性出發，提出一種在障礙物邊緣放置虛擬障礙物形成緩沖區和改進搜索啟發函數的算法。

(2)仿真結果表明，改進A*算法相比基本A*算法規劃的路徑更加遠離障礙物且累計轉折角度變小，并能有效避開動態障礙物，規劃時間相對縮短，證明了改進算法的有效性。

(3)最后對靜態障礙物環境和動態障礙物環境規劃后的路徑進行二次平滑處理。結果表明，平滑后的路徑更符合實體機器人通過狹窄的區域或通道，提高了機器人的穩定性、安全性、控制性和實時跟蹤效率。