基于A*和動(dòng)態(tài)窗口法的移動(dòng)機(jī)器人路徑規(guī)劃?

王佳軍 孟令軍 薛志凌 李曉宇

（中北大學(xué)儀器與電子學(xué)院 太原 030000）

1 引言

隨著科技水平的快速提高，機(jī)器人由最初的結(jié)構(gòu)簡單、功能單一發(fā)展為能夠適應(yīng)多樣化環(huán)境、執(zhí)行復(fù)雜任務(wù)的智能化機(jī)器人，廣泛應(yīng)用于物流業(yè)、制造業(yè)、服務(wù)業(yè)等領(lǐng)域，極大地提高了社會(huì)的生產(chǎn)力水平。而路徑規(guī)劃是實(shí)現(xiàn)自主導(dǎo)航的關(guān)鍵一步，也是機(jī)器人智能化的關(guān)鍵技術(shù)之一，是機(jī)器人以及自動(dòng)駕駛領(lǐng)域的研究熱點(diǎn)。目前，機(jī)器人路徑規(guī)劃主要集中在三類算法：1）基于圖的算法，如：Dijkstra、A*算法［1］，D*算法［2］，D*Lite算法［3］等；2）隨機(jī)采樣類算法，如：快速隨即搜索樹法（RRT）［4］、概率路圖法（PRM）等；3）智能優(yōu)化算法：如蟻群算法、遺傳算法、神經(jīng)網(wǎng)絡(luò)等，此外，還有人工勢(shì)場(chǎng)法［5］，動(dòng)態(tài)窗口法［6］等。

Dijkstra 算法基于廣度優(yōu)先遍歷，向四周盲目拓展，搜索節(jié)點(diǎn)過多。A*算法在Dijkstra 算法的基礎(chǔ)上引入啟發(fā)式函數(shù)，增強(qiáng)了搜索的目的性，在機(jī)器人全局路徑規(guī)劃中使用廣泛。但是該算法存在節(jié)點(diǎn)過于密集、軌跡不平滑等問題。文獻(xiàn)［7］利用跳點(diǎn)搜索理論，大大減小了搜索過程中節(jié)點(diǎn)的數(shù)量；文獻(xiàn)［8］引入電動(dòng)車的能耗約束建立綜合估價(jià)函數(shù)，同時(shí)考慮中途充電站，規(guī)劃出能耗最優(yōu)的行車路徑；文獻(xiàn)［9］將傳統(tǒng)A*算法八角度拓展節(jié)點(diǎn)優(yōu)化為任意角度路線，減小了路徑長度，與蟻群算法進(jìn)行融合實(shí)現(xiàn)多目標(biāo)點(diǎn)路徑規(guī)劃。

動(dòng) 態(tài) 窗 口 法（Dynamic Window Approach，DWA）對(duì)復(fù)雜環(huán)境具有很強(qiáng)的適應(yīng)能力，實(shí)時(shí)結(jié)合機(jī)器人自身傳感器進(jìn)行局部路徑規(guī)劃，能夠有效躲避障礙物且路徑為平滑曲線，但是該方法容易陷入局部最優(yōu)解，無法抵達(dá)目標(biāo)點(diǎn)。文獻(xiàn)［10］結(jié)合與全局路徑距離信息定義新的評(píng)價(jià)函數(shù)，能夠提前規(guī)避“C”型障礙物；文獻(xiàn)［11］結(jié)合自適應(yīng)思想使得機(jī)器人在通過不同密度障礙物區(qū)域時(shí)，運(yùn)動(dòng)參數(shù)能夠自主變化，機(jī)器人的速度與路徑更加合理；文獻(xiàn)［12］利用動(dòng)態(tài)窗口法對(duì)可選避障速度進(jìn)行速度約束，同時(shí)，增大了移動(dòng)機(jī)器人可選避障速度區(qū)域。

