融合環境信息和運動約束的改進A*算法研究

白 雄，魯吉林，路 寬，陳鵬云，崔俊杰，劉澤華

1.中北大學 機電工程學院，太原030051

2.中國人民解放軍 第32381部隊，北京100072

目前機器人技術得到廣泛的發展，移動機器人在各行各業當中得到了廣泛的使用[1]。其中，路徑規劃作為移動機器人自主導航的重要前提，在機器人領域扮演了重要的角色[2]。路徑規劃A*算法作為一種深度優先啟發式搜索算法[3]，相比于圖搜索算法，A*算法具有計算量小，算法搜索朝著目標點方向等優點，但是A*算法的啟發函數簡單，算法在尋路過程當中會出現較多的冗余點，花費時間長。針對這一問題已經有眾多學者改進了A*算法。文獻[4]提出一種基于改進的A*算法與動態窗口法結合的溫室機器人路徑規劃算法，改進后的A*算法路徑規劃更為平滑和高效。文獻[5]在A*算法的啟發函數中加入余弦相似性和方法信息，選取36 階領域矩陣解決了移動機器人貼合U型彎的問題，改進了路徑曲率的變化。文獻[6]在游戲地圖中通過改進區域搜索點改進了A*算法，有效解決了路徑轉折問題。文獻[7]針對公交線路提出一種改進A*算法，將乘客人數加入到估價函數中，提高了路徑搜索的效率。文獻[8]采用“視野線”以及“圓弧-直線-圓弧”改善了路徑的平滑度。文獻[9]將物理信息添加到啟發函數當中并且刪除冗余點，增強路徑搜索效率和平滑度。文獻[10]針對標準A*算法存在對稱路徑，使用定向搜索方法減少搜索點，提高了算法的效率。文獻[11]通過引入三次均勻B樣條插值函數，改進了A*算法，改進后的算法規劃的路徑更加平滑。文獻[12]針對城市地圖進行預處理，同時改進真實代價函數和估價函數對A*算法進行改進，改進后的算法能有效提高無人機的工作效率。文獻[13]在全局路徑的基礎上選擇關鍵節點對全局路徑進行分段，然后結合局部路徑算法進行優化，改善了路徑的平滑度。文獻[14]通過全局路徑重規劃解決了障礙物大面積阻礙全局路徑的問題，提高了移動機器人避障安全性。上述文獻可以看出，之前的改進算法主要局限于對代價函數和搜索點的改進，適用于簡單的環境地圖，算法并沒有考慮移動機器人的運動特性、環境信息和避障能力。

為了解決以上問題，本文基于機器人運動特性和環境信息改進A*算法進行路徑規劃。首先通過環境地圖中障礙物權重系數對代價函數進行改進；其次考慮機器人運動約束和最小安全距離，保證機器人運動的安全性。最后通過與其他經典算法進行對比分析，驗證改進的算法在路徑拐彎次數、擴展節點、規劃時間和路徑長度方面的有效性。

1 改進A＊算法

針對標準A*算法代價函數缺乏通用性和路徑規劃算法沒有考慮機器人運動特性。改進A*算法首先通過環境地圖信息設定啟發函數；其次根據搜索方向計算子節點的代價值；最后結合關鍵節點和運動約束生成最優路徑，具體流程如圖1所示。

圖1 改進A*算法的優化流程Fig.1 Improved A* algorithm optimization process

1.1 環境信息模型

移動機器人在進行路徑規劃之前要對環境地圖進行合理的建模，由于柵格地圖具有簡單、高效的特點，則采用柵格法對環境信息建模，地圖中黑色柵格表示障礙物柵格，在數組中表示為1；白色地圖為機器人可通行柵格，在數組中表示為0，S為機器人人起始位置，G為機器人目標位置。如圖2 所示。機器人最短路徑就是通過搜索這張柵格地圖來得到的。

圖2 柵格地圖Fig.2 Raster map

通過設置障礙物權重系數表達柵格地圖的復雜度，對環境信息進行的分析，并且定義障礙物權重系數為當前地圖中障礙物個數和整個柵格地圖中的柵格個數占比。障礙柵格數為N個，移動機器人起始坐標為(xs,ys),終止坐標為(xg,yg),障礙物權重系數定義為：

1.2 啟發函數的改進

標準A*算法規劃出的路徑存在較多的拐點和轉折，不利于機器人運動。本文基于移動機器人運動特性和環境信息提出一種改進A*算法。

