基于改進動態窗口法的機器人路徑規劃

鄒文 韓丙辰

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

[關鍵詞]移動機器人；路徑規劃；安全性；空間模型

[中圖分類號]TP 242

[文獻標志碼]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 引言

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

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

1 環境空間模型

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

1.1 環境描述

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

1.2 障礙物空間模型

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

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

1.3 障礙物分類檢測

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

2 改進動態窗口法

2.1 傳統動態窗口法

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

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

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

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

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

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

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

2.2 擴展評價函數

動態窗口法的評價函數結構清晰，擴展性強。本文在此評價函數的基礎上增加了距離評價子函數G，取值范圍為[0，1]，其計算方法如式（5）所示：

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

式中，δ為距離評價子函數的加權系數。

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

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

2.3 融合A*算法

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

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

3 實驗及分析

為了驗證本文改進動態窗口法的性能及合理性，在Matlab 2021b下進行仿真實驗，并設置了障礙物環境進行導航實驗，同時對各實驗結果進行分析。

3.1 仿真實驗

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

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

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

3.2 自主導航實驗

為驗證本文的改進算法在機器人路徑規劃中的實用性，本文在如圖8所示的環境中進行自主導航實驗。本實驗用Gmapping算法構建柵格代價地圖后，在環境中設置“凹”型陷阱，并添加其他障礙物。機器人自主導航的移動路徑如圖9所示。

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

4 結束語

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

[參考文獻]

[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]何玉慶，趙憶文，韓建達，等.與人共融——機器人技術發展的新趨勢[J].機器人產業，2015（5）：74-80.

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

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

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

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

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

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

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

[10]槐創鋒，郭龍，賈雪艷，等.改進A*算法與動態窗口法的機器人動態路徑規劃[J].計算機工程與應用，2021，57（8）：244-248.

[11]趙偉，吳子英.雙層優化A*算法與動態窗口法的動態路徑規劃[J].計算機工程與應用，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]阮曉鋼，張家輝，黃靜，等.一種結合內在動機理論的移動機器人環境認知模型[J].控制與決策，2021，36（9）：2211-2217.

（責任編輯 白麗媛；責任校對 柴 智）

[收稿日期]2022-10-11

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

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