基于定向搜索A*算法的平滑路徑規劃

李圣達，鄭宇鋒，呂娜，李詩瑤，祁宇峰

(1.大連交通大學 電氣信息工程學院，遼寧 大連 116028；2.大連交通大學 機械工程學院，遼寧 大連 116028；3.大連交通大學 軟件學院，遼寧 大連 116028；4.大連理工大學 化工學院，遼寧 大連 116023)

移動機器人在當今物流行業中具有舉足輕重的地位.為了適應物流行業的高速發展，移動機器人路徑規劃問題對于時效性的需求日益增加，已逐步成為國內外機器人學者研究的熱點問題.路徑規劃指在當前環境下，移動機器人能夠依據先驗地圖信息及實時環境數據，自主規劃出一條從起始點(A點)到目標點(B點)的最優或次優無碰撞路徑[1].當前主流路徑規劃算法包括：蟻群算法、Dijkstra算法、A*算法、遺傳算法及可視圖法等.其中A*算法因原理通俗易懂、搜索方式直接及準確度高等特點，在靜態全局路徑規劃算法中占據了重要地位.然而，傳統A*算法在路徑規劃中，存在冗余路徑節點過多、路徑較長、算法效率低、移動機器人與障礙物之間無安全距離且所得路徑非平滑曲線等問題.為此,任玉潔等[2]提出了一種拓展搜索領域的平滑A*算法，消除了路徑中的部分冗余節點，但是其算法拓展了搜索領域，使得搜尋路徑時間變長；喬云俠等[3]提出了一種背向障礙物的搜索方法，避免了移動機器人貼合障礙物的邊緣前進的可能，但增加了路徑長度、搜尋時間；陳靖輝等[4]提出了一種改進A*算法，消除了對稱路徑，縮短了路徑長度、搜尋時間，但所得路徑平滑效果不明顯.

通過分析上述各方法的優缺點，本文對傳統A*算法進行改進.改進后的A*算法增加了定向搜索策略、關鍵點提取策略以及“拉直”處理策略，不僅使路徑規劃效率得到提升，而且減少了路徑轉折次數并去除了路徑冗余節點，此外引入貝塞爾曲線對所得路徑節點進行路徑平滑優化處理，使移動機器人更加順暢的移動至目標點.

1 傳統A*算法

傳統A*算法是靜態全局路徑規劃算法中的一種經典的啟發式搜索算法[5-6].其中的代價函數可表示為：

f(x)=g(x)+h(x)

(1)

式中:g(x)表示從起始節點(STRAT)至指定節點的(x)實際代價；h(x)表示點(x)至目標點(GOAL)的估計代價；f(x)表示點(x)的優先搜索程度，其值越小代表該點(x)搜索優先級越高.

傳統A*算法中，啟發函數h(x)的計算方法直接決定了A*算法的結果.一般來說，傳統A*算法中有三種啟發函數計算方法：曼哈頓距離、對角線距離及歐氏距離.其中歐式距離是在N維空間中的兩點之間的真實最短距離，精確度最高且不易產生誤差.本文也是應用歐氏距離方法計算啟發函數h(x)：

(2)

2 定向搜索的A*算法

傳統的A*算法采取8鄰域搜索方式[7]進行全局路徑規劃.該方法不但存在搜索鄰域過多、耗時長及計算量大等不足，而且無法避免移動機器人貼合障礙物邊緣前進的不良情況.為解決上述問題,本文提出定向搜索方式改進的A*算法.首先在傳統的A*算法的基礎上增加定向引導啟發函數γs作為搜索最高優先級，隨后加入避障約束條件，明確搜索方向；其次對所得初始路徑節點進行關鍵節點提取操作并進行路徑“拉直”處理；最后通過貝塞爾曲線對經過上述步驟處理后的路徑節點進行平滑路徑優化，得到一條最優平滑路徑.

(1)定向搜索策略

通過當前節點與目標點坐標所組成的直線求出γs值，并選擇合適的方位判定閾值(ε1，ε2)與其進行差值比較，確定搜索方向.其中定向導引啟發函數公式為：

(3)

具體為以下四種情況：

1)γs大于ε1且小于ε2時，優先搜索方向為東北、西南方向；

2)γs大于等于-ε2且小于等于-ε1時，優先搜索東南、西北方向；

3)γs大于-ε1且小于-ε2時，優先搜索東、西方向；

4)γs大于ε2或者小于-ε2或不存在時，優先搜索南、北方向.

再依據起始點與目標點之間的橫坐標或縱坐標的關系和障礙物約束條件進一步明確搜索方向，并確定下一待擴展節點，解決了傳統A*算法盲目搜索和耗時長的問題.定向搜索策略示意圖見圖1.

(a) 障礙物在直線上