A*算法的評價函數由代價函數g(n)和啟發函數h(n)組成，其中算法的最優搜索性取決于啟發函數的選取，如式（2）、式（3）：

改進A*算法將環境地圖的障礙物權重系數引入啟發函數當中，如式（4）。在不同的環境地圖當中，通過修改代價函數與啟發函數的權重，提高算法的效率，保證移動機器人路徑規劃的最優性。

其中，g(n)為起始節點到當前節點的代價函數，h(n)為當前節點到目標節點的啟發函數值，(xn,yn)為當前節點的坐標，a為代價函數g(n)的權重，即當前節點到目標點的距離與起始節點到目標點的距離比值。

A*算法中評價函數的好壞直接影響路徑規劃的效率。代價函數g(n)較大時，評價函數的值則會大于的實際代價，算法效率降低，從而導致機器人運動的實際代價過大，則算法將當前節點到目標點的距離與起始節點到目標點的距離比值設為代價函數的權重。通過式（4）可知，當障礙物權重系數較小時，增加啟發函數的權重，A*算法減少搜索空間，提高了路徑規劃速度，減少路徑的拐點和轉折點；當障礙物權重系數較大時，減少啟發函數的權重，增大搜索空間，避免算法陷入局部最優。因此，改進的評價函數可以在不同環境地圖中提高路徑搜索能力，有利于機器人找到最短路徑。

1.3 搜索領域的選取

在搜索過程中A*算法向父節點的任意方向擴展，增加了算法的規劃時間，改進的A*算法提出了一種新的擴展方式，即8 節點擴展方式改為5 節點擴展方式。定義終點坐標G(xG,yG)，當前節點坐標S(xS,yS)，通過坐標作差所得值與0進行比較來確定算法的擴展方向，如圖3（a）所示。

xG-xS>0，且yG-yS>0，則可選節點為（1，2，3，6，9）；

xG-xS>0，且yG-yS<0，則可選節點為（3，6，7，8，9）；

xG-xS<0，且yG-yS>0，則可選節點為（1，2，3，4，7）；

xG-xS<0，且yG-yS<0，則可選節點為（1，4，7，8，9）。

通過確定節點擴展方向，然后從剩余的五個子節點選取下一個所要擴展的節點，每次選擇評價函數值最小的子節點作為下一個路徑規劃的節點。

圖3（b）顯示父節點與其相鄰的各個子節點的關系，主要將父節點的各子節點分為2類：父節點的上下左右四個節點和其余的四個節點。

圖3 節點搜索方式Fig.3 Node search mode

針對標準的A*算法斜穿障礙物邊緣可能性較大，增加子節點選取規則去避免此缺點，所用節點選取規則為：當障礙物節點為該父節點上下左右的四個子節點時，則四個子節點各自兩邊的子節點不作為擴展節點，如圖4 所示1 號路徑；當障礙物節點為其余的四個節點時，則不作任何處理，如圖4所示2號路徑。經過上述處理之后，改進A*算法可有效避免斜穿障礙物邊緣。

圖4 避開障礙物邊緣后的路徑Fig.4 Path after avoiding edge of obstacle

2 基于運動約束的路徑生成

2.1 機器人運動學模型

移動機器人運動過程中，一般將移動機器人作為一個質點處理，而在實際中機器人具有一定的尺寸限制。機器人運動過程中是保持一定速度的，為防止機器人發生原地轉向、側滑等不安全動作，需對機器人路徑進行優化。而移動機器人受到運動特性的限制，存在最小轉彎角度，若機器人轉彎時角度過大時，會導致機器人發生不安全的動作?？紤]到移動機器人作為一個非完整約束系統，移動機器人的簡化模型如圖5所示。

圖5 移動機器人運動模型Fig.5 Mobile robot motion model

某時刻下，機器人位置坐標為(x,y,θ)，其中x、y和θ分別是移動機器人質心位置的橫縱坐標和機器人的航向角。機器人的導向角為?（機器人中軸線和前輪行駛方向的夾角），R為機器人轉向半徑，l為前后軸距，b為輪距，v為當前速度。移動機器人的運動特性需滿足如下約束：

由于機器人受運動特性所限，導致移動機器人的路徑轉角不能過大，否則將發生不可預測的不安全動作，改進A*算法將其作為路徑規劃的約束條件，使規劃出的路徑滿足該運動約束。

2.2 Floyd算法路徑平滑優化

