基于改進(jìn)A*算法融合DWA 算法的機(jī)器人路徑規(guī)劃

王海瑤，安天洋

（沈陽(yáng)航空航天大學(xué) 電子信息工程學(xué)院，遼寧沈陽(yáng)，110000）

0 引言

自主移動(dòng)機(jī)器人[1]作為自動(dòng)化技術(shù)的應(yīng)用之一，在新能源汽車，智能家居，商場(chǎng)智能向?qū)В蛷d自動(dòng)送餐等應(yīng)用領(lǐng)域作用極大，而其路徑規(guī)劃[2]是機(jī)器人自主導(dǎo)航中的關(guān)鍵問題，目標(biāo)是搜尋出一條無(wú)障礙且最優(yōu)先的路徑。目前生產(chǎn)生活實(shí)際應(yīng)用中普遍使用的定位建圖方式有粒子濾波法[3]，Gmapping[4]等，建圖之后，考慮到醫(yī)院等地形的復(fù)雜性[5]以及算法效率問題，被廣泛使用的路徑規(guī)劃算法有A*[6]，蟻群[7]，粒子群算法[8]，Dijkstra 算法[9]，動(dòng)態(tài)窗口法[10]等。普通路徑規(guī)劃算法不能夠同時(shí)實(shí)現(xiàn)全局路徑規(guī)劃[11]與局部路徑規(guī)劃[12]，為實(shí)現(xiàn)實(shí)際需求，本文提出了一種改進(jìn)傳統(tǒng)A*算法[13]以此提升效率，接著融合DWA 算法[14]使路徑在全局正確的前提下能夠做到局部避障規(guī)劃且更加平滑化的融合性算法。本文章主要論述路徑規(guī)劃算法的創(chuàng)新想法與實(shí)用性分析，從結(jié)果可以看出，在有障礙物的環(huán)境下，提出的將改進(jìn) A *算法與動(dòng)態(tài)窗口法 DWA 算法相結(jié)合的算法提高了移動(dòng)機(jī)器人在復(fù)雜環(huán)境中規(guī)劃路徑的效率，縮短了路徑規(guī)劃時(shí)間及軌跡距離，更適于應(yīng)用在實(shí)際中，以實(shí)現(xiàn)移動(dòng)機(jī)器人的路徑規(guī)劃和及時(shí)避障，快速準(zhǔn)確地找出最佳路徑。

1 傳統(tǒng)A*算法

傳統(tǒng)A*算法同時(shí)結(jié)合了廣度優(yōu)先搜索、Dijkstra 算法、最佳優(yōu)先搜索這三種算法的特點(diǎn)于一身，是一種常用的路徑規(guī)劃和圖形遍歷算法。廣度優(yōu)先搜索是以廣度為優(yōu)先級(jí)進(jìn)行，這種算法會(huì)遍歷圖中的每一個(gè)節(jié)點(diǎn)，像洪水一樣，實(shí)際對(duì)于大部分情況來(lái)說(shuō)是沒有必要的；Dijkstra 算法是1956 年由計(jì)算機(jī)科學(xué)家Edsger W.Dijkstra 提出的一種用來(lái)尋找節(jié)點(diǎn)之間最短路徑的方法，考慮到相鄰節(jié)點(diǎn)之間移動(dòng)代價(jià)的不同，列出一個(gè)優(yōu)先隊(duì)列，將每次移動(dòng)時(shí)計(jì)算到的從當(dāng)前節(jié)點(diǎn)到起點(diǎn)的總移動(dòng)代價(jià)最小的路徑放入優(yōu)先隊(duì)列，直到遍歷到終點(diǎn)結(jié)束；最佳優(yōu)先搜索與Dijkstra 算法類似，同樣列出一個(gè)優(yōu)先隊(duì)列，將當(dāng)前節(jié)點(diǎn)到終點(diǎn)的距離作為優(yōu)先級(jí)，最終選取離終點(diǎn)最近的路徑，但是同時(shí)這種算法也存在缺陷即沒有考慮到中間有障礙物的情況。

通過上述可知，A*是一種結(jié)合評(píng)價(jià)函數(shù)使用的、通過不斷尋找節(jié)點(diǎn)的方式最終組成一條完整路徑的啟發(fā)式算法，傳統(tǒng)A*算法以從當(dāng)前節(jié)點(diǎn)到下一個(gè)節(jié)點(diǎn)的實(shí)際成本（或稱代價(jià))和估計(jì)成本之和作為評(píng)價(jià)標(biāo)準(zhǔn)，評(píng)價(jià)函數(shù)如下：

