融合改進(jìn)A*與DWA算法的移動(dòng)機(jī)器人路徑規(guī)劃

龐永旭,袁德成

(沈陽化工大學(xué)信息工程學(xué)院,遼寧 沈陽 110142)

0 引 言

隨著科技的日益發(fā)展，移動(dòng)機(jī)器人與人們的生活交集日漸增多。路徑規(guī)劃[1]作為當(dāng)前移動(dòng)機(jī)器人研究領(lǐng)域的前沿課題之一，其目的是在復(fù)雜的環(huán)境中躲避障礙，找尋一條能夠?qū)崿F(xiàn)從起始點(diǎn)無碰撞快速到達(dá)目標(biāo)點(diǎn)的最佳路徑。為解決路徑規(guī)劃中出現(xiàn)的問題，該領(lǐng)域內(nèi)的學(xué)者進(jìn)行了大量研究，提出了各種行之有效的算法。其中全局路徑規(guī)劃[2]算法有傳統(tǒng)的A*[3]算法、Dijkstra[4]算法；智能仿生的蟻群算法[5]、粒子群算法[6]、遺傳算法[7]等；局部路徑規(guī)劃算法有動(dòng)態(tài)窗口法[8-9](DWA)和人工勢(shì)場(chǎng)法[10]等。王小紅等人[11]基于A*算法成功優(yōu)化其評(píng)價(jià)函數(shù)，提高了搜索效率，并且通過關(guān)鍵點(diǎn)選取策略去除冗余節(jié)點(diǎn)?；眲?chuàng)鋒等人[12]通過擴(kuò)大傳統(tǒng)的A*算法搜索鄰域，將傳統(tǒng)的3×3鄰域分別擴(kuò)展為5×5鄰域與7×7鄰域，優(yōu)化了搜索角度，顯著提高了其搜索效率。張超超等人[13]通過定向加權(quán)的方法改進(jìn)A*算法，成功解決了機(jī)器人運(yùn)動(dòng)軌跡穿過障礙物的問題，但是仍然存在機(jī)器人運(yùn)動(dòng)軌跡與障礙物頂點(diǎn)相交的問題。張丹紅等人[14]提出了一種將A*算法與蟻群算法相融合的算法，此算法去除了冗余節(jié)點(diǎn)，使路徑更平滑，但是缺乏動(dòng)態(tài)實(shí)時(shí)避障的功能，而且計(jì)算復(fù)雜度較大。徐保來等人[15]改進(jìn)了動(dòng)態(tài)窗口法，雖然能夠?qū)崿F(xiàn)移動(dòng)機(jī)器人的實(shí)時(shí)避障，軌跡卻無法滿足全局最優(yōu)。傳統(tǒng)的路徑規(guī)劃算法在面對(duì)復(fù)雜多變的障礙環(huán)境時(shí)已然力不從心。程傳奇等人[16]、吳飛龍等人[17]分別提出了2種基于A*與動(dòng)態(tài)窗口的融合算法。根據(jù)上述路徑規(guī)劃問題，本文提出一種新的融合算法。通過引入障礙物占比優(yōu)化并設(shè)計(jì)A*算法的啟發(fā)函數(shù)，提高搜索效率，通過優(yōu)化子節(jié)點(diǎn)的選擇方式解決移動(dòng)機(jī)器人路徑與障礙物頂點(diǎn)相交的問題，通過刪除軌跡中的冗余節(jié)點(diǎn)實(shí)現(xiàn)路徑平滑度的優(yōu)化，并且融合動(dòng)態(tài)窗口法，在滿足全局最優(yōu)的情況下實(shí)現(xiàn)動(dòng)態(tài)實(shí)時(shí)避障。

1 A*算法的改進(jìn)

A*算法是一種能夠?qū)崿F(xiàn)全局路徑規(guī)劃的啟發(fā)式搜索算法，可以在靜態(tài)網(wǎng)絡(luò)環(huán)境中根據(jù)評(píng)價(jià)函數(shù)來搜索最佳路徑。但是傳統(tǒng)的A*算法在復(fù)雜環(huán)境下存在以下問題：1)冗余節(jié)點(diǎn)多，搜索效率低；2)規(guī)劃后路徑的軌跡與障礙物頂點(diǎn)相交，安全性低；3)路徑拐點(diǎn)多，平滑度低。因此本文通過對(duì)傳統(tǒng)A*算法的評(píng)價(jià)函數(shù)做出改進(jìn)，對(duì)子節(jié)點(diǎn)選擇方式和路徑平滑度進(jìn)行優(yōu)化以解決上述問題。

