融合改進A＊與動態窗口算法的室內機器人路徑規劃

朱玉華，王飛

（沈陽工業大學 化工過程自動化學院，遼寧遼陽，111003）

0 引言

隨著機器人技術的日益成熟，移動機器人智能化和自動化的水平逐步提高[1]，它們被廣泛用于生產和生活之中。由于機器人工作環境的復雜性，如何快速無碰撞地規劃出一條從起點到終點的路線成為當前移動機器人的關鍵技術之一，因此機器人路徑規劃算法已成為國內外研究的熱點[2～3]。經典的全局規劃算法主要有Dijkstra 算法[4]、蟻群算法[5～6]、概率路圖算法[7～8]、A＊[9]等。其中A＊算法是一種以柵格地圖為基礎的路徑規劃算法，A＊由于采用了較為單一的啟發式搜索算法，使得在較大的場景地圖中，會搜索一些不必要的節點且存在較多的拐角，因此提高搜索效率減少拐點、減少機器人導航到目標點路徑距離一直是工程應用中改進的方向。

目前很多基于改進A＊的算法被提出。文獻[10]、[11]通過擴展鄰域使搜索效率提高，但是有些場景地圖中會出現穿越地圖中障礙物的情況。文獻[12]將父節點加入到評價函數中加快了搜索效率，但是沒有考慮引入父節點失去平衡問題。文獻[13]通過引入父節點的影響力提高了搜索的速度，引入轉彎懲罰函數和鄰域擴展使拐點減少，但是最終規劃處理的路線沒有進行平滑處理仍然存在較尖銳的拐角。文獻[14]利用動態加權并修改搜索領域的方法提高搜索速度但是未進行拐點去除后就直接進行路徑平滑導致平滑曲線較為復雜。

綜合上述文獻為了減少傳統A＊算法運行時間、拐點數量和規劃路徑的距離，本文通過探究啟發式函數、擴展領域對算法的影響，在保留傳統A＊算法優勢的基礎上重新對啟發式函數進行設計，使改進的算法在路徑規劃上具有較高的運行效率，在最優路徑的基礎上采用拐點去除策略使其平滑度也相對較好，采用動態窗口法[15]完成實時躲避環境中的障礙物，最后進行仿真測試并將該算法部署到實際機器人中進行驗證。

1 傳統A＊路徑規劃算法

傳統的A＊算法綜合考慮Dijkstra 算法和最佳優先搜索算法的優缺點并將兩者結合起來。開始節點到任意節點n的代價g(n)與從節點n到目標節點的啟發式評估代價h(n)進行相加以評價當前節點，其評價函數公式如下：

n表示當前進行評價的節點；g(n)表示從起始節點移動到當前節點的距離；h(n)表示當前節點到目標節點的直接距離也可以稱為預估代價，其中預估代價對A＊算法的搜索效率起著關鍵性的作用。如果h(n)的權重占比很小以至于忽略，此時就變成了Dijkstra 算法，此時算法搜索效率較低。若g(n)占比權重較小則此時就變成了最佳優先搜索算法，路徑搜索速度變快但是會導致計算出現局部最優解。

本文選擇的預估代價為歐氏距離。其計算公式如下：

(xn,yn)表示當前節點的坐標，(xend,yend)表示最終目標節點的坐標。

室內機器人路徑規劃一般采用柵格地圖，將A＊引入兩個表用來保存擴展的節點和最優的節點，記為OPEN 和CLOSE。主要路徑規劃過程如下：

（1）從起點S 開始，將其放入到OPEN 中。

（2）尋找S 上下左右、左上、右上、左下、右下的8個方格，并將其它們放入到OPEN 中，設置它們的父節點為S。

（3）將S 從OPEN 中刪除并將其加入到CLOSE 中。

（4）計算周圍方格的評價函數值，OPEN 選擇評價函數最小的節點a將其納入到CLOSE 后從OPEN 刪除。

（5）檢查a周圍8 個節點；障礙物以及CLOSE 中的節點不予考慮，計算周圍節點評價函數值，并設置父節點為a,若相鄰的節點b已經在OPEN 中，檢查比較S到b與經過a到b的g(n)，若經過a的低則更新父節點為a反之則不變。

（6）執行步驟4-5，找出周圍可以到達的節點，如此循環。

（7）當OPEN 中出現目標節點E 時，則路徑已經找到，當OPEN 中沒有數據的時候則沒有合適的路徑。

2 改進A＊算法

■2.1 障礙物占比啟發函數

針對傳統算法搜索效率較低問題，本文對啟發式評估函數進行改進，在原式的基礎上加入當前節點的父節點對擴展節點的影響，如下式所示：

