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

朱玉華，王飛

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

0 引言

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

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

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

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

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

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

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

(xn,yn)表示當(dāng)前節(jié)點(diǎn)的坐標(biāo)，(xend,yend)表示最終目標(biāo)節(jié)點(diǎn)的坐標(biāo)。

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

（1）從起點(diǎn)S 開(kāi)始，將其放入到OPEN 中。

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

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

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

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

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

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

2 改進(jìn)A＊算法

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

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

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

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

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

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

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

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

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

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

■2.3 路徑平滑

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

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

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

3 算法融合

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

動(dòng)態(tài)窗口法是在二維速度空間中實(shí)時(shí)對(duì)運(yùn)動(dòng)的機(jī)器人

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

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

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

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

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

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

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

4 實(shí)驗(yàn)及仿真分析

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

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

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

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

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

■4.2 融合算法仿真實(shí)驗(yàn)

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

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

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

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

圖5 運(yùn)行效果對(duì)比

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

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

圖6 避障仿真實(shí)驗(yàn)

5 結(jié)束語(yǔ)

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