1.1 傳統(tǒng)A*算法的基本原理

傳統(tǒng)A*算法搜索原理是將起始點(diǎn)加入open列表，將該點(diǎn)作為父節(jié)點(diǎn)加入close列表，搜索其相鄰的可到達(dá)節(jié)點(diǎn)加入open列表，依據(jù)評(píng)價(jià)函數(shù)計(jì)算open列表中節(jié)點(diǎn)的代價(jià)，選取代價(jià)最低的節(jié)點(diǎn)作為下一個(gè)父節(jié)點(diǎn)并放入close列表，再次搜索父節(jié)點(diǎn)可到達(dá)節(jié)點(diǎn)并計(jì)算其代價(jià)，依次循環(huán)，直到父節(jié)點(diǎn)為目標(biāo)點(diǎn)的位置。

傳統(tǒng)A*算法的評(píng)價(jià)函數(shù)為：

f(n)=g(n)+h(n)

(1)

式(1)中，n代表當(dāng)前節(jié)點(diǎn)，f(n)表示的是移動(dòng)機(jī)器人在當(dāng)前節(jié)點(diǎn)的評(píng)價(jià)函數(shù)，用來選取最優(yōu)路徑，g(n)代表移動(dòng)機(jī)器人從起始點(diǎn)到當(dāng)前點(diǎn)的實(shí)際代價(jià)值[18-20]，h(n)為啟發(fā)函數(shù)，代表從當(dāng)前點(diǎn)n到目標(biāo)點(diǎn)的估計(jì)代價(jià)值[21-22]。一般情況下，h(n)小于從當(dāng)前點(diǎn)n到目標(biāo)點(diǎn)的實(shí)際代價(jià)值，當(dāng)h(n)值為0時(shí)，此時(shí)只有g(shù)(n)起作用，算法即為Dijkstra算法。h(n)的估計(jì)值比實(shí)際代價(jià)值小時(shí)，算法搜索時(shí)間會(huì)因搜索空間的增大而增加。h(n)估計(jì)值比實(shí)際代價(jià)值大時(shí)，算法搜索速度會(huì)增加，但是算法可能無法搜索到最短路徑。

不難看出h(n)對(duì)于搜索效率有很大的影響，h(n)有幾種常見的表現(xiàn)形式：1)曼哈頓距離；2)切比雪夫距離；3)歐幾里得距離。其幾種表現(xiàn)形式如下。

h(n)=abs(nx-gx)+abs(ny-gy)

(2)

h(n)=max[abs(nx-gx),abs(ny-gy)]

(3)

h(n)=sqrt[(nx-gx)2+(ny-gy)2]

(4)

在本文中h(n)采用的是歐幾里得距離，并且通過引入障礙物占比優(yōu)化改進(jìn)啟發(fā)函數(shù)。

1.2 啟發(fā)函數(shù)的改進(jìn)

當(dāng)柵格地圖中的障礙物即障礙柵格適度增加時(shí)，地圖中從起始點(diǎn)到達(dá)目標(biāo)點(diǎn)的路徑的選擇也會(huì)相應(yīng)增多，優(yōu)化后過程中容易陷入局部最優(yōu)，導(dǎo)致搜索效率降低，依據(jù)傳統(tǒng)A*算法的基本原理可看出，評(píng)價(jià)函數(shù)影響著A*算法的搜索效率，因此對(duì)評(píng)價(jià)函數(shù)中的啟發(fā)函數(shù)進(jìn)行改進(jìn)。

1.2.1 引入障礙物占比P

通過引入障礙物占比P來影響啟發(fā)函數(shù)h(n)在不同環(huán)境下的權(quán)重。假設(shè)一局部區(qū)域中障礙物的數(shù)量為a，所有柵格數(shù)量為A，設(shè)當(dāng)前點(diǎn)的坐標(biāo)為(nx，ny)，目標(biāo)點(diǎn)的坐標(biāo)為(gx，gy)，則A可以由式(5)表示。

A=[1+abs(nx-gx)]×[1+abs(ny-gy)]

(5)

障礙物占比P表示在當(dāng)前節(jié)點(diǎn)與目標(biāo)節(jié)點(diǎn)所構(gòu)成的局部區(qū)域中，障礙物在所有柵格中占的比重。則P如式(6)所示。

P=a/A

(6)

1.2.2 優(yōu)化啟發(fā)函數(shù)