其中f(n)是節(jié)點(diǎn)的綜合優(yōu)先級(jí)，h(n)和g(n)分別表示到目標(biāo)點(diǎn)的實(shí)際和估計(jì)代價(jià)。h(n)的選擇將直接影響到算法成功率、速率和準(zhǔn)確率，h(n)的值與準(zhǔn)確值間的誤差越小，搜索效率越高。極端情況下，當(dāng)h(n)=0 時(shí)，f(n)完全由g(n)決定，此時(shí)算法退化成Dijkstra 算法；當(dāng)h(n)始終小于等于節(jié)點(diǎn)到終點(diǎn)的代價(jià)時(shí)，一定可以找到最短路徑，但隨著h(n)值的減小，被遍歷到的節(jié)點(diǎn)數(shù)會(huì)越多，算法整體速率變慢；當(dāng)h(n)完全等于節(jié)點(diǎn)到終點(diǎn)的代價(jià)時(shí)，此時(shí)的路徑為最佳路徑并且速率很快，但這種情況為理想狀態(tài)，很難實(shí)現(xiàn)；當(dāng)h(n)大于節(jié)點(diǎn)到終點(diǎn)的代價(jià)時(shí)，不能保證可以找到最短路徑，但此時(shí)算法速率很快；另一種極端情況下，當(dāng)h(n)遠(yuǎn)大于g(n)時(shí)，此時(shí)只有h(n)發(fā)揮作用，此時(shí)算法退化成最佳優(yōu)先搜索。

對(duì)于網(wǎng)格形式的圖形，設(shè)定兩個(gè)距離dx、dy：

如果節(jié)點(diǎn)只允許朝上下左右四個(gè)方向移動(dòng)的話，h(n)可以使用曼哈頓距離，一般情況下兩個(gè)相鄰節(jié)點(diǎn)之間的移動(dòng)代價(jià)為一個(gè)固定的常數(shù)，記作D，曼哈頓距離的公式：

如果節(jié)點(diǎn)允許朝斜著的方向移動(dòng)即朝著八個(gè)方向移動(dòng)，則h(n)可以使用對(duì)角距離。D2 指的是兩個(gè)斜著相鄰節(jié)點(diǎn)之間的移動(dòng)代價(jià)。如果所有節(jié)點(diǎn)都正方形，其值是：

對(duì)角距離的公式：

如果節(jié)點(diǎn)允許朝任何方向移動(dòng)，h(n)可以使用歐幾里得距離，由于本系統(tǒng)采用雙輪差速驅(qū)動(dòng)，機(jī)器人的移動(dòng)方位不受機(jī)械結(jié)構(gòu)限制，故采用可以直接反映兩點(diǎn)距離的歐幾里得距離模型作為實(shí)際代價(jià)函數(shù)：

2 優(yōu)化A*算法

■2.1 代價(jià)函數(shù)的優(yōu)化

在傳統(tǒng)A*算法中，代價(jià)評(píng)價(jià)函數(shù)中的h(n)和g(n)權(quán)重相同，而在實(shí)際問題中，當(dāng)距離目標(biāo)點(diǎn)距離較大時(shí)，實(shí)際代價(jià)遠(yuǎn)大于估計(jì)代價(jià)，這將使得系統(tǒng)遍歷許多無(wú)效節(jié)點(diǎn)，使算法效率降低。而在接近目標(biāo)點(diǎn)時(shí)，估計(jì)代價(jià)逐漸增大并接近于估計(jì)代價(jià)。針對(duì)以上問題，可以給h(n)賦予動(dòng)態(tài)權(quán)重w(n)，使系統(tǒng)能根據(jù)自身狀態(tài)自適應(yīng)調(diào)整h(n)的影響力，以此提高算法效率。賦予動(dòng)態(tài)權(quán)重的原則是在開始尋找路徑后，快速地到達(dá)終點(diǎn)比重占的更大；尋找結(jié)束后，獲得最佳路徑比重占的更大。w(n)與h(n)呈線性變化關(guān)系，當(dāng)h(n)較大時(shí)，A*算法搜索速率增大，很快地向終點(diǎn)擴(kuò)散但會(huì)錯(cuò)過最佳路徑；當(dāng)h(n)較小時(shí)，A*算法會(huì)降低搜索速率而更傾向于尋找最佳路徑

此時(shí)，再對(duì)當(dāng)前節(jié)點(diǎn)與目標(biāo)點(diǎn)的長(zhǎng)度dn、起始點(diǎn)與目標(biāo)點(diǎn)間的長(zhǎng)度ds加以考慮。則優(yōu)化后的代價(jià)函數(shù)：