其中：w表示權重函數，取值范圍w∈[0,1]。引入父節點之后會導致原本g(n) 與h(n)不平衡，在縮短時間的同時出現局部最優解，導致搜索路徑較長。考慮到當地圖中障礙物較多時也會出現局部最優解，因此首先通過實驗進一步選取合適的權重取值范圍，同時構建相對合理的障礙物占比函數以適應不同的障礙物占比場景，針對障礙物占比啟發函數的實驗分析將在4.1 節詳細闡述。

■2.2 拐點優化策略

由于傳統的A＊算法一般只搜索當前節點的8 鄰域或者4 鄰域，使得機器人只能沿著8 或者4 個方向移動，所以導致規劃的路徑較為曲折，文獻[10]擴展A＊算法的搜索鄰域為32 鄰域，文獻[11]將鄰域擴展為16 鄰域上述改進方法能夠使機器人能夠沿著更多的方向運行，使路徑看起來相對順滑，將A＊搜索鄰域分別擴展16 鄰域和32 鄰域，如圖1所示。

圖1 擴展鄰域路徑規劃效果

由上圖知文獻[10][11]規劃出的全局路徑會跨過障物，這是由于擴展鄰域導致當前節點搜索的范圍變大，有些障礙物被包括進來，當前節點搜索下一個最優節點可能在障礙物后面，故搜索的路徑可能會出現穿越障礙物的情況。為了避免上述現象的發生且不降低節點搜索效率，本文依舊采用傳統的8 鄰域搜索算法來進行尋路，然后對路徑進行拐點優化以消除多余拐點，進一步縮短規劃路徑的距離。

先找出路徑中的拐點，選取拐點的方法：將起始記為p1點，然后找到起始點后面的兩個點分別記為p2和p3，計算p1和p2的坐標差值分別記為dx21和dy21，計算p3和p2的坐標差值記為dx32和dy32，分別比較坐標差值是否相等，若不相等則將p2點記錄到拐點列表inflection_list 中，然后再從p2依次順序取相鄰的三個節點按照上述算法計算，直到路徑終點為止，再將起點和終點分別加入到inflection_list 的首位和末位中。

從inflection_list 中依次取出三個相鄰的拐點，按照取出的先后分別記為i1、i2、i3，利用i1、i3這兩點構造一條直線I 并利用點到直線的距離如公式(4)所示，計算障礙物的到直線的距離，其中 (x0,y0)表示障礙物的坐標。

障礙物選取的標準為該障礙物是否處于i1、i3所形成的矩形內。這里可以設置距離閾值以增強拐點去除的靈活性，當距離d大于我們設定的閾值或矩形內沒有障礙物的時候，則i2剔除，當d小于閾值的時候則保留i2。然后再順序取連續的三個拐點i2、i3、i4進行計算直到取到最后一個拐點為止，這里循環設置的大小為初始拐點個數減去2。

■2.3 路徑平滑

室內機器人一般采用的大多為差分運動模型，該模型可以直線運動、弧線運動。在拐點優化之后，依舊有一定數量的拐角，為了為局部規劃路徑提供相對平滑的目標點序列，對去除拐點后的路徑使用貝塞爾曲線進行平滑，使機器人大致沿著曲線運行。貝塞爾曲線具有如下特性：使用n個控制節點 {P1,P2,...,Pn}來控制曲線的形狀；曲線經過起點P1和終點Pn，但是不經過中間點P2到Pn-1。考慮到要在拐角處進行平滑且要保證規劃線路的連續性，三階貝塞爾曲線更符合我們的需求，其具體表達式如下所示：

其中P1，P2，P3，P4表示四個控制節點，t為參數其范圍為0 到1。

獲取四個控制點的方法為從路徑起點s1取第一組順序拐點s1、s2、s3，根據s2與s1、s3與s2之間的橫坐標差值進行拓展采點，并設置采樣步長stride為可調步長。選取采樣點的規則如下：當s2與s1的橫坐標差值大于零則取構建貝塞爾曲線的控制點坐標為 (xs1+stride,P1y)其中xs1為s1的橫坐標P1y為s1、s2構成直線xs1+stride處的縱坐標；類似地，當s2與s1的橫坐標小于零則取坐標為 (xs1+stride,P1y)，當s2與s1的橫坐標相等的時候則點為 (xs1,P1y+stride)，依此規則再計算s3與s2新的采樣點P2，我們根據s1、s2、P1、P2繪制貝塞爾曲線，下一次取點的時候則以P2為起點順序選取兩個拐點并按照上述過程進行計算。

3 算法融合

■3.1 動態窗口算法

動態窗口法是在二維速度空間中實時對運動的機器人