本文將A*算法與動(dòng)態(tài)窗口法結(jié)合，首先對(duì)A*算法產(chǎn)生的路徑節(jié)點(diǎn)進(jìn)行預(yù)處理，剔除冗余節(jié)點(diǎn)，得到路徑坐標(biāo)點(diǎn)作為動(dòng)態(tài)窗口算法的局部目標(biāo)點(diǎn)，直至最終目標(biāo)點(diǎn)，有效解決了單一算法在路徑規(guī)劃中存在的問題。

2 A*算法

2.1 環(huán)境構(gòu)建

在機(jī)器人工作過程中，需要通過自身傳感器感知環(huán)境，構(gòu)建一個(gè)機(jī)器人可以理解地表示所在環(huán)境的地圖模型。柵格法是經(jīng)典的環(huán)境建模方法，簡單高效，將機(jī)器人構(gòu)建的地圖劃分為大小相同、彼此相連的柵格，柵格帶有環(huán)境信息及機(jī)器人位置信息，機(jī)器人以柵格為基本單元進(jìn)行移動(dòng)和搜索，完成路徑規(guī)劃任務(wù)。如圖1所示，深色柵格表示環(huán)境中存在障礙物，機(jī)器人無法通過，淺色柵格表示允許搜索區(qū)域。

圖1 柵格地圖

2.2 A*算法基本原理

A*算法在搜索過程中假設(shè)環(huán)境不再改變，是求解最短路徑的有效方法。Dijkstra 盲目向四周搜索，而A*算法的改進(jìn)在于結(jié)合當(dāng)前位置與目標(biāo)點(diǎn)的距離建立具有啟發(fā)性的搜索函數(shù)：

式中：g(n)為起始節(jié)點(diǎn)至當(dāng)前節(jié)點(diǎn)的實(shí)際距離代價(jià)；?(n)為當(dāng)前節(jié)點(diǎn)至目標(biāo)點(diǎn)的估計(jì)距離代價(jià)。

代價(jià)函數(shù)f(n)來進(jìn)行節(jié)點(diǎn)的擴(kuò)展和搜索，g(n)代表搜索的完整性，g(n)越大越趨向于搜索的廣度，但會(huì)降低搜索的效率，h（n）具有啟發(fā)性，h（n）越大，搜索的效率高，但通常難以規(guī)劃到最優(yōu)路徑。

A*算法沿著柵格地圖搜索的過程中，更新維護(hù)open list和close list兩個(gè)列表。Open存儲(chǔ)拓展點(diǎn)周圍節(jié)點(diǎn)，用于計(jì)算選擇最小代價(jià)節(jié)點(diǎn)作為下一個(gè)拓展點(diǎn)，close 存儲(chǔ)經(jīng)過的節(jié)點(diǎn)以及障礙物。A*算法具體流程如下：

1）初始化環(huán)境信息，包括障礙物、起始點(diǎn)、目標(biāo)點(diǎn)，以及open和close列表；

2）判斷open 列表是否為空或只有目標(biāo)點(diǎn)，若為真，結(jié)束搜索；

3）選取open 列表中評(píng)價(jià)函數(shù)最小的節(jié)點(diǎn)作為父節(jié)點(diǎn)，調(diào)用拓展函數(shù)，計(jì)算g（n），h（n），將此節(jié)點(diǎn)放入close列表；

4）將拓展出來的節(jié)點(diǎn)放入open 列表中，保存父節(jié)點(diǎn)；

5）跳至步驟2），直至找到終點(diǎn)；

6）保存的父節(jié)點(diǎn)信息，即為路徑關(guān)鍵節(jié)點(diǎn)。

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

動(dòng)態(tài)窗口法由機(jī)器人運(yùn)動(dòng)學(xué)方程推導(dǎo)而來，在預(yù)測(cè)時(shí)間dt內(nèi)，對(duì)機(jī)器人線速度和角速度采樣，形成速度空間(v,ω)，描繪出所有可能的運(yùn)動(dòng)軌跡，構(gòu)建評(píng)價(jià)函數(shù)對(duì)軌跡進(jìn)行評(píng)價(jià)，從中選擇最合理的路徑。