■2.2 剔除冗余節(jié)點(diǎn)

傳統(tǒng)A*算法選取的節(jié)點(diǎn)位于柵格中心位置，可能導(dǎo)致原本可以互相直達(dá)的兩個(gè)節(jié)點(diǎn)之間存在第三個(gè)冗余節(jié)點(diǎn)，這種冗余節(jié)點(diǎn)每存在一個(gè)就會(huì)使機(jī)器人做出兩次多余的轉(zhuǎn)向動(dòng)作，將促進(jìn)系統(tǒng)誤差的累積，縮短機(jī)械系統(tǒng)壽命，降低路徑效率，本文嘗試通過計(jì)算向量角和作線段的方式消除冗余節(jié)點(diǎn)：設(shè)第一個(gè)節(jié)點(diǎn)為P1，后一個(gè)節(jié)點(diǎn)為P2，P2后一個(gè)節(jié)點(diǎn)P3，而后計(jì)算∠P3P2P1，若∠P3P2P1=180°，則P2為冗余節(jié)點(diǎn)，剔除；若∠P3P2P1≠180°，則作線段L(P3P1)，若L(P3P1)經(jīng)過障礙物，則保留P2；若L(P3P1)不經(jīng)過障礙物，則P2為冗余節(jié)點(diǎn)，剔除。接著前往下一個(gè)節(jié)點(diǎn)直至終點(diǎn)。此時(shí)得出的路徑對(duì)比傳統(tǒng)A*算法，轉(zhuǎn)彎次數(shù)和軌跡總長(zhǎng)都會(huì)有所下降。

■2.3 局部路徑規(guī)劃

在建圖成功的前提下，進(jìn)行優(yōu)化代價(jià)函數(shù)和剔除冗余節(jié)點(diǎn)后的A*算法（以下統(tǒng)稱改進(jìn)A*算法)可以規(guī)劃出全局路徑，但是當(dāng)有各種各樣移動(dòng)中的未知障礙物時(shí)，僅靠改進(jìn)A*算法是無(wú)法及時(shí)避開未知移動(dòng)障礙的，于是引入動(dòng)態(tài)窗口法（DWA)來(lái)進(jìn)行局部路徑規(guī)劃，在DWA 中，機(jī)器人在速度空間內(nèi)采樣，生成模擬軌跡，最后根據(jù)評(píng)價(jià)函數(shù)選擇并沿著最合理的軌跡移動(dòng)。

(1)根據(jù)硬件條件與周圍具體環(huán)境條件將機(jī)器人的速度空間控制在：

機(jī)器人在模擬運(yùn)行的一定時(shí)間間隔?t內(nèi)，機(jī)器人所能達(dá)到的速度被加速度限制在：

由于機(jī)器人減速性能有限，為確保機(jī)器人不會(huì)撞到障礙物，于是將速度限制在：

于是，得出機(jī)器人的有效速度采樣區(qū)間：

其中，vmin,vmax為最小，最大線速度；ωmin,ωmax為最小，最大角速度；vc，ωc為機(jī)器人線速度，角速度；，為機(jī)器人最小，最大線加速度；，為機(jī)器人最小，最大旋轉(zhuǎn)角加速度；dist(v,ω)為機(jī)器人與最近障礙物間的距離。

機(jī)器人在有效速度采樣區(qū)間采樣后，就可以利用評(píng)價(jià)函數(shù)來(lái)評(píng)價(jià)所采樣的速度對(duì)應(yīng)的軌跡：

其中，α,β,γ是權(quán)重因子；heading(v,ω)表示機(jī)器人采樣區(qū)間使，所在軌跡方向與目標(biāo)角度值θ的函數(shù)；vel(v,ω)表示運(yùn)動(dòng)軌跡速度大小。

■2.4 全局路徑規(guī)劃與局部路徑規(guī)劃的融合

改進(jìn)A*算法可以計(jì)算出兩點(diǎn)之間的最佳軌跡，DWA 算法可以利用傳感器及時(shí)躲避事先未知的障礙物，進(jìn)行局部路徑規(guī)劃。故將兩者結(jié)合，便可以得到可在實(shí)際醫(yī)院環(huán)境中使用的融合路徑規(guī)劃算法，流程如圖1 所示。

圖1 路徑規(guī)劃流程

■2.5 仿真實(shí)驗(yàn)

根據(jù)計(jì)劃元件組裝簡(jiǎn)易機(jī)器人，安裝系統(tǒng)后，放置在模擬的走廊和儲(chǔ)物室環(huán)境中，運(yùn)行地圖構(gòu)建算法節(jié)點(diǎn)，打開rviz，可獲得如圖2，圖3 兩種模擬環(huán)境的室內(nèi)環(huán)境效果圖。

