基于改進(jìn)動(dòng)態(tài)窗口法的機(jī)器人路徑規(guī)劃

鄒文 韓丙辰

[摘 要] 針對(duì)傳統(tǒng)動(dòng)態(tài)窗口法安全性低、規(guī)劃范圍有限等問題，提出一種聯(lián)合動(dòng)態(tài)窗口法和A*算法的路徑規(guī)劃方法。該方法首先根據(jù)路徑前方障礙物種類的不同，在代價(jià)地圖中投影不同的空間模型；然后在動(dòng)態(tài)窗口法的評(píng)價(jià)函數(shù)中引入距離評(píng)價(jià)子函數(shù)，使機(jī)器人能對(duì)不同類別的障礙物保持不同的避讓距離；最后融合A*算法，提供全局路徑指引。對(duì)比實(shí)驗(yàn)表明，與傳統(tǒng)算法相比，改進(jìn)算法擁有全局規(guī)劃能力，并且提升了機(jī)器人路徑規(guī)劃的安全性。

[關(guān)鍵詞]移動(dòng)機(jī)器人；路徑規(guī)劃；安全性；空間模型

[中圖分類號(hào)]TP 242

[文獻(xiàn)標(biāo)志碼]A

[文章編號(hào)]1005-0310（2023）06-0067-06

Robot Path Planning Based on Improved Dynamic

Window Approach

ZOU Wen，HAN Bingchen

（1.College of Electronic Engineering， Sichuan Vocational and Technical College， Suining Sichuan 629000， China;

2.Department of Physics， Taiyuan Normal University， Jinzhong Shanxi 030619， China）

Abstract： A path planning method combining dynamic window method and A* algorithm is proposed to address the issues of low security and limited planning scope of traditional dynamic window method. This method first projects different spatial models in the cost map based on the types of obstacles in front of the path， and then introduces a distance term subfunction in the evaluation function of the dynamic window method， allowing the robot to maintain different avoidance distances for non-classified obstacles. Finally， it integrates the A* algorithm to provide global path guidance. Comparative experiments show that compared with traditional algorithms， the improved algorithm has global planning ability and improves the safety of robot path planning.

Keywords： Mobile robot；Path planning；Safety performance；Space model

0 引言

隨著機(jī)器人技術(shù)的飛速發(fā)展，越來越多的機(jī)器人進(jìn)入社會(huì)生產(chǎn)生活中。路徑規(guī)劃是實(shí)現(xiàn)機(jī)器人自主導(dǎo)航的重要組成部分，可分為全局靜態(tài)路徑規(guī)劃和局部動(dòng)態(tài)路徑規(guī)劃。其中，動(dòng)態(tài)規(guī)劃是指機(jī)器人在運(yùn)動(dòng)時(shí)，依靠自身傳感器檢測(cè)到環(huán)境中出現(xiàn)的未知障礙物，使用路徑規(guī)劃算法實(shí)現(xiàn)合理、安全地避開障礙物并到達(dá)目標(biāo)位置的過程。

動(dòng)態(tài)窗口法（Dynamic Window Approach， DWA）是常用的機(jī)器人局部規(guī)劃算法，它既能有效避開路徑中出現(xiàn)的未知障礙物，又能提升路徑的平滑度。傳統(tǒng)的DWA算法對(duì)所有障礙物的避讓距離相同，規(guī)劃范圍有限，容易陷入地圖陷阱，無法滿足多種場(chǎng)景的局部動(dòng)態(tài)規(guī)劃需求，因此，移動(dòng)機(jī)器人局部規(guī)劃算法的改進(jìn)成為當(dāng)前的研究熱點(diǎn)之一。王永雄等提出自適應(yīng)動(dòng)態(tài)窗口法，根據(jù)障礙物的距離和密度自動(dòng)調(diào)整機(jī)器人的最大速度限制，從而獲得最佳運(yùn)行速度組和最優(yōu)路徑。封碩等在動(dòng)態(tài)窗口法評(píng)價(jià)函數(shù)中增加了動(dòng)態(tài)障礙物速度的評(píng)價(jià)函數(shù)，從而避開高速移動(dòng)的障礙物。張森等通過對(duì)人的緊張空間和機(jī)器人的運(yùn)動(dòng)趨勢(shì)進(jìn)行建模，進(jìn)而優(yōu)化人在機(jī)器人運(yùn)動(dòng)過程中的舒適感受。王洪斌等用預(yù)瞄偏差角算法追蹤動(dòng)態(tài)目標(biāo)，得到動(dòng)態(tài)障礙物的位置信息及運(yùn)動(dòng)趨勢(shì)，進(jìn)而提升路徑規(guī)劃效率。宋莉等對(duì)Q-Learning（QL）算法進(jìn)行離散化處理及降低狀態(tài)空間維度，并在獎(jiǎng)懲函數(shù)上增加了ε-探索和動(dòng)作選擇算法來選擇執(zhí)行速度組，增強(qiáng)了機(jī)器人在復(fù)雜情況下的適應(yīng)能力。鄒文等通過增加動(dòng)態(tài)窗口法的運(yùn)動(dòng)狀態(tài)，提高了機(jī)器人的靈活性。文獻(xiàn)[9]～[11]均將不同程度改進(jìn)的A*算法與動(dòng)態(tài)窗口法融合，既能做到全局不丟失路徑，又能避讓運(yùn)動(dòng)過程中遇到的未知物體。