局部區(qū)域內(nèi)障礙物較多時(shí)，需要確保最優(yōu)路徑的精度，避免陷入局部最優(yōu)，此時(shí)應(yīng)該減少h(n)的權(quán)重，降低搜索速度；局部區(qū)域內(nèi)障礙物較少時(shí)，如果按照傳統(tǒng)A*算法的啟發(fā)函數(shù)h(n)尋找最優(yōu)路徑，會(huì)導(dǎo)致搜索節(jié)點(diǎn)過多，搜索效率降低，此時(shí)應(yīng)該加大h(n)的權(quán)重。如式(7)所示，改進(jìn)后的評(píng)價(jià)函數(shù)提高了A*算清的適應(yīng)性。

f(n)=g(n)+h(n)-lgP×h(n)

(7)

1.2.3 子節(jié)點(diǎn)的優(yōu)化

傳統(tǒng)的A*算法在尋找最優(yōu)路徑時(shí)，會(huì)出現(xiàn)最優(yōu)路徑與障礙柵格頂點(diǎn)相交的情況，為解決這一問題，本文優(yōu)化子節(jié)點(diǎn)的選擇方式。如圖1所示，中間節(jié)點(diǎn)為父節(jié)點(diǎn)，父節(jié)點(diǎn)周圍相鄰的8個(gè)鄰域?yàn)?個(gè)子節(jié)點(diǎn)。根據(jù)8個(gè)子節(jié)點(diǎn)與父節(jié)點(diǎn)的位置關(guān)系，將子節(jié)點(diǎn)1、3、5、7歸類為A類子節(jié)點(diǎn)，將父節(jié)點(diǎn)上下方向上的子節(jié)點(diǎn)2、6歸類為B類子節(jié)點(diǎn)，將父節(jié)點(diǎn)左右方向上的子節(jié)點(diǎn)4、8歸類為C類子節(jié)點(diǎn)。

圖1 子節(jié)點(diǎn)圖

根據(jù)A、B、C這3類子節(jié)點(diǎn)，定義了以下子節(jié)點(diǎn)優(yōu)化選擇方式，如表1所示。

表1 子節(jié)點(diǎn)選擇規(guī)則

1.2.4 優(yōu)化路徑平滑度

傳統(tǒng)的A*算法在路徑規(guī)劃過程中會(huì)出現(xiàn)冗余節(jié)點(diǎn)，影響移動(dòng)機(jī)器人的正常工作。冗余節(jié)點(diǎn)包括共線節(jié)點(diǎn)和多余拐點(diǎn)，如圖2所示，X1→X2→X3→X4→X5→X6→X7→X8路徑存在較多冗余節(jié)點(diǎn)，為傳統(tǒng)A*算法所尋路徑。本文通過從起始點(diǎn)到目標(biāo)點(diǎn)刪除冗余節(jié)點(diǎn)，然后從目標(biāo)點(diǎn)反向到起始點(diǎn)再次刪除多余節(jié)點(diǎn)進(jìn)行路徑平滑度優(yōu)化。

圖2 刪除冗余節(jié)點(diǎn)圖

1)刪除共線節(jié)點(diǎn)，從目標(biāo)點(diǎn)方向在X2開始，沿著傳統(tǒng)A*算法的路徑判斷當(dāng)前節(jié)點(diǎn)與前一節(jié)點(diǎn)是否共線，若共線，則前一節(jié)點(diǎn)被認(rèn)為需要?jiǎng)h除的冗余節(jié)點(diǎn)。優(yōu)化后軌跡變?yōu)閄1→X5→X6→X7→X8。

2 動(dòng)態(tài)窗口法

動(dòng)態(tài)窗口法(DWA)是一種解決移動(dòng)機(jī)器人局部避障時(shí)的常用算法。此算法是指在速度空間中，依據(jù)移動(dòng)機(jī)器人的運(yùn)動(dòng)模型，對(duì)多組速度進(jìn)行采樣，分析并預(yù)測(cè)出一段時(shí)間內(nèi)在各組速度下移動(dòng)機(jī)器人的移動(dòng)軌跡，然后根據(jù)算法的評(píng)價(jià)函數(shù)選出最優(yōu)軌跡所對(duì)應(yīng)的速度，根據(jù)最優(yōu)軌跡對(duì)應(yīng)的速度驅(qū)動(dòng)移動(dòng)機(jī)器人進(jìn)行局部路徑規(guī)劃。

2.1 移動(dòng)機(jī)器人的運(yùn)動(dòng)模型

