基于擴大搜索鄰域A＊算法的平滑路徑規劃

張敬寒，陶兆勝，彭澎，王麗華

（安徽工業大學 機械工程學院，馬鞍山 243032）

路徑規劃[1]作為機器人的研究方向之一，一直是學者研究的熱點。柵格法[2]將機器人運行環境特征信息轉換為具有二值信息的單元網格存儲，因其簡便、易處理的特點在環境建模中具有顯著優勢。為此，學者基于柵格法研究了如 A*[3]、D*[4]和D*Lite[5]等算法，其中啟發式搜索[6]A*算法因其簡單高效、可操作性強和準確性高的特點而被廣泛應用于解決靜態全局路徑規劃問題。但A*算法受節點搜索策略影響，存在規劃路徑不是理論上的最優路徑、路徑拐點過多[7，8]，且當環境信息過于復雜時算法搜索效率降低的缺陷。

本文提出一種基于改進A*算法的平滑路徑規劃方法。首先，針對A*算法規劃路徑長度不是最優和路徑拐點較多的不足，提出一種擴大算法搜索鄰域的改進方法，消除了節點移動方向0.25π整數倍和8鄰域搜索的限制；其次為提高算法尋路效率，利用最小二叉堆優化A*算法OPEN列表數據存儲結構；最后采用三次均勻B樣條曲線平滑處理，綜合改進A*算法規劃路徑，以滿足機器人運動時對速度和加速度連續性的實時要求。

1 A*算法基本理論

A*算法通過啟發信息引導算法搜索下一待擴展節點，其估價函數為：

式中，g(n)為當前節點n與起始節點之間的移動代價；h(n)為當前節點n與目標節點之間的移動代價，即啟發函數。

設機器人運行環境的目標節點為G，坐標為，則啟發函數h(n)：

2 擴大搜索鄰域A*算法

標準A*算法搜索下一待擴展節點采用8領域算法，如圖1所示。每個搜索方向之間夾角均為0.25π，節點轉折角為0.25π的整數倍，為此標準A*算法的規劃路徑距離不是最短，且規劃路徑上因轉折點較多不夠平滑。

圖1 8鄰域搜索示意圖

為消除8鄰域搜索缺陷，本文提出擴大A*算法當前節點搜索鄰域范圍的改進算法。以圖1中7×7無障礙物柵格地圖為例，中心節點為當前節點n，其周圍第一層8個節點為標準A*算法待擴展節點。本文算法除了采用8領域搜索算法外，還將當前節點n周圍第二層16個節點（如圖2所示）納入算法下一步待擴展節點，此時算法待搜索鄰域節點數為24個，節點移動方向為16個方向。以此類推，A*算法的可擴展搜索鄰域節點和可移動方向個數隨著搜索層數的擴展一直增加，如圖3所示。

設R表示由當前節點n搜索下一節點時待擴展的節點層數，NR表示待搜索鄰域節點個數，SR表示隨節點可移動方向個數，則：

式中，R≥1,R∈Z。

圖2 24鄰域搜索示意圖

圖3 48鄰域搜索鄰域示意圖

由此得到與R取值對應的不同NR和SR值，如表1所示。

表1 搜索層數與待搜索鄰域個數和可移動方向的對應關系

3 最小二叉堆優化A*算法OPEN列表

本文提出的擴大搜索鄰域算法導致OPEN列表中待搜索節點數量增多，數據存儲標記更加復雜，嚴重影響算法效率。為此利用最小二叉堆優化OPEN列表節點存儲方式。圖4所示為典型的最小二叉堆：鍵值最小點在堆的頂端，并存儲在一維數組的首位，其子節點的鍵值均高于該節點，分別存儲在數組的2和3位置，以此類推。

圖4 最小二叉堆及其數據存儲

最小二叉堆的堆序特性保證OPEN列表節點排列的穩定性，算法搜索OPEN列表中估價函數f(n)值最小節點的時間復雜度為O(1)[9-10]。

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

4.1 環境建模

本文采用直角坐標法標記柵格環境信息，以每個柵格的中心點坐標表示整個柵格信息。設置柵格粒度值為1，坐標系原點坐標（0.5，0.5），即所有柵格中心點橫、縱坐標均為整數。圖5所示的30×30大小仿真實驗地圖以課題組實驗室環境為模型建立。