利用定向導引函數值確定搜索方向后，若出現圖1(a)所示情況：當障礙物在目標節點與當前節點之間的連線上時，因需要避開障礙物，則終止該方向搜索；若出現圖1(b)所示情況：當障礙物在連線的某一側時，則可以繼續搜索；若出現圖1(c)所示情況：當障礙物在連線的兩側且位于正方向時，可以繼續搜索.待在初步確定的方向搜索完成后，轉而搜索東、南、西、北四個方向，以其中代價函數值最小的節點作為下一待擴展節點.

本文中定向搜索的A*算法不再對當前節點的周圍8鄰域進行搜索，而是依據定向導引啟發函數γs進行定向搜索，有效縮短搜索時間，提高路徑規劃效率.

(2)關鍵節點提取策略

柵格法規劃出的路徑包含許多路徑節點，其中有大量路徑節點為路徑冗余節點，只有少數節點為生成路徑所需的必要節點，如：起始點、目標點及路徑轉折點.針對此問題，本文采用了關鍵節點提取策略.具體策略如下：

若生成的路徑節點集合為L={Qi|i=0,1,2,…，n}，其中Qi表示路徑上的第i個路徑節點.依據三點共線原理判斷Q0、Q1、Q2三個節點是否共線，若三個節點處于同一條直線上，則說明Q1節點為可舍節點，執行去除Q1節點并更新路徑L的操作.隨后繼續判斷Q0、Q1、Q2三個節點是否存在可舍節點，若不存在，則繼續遍歷Q2、Q3、Q4三個節點，直至遍歷所有的路徑節點為止.遍歷完成后，得到僅含有起始節點、路徑轉折點以及目標節點的集合L.

(3)“拉直”處理策略

利用定向搜索策略所得路徑，雖然減少了搜索鄰域、提高了路徑規劃效率，但可能存在鋸齒效應，產生“Z”形路徑，且該現象不利于移動機器人的運動.因此將最初得到的路徑進行“拉直”處理，“拉直”處理策略示意圖見圖2.具體流程如下:

(a) 起始點與目標點之間無障礙情況示意圖

經過關鍵節點提取策略優化后的路徑節點集合為S={Qi|i=0,1,2,…,n}，確定起始點Q0與目標節點Qn所在直線方程，遍歷該線段所經過的節點坐標，判斷兩者之間是否存在障礙物.若無障礙物，則刪除兩者之間的其他路徑點，并將Q0和Qn兩點保留至新的路徑，同時退出程序，見圖2 (a)；否則連接Q0，Qn-1，若兩者之間的連線滿足無障礙物條件，則保留Q0，Qn-1，Qn三個路徑節點作為最終路徑節點集合；否則連接Q0，Qn-2，以此類推，直至找到與起始點之間的連線滿足條件的Qi節點.隨后將Qi節點作為新的起始點Q0，重復上述步驟，直至路徑中不存在多余路徑節點，見圖2 (b).采取關鍵點提取策略及“拉直”處理策略后與無處理策略的對比效果,見圖3.

(a)無處理策略 (b)本文處理策略

本文通過定向搜索策略、關鍵節點提取策略及“拉直”處理策略，形成優化后的A*算法.改進后的A*算法流程圖見圖4，具體流程如下：

1)初始化節點信息.

2)若指定節點是目標節點，跳轉至7)，否則繼續執行.

3) 計算出起始節點或指定節點與目標節點之間的定向導引函數值，確定優先搜索方向.

4)依據障礙物的分布情況，選擇待拓展節點.將待指定節點和東、南、西、北四個方向添加到Open_list，同時將障礙物節點添加到Close_list.

圖4 改進后的A*算法流程圖

5)找出最小代價函數f(x)對應的節點作為指定節點，并將該指定節點標記為Close_list中的

元素，記錄其父節點并繼續執行2).

6)路徑關鍵節點提取及“拉直”處理.

7)輸出路徑.

3 貝塞爾曲線路徑優化

本文提出的定向搜索的A*算法雖然減少了搜索路徑所耗時長，避免了移動機器人無限接近障礙物的問題，但其生成的部分路徑仍為折線并且存在尖峰拐點，并不利于移動機器人的轉彎運動.而貝塞爾曲線[8-10]具有端點性質以及凸包性，非常適用于路徑平滑工作，也使其成為在全局路徑優化領域[11-12]中最重要的方法.因此本文中引用貝塞爾曲線對改進后的A*算法所生成的路徑進行平滑處理.

貝塞爾曲線的定義取決于控制點的個數，當有n+1個控制點時，就可以確定n次多項式的貝塞爾曲線參數方程：

(4)

式中:Bi,n(t)為貝塞爾曲線基函數；t為歸一化時間變量；n為n次貝塞爾曲線優化.

由于貝塞爾曲線的導數由控制點所決定，因此貝塞爾曲線的一階導數可以由式(5)、式(6)表示：