DWA對(duì)窗口區(qū)域內(nèi)移動(dòng)機(jī)器人的線速度和角速度進(jìn)行采樣，因此第一步需要對(duì)移動(dòng)機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)建模。假設(shè)在一段時(shí)間內(nèi)(Δt)移動(dòng)機(jī)器人做勻速直線運(yùn)動(dòng)。式(8)表示移動(dòng)機(jī)器人在該段時(shí)間內(nèi)的運(yùn)動(dòng)學(xué)模型。

(8)

2.2 移動(dòng)機(jī)器人速度采樣

在移動(dòng)機(jī)器人速度組空間中，理論上有無窮多組速度集[23](v,ω)，但是移動(dòng)機(jī)器人容易受到自身的硬件、工作環(huán)境的制約，因此獲得機(jī)器人運(yùn)動(dòng)模型后需要根據(jù)實(shí)際情況對(duì)采樣的速度集范圍進(jìn)行約束。

1)移動(dòng)機(jī)器人受自身?xiàng)l件制約的最大、最小速度范圍可表示為：

Vm={(v,ω)|v∈[vmin,vmax]∩ω∈[ωmin,ωmax]}

(9)

2)移動(dòng)機(jī)器人受到自身電機(jī)的影響，存在加速減速約束，在動(dòng)態(tài)窗口顯示內(nèi)，移動(dòng)機(jī)器人受加減速影響的最大、最小速度范圍為：

(10)

式中，vt代表移動(dòng)機(jī)器人的當(dāng)前線速度，ωt代表機(jī)器人當(dāng)前角的速度，va、ωa為移動(dòng)機(jī)器人的最大加減速度，Δt為模擬預(yù)測(cè)時(shí)間。

3)移動(dòng)機(jī)器人制動(dòng)距離約束。實(shí)現(xiàn)動(dòng)態(tài)避障主要依賴制動(dòng)距離的約束。動(dòng)態(tài)窗口法在選擇速度和軌跡評(píng)價(jià)的過程中尋找障礙物，為保證移動(dòng)機(jī)器人工作時(shí)的可靠性與安全性，需要其在與障礙物發(fā)生碰撞前，在最大減速度條件下，把速度減到0。約束公式如式(11)所示。

(11)

式(11)中dist(v，ω)表示此速度下該軌跡與障礙物之間的最短距離。根據(jù)上述3種速度約束可得移動(dòng)機(jī)器人所取速度集為滿足上述3種約束條件的速度矢量空間交集，如圖3所示。

圖3 速度采樣示意圖

2.3 評(píng)價(jià)函數(shù)

動(dòng)態(tài)窗口法中的評(píng)價(jià)函數(shù)用于選取最優(yōu)軌跡。軌跡評(píng)判的準(zhǔn)則為：準(zhǔn)確避開障礙物，耗時(shí)最短靠近目標(biāo)點(diǎn)。設(shè)計(jì)評(píng)價(jià)函數(shù)為：

G(v,ω)=σ(α·head(v,ω))+β·dist(v,ω)+γ·vel(v,ω)

(12)

式中head(v，ω)為方位角評(píng)價(jià)函數(shù)[24]，表示目標(biāo)位置與預(yù)測(cè)軌跡終點(diǎn)位置方向的方位角偏差大小；dist(v,ω)為距離評(píng)價(jià)函數(shù)；vel(v，ω)為速度評(píng)價(jià)函數(shù)；α、β、γ為3項(xiàng)加權(quán)系數(shù)，σ為歸一化平滑系數(shù)。

3 算法融合

改進(jìn)后的A*算法本質(zhì)上仍然是一種全局路徑規(guī)劃算法，在錯(cuò)綜復(fù)雜的環(huán)境下無法進(jìn)行動(dòng)態(tài)避障，路徑也不夠平滑。而動(dòng)態(tài)窗口法作為一種解決移動(dòng)機(jī)器人局部避障時(shí)的常用算法，能夠動(dòng)態(tài)實(shí)時(shí)規(guī)劃路徑，從而彌補(bǔ)改進(jìn)A*算法的不足。而且傳統(tǒng)的動(dòng)態(tài)窗口法缺少全局路徑規(guī)劃的指引，在障礙環(huán)境復(fù)雜時(shí)容易陷入局部最優(yōu)，對(duì)此提取改進(jìn)后A*算法的全局路徑節(jié)點(diǎn)，設(shè)計(jì)一種融合全局路徑信息的評(píng)價(jià)函數(shù)。

G(v,ω)=σ(α·Head(v,ω))+β·dist(v,ω)+γ·vel(v,ω)+λDist(v,ω)

(13)