平滑路徑可使機器人更好地完成任務，提高機器人的運動精度。當前機器人路徑的平滑方式有貝塞爾曲線和利用幾何方式對移動機器人路徑進行優化處理，但其復雜度高，難用在機器人頻繁轉向和轉角過大的情況。文獻[15]提出基于多項式的方法優化移動機器人路徑，但該方法在復雜情況下多項式無解，不能保證算法的完整性。文獻[16]在機器人路徑規劃參數單元中設定狀態參數，依據狀態參數生成貝塞爾曲線的控制點，優化了機器人路徑，但是該方法存在著速度方面的限制，在運動過程中需不斷校正參數。本研究融合移動機器人運動特性進行路徑點的處理，通過Floyd 算法使得修正后的路徑更加符合移動機器人的運動約束。

Floyd算法是解決地圖中任意兩點之間最短路徑的一種經典算法。通過柵格地圖規劃出來的路徑存在較多的轉折點、搜索節點只能在柵格中心位置，導致機器人路徑平滑度較差。本研究將Floyd 算法與A*算法相融合可以有效減少移動機器人路徑的轉折，提高機器人運動效率，有利于移動機器人的應用需求?？紤]運動學模型判斷兩個路徑點之間的距離、時間和轉角是否比原來的短，算法優化路徑對比如圖6、圖7所示。

圖6 Floyd算法原理Fig.6 Principle of Floyd algorithm

圖7 Floyd算法優化路徑Fig.7 Floyd algorithm optimization path

如圖6所示，節點A、D之間并沒有可直達的路徑，則在Floyd算法當中視為無窮大，那么有：

且

則，節點A到節點D的路徑可表示為A、B、D。進一步有：

即：

則節點A到節點D的路徑表示為A、C、D。

如圖7所示，采用標準A*算法的規劃路徑為(S、S1、S2、S3、S4、S5、S6、S7、S8、G)，圖中所示路徑拐點及轉折點較多，導致路徑平滑度較差。

通過融合移動機器人運動特性的Floyd平滑算法剔除路徑冗余節點，從而降低路徑的轉折次數以及優化路徑長度，改善路徑的平滑度。并通過判斷兩個節點之間是否有障礙物，以及兩節點連線到障礙物的距離d和安全距離D的大小，來判斷該路徑是否可行，如圖7 中的路徑為S、S1、S7、G。

2.3 改進算法約束條件

文章改進的路徑規劃算法通過融合環境信息、搜索節點方式、安全距離和運動特性來進行全局路徑規劃，算法中所涉及的主要約束條件以及詳細的數學描述如下：

（1）安全距離

改進A*算法設置路徑節點到障礙物的安全距離，防止移動機器人和障礙物發生碰撞。障礙物到路徑的垂直距離OE（點O到線段KG的垂直距離）與預先設置的安全距離進行比較來判斷所規劃的路徑是否安全可行，安全距離如圖8所示。假設節點K的坐標為(x1,y1)，終止節點G的坐標為(x2,y2)，障礙物節點O坐標為(x3,y3)；節點F的坐標為(x4,y4)線段OF為障礙物到線段KG沿縱軸方向的距離，記作l；KG之間連線與x軸之間的夾角為α，具體原理圖如下：

圖8 安全距離設計Fig.8 Safety distance design

其中，l為線段OF為障礙物節點到線段KG沿縱軸方向的距離，α為線段OE與線段OF之間的夾角，d為障礙物到路徑的距離。

通過式（13）計算得障礙物到路徑的距離d，設置安全距離為D。比較d與安全距離D的大小確定路徑是否可作為備選路徑。具體判斷方式如下所示：

若d≤D，那么放棄該路徑；

若d>D，那么選擇該路徑。

（2）機器人運動特性

首先設置移動機器人在移動過程中只能進行前后運動，當移動機器人路徑角度發生改變時，調整移動機器人的姿態，然后按照已經規劃好的路徑移動到終點。

如圖6 所示優化路徑起點A(xa,ya)，節點B(xb,yb)和節點C(xc,yc)，移動機器人旋轉角度計算方法為：

其中

β表示為機器人前一運動狀態。并且移動機器人從A點到C點之間的路徑為：改進A*算法主要融合上述約束條件對路徑進行全局路徑規劃。

3 實驗和分析

3.1 仿真環境

