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

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

（安徽工業(yè)大學(xué) 機械工程學(xué)院，馬鞍山 243032）

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

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

1 A*算法基本理論

A*算法通過啟發(fā)信息引導(dǎo)算法搜索下一待擴展節(jié)點，其估價函數(shù)為：

式中，g(n)為當(dāng)前節(jié)點n與起始節(jié)點之間的移動代價；h(n)為當(dāng)前節(jié)點n與目標(biāo)節(jié)點之間的移動代價，即啟發(fā)函數(shù)。

設(shè)機器人運行環(huán)境的目標(biāo)節(jié)點為G，坐標(biāo)為，則啟發(fā)函數(shù)h(n)：

2 擴大搜索鄰域A*算法

標(biāo)準(zhǔn)A*算法搜索下一待擴展節(jié)點采用8領(lǐng)域算法，如圖1所示。每個搜索方向之間夾角均為0.25π，節(jié)點轉(zhuǎn)折角為0.25π的整數(shù)倍，為此標(biāo)準(zhǔn)A*算法的規(guī)劃路徑距離不是最短，且規(guī)劃路徑上因轉(zhuǎn)折點較多不夠平滑。

圖1 8鄰域搜索示意圖

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

設(shè)R表示由當(dāng)前節(jié)點n搜索下一節(jié)點時待擴展的節(jié)點層數(shù)，NR表示待搜索鄰域節(jié)點個數(shù)，SR表示隨節(jié)點可移動方向個數(shù)，則：

式中，R≥1,R∈Z。

圖2 24鄰域搜索示意圖

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

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

表1 搜索層數(shù)與待搜索鄰域個數(shù)和可移動方向的對應(yīng)關(guān)系

3 最小二叉堆優(yōu)化A*算法OPEN列表

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

圖4 最小二叉堆及其數(shù)據(jù)存儲

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

4 A*改進(jìn)算法的路徑規(guī)劃

4.1 環(huán)境建模

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

圖5 實驗室環(huán)境建模地圖

4.2 仿真實驗

本文算法的代碼編寫和仿真均在Windows8 64位系統(tǒng)Matlab2013b軟件中實現(xiàn)。

（1）標(biāo)準(zhǔn)A*算法、24鄰域A*算法和48鄰域A*算法對圖5環(huán)境的路徑規(guī)劃仿真實驗結(jié)果分別如圖6、圖7和圖8所示，每組重復(fù)仿真10次，記錄每次路徑規(guī)劃的時間ti和規(guī)劃路徑包括的節(jié)點個數(shù)及其坐標(biāo)，并計算路徑長度。

圖6 標(biāo)準(zhǔn)A＊算法

圖7 24鄰域

圖8 48鄰域

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

本文提出的算法規(guī)劃路徑節(jié)點個數(shù)、長度和時間代價如表2所示。

圖9 二叉堆標(biāo)準(zhǔn)A＊算法

圖10 二叉堆24鄰域

圖11 二叉堆48鄰域

表2 算法性能對比

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

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

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

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

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

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

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

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

由公式（4）可知當(dāng)k=3時，三次均勻B樣條曲線的數(shù)學(xué)表達(dá)式為：

三次均勻B樣條曲線的基函數(shù)數(shù)學(xué)表達(dá)式為：

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

5.2 仿真實驗

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

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

圖13 節(jié)點（19，10）和（18，9）處區(qū)域放大

6 結(jié)論

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