首先，我們建立機(jī)器人的運(yùn)動(dòng)模型，機(jī)器人的運(yùn)動(dòng)軌跡由一系列的圓弧組成，在dt時(shí)間內(nèi)由速度對(duì)(v,ω)決定，運(yùn)動(dòng)模型為

依據(jù)上述機(jī)器人運(yùn)動(dòng)模型，在機(jī)器人的實(shí)際工作中，具有無窮多(v,ω)，需要根據(jù)環(huán)境和工作狀態(tài)添加約束，選擇出朝向目標(biāo)、躲避障礙物的(v,ω)。

出于安全考慮，對(duì)機(jī)器人行駛的速度范圍進(jìn)行約束，定義Vs為機(jī)器人最大線速度和角速度，所以動(dòng)態(tài)窗口所能搜索的最大范圍為

由于機(jī)器人電機(jī)所提供轉(zhuǎn)矩的限制，機(jī)器人在dt時(shí)間內(nèi)所能達(dá)到的實(shí)際速度受到一定的約束，即：

為了保證機(jī)器人在行駛過程中避免與障礙物發(fā)生碰撞，當(dāng)機(jī)器人檢測(cè)到環(huán)境中障礙物時(shí)，能夠以最大減速度停下來，即：

最終，由以上三個(gè)速度構(gòu)成動(dòng)態(tài)窗口速度集合Vr：

經(jīng)過速度采樣后得到速度空間，如何在該空間內(nèi)選擇一條最優(yōu)的軌跡，需要我們?cè)O(shè)計(jì)適宜的評(píng)價(jià)函數(shù)，使得機(jī)器人朝向目標(biāo)快速前進(jìn)，局部避開障礙物。評(píng)價(jià)函數(shù)設(shè)計(jì)為

式中：σ為平滑系數(shù)，α,β,γ為各子函數(shù)加權(quán)系數(shù)；heading（v，ω）表示當(dāng)前位置與全局之間的角度偏差，使機(jī)器人朝向目標(biāo)點(diǎn)方向；dist（v，ω）表示生成的速度軌跡至障礙物的最近距離，防止機(jī)器人發(fā)生碰撞；vel（v，ω）表示采樣軌跡中的速度大小，盡快抵達(dá)。

4 優(yōu)化與改進(jìn)

A*算法根據(jù)評(píng)價(jià)函數(shù)，遍歷整個(gè)柵格地圖，得到一系列路徑點(diǎn)，導(dǎo)致路徑點(diǎn)過于密集，曲率不連續(xù)，關(guān)鍵拐點(diǎn)選取策略，剔除不相關(guān)的冗余節(jié)點(diǎn)，提升路徑的平滑度。設(shè)A→B→C 為路徑中三個(gè)節(jié)點(diǎn)的行駛路線，若A 與B 共線，B 與C 共線，則認(rèn)為B節(jié)點(diǎn)為冗余節(jié)點(diǎn)，刪去該結(jié)點(diǎn)。

傳統(tǒng)評(píng)價(jià)函數(shù)從方向性、安全性、效率性方面充分評(píng)價(jià)速度空間內(nèi)最合理的速度。向方向子函數(shù)加入全局路徑信息約束：全局路徑距離當(dāng)前位置最近的節(jié)點(diǎn)之間的角度，新的方向函數(shù)保證機(jī)器人不斷跟隨新的目標(biāo)點(diǎn)前進(jìn)。該改進(jìn)方法有效地解決了動(dòng)態(tài)窗口法中單一目標(biāo)點(diǎn)容易陷入局部最優(yōu)的缺陷，規(guī)劃出的路徑即為全局最佳路徑，改進(jìn)后算法具體執(zhí)行步驟如下：

1）根據(jù)機(jī)器人傳感器構(gòu)建柵格地圖；

2）初始化A*及DWA參數(shù)；

3）A*算法進(jìn)行搜索，獲取關(guān)鍵全局路徑點(diǎn)信息；

