融合改進A＊與動態(tài)窗口算法的室內(nèi)機器人路徑規(guī)劃

朱玉華，王飛

（沈陽工業(yè)大學(xué) 化工過程自動化學(xué)院，遼寧遼陽，111003）

0 引言

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

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

綜合上述文獻為了減少傳統(tǒng)A＊算法運行時間、拐點數(shù)量和規(guī)劃路徑的距離，本文通過探究啟發(fā)式函數(shù)、擴展領(lǐng)域?qū)λ惴ǖ挠绊懀诒Ａ魝鹘y(tǒng)A＊算法優(yōu)勢的基礎(chǔ)上重新對啟發(fā)式函數(shù)進行設(shè)計，使改進的算法在路徑規(guī)劃上具有較高的運行效率，在最優(yōu)路徑的基礎(chǔ)上采用拐點去除策略使其平滑度也相對較好，采用動態(tài)窗口法[15]完成實時躲避環(huán)境中的障礙物，最后進行仿真測試并將該算法部署到實際機器人中進行驗證。

1 傳統(tǒng)A＊路徑規(guī)劃算法

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

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

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

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

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

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

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

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

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

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

（6）執(zhí)行步驟4-5，找出周圍可以到達的節(jié)點，如此循環(huán)。

（7）當OPEN 中出現(xiàn)目標節(jié)點E 時，則路徑已經(jīng)找到，當OPEN 中沒有數(shù)據(jù)的時候則沒有合適的路徑。

2 改進A＊算法

■2.1 障礙物占比啟發(fā)函數(shù)

針對傳統(tǒng)算法搜索效率較低問題，本文對啟發(fā)式評估函數(shù)進行改進，在原式的基礎(chǔ)上加入當前節(jié)點的父節(jié)點對擴展節(jié)點的影響，如下式所示：

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

■2.2 拐點優(yōu)化策略

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

圖1 擴展鄰域路徑規(guī)劃效果

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

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

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

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

■2.3 路徑平滑

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

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

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

3 算法融合

■3.1 動態(tài)窗口算法

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

■3.2 算法融合的路徑規(guī)劃

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

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

（b）設(shè)置導(dǎo)航的目標點，執(zhí)行拐點優(yōu)化改進A＊算法，規(guī)劃出一條全局最優(yōu)路徑,提取路徑中的關(guān)鍵節(jié)點。

（c）將全局路徑中的關(guān)鍵點作為DWA 的目標點，DWA 算法選擇最優(yōu)軌跡對應(yīng)的速度和角速度，并實時避障。

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

case1：若節(jié)點為目標節(jié)點但不為終點，則跳到（c）更換下一個關(guān)鍵節(jié)點。case2：若節(jié)點為終點則結(jié)束循環(huán)。進行速度采樣，然后利用機器人運動學(xué)模型預(yù)測機器人在采樣速度組下相對一段時間內(nèi)的軌跡，根據(jù)評價函數(shù)選擇相對最優(yōu)路徑下的速度，發(fā)送給機器人執(zhí)行機構(gòu)。

4 實驗及仿真分析

■4.1 障礙物占比函數(shù)分析實驗

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

圖3 權(quán)重對耗時、距離的影響

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

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

■4.2 融合算法仿真實驗

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

圖4 仿真環(huán)境及地圖

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

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

圖5 運行效果對比

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

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

圖6 避障仿真實驗

5 結(jié)束語

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