■3.2 算法融合的路徑規劃

為了讓室內機器人在規劃處最優全局路徑的同時實現安全避障，將上述兩種算法進行融合。改進A＊算法提供一條相對最優的全局路徑，DWA 算法根據全局路徑提供的目標點評估出機器人運動的最優速度，二者融合的實現思路簡述如下：

（a）利用建圖算法繪制室內環境柵格地圖，加載環境地圖并給予機器人正確的初始化位姿。

（b）設置導航的目標點，執行拐點優化改進A＊算法，規劃出一條全局最優路徑,提取路徑中的關鍵節點。

（c）將全局路徑中的關鍵點作為DWA 的目標點，DWA 算法選擇最優軌跡對應的速度和角速度，并實時避障。

（d）判斷是否到達目標點且是否為導航終點？

case1：若節點為目標節點但不為終點，則跳到（c）更換下一個關鍵節點。case2：若節點為終點則結束循環。進行速度采樣，然后利用機器人運動學模型預測機器人在采樣速度組下相對一段時間內的軌跡，根據評價函數選擇相對最優路徑下的速度，發送給機器人執行機構。

4 實驗及仿真分析

■4.1 障礙物占比函數分析實驗

本文仿真運行環境為Ubuntu20.04，采Python 版本為3.8.10，選擇地圖的尺寸大小為51×40。針對權重對A＊算法的影響進行五次仿真實驗，統計權重對搜索算法耗時以及路徑規劃距離的影響，如圖3 所示。

圖3 權重對耗時、距離的影響

通過實驗發現當w的取值逐漸增大的時候路徑距離不斷增大然后減少，搜索耗時在驟降后逐漸減少趨于不變。綜合考慮到搜索效率和路徑代價選擇w的取值范圍為0.5～0.6，設計障礙物占比啟發函數如式(6)所示：

其中障礙物占比p為起點和終點形成矩形內的障礙物面積與矩形總面積的比值。

■4.2 融合算法仿真實驗

本文融合算法仿真實驗采用開源機器人仿真平臺Gazebo。搭建20m×20m 的室內仿真環境，在室內布置若干靜態物體，仿真機器人為turtlebot3_waffle，利用開源建gmapping 算法建立仿真環境地圖。機器人的起點設置為開始建圖的坐標原點，在仿真環境中分別不設置障礙物和設置障礙物分別進行算法驗證。仿真環境及環境地圖如圖4 所示。

圖4 仿真環境及地圖

選取終點坐標為(8,-9)方向角(0,0,0)，分別運行傳統A＊與拐點優化改進A＊算法。

為了驗證本文改進效果，截取傳統A＊和拐點優化改進算法進行對比如圖5 所示，其中圖5(a)中實線表示傳統A＊規劃的路線，圖5(b)中實線表示本文拐點優化改進A＊規劃處的路線，由圖5 知傳統A＊算法在規劃路徑后存在較多的拐點，利用本文拐點濾除方法可以有效濾除拐點使規劃出的路徑相對平滑，利用三階貝塞爾曲線在一定程度上可以進一步優化拐點。

圖5 運行效果對比

由表1 知傳統A＊運行平均耗時79.27s，平均運行的里程為14.98m，本文拐點優化改進A＊運行平均耗時為74.41s，平均運行的里程為14.35m，其中運行里程在本場景下減少4.19%，時間減少6.13%。

進一步驗證融合算法的避障能力，在行駛到終點坐標為(5,5)方向角(0,0,0)的路徑上加上五個仿真箱子。啟動程序發送導航終點，本文改進算法規劃出全局路徑，當檢測到障礙物遮擋全局路徑中的路徑節點時則重新進行全局路徑規劃，當全局路徑沒有被遮擋時則DWA 進行局部路徑規劃直到行駛到終點為止，規劃出全局路徑如圖6 所示，其中附著在地圖邊上的點表示仿真中的激光點云，其中實線表示全局路徑。

圖6 避障仿真實驗

5 結束語

本文提出改進的A＊算法，在評價函數中引入當前節點的父節點，為了更好地平衡評價函數且考慮到地圖中障礙物對算法的影響，構造障礙物占比啟發函數，考慮到可能穿越障礙物的情況采用了傳統的8 鄰域搜索。由于路徑規劃會出現較多的冗余拐點，本文提出拐點優化策略，在拐點優化完畢之后再利用三階貝塞爾曲線進一步對路徑做平滑。用Gazebo 和python 對相關的設計進行仿真，根據仿真結果知本文改進A＊算法搜索效率、路徑平滑度上有一定的提升。后續將進一步深入研究并優化DWA 算法進一步提升其效率。