1 環(huán)境空間模型

機(jī)器人在路徑規(guī)劃前，需要根據(jù)環(huán)境信息建立環(huán)境模型地圖。機(jī)器人根據(jù)傳感器采集到的信息，將障礙物的種類和空間位置投影在二維地圖模型中，并根據(jù)代價(jià)地圖來完成規(guī)劃和移動(dòng)。

1.1 環(huán)境描述

環(huán)境模型地圖可分為全局靜態(tài)地圖和局部動(dòng)態(tài)模型地圖。全局靜態(tài)地圖用來表示在機(jī)器人整個(gè)運(yùn)動(dòng)環(huán)境中相對(duì)固定不變的物體，如墻體、家具等。本文將全局環(huán)境投影在連續(xù)緊湊的二維柵格平面中，構(gòu)建全局靜態(tài)環(huán)境模型。每個(gè)柵格以概率形式表示是否被固定障礙物占據(jù)，黑色柵格表示占據(jù)狀態(tài)，白色柵格表示空閑狀態(tài)，如圖1所示。

1.2 障礙物空間模型

機(jī)器人在運(yùn)動(dòng)過程中，除相對(duì)固定不變的靜態(tài)障礙物外，往往還會(huì)遇到位置改變的靜態(tài)障礙物和移動(dòng)的未知障礙物。本文按照物體自身能否移動(dòng)將障礙物分為靜態(tài)障礙物和動(dòng)態(tài)障礙物兩種。其中，將自身不會(huì)發(fā)生移動(dòng)的物體（如墻體、家具、植物等）定義為靜態(tài)障礙物，將自身會(huì)發(fā)生移動(dòng)的人和動(dòng)物等定義為動(dòng)態(tài)障礙物，分別賦予其不同的安全范圍值。本文基于障礙物的特性和種類建立空間模型，來描述障礙物的安全范圍空間和舒適范圍空間。

靜態(tài)障礙物和動(dòng)態(tài)障礙物安全區(qū)域的半徑大小不同，對(duì)于動(dòng)態(tài)障礙物，除安全區(qū)域外，本文還在其外添加了一層舒適區(qū)域。其中，安全區(qū)域表示機(jī)器人禁止進(jìn)入的區(qū)域，舒適區(qū)域表示機(jī)器人可以進(jìn)入的區(qū)域，但進(jìn)入舒適區(qū)域后機(jī)器人會(huì)減速并優(yōu)先選擇遠(yuǎn)離障礙物的模擬軌跡。圖2（a）為傳統(tǒng)方法對(duì)障礙物的膨脹模型，圖2（b）是本文提出的膨脹模型，圖2（b）中左邊為動(dòng)態(tài)障礙物，右邊為靜態(tài)障礙物。

1.3 障礙物分類檢測(cè)

本文使用Yolov3算法來識(shí)別機(jī)器人在移動(dòng)過程中遇到的障礙物。機(jī)器人首先識(shí)別出障礙物，然后對(duì)物體進(jìn)行靜態(tài)或動(dòng)態(tài)分類，最后將障礙物對(duì)應(yīng)的空間模型投影在代價(jià)地圖中，并在代價(jià)地圖中對(duì)障礙物進(jìn)行擴(kuò)展。機(jī)器人使用搭載的攝像頭對(duì)環(huán)境中物體的識(shí)別效果如圖3所示。