4）根據(jù)機(jī)器人運(yùn)動(dòng)模型，計(jì)算當(dāng)前動(dòng)態(tài)窗口Vr；

5）根據(jù)機(jī)器人運(yùn)動(dòng)模型，計(jì)算預(yù)測(cè)時(shí)間dt內(nèi)運(yùn)動(dòng)軌跡點(diǎn)，同時(shí)根據(jù)評(píng)價(jià)函數(shù)對(duì)各軌跡點(diǎn)打分，分?jǐn)?shù)最高者為下一個(gè)dt時(shí)間機(jī)器人的運(yùn)動(dòng)速度；

6）判斷機(jī)器人是否到達(dá)目標(biāo)點(diǎn)，若抵達(dá)，尋路成功，否則，返回第4）步，繼續(xù)搜索。

5 仿真與分析

為了驗(yàn)證本文算法的有效性，在Win10 系統(tǒng)Matlab R2016a 環(huán)境下構(gòu)建柵格地圖進(jìn)行仿真實(shí)驗(yàn)。根據(jù)文獻(xiàn)［13］，機(jī)器人運(yùn)動(dòng)參數(shù)設(shè)置為Vmax=1.0m/s，ωmax=20°/s，V.max=0.2m/s2，ω?max=50°/s，速度分辨率和角速度分辨率分別為0.01m/s，1°/s。各評(píng)價(jià)子函數(shù)系數(shù)分別為0.05，0.4，0.1。起始點(diǎn)、目標(biāo)點(diǎn)分別為（1.5，2.5），（10.5，9.5）。

傳統(tǒng)A*算法規(guī)劃路徑如圖2所示，當(dāng)搜索鄰域由4鄰域變?yōu)?鄰域時(shí)，路徑節(jié)點(diǎn)數(shù)、路徑長度和轉(zhuǎn)彎角度有了明顯改善。實(shí)驗(yàn)數(shù)據(jù)如表1所示。可見，隨著搜索鄰域的增加，比如：24 鄰域［14］，48 鄰域［15］，運(yùn)行時(shí)間沒有明顯增加，轉(zhuǎn)彎角度降低，路徑更加平滑。同時(shí)，坐標(biāo)（3，3），（6，5），（8，9）處距離障礙物距離過近，導(dǎo)致路徑的安全性降低。

表1 仿真實(shí)驗(yàn)數(shù)據(jù)

圖2 全局最優(yōu)路徑及節(jié)點(diǎn)

圖3 全局最優(yōu)路徑及關(guān)鍵節(jié)點(diǎn)

由圖4 可以看出，傳統(tǒng)DWA 算法規(guī)劃路徑時(shí)會(huì)陷入局部最優(yōu)，尋路失?。挥蓤D5可知，與A*算法融合改進(jìn)后得到新的路徑為平滑曲線，距離障礙物距離更加合理，提高機(jī)器人行駛的安全性。本文得到運(yùn)動(dòng)學(xué)參數(shù)結(jié)果圖6所示，速度和角速度連續(xù)變化，整體符合要求。在迭代次數(shù)為300 次通過稠密障礙物時(shí)，機(jī)器人方向出現(xiàn)小幅度震蕩，在今后的研究中還需要改進(jìn)。

圖4 DWA路徑

圖5 改進(jìn)型DWA路徑

圖6 速度曲線

6 結(jié)語

為了提高機(jī)器人在復(fù)雜動(dòng)態(tài)環(huán)境下的表現(xiàn)，結(jié)合A*算法全局路徑最優(yōu)的特點(diǎn)和動(dòng)態(tài)窗口法局部最優(yōu)特點(diǎn)，提出一種新的算法。仿真結(jié)果表明，相較于單一算法，混合算法克服了局部最優(yōu)問題，平滑度上有了巨大改進(jìn)。但是在經(jīng)過稠密障礙物時(shí)，還需進(jìn)一步改進(jìn)。本文僅僅做了初步的研究和實(shí)驗(yàn)，在今后還將在多目標(biāo)、實(shí)時(shí)性方面做更多的探索。