圖2 走廊環(huán)境

圖3 儲(chǔ)物室環(huán)境

如圖2，圖3 所示，使用計(jì)劃系統(tǒng)的硬件和算法構(gòu)建出的地圖能準(zhǔn)確表現(xiàn)出墻體、路面以及障礙物的位置，存在些許傾斜、重影誤差但不會(huì)影響到后續(xù)的自主路徑規(guī)劃工作。為驗(yàn)證算法可行性，配置測(cè)試環(huán)境，運(yùn)行系統(tǒng)為Windows11X64，平臺(tái)為Matlab R2018a，利用Matlab 中的colormap 函數(shù)搭建20*20 柵格地圖，其中白色為空區(qū)域，黑色為障礙物，左上角為三角形為起點(diǎn)，右 下角圓形為目標(biāo)點(diǎn)，設(shè)定表1 參數(shù)，運(yùn)行算法，觀察軌跡。

表1 機(jī)器人及算法參數(shù)

首先進(jìn)行傳統(tǒng)A*算法與改進(jìn)A*算法的對(duì)照實(shí)驗(yàn)。

圖4 中黑色實(shí)線顯示的是傳統(tǒng)A*算法計(jì)算得出的軌跡，圖5 中黑色實(shí)線顯示的是改進(jìn)A*算法計(jì)算得出的軌跡，統(tǒng)計(jì)轉(zhuǎn)彎次數(shù)，總長(zhǎng)度，計(jì)算時(shí)間得表2。

圖4 傳統(tǒng)A*算法

圖5 改進(jìn)A*算法

根據(jù)表2 數(shù)據(jù)可輕易看出，改進(jìn)A*算法可以顯著減少機(jī)器人的轉(zhuǎn)彎次數(shù)，同時(shí)能夠減少軌跡長(zhǎng)度，降低計(jì)算時(shí)間，在系統(tǒng)整體性能方面對(duì)比傳統(tǒng)A*算法有顯著提升。

為驗(yàn)證DWA 算法的融合是否能對(duì)改進(jìn)A*算法起到輔助躲避未知障礙的作用，在柵格地圖的改進(jìn)A*算法的軌跡上放置灰色塊作為事先未知的障礙物（如圖6)，后使用改進(jìn)A*與DWA 融合后的算法（以下統(tǒng)稱融合算法)再次運(yùn)行，得出圖7 所示軌跡。

圖6 放置未知障礙

圖7 融合算法軌跡

如圖7 可見，融合算法得出的路徑在避開了所有臨時(shí)加入的障礙的基礎(chǔ)上對(duì)比改進(jìn)A*算法的路徑更加平滑，更符合普通雙輪差速機(jī)械結(jié)構(gòu)的運(yùn)動(dòng)特性，綜上所述，最終的融合算法得出了一條可在實(shí)際問題中使用的優(yōu)質(zhì)軌跡。

3 結(jié)語(yǔ)

本文提出了一種針對(duì)A*路徑規(guī)劃算法的改進(jìn)方法，首先優(yōu)化評(píng)價(jià)函數(shù)使實(shí)際成本值在系統(tǒng)運(yùn)行過程中具有動(dòng)態(tài)影響力，減少節(jié)點(diǎn)數(shù)，提升了算法效率，通過剔除冗余節(jié)點(diǎn)的方式保留了重要節(jié)點(diǎn)，刪除了非必要節(jié)點(diǎn)，最大程度上提升算法效率，并減少了機(jī)器人轉(zhuǎn)彎總次數(shù)，縮短路徑，使機(jī)械誤差的累計(jì)值達(dá)到最小。同時(shí)引入DWA（動(dòng)態(tài)窗口)算法，解決了運(yùn)行過程中躲避未知障礙的問題，同時(shí)增加了軌跡平滑度。該融合算法經(jīng)過仿真實(shí)驗(yàn)后得知具備有效的可行性，可以提高機(jī)器人的自動(dòng)路徑規(guī)劃效率與準(zhǔn)確度，通過更快更優(yōu)的路徑規(guī)劃，一定程度上提高了全局路徑規(guī)劃中躲避障礙的效果，可以實(shí)現(xiàn)動(dòng)態(tài)路徑規(guī)劃中的全局避障。本文僅介紹了簡(jiǎn)單二維環(huán)境下的自主導(dǎo)航處理辦法，針對(duì)更加復(fù)雜的上下寬窄不一的障礙物或高度障礙問題未做論述，未來(lái)還需進(jìn)一步的針對(duì)性研究。