仿真實驗主要環境為Intel?CoreTMi5-8400 CPU@2.80 GHz，Windows10。通過建立柵格地圖來驗證此次改進的A*算法的有效性以及改進A*算法對于復雜地圖的適應性，同時將改進A*算法與標準A*算法、Dijkstra 算法以及蟻群算法進行對比，檢驗改進A*算法的優越性。仿真實驗所用的柵格地圖為4 個尺寸為50×50，障礙物權重占比不同的柵格地圖。地圖中的單位柵格邊長為1 m，柵格地圖中空白區域為可通行區域，黑色區域為不可通行區域，仿真起點為地圖左下角頂點，終點為地圖右上角頂點。安全距離D設為0.8 m。

3.2 仿真結果與分析

改進A*算法的約束條件可分別計算路徑長度和路徑轉角，如式（13）、（14）所示；搜索節點數通過節點選取規則加入Open 列表中節點個數可知，通過上述評價指標對仿真實驗進行評價。通過50×50 的柵格地圖對蟻群算法、標準A*算法、Dijkstra 算法以及改進A*算法在Matlab中進行測試，測試結果如圖9～12所示。

圖9 地圖1仿真結果Fig.9 Simulation results of map 1

上述四個柵格地圖障礙物權重占比逐漸增大，環境地圖越來越復雜，地圖主要包含了排列整齊的障礙物以及排列混亂的障礙物，所利用的環境地圖有較好的代表性。仿真實驗通過在Matlab中對比改進A*算法與標準A*算法、Dijkstra 算法以及蟻群算法得到路徑規劃的時間、轉折次數、轉角度數、搜索節點個數以及路徑長度等，如表1所示。

表1 不同地圖下幾種算法對比實驗Table 1 Comparison tests of several algorithms under different maps

圖10 地圖2仿真結果Fig.10 Simulation results of map 2

圖11 地圖3仿真結果Fig.11 Simulation results of map 3

圖12 地圖4仿真結果Fig.12 Simulation results of map 4

由圖9～12 所示的仿真結果可知，在所使用的四種環境地圖中，地圖復雜度由易到難，障礙物所占比重逐漸增加。改進A*算法和其余三種對比算法都可以規劃出路徑，但是改進算法和其余路徑規劃算法在轉折點、規劃時間、路徑角度、搜索節點以及路徑長度上有較大的不同。在不同環境地圖中，改進A*算法規劃出來的路徑更有利于機器人運動，改進算法主要在機器人運動特性的基礎上通過Floyd算法進行處理路徑，Floyd算法是解決地圖中任意兩點之間最短路徑的一種經典算法，在兩點之間無障礙物時，刪除冗余點進行規劃路徑，所以改進A*算法規劃路徑與搜索節點并不能一一對應，在路徑長度以及路徑彎曲程度有很大的改進。

從表中數據可以看出：標準A*算法是經典的啟發式算法，規劃出來的路徑通往目標點所在的方向，并沒有考慮障礙物的影響，而是選取了局部最短路徑；蟻群算法規劃出來的路徑由于信息素的影響，路徑轉角過大，并且路徑規劃時間長，算法在1 500 代左右達到收斂，并沒有找到最優路徑；Dijkstra 算法簡單，能夠得到最優解，但是其搜索節點過多，效率比較低，占用過多內存，路徑搜索時間長等缺點導致它并不適合復雜地圖；在復雜地圖當中改進A*算法相比其他算法有較大的優勢，相比于以上四種算法，此次改進的算法路徑轉折點更少，規劃出來的路徑轉角降低了82%，更適合機器人運動；并且始終與障礙物保持一定的距離，避免移動機器人與障礙物發生碰撞；路徑規劃時間平均減少了80%；路徑長度在不同地圖中相比其他算法都有很大的改進。上述分析可知，改進后的A*算法在移動機器人路徑規劃當中會起到更好的效用。

4 結束語

基于運動特性的改進A*算法，針對標準A*算法在路徑規劃過程中拐點較多、轉角大不利于機器人運動、路徑規劃時間長以及搜索路線緊貼障礙物邊緣等缺點，導致移動機器人容易發生不安全動作，不利于機器人的行進，提出一種改進A*算法。主要結論如下：

（1）通過環境地圖中障礙物權重系數對代價函數進行改進，算法搜索節點更具有方向性，提高了算法的效率，也保證了算法的實時性。

（2）在路徑優化過程中考慮機器人運動特性和安全距離等約束條件，保證移動機器人運動安全性。

（3）實驗結果表明，改進后的A*算法在路徑規劃中始終與障礙物保持一定的距離，搜索的節點和路徑規劃時間減少；融合運動特性的路徑規劃更為平滑，路徑轉角更小，更有利于移動機器人的運動。