2 改進(jìn)動(dòng)態(tài)窗口法

2.1 傳統(tǒng)動(dòng)態(tài)窗口法

動(dòng)態(tài)窗口法主要通過采樣多個(gè)速度組（線速度、角速度），并模擬它們?cè)陬A(yù)測(cè)時(shí)間范圍內(nèi)的軌跡，把機(jī)器人與障礙物碰撞的軌跡刪掉，最后通過評(píng)價(jià)函數(shù)選取出最優(yōu)的速度組。

假定在一個(gè)最小單位時(shí)間間隔內(nèi)，機(jī)器人做勻速直線運(yùn)動(dòng)。線速度v和角速度ω反映機(jī)器人的運(yùn)動(dòng)狀態(tài)，其運(yùn)動(dòng)模型可用式（1）表示：

在速度空間（v，ω）范圍內(nèi)存在無數(shù)個(gè)速度組，需要根據(jù)實(shí)際情況對(duì)采樣速度進(jìn)行限制。速度組約束如式（2）所示：

因加速度受機(jī)器人自身電機(jī)的性能約束，在一個(gè)時(shí)間間隔內(nèi)，實(shí)際能到達(dá)的速度組如式（3）所示：

式中：（vc，ωc）代表當(dāng)前速度組，（va，ωa）代表機(jī)器人的最大加速度組，（vb，ωb）代表機(jī)器人的最大減速度組。

在采樣并模擬的各條軌跡中，除機(jī)器人與障礙物發(fā)生碰撞的軌跡之外，還有很多可行的模擬軌跡，最后通過評(píng)價(jià)函數(shù)來篩選出最優(yōu)的模擬軌跡。評(píng)價(jià)函數(shù)如式（4）所示：

式中：G為方位角評(píng)價(jià)子函數(shù)，表示模擬速度組在軌跡末端時(shí)的朝向與目標(biāo)位置航向角之間的角度偏差；G為障礙物評(píng)價(jià)子函數(shù)，表示模擬軌跡與障礙物之間最近的距離；G為速度評(píng)價(jià)子函數(shù)，表示模擬軌跡對(duì)應(yīng)的速度大??；σ為平滑系數(shù)，α、β、γ為各個(gè)子函數(shù)的加權(quán)系數(shù)。

2.2 擴(kuò)展評(píng)價(jià)函數(shù)

動(dòng)態(tài)窗口法的評(píng)價(jià)函數(shù)結(jié)構(gòu)清晰，擴(kuò)展性強(qiáng)。本文在此評(píng)價(jià)函數(shù)的基礎(chǔ)上增加了距離評(píng)價(jià)子函數(shù)G，取值范圍為[0，1]，其計(jì)算方法如式（5）所示：

式中：d代表采樣速度組模擬軌跡與障礙物之間的最小距離，取值范圍為（r，r）；r表示各類障礙物的禁止區(qū)域半徑；r表示各類障礙物的舒適區(qū)域半徑。優(yōu)化后的評(píng)價(jià)函數(shù)如式（6）所示：

式中，δ為距離評(píng)價(jià)子函數(shù)的加權(quán)系數(shù)。

改進(jìn)算法對(duì)障礙物空間模型的模擬軌跡示意圖如圖4所示。

圖4中：軌跡①和軌跡②進(jìn)入安全范圍內(nèi)，所以將其剔除；軌跡③和軌跡④在舒適范圍內(nèi)且在安全范圍外，距離評(píng)價(jià)子函數(shù)的得分按式（5）求得，越靠近障礙物中心的模擬軌跡，其得分越低；軌跡⑤和軌跡⑥在舒適范圍以外，距離評(píng)價(jià)子函數(shù)的得分均為1。

2.3 融合A*算法

由于動(dòng)態(tài)窗口法僅向前模擬一定時(shí)間內(nèi)的軌跡，所以其規(guī)劃的路徑范圍有限，很容易陷入“L”型、“凹”型等陷阱中。如圖5中綠色虛線所示，由于DWA算法規(guī)劃的范圍有限，機(jī)器人會(huì)從①處運(yùn)動(dòng)到②處，進(jìn)入地圖陷阱中，而最優(yōu)路徑應(yīng)該是與圖5中紅色實(shí)線類似的路徑。