式中Head(v，ω)為方位角評(píng)價(jià)函數(shù)，表示模擬預(yù)測(cè)路徑終點(diǎn)位置方向與全局路徑節(jié)點(diǎn)的方位角偏差，依照改進(jìn)后A*算法的全局路徑節(jié)點(diǎn)順序設(shè)置當(dāng)前目標(biāo)節(jié)點(diǎn)，在抵達(dá)目標(biāo)點(diǎn)時(shí)更新，依次動(dòng)態(tài)設(shè)置目標(biāo)節(jié)點(diǎn)。dist(v,ω)、Dist(v,ω)為距離評(píng)價(jià)函數(shù)，分別表示全局已知障礙物與模擬軌跡的最短距離和局部未知障礙物與模擬軌跡的最短距離。本文采用改進(jìn)后的A*算法做全局路徑規(guī)劃，然后采用融合全局路徑信息的DWA進(jìn)行局部路徑規(guī)劃，最終實(shí)現(xiàn)移動(dòng)機(jī)器人動(dòng)態(tài)避障，尋找最優(yōu)路徑，融合算法流程如圖4所示。

圖4 融合算法流程圖

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

本文在MATLAB下進(jìn)行仿真實(shí)驗(yàn)，從而驗(yàn)證改進(jìn)算法及融合算法的有效性。

4.1 A*算法優(yōu)化后仿真結(jié)果

上述操作實(shí)現(xiàn)了搜索效率的提高，刪除了多余節(jié)點(diǎn)并且優(yōu)化了路徑平滑度，本文在MATLAB下進(jìn)行不同環(huán)境的仿真，分別為環(huán)境1(簡(jiǎn)易環(huán)境)、環(huán)境2(復(fù)雜環(huán)境)，其中仿真環(huán)境2的尺寸相比環(huán)境1擴(kuò)大了4倍。通過圖5、圖6可看出簡(jiǎn)易環(huán)境下優(yōu)化后的A*算法有障礙物信息的引導(dǎo)，搜索空間減少，搜索時(shí)間更快，全局路徑效果更佳；通過圖7、圖8可看出復(fù)雜環(huán)境下的改進(jìn)A*算法路徑長(zhǎng)度上短于傳統(tǒng)A*算法，避免了路徑與障礙物頂點(diǎn)相切，安全系數(shù)提高，路徑精度高于傳統(tǒng)A*算法。表2為不同環(huán)境下路徑規(guī)劃后的指標(biāo)，改進(jìn)后的A*算法優(yōu)于傳統(tǒng)算法。

圖5 環(huán)境1下傳統(tǒng)A*算法仿真

圖6 環(huán)境1下改進(jìn)A*算法仿真

圖7 環(huán)境2下傳統(tǒng)A*算法仿真

圖8 環(huán)境2下改進(jìn)A*算法仿真

表2 路徑規(guī)劃對(duì)比

4.2 融合算法仿真結(jié)果

圖9 避障過程仿真圖1

圖10 避障過程仿真圖2

圖11 改進(jìn)A*算法仿真圖

圖12 融合算法仿真圖

58 結(jié)束語

本文提出了一種融合改進(jìn)A*算法與動(dòng)態(tài)窗口法的融合算法，以提升移動(dòng)機(jī)器人在不同復(fù)雜度環(huán)境下工作的靈活性，提高其工作效率。與傳統(tǒng)算法相比，改進(jìn)后的A*算法解決了軌跡與障礙柵格頂點(diǎn)相交的問題，路徑節(jié)點(diǎn)減少，路徑更加平滑，引入障礙物占比，提高了不同環(huán)境下的靈活性與搜索效率。并采用動(dòng)態(tài)窗口算法進(jìn)行局部避障，結(jié)合全局路徑規(guī)劃信息的指引規(guī)劃出最優(yōu)路徑，彌補(bǔ)了A*算法無法完成動(dòng)態(tài)避障的缺點(diǎn)。通過MATLAB仿真對(duì)比實(shí)驗(yàn)，表明了本文所提出融合算法的有效性。未來預(yù)計(jì)從以下幾個(gè)方面進(jìn)行研究：1)針對(duì)復(fù)雜動(dòng)態(tài)環(huán)境下多數(shù)目移動(dòng)機(jī)器人相互協(xié)調(diào)，多目標(biāo)移動(dòng)機(jī)器人路徑規(guī)劃問題進(jìn)行探索；2)針對(duì)未知?jiǎng)討B(tài)環(huán)境，結(jié)合機(jī)器學(xué)習(xí)進(jìn)行移動(dòng)機(jī)器人路徑規(guī)劃的進(jìn)一步研究。