圖5 實驗室環境建模地圖

4.2 仿真實驗

本文算法的代碼編寫和仿真均在Windows8 64位系統Matlab2013b軟件中實現。

（1）標準A*算法、24鄰域A*算法和48鄰域A*算法對圖5環境的路徑規劃仿真實驗結果分別如圖6、圖7和圖8所示，每組重復仿真10次，記錄每次路徑規劃的時間ti和規劃路徑包括的節點個數及其坐標，并計算路徑長度。

圖6 標準A＊算法

圖7 24鄰域

圖8 48鄰域

（2）最小二叉堆優化OPEN列表后的8鄰域、24鄰域和48鄰域A*算法對圖5環境的路徑規劃仿真實驗結果分別如圖9、圖10和圖11所示，每組重復仿真10次，記錄每次路徑規劃的時間ti和規劃路徑包括的節點個數及其坐標，并計算路徑長度。

本文提出的算法規劃路徑節點個數、長度和時間代價如表2所示。

圖9 二叉堆標準A＊算法

圖10 二叉堆24鄰域

圖11 二叉堆48鄰域

表2 算法性能對比

通過表2和圖6、圖7、圖8可知24鄰域和48鄰域的改進A*算法相對于標準A*算法：（1）規劃路徑上節點個數依次降低，分別為40、30和22；規劃路徑距離即移動代價逐漸減小，路徑漸為平滑；（2）路徑規劃效率不斷降低，時間代價依次增加了0.34373s和0.94416s，算法效率降低。

通過表2和圖6與圖9、圖7與圖10、圖8與圖11可知：（1）最小二叉堆不影響A*算法的節點選擇策略，優化OPEN列表前后算法規劃路徑完全一致；（2）最小二叉堆優化OPEN列表使得8鄰域、24鄰域和48鄰域A*算法路徑規劃并的平均時間分別降低了47.86%、49.84%和45.97%，有效改善因搜索鄰域擴大導致算法路徑規劃時間增加的不足，算法效率顯著提高，提高了路徑規劃實時性。

5 三次均勻B樣條曲線平滑路徑

本文提出的擴大搜索鄰域A*算法雖然在一定程度上平滑了路徑，但依然存在部分路徑區域轉折角度過大，路徑尖峰。機器人實際移動時突然轉向加減速會損害伺服電機和齒輪，并不易追蹤規劃路徑[11]。而具有分段多項式形式的B樣條曲線因其參數及表達式簡單、凸包性及局部修改性等優點而廣泛應用于路徑平滑處理[12]。其中三次B樣條曲線在節點矢量處二階連續[13-14]滿足對于機器人速度和加速度連續性的要求，故本文采用三次均勻B樣條曲線平滑綜合改進上述本文算法已規劃的路徑。

5.1 三次均勻B樣條曲線方程

設n+1個頂點Pi(i=0,1,…,n)構成的特征多邊形，則k次（k+1階）的B樣條曲線表達式[15]：

令Ni,k(u)稱為基函數，則：

式中，0≤u≤1,i=0,1,…,k-1

由公式（4）可知當k=3時，三次均勻B樣條曲線的數學表達式為：

三次均勻B樣條曲線的基函數數學表達式為：

將公式（8）上式帶回公式（4）得三次均勻B樣條曲線：

5.2 仿真實驗

三次均勻B樣條曲線平滑綜合改進24鄰域A*算法規劃路徑（圖7）的仿真結果如圖12所示，放大節點（19，10）和（18，9）處區域如圖12所示，因篇幅關系其余區域放大圖并未貼出，通過對比可知三次均勻B樣條曲線消除了原規劃路徑上節點（19，10）、（18，9）、（13，10）、（12，9）、（11，7）、（11，5）、（11，4）處的路徑尖峰點，平滑后的路徑由平滑連續的曲線組成，有利于機器人移動時保持速度與加速度的連續性。

圖12 三次均勻B樣條曲線路徑平滑

圖13 節點（19，10）和（18，9）處區域放大

6 結論

本文提出的基于擴大鄰域和最小二叉堆優化A*算法OPEN列表存儲結構的算法相對傳統A*算法，縮短路徑長度、減少路徑拐點，提高算法路徑規劃效率；其次，三次均勻B樣條曲線對本文算法所規劃路徑的平滑處理有效消除了原規劃路徑上的路徑尖峰拐點。