本文在機(jī)器人局部路徑規(guī)劃前融合全局規(guī)劃算法（A*算法）。首先，將A*算法規(guī)劃的路徑點(diǎn)提取出來，然后將該路徑點(diǎn)中多余的轉(zhuǎn)折點(diǎn)刪除，留下各階段的關(guān)鍵點(diǎn)，再將各個(gè)關(guān)鍵點(diǎn)作為局部規(guī)劃的臨時(shí)目標(biāo)點(diǎn)，指引機(jī)器人到達(dá)最終的目標(biāo)位置。融合A*算法使機(jī)器人得到全局路徑的指引，從而避免陷入局部路徑陷阱中。

3 實(shí)驗(yàn)及分析

為了驗(yàn)證本文改進(jìn)動(dòng)態(tài)窗口法的性能及合理性，在Matlab 2021b下進(jìn)行仿真實(shí)驗(yàn)，并設(shè)置了障礙物環(huán)境進(jìn)行導(dǎo)航實(shí)驗(yàn)，同時(shí)對(duì)各實(shí)驗(yàn)結(jié)果進(jìn)行分析。

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

仿真實(shí)驗(yàn)分別用A*算法、改進(jìn)的DWA算法、融合A*的傳統(tǒng)DWA算法、融合A*的改進(jìn)DWA算法，在相同環(huán)境下進(jìn)行。各算法規(guī)劃出的路徑如圖6所示，避讓距離統(tǒng)計(jì)如表1和圖7所示。

由圖6（a）可知，A*算法規(guī)劃出的路徑轉(zhuǎn)折角度大，離障礙物的距離約為0.5個(gè)單位柵格。由圖6（b）可知，未融合A*算法的改進(jìn)DWA算法的規(guī)劃范圍有限，容易陷入局部路徑陷阱中，此時(shí)路徑規(guī)劃失敗。由圖6（c）和圖6（d）可知，融合A*的DWA算法能到達(dá)終點(diǎn)。

對(duì)比圖6（c）和圖6（d），并聯(lián)合表1和圖7可知，融合A*的傳統(tǒng)DWA算法與所有障礙物的避讓距離均為1個(gè)單位柵格，而融合A*的改進(jìn)DWA算法與動(dòng)態(tài)障礙物的避讓距離為1.5個(gè)單位柵格，與靜態(tài)障礙物的避讓距離為1個(gè)單位柵格。由此可知，傳統(tǒng)算法對(duì)所有障礙物只能保持相同的安全距離，融合A*算法的改進(jìn)DWA算法對(duì)動(dòng)態(tài)障礙物、靜態(tài)障礙物能有針對(duì)性地選擇不同的避讓距離。改進(jìn)算法不僅提升了路徑規(guī)劃的安全性，也擴(kuò)大了路徑規(guī)劃的范圍。

3.2 自主導(dǎo)航實(shí)驗(yàn)

為驗(yàn)證本文的改進(jìn)算法在機(jī)器人路徑規(guī)劃中的實(shí)用性，本文在如圖8所示的環(huán)境中進(jìn)行自主導(dǎo)航實(shí)驗(yàn)。本實(shí)驗(yàn)用Gmapping算法構(gòu)建柵格代價(jià)地圖后，在環(huán)境中設(shè)置“凹”型陷阱，并添加其他障礙物。機(jī)器人自主導(dǎo)航的移動(dòng)路徑如圖9所示。

圖9表明，機(jī)器人能提前規(guī)劃全局路徑，避開“凹”型陷阱，并且其對(duì)動(dòng)態(tài)障礙物（人）的避讓距離大于靜態(tài)障礙物，證明了改進(jìn)算法在實(shí)際應(yīng)用中的可行性。

4 結(jié)束語

機(jī)器人與障礙物的合理避讓距離是路徑規(guī)劃時(shí)應(yīng)考慮的重要安全性因素。針對(duì)傳統(tǒng)動(dòng)態(tài)窗口法對(duì)所有障礙物采取相同避讓距離的問題，本文通過構(gòu)建不同障礙物空間模型和擴(kuò)展評(píng)價(jià)函數(shù)來改進(jìn)動(dòng)態(tài)窗口法；針對(duì)動(dòng)態(tài)窗口法規(guī)劃范圍有限的問題，本文融合A*算法為機(jī)器人提供全局指引。仿真對(duì)比實(shí)驗(yàn)和自主導(dǎo)航實(shí)驗(yàn)表明，融合改進(jìn)算法能讓機(jī)器人根據(jù)環(huán)境中各類物體的運(yùn)動(dòng)特性合理規(guī)劃出不同的安全避讓距離，而且不會(huì)落入地圖陷阱。相比傳統(tǒng)算法，改進(jìn)算法能讓機(jī)器人的移動(dòng)過程更安全、更合理。

