鄒文 韓丙辰

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

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

[中圖分類號]TP 242

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

[文章編號]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 引言

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

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

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

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

1.1 環(huán)境描述

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

1.2 障礙物空間模型

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

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

1.3 障礙物分類檢測

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

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

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

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

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

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

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

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

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

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

2.2 擴展評價函數(shù)

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

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

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

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

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

2.3 融合A*算法

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

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

3 實驗及分析

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

3.1 仿真實驗

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

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

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

3.2 自主導(dǎo)航實驗

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

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

4 結(jié)束語

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

[參考文獻(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á)，等.與人共融——機器人技術(shù)發(fā)展的新趨勢[J].機器人產(chǎn)業(yè)，2015（5）：74-80.

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

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

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

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

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

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

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

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

[11]趙偉，吳子英.雙層優(yōu)化A*算法與動態(tài)窗口法的動態(tài)路徑規(guī)劃[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)在動機理論的移動機器人環(huán)境認(rèn)知模型[J].控制與決策，2021，36（9）：2211-2217.

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

[收稿日期]2022-10-11

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

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