(5)

貝塞爾曲線上任意一點的曲率公式為：

圖5 貝塞爾曲線優化流程圖

(6)

式中，x′(t)、y′(t)、x″(t)和y″(t)分別為貝塞爾曲線的坐標x和y方向上的分量對x和y的一階導數和二階導數.貝塞爾曲線優化的具體流程見圖5.

4 基于ROS平臺的仿真實驗

ROS機器人操作系統是針對機器人編程及應用的一種開源操作系統.ROS能夠同時支持多種編程語言來編寫所用的功能包，比如最常見的C++、PYTHON、JAVA等.與此同時，還能支持GAZEBO仿真平臺和計算圖可視化工具、數據繪圖工具、圖像渲染工具及RVIZ三維可視化工具等，為廣大ROS使用者提供方便.

4.1 定向搜索的A*算法仿真實驗

在GAZEBO仿真平臺下，采用RBPF粒子濾波算法生成20×20的柵格地圖，單位為m，其中生成的先驗地圖的分辨率為0.05 m/柵格；同時為保證粒子濾波算法中的粒子多樣性及地圖的精確度，將其粒子數設置為30.在已經生成的先驗地圖的基礎上，分別選取其中無障礙物區間和有障礙物區間進行了路徑規劃的仿真實驗.其中仿真實驗結果見圖6，點劃線為傳統A*算法所生成路徑；直線為改進后A*算法所生成的路徑.

圖6 A*算法改進前后仿真實驗結果對比

為進一步驗證實驗的準確性，在無障礙物區間選取 (31,38)，(99,32) 作為X向無障礙物區間實驗的起始點與目標點；在無障礙物區間選取(64,86)，(92,154)作為Y向無障礙物區間實驗的起始點與目標點；在有障礙物區間選取(98,70)，(123,159)作為一般復雜障礙物區間實驗的起始點與目標點，選取(98,70)，(123,183)作為較復雜障礙物區間實驗的起始點與目標點.每組進行10次重復仿真實驗，并記錄10次實驗中的傳統A*算法、定向搜索的A*算法路徑規劃過程中的拓展節點集合N、路徑節點集合L、路徑轉折點P、搜尋路徑時間T1及計算各路徑所走長度S，并將各項數據的平均值記錄到表1中.

表1 不同算法實驗數據結果對比

從表1、圖6中的數據和仿真結果可以清晰地得出以下結論：a)在無障礙物區間內：雖然改進后的A*算法路徑搜索時間較慢，但減少了移動機器人轉彎次數并且得到較短的路徑；b)在障礙物區間內：較傳統A*算法，改進后A*算法在搜索過程中產生的拓展節點數平均減少了26.90%，路徑轉折點數平均減少了50%，路徑節點數平均減少了96.76%，為改進A*算法的高效完成提供了有力的保障.同時也因路徑轉折點數的減少以及更短的路徑長度，更有利于移動機器人運動.與傳統A*算法相比，應用改進后的A*算法所生成的路徑不再有與障礙物無限接近的位置，能夠保證一定的安全距離，滿足機器人基本避障需求；改進后的A*算法尋路效率與傳統A*算法尋路效率優勢會隨著待搜索路徑的長度以及復雜程度突顯出來，由較復雜障礙物區間實驗數據可知，改進后A*算法尋路時間比傳統A*算法減少了50.32%，大大提升了算法搜索效率.

4.2 貝塞爾曲線平滑路徑仿真實驗

將利用定向搜索的A*算法所得到的路徑，進行貝塞爾曲線平滑處理，平滑路徑結果如圖7所示，灰色線代表改進后A*算法生成的路徑，沒有進行平滑優化；黑色線代表貝塞爾曲線平滑后的路徑.結果顯而易見：定向搜索的A*算法在沒有加入貝塞爾曲線優化之前，所得路徑存在尖峰拐點，不利于移動機器人轉向運動；而加入貝塞爾曲線之后，路徑變得平滑有利于移動機器人快速通過路徑拐角.

圖7 貝塞爾曲線平滑路徑

5 結論

針對傳統A*算法在規劃路徑時存在盲目搜索、算法效率低、規劃路徑轉折點尖銳及路徑與障礙物之間安全距離不足等問題，本文提出了基于定向搜索A*算法的平滑路徑規劃.改進后的A*算法雖然在無障礙物區域進行路徑規劃時耗時略長，但在有障礙物區域的路徑規劃效率的優勢會隨著路徑長度的增加而顯現出來，同時通過增加關鍵點提取策略以及“拉直”處理策略去除了路徑冗余點并減少了路徑轉折次數；此外引入了貝塞爾曲線，明顯使路徑中的尖峰拐點部分變得平滑，能夠更好地滿足移動機器人路徑規劃的實際需求.