[參考文獻(xiàn)]

[1]SARIFF N， BUNIYAMIN N. An overview of autonomous mobile robot path planning algorithms[C]//4th Student Conference on Research and Development. Piscataway， USA： IEEE，2007： 182-187.

[2]何玉慶，趙憶文，韓建達(dá)，等.與人共融——機(jī)器人技術(shù)發(fā)展的新趨勢(shì)[J].機(jī)器人產(chǎn)業(yè)，2015（5）：74-80.

[3]王永雄，田永永，李璇，等.穿越稠密障礙物的自適應(yīng)動(dòng)態(tài)窗口法[J].控制與決策，2019，34（5）：927-936.

[4]封碩，吉現(xiàn)友，程博，等.融合動(dòng)態(tài)障礙物運(yùn)動(dòng)信息的路徑規(guī)劃算法[J].計(jì)算機(jī)工程與應(yīng)用，2022，58（21）：279-285.

[5]張森，劉景泰.基于多維度服務(wù)情景的人的舒適需求建模[J].機(jī)器人，2019，41（4）：493-506.

[6]王洪斌，尹鵬衡，鄭維，等.基于改進(jìn)的A*算法與動(dòng)態(tài)窗口法的移動(dòng)機(jī)器人路徑規(guī)劃[J].機(jī)器人，2020，42（3）：346-353.

[7]宋莉，李彩虹，王小宇，等.移動(dòng)機(jī)器人局部避障路徑規(guī)劃仿真研究[J].計(jì)算機(jī)仿真，2018，35（7）：279-284.

[8]鄒文，韓丙辰，李鵬飛，等.融合改進(jìn)A*算法和優(yōu)化動(dòng)態(tài)窗口法的路徑規(guī)劃[J/OL].計(jì)算機(jī)集成制造系統(tǒng).（2022-04-12）[2022-10-11].http：//kns.cnki.net/kcms/detail/11.5946.TP.20220409.1931.002.html.

[9]劉建娟，薛禮啟，張會(huì)娟，等.融合改進(jìn)A*與DWA算法的機(jī)器人動(dòng)態(tài)路徑規(guī)劃[J].計(jì)算機(jī)工程與應(yīng)用，2021，57（15）：73-81.

[10]槐創(chuàng)鋒，郭龍，賈雪艷，等.改進(jìn)A*算法與動(dòng)態(tài)窗口法的機(jī)器人動(dòng)態(tài)路徑規(guī)劃[J].計(jì)算機(jī)工程與應(yīng)用，2021，57（8）：244-248.

[11]趙偉，吳子英.雙層優(yōu)化A*算法與動(dòng)態(tài)窗口法的動(dòng)態(tài)路徑規(guī)劃[J].計(jì)算機(jī)工程與應(yīng)用，2021，57（22）：295-303.

[12]LIU Y J，ZHAO C S，WEI Y L.A particle PHD filter for dynamic grid map building towards indoor environment[J].Applied Sciences，2021，11（15）：6891.

[13]阮曉鋼，張家輝，黃靜，等.一種結(jié)合內(nèi)在動(dòng)機(jī)理論的移動(dòng)機(jī)器人環(huán)境認(rèn)知模型[J].控制與決策，2021，36（9）：2211-2217.

（責(zé)任編輯 白麗媛；責(zé)任校對(duì) 柴 智）

[收稿日期]2022-10-11

[作者簡(jiǎn)介]鄒文（1996—），男，四川遂寧人，四川職業(yè)技術(shù)學(xué)院電子工程學(xué)院助教，主要研究方向?yàn)闄C(jī)器人路徑規(guī)劃。

[通訊作者]韓丙辰（1976—），男，山西侯馬人，太原師范學(xué)院物理系教授，博士，主要研究方向?yàn)楣怆姍z測(cè)技術(shù)。E-mail：hanbc@tynu.edu.cn