摘 要：針對傳統(tǒng)動態(tài)窗口法應用在移動機器人路徑規(guī)劃中存在目標不可達、陷入局部最優(yōu)等缺陷，提出了一種改進動態(tài)窗口法。首先，在傳統(tǒng)動態(tài)窗口法的基礎上，考慮到移動機器人自身狀態(tài)和動力學模型，提出了障礙物膨脹模型，提高在真實環(huán)境下機器人運行的安全性。其次，針對傳統(tǒng)動態(tài)窗口法評價函數(shù)，提出了目標距離評價子函數(shù)，增強了移動機器人運動的穩(wěn)定性和軌跡的平滑性。利用MATLAB進行仿真實驗對比，結果表明：在靜態(tài)環(huán)境和動態(tài)環(huán)境下，改進動態(tài)窗口法的規(guī)劃速度和路徑規(guī)劃性能較傳統(tǒng)動態(tài)窗口法獲得了一定的提升，能夠較好適應移動機器人在復雜環(huán)境下的路徑規(guī)劃需要。

關鍵詞：移動機器人；路徑規(guī)劃；動態(tài)窗口法；柵格地圖；靜態(tài)避障；動態(tài)避障

中圖分類號：TP391.9；TP242 文獻標識碼：A 文章編號：2095-1302（2024）03-0-06

0 引 言

自主避障技術作為移動機器人路徑規(guī)劃問題中一個重要組成部分，其任務是在有障礙物的環(huán)境中，特別是人機共存的復雜環(huán)境下，尋找一條從起始點到目標點的無碰撞運動軌跡[1]。在實際應用過程中，由于環(huán)境變化且復雜，因此，能夠獲取環(huán)境信息的局部路徑規(guī)劃成為移動機器人實現(xiàn)自主避障技術的研究重點[2]。

在局部路徑規(guī)劃的研究中，常采用人工勢場法[3]、神經(jīng)網(wǎng)絡[4-5]、動態(tài)窗口法（Dynamic Window Approach， DWA）[6]等算法解決機器人路徑規(guī)劃問題。但由于人工勢場法易受外界環(huán)境變化的影響，使其在復雜環(huán)境中規(guī)劃能力不足；神經(jīng)網(wǎng)絡存在路徑規(guī)劃實時性較低、計算量較高等問題，因此不適合應用于大型復雜地圖中。除此之外，大部分局部路徑規(guī)劃算法并未考慮到移動機器人的自身狀態(tài)和動力學性能變化，因此在實際環(huán)境中難以滿足復雜多變的工作需求。

動態(tài)窗口法作為一種基于機器人動力學的在線避障算法，首先生成二維空間內有效的多組速度對（線速度、角速度），并在采樣時間內模擬每組速度對生成的運動軌跡，最后利用評價函數(shù)對多組軌跡進行選擇，選取評價最高的軌跡對應的速度作為下一時刻機器人的運動速度[7]。

針對傳統(tǒng)DWA算法容易陷入局部最優(yōu)、導致目標不可達等問題，國內外專家進行了廣泛的研究。王永雄等人[8]通過自適應調整目標函數(shù)中的權值，以提高機器人的運行速度和效率，但并未考慮環(huán)境中動態(tài)障礙物的未知干擾。卞永明等人[9]提出利用A*全局規(guī)劃路徑軌跡中的關鍵點，定義新的評價函數(shù)，為路徑規(guī)劃問題提出了一種新的思路。Liu 等人[10]提出了變化角度評估子函數(shù)，在一定程度上提高了算法的避障性能，但容易導致機器人在接近目的地時繞行過多，導致時間成本增加。

綜上，本文提出一種改進的DWA路徑規(guī)劃算法，首先通過對目標點和障礙物的分析，提出了障礙物膨脹模型。其次，針對算法易陷入局部最優(yōu)導致目標點不可達、路徑繞行過多等問題，定義了目標評價子函數(shù)，改進了傳統(tǒng)DWA算法的評價函數(shù)。在提升移動機器人路徑安全性的同時，提高了路徑避障性能。

1 基本動態(tài)窗口法

1.1 移動機器人運動學模型

動態(tài)窗口法需要根據(jù)采樣的速度獲取機器人的運動軌跡，因此，需要分析移動機器人運動學模型。設移動機器人的線速度為ν，角速度為ω，則t時刻移動機器人的位置坐標為（xt， yt），θt是t時刻機器人在世界坐標系XOY中與x軸的夾角，則（xt， yt， θt）可以表示t時刻下機器人的運動學位姿，其運動學模型如圖1所示。

假設在[t， t+1]兩個相鄰時刻移動機器人運動速度不發(fā)生變化，即機器人運動狀態(tài)為勻速直線運動，則兩相鄰時刻間機器人的運動路徑可看成直線，在t+1時刻下移動機器人的位姿為：

（1）

式中：Δt是兩時刻間的采樣間隔；ν（t）是移動機器人在t時刻的線速度；ω（t）為機器人在t時刻的角速度。

但在實際情況中，移動機器人的角速度和線速度不全為零，因此機器人的實際運動軌跡是一條曲線。在上述推導中，假設兩相鄰時刻間的移動軌跡是直線，這一假設并不是十分準確的，用圓弧代替直線將更能符合實際情況，圓弧半徑為：

（2）

則移動機器人運動軌跡可表示為：

（3）

1.2 移動機器人速度采樣空間

動態(tài)窗口法的基本原理是通過建立移動機器人運動模型，并對機器人的速度向量空間進行采樣，就可以模擬出機器人在下一時刻的軌跡，最后根據(jù)評價函數(shù)選擇最優(yōu)軌跡對應的速度作為機器人的運動速度。在理想條件下，移動機器人的速度（ν， ω）存在無窮多個取值，但由于機器人自身機械結構的限制和環(huán)境空間的約束，限制了機器人速度的采樣范圍，其約束條件如下：

（1）機器人受自身機械結構的限制，其速度存在最大值和最小值。

（4）

式中：Vm表示機器人所有速度的集合；vmin、vmax分別為機器人線速度的最小值和最大值；ωmin、ωmax分別為機器人角速度的最小值和最大值。

（2）移動機器人受電機性能等限制，存在最大加速度限制。

（5）

式中：Vd表示機器人可達的速度空間；vc和ωc表示機器人當前時刻的線速度和角速度；av表示最大線加速度；aω表示最大角加速度。

（3）為了避免機器人與障礙物發(fā)生碰撞，需要限制機器人的速度空間，以確保機器人在碰撞之前停止。

（6）

式中：dist（ν， ω）是速度（ν， ω）對應軌跡上與最近障礙物的

距離。

綜上所述，移動機器人的速度空間為：

（7）

在確定移動機器人的速度空間后，通過結合機器人運動學模型，生成相應的運動軌跡。動態(tài)窗口法采樣軌跡如圖2所示。

1.3 移動機器人軌跡評價函數(shù)

動態(tài)窗口法通過對速度空間的約束，在下一時刻機器人可獲取若干組采樣軌跡。為獲取最優(yōu)軌跡，采用評價函數(shù)對軌跡進行評價，設評價函數(shù)為G（ν， ω），綜合考慮方位角評價函數(shù)、距離評價函數(shù)及速度評價函數(shù)三個指標。

（8）

式中，heading（ν， ω）為目標方向角評價子函數(shù)，用以評價機器人當前方向和目標位置方向的偏移程度，其計算公式如下：

（9）

式中，θ為機器人預測軌跡末端與目標點相對應的機器人方向角之間的角度差，如圖3所示。

障礙物間隙dist（ν， ω）為距離評價函數(shù)，表示模擬軌跡末端與最近障礙物之間的距離，若dist（ν， ω）足夠大，則反映附近沒有障礙物，移動機器人很安全。

速度評價函數(shù)vel（ν， ω）是下一時刻機器人的線速度，用于評估移動機器人在向目標點移動過程中的前進速度。

評價函數(shù)G（ν， ω）由方位角評價函數(shù)、距離評價函數(shù)及速度評價函數(shù)三個指標組成。其中，σ為歸一化系數(shù)，α、β、γ則為三項指標對應權值。通過將三項指標歸一化處理，并結合指標相對應的權值，即可消除機器人移動軌跡的不連續(xù)性，提高軌跡的平滑性。

（10）

式中：n為移動機器人在一個模擬周期內所有的采樣軌跡；i為當前所評價的采樣軌跡。

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

2.1 動態(tài)窗口法的改進

傳統(tǒng)DWA算法在實際運行過程中，由于缺少對全局路徑的考慮，在采樣軌跡周圍存在障礙物時，需要通過對模擬軌跡進行評價，選取最優(yōu)方位角及速度進行避障。即使機器人與局部目標點的直線軌跡絕對安全，動態(tài)窗口法也會選擇距離障礙物較遠的采樣軌跡，導致軌跡轉彎頻率較高、繞行距離較遠、時間成本增加等問題。因此，當空間中的動態(tài)障礙物增加，移動機器人與障礙物發(fā)生碰撞的概率也會增加。除此之外，傳統(tǒng)DWA算法并未考慮障礙物形狀、特性等物理因素，而是將障礙物視為質點，但在實際應用中會增大與障礙物碰撞的風險，因此提出改進DWA路徑規(guī)劃算法。

2.1.1 障礙物虛擬碰撞模型

為保證機器人在路徑規(guī)劃方面的安全性，采用安全邊界對障礙物進行膨脹處理。而障礙物膨脹的尺寸和形狀對移動機器人規(guī)避障礙物有至關重要的作用。為滿足路徑規(guī)劃算法效率高、計算量小的需求，膨脹安全邊界需貼合實際地圖，隨地圖環(huán)境動態(tài)變化。因此，采用矩形安全邊界膨脹處理，在柵格地圖中對動態(tài)障礙物進行膨脹處理。其膨脹半徑按照機器人底盤的半徑進行選擇，以滿足機器人避障需求。本文提出了一種障礙物膨脹模型，如圖4所示。

2.1.2 改進評價函數(shù)

在傳統(tǒng)的DWA算法中，原評價函數(shù)的評價標準相對獨立，在不同環(huán)境中最優(yōu)權重也有較大差異。若障礙物直接存在于全局規(guī)劃路徑上，則機器人將保持朝向障礙物移動，直到機器人接近障礙物再進行轉彎，此時將不可避免地會導致機器人與障礙物之間的規(guī)避距離減少。隨著時間的增加，移動機器人的碰撞預警和緊急停止的反應時間也相應縮短，給機器人增加了潛在的危險。一旦全局路徑上障礙物分布密集，由于機器人無法及時規(guī)避障礙物，容易陷入障礙物密集區(qū)域中，導致機器人需要更多的轉彎、加速和減速操作才能擺脫困境。為此，我們在評估函數(shù)中添加了一個目標距離評價子函數(shù)，以提高機器人運動的穩(wěn)定性和路徑的平滑性。其數(shù)學模型如下：

（11）

式中：（xstart， ystart）為移動機器人初始點的位置坐標；（xgoal， ygoal）為局部目標點的位置坐標；（xcurrent， ycurrent）為移動機器人軌跡末端點的位置坐標。

因此改進后的DWA算法評價函數(shù)為：

（12）

2.2 改進DWA算法流程

改進DWA算法流程具體步驟如下：

步驟1：根據(jù)移動機器人搭載的傳感器獲取環(huán)境信息構建柵格地圖，以機器人底盤半徑對障礙物點進行膨脹。

步驟2：初始化移動機器人參數(shù)，獲取當前方向角、角速度及線速度。

步驟3：根據(jù)機器人運動學狀態(tài)和機械特性，確定有效的速度及加速度范圍。

步驟4：根據(jù)機器人運動學狀態(tài)，對采樣速度進行模擬，生成運動軌跡樣本。

步驟5：根據(jù)改進DWA算法評價函數(shù)對采樣軌跡進行評價，從中選取最優(yōu)軌跡，并將最優(yōu)軌跡對應的速度作為下一時刻機器人的運動速度。

步驟6：執(zhí)行最優(yōu)速度，判斷機器人是否到達目標點，若到達，則結束運行，輸出最優(yōu)軌跡；若未到達，則重復步驟4。

3 改進動態(tài)窗口法仿真實驗

為了驗證改進DWA算法的路徑規(guī)劃性能，本文在10×10規(guī)模的柵格地圖下進行仿真實驗。設置起始點用星號表示，位置坐標為（0.5，0.5）；目標點用圓圈表示，位置坐標為（9.5，9.5）。為了客觀評價算法的性能指標，驗證改進的DWA算法和傳統(tǒng)DWA算法在靜態(tài)環(huán)境和動態(tài)環(huán)境下的路徑規(guī)劃效果，對比算法需基于相同實驗條件，其仿真實驗參數(shù)見表1所列。在仿真實驗中用實線線條表示改進DWA算法的路徑規(guī)劃和性能指標曲線，虛線線條表示基本DWA算法的路徑規(guī)劃和性能指標曲線。

在仿真實驗中，定義移動機器人到達目標點0.5 m范圍內，則視為到達。

3.1 改進DWA靜態(tài)路徑規(guī)劃仿真實驗

靜態(tài)環(huán)境下，改進DWA算法和基本DWA算法的路徑規(guī)劃性能結果對比如圖5所示。其中，圖5（a）為移動機器人從起始點到目標點的最優(yōu)路徑軌跡，圖5（b）～圖5（d）為運動過程中機器人的方向角波動曲線、角速度波動曲線及線速度波動曲線。表2展示了在10×10規(guī)模的靜態(tài)柵格地圖下，改進DWA算法和傳統(tǒng)DWA算法的性能指標。

綜合分析圖5和表2，可以得出，在靜態(tài)地圖下，改進的DWA算法相較基本的DWA算法在最優(yōu)路徑規(guī)劃長度、算法運行時間方面都有了一定的提升。其中，在路徑長度方面改進的DWA算法較基本DWA算法提升了9.18%，算法運行時間提升了26.62%。算法運行時間縮短，不僅因為改進的DWA算法選擇的最優(yōu)路徑較短，還因為改進的DWA算法在運行過程中通過調整方向角轉向目標點位置，使機器人方向角轉彎幅度小于基本的DWA算法，其轉彎幅度減少了37.13%，如圖5（b）所示。

在圖5（c）所示的角速度波動曲線中，改進DWA算法較基本DWA算法在角速度波動幅度方面提升了17.51%。在圖5（d）中，改進的DWA算法線速度在運行過程的大部分時間都處于最大線速度0.62 m/s，雖然基本DWA算法的最大線速度略高于改進DWA算法的最大線速度，但改進DWA算法在第58～129次迭代中由于距障礙物較近，需要減緩速度轉變方向角，其線速度低于最大線速度，而基本DWA算法僅在第34～59、159～171次迭代內達到最大線速度，其余時間由于算法的大幅度波動，基本DWA算法的線速度都低于改進DWA算法線速度。從整條軌跡上看，改進DWA算法轉向角更小，線速度和角速度波動幅度、次數(shù)方面均小于基本DWA算法，因此得到的最優(yōu)軌跡更為平滑，算法運行時間更短。

3.2 改進DWA動態(tài)路徑規(guī)劃仿真實驗

由于靜態(tài)環(huán)境具有一定的局限性，并不能準確評價算法在障礙物移動的動態(tài)環(huán)境下算法的性能指標。因此設計動態(tài)環(huán)境驗證改進DWA算法的性能，定義動態(tài)障礙物可平移運動，坐標為（1.5，2.0）、（4.5，5.0）、（5.5，4.0）和（4.5，9.5）。動態(tài)仿真實驗避障結果如圖6所示，圖中灰色柵格表示靜態(tài)障礙物，其規(guī)模為1個單位長度的正方形，動態(tài)障礙物為0.5個單位長度的圓形。

在動態(tài)障礙物柵格地圖中，改進的DWA算法同樣也展現(xiàn)出較好的路徑規(guī)劃性能。由表1可知，在最優(yōu)路徑軌跡長度方面，改進的DWA算法獲得的路徑長度為13.501 3，相較傳統(tǒng)DWA算法獲得的路徑長度14.268 9，縮短了5.38%。在算法運行時間方面，改進的DWA算法相較基本DWA算法減少了15.12%。在軌跡平滑度方面，從圖6（a）中可以看出，改進的DWA算法獲得的軌跡更為平滑，方向角朝向目標方向，因此機器人在運動過程中的轉向次數(shù)較少、軌跡更為平滑。在算法性能波動曲線中，改進的DWA算法的角速度和方向角的波動次數(shù)和波動幅度都小于基本DWA算法。相較于基本DWA算法，改進DWA算法的角速度波動幅度降低了20.51%、方向角波動幅度降低了1.95%。在圖6（d）的線速度波動曲線中，雖然改進的DWA算法和基本DWA算法的最大線速度都為0.62 m/s，但從線速度波動曲線中可以看出，基本的DWA算法在機器人運行過程中，其線速度處于最大線速度的時間略大于改進的DWA算法，通過舍棄部分方向的穩(wěn)定性，從而獲得了更快的線速度。而改進的DWA算法通過調整自身方向角轉向目標點，在方向角和角速度方面的波動小于基本DWA算法的波動幅度，從而獲得了軌跡更為平滑和更短的最優(yōu)路線。綜上所述，改進的DWA算法的運行效率較基本的DWA算法獲得了一定的提升，在速度控制方面具有更強的穩(wěn)定性，從而生成的軌跡更為平滑、路徑更短。

4 結 語

本文針對基本DWA算法出現(xiàn)的目標點不可達、局部最優(yōu)等問題，提出了一種改進的DWA路徑規(guī)劃算法。通過充分考慮了移動機器人的運動學模型和機械特征，引入了障礙物膨脹方法，根據(jù)目標與待評價軌跡的距離，定義了目標評價子函數(shù)，對評價函數(shù)進行了改進。仿真實驗結果表明，本文提出的改進DWA算法能夠有效提高軌跡的平滑性和速度的穩(wěn)定性，獲得了較好的規(guī)劃軌跡。在后續(xù)的研究中，會進一步將改進算法應用于實物實驗中，驗證改進算法的有效性。

參考文獻

[1] OROKO J A，NYAKOE G N. Obstacle avoidance and path planning schemes for autonomous navigation of a mobile robot：a review [C]// Proceedings of the Sustainable Research and Innovation Conference. [S.l.]：[s.n.]：2022：314-318.

[2] PATLE B K，PANDEY A，PARHI D R K，et al. A review： on path planning strategies for navigation of mobile robot [J]. Defence technology，2019，15（4）：582-606.

[3] WANG P，GAO S，LI L，et al. Obstacle avoidance path planning design for autonomous driving vehicles based on an improved artificial potential field algorithm [J]. Energies，2019，12（12）：2342.

[4]陳秋蓮，鄭以君，蔣環(huán)宇，等.基于神經(jīng)網(wǎng)絡改進粒子群算法的動態(tài)路徑規(guī)劃[J].華中科技大學學報（自然科學版），2021，49（2）：51-55.

[5]鄭以君. 基于神經(jīng)網(wǎng)絡改進粒子群算法的機器人路徑規(guī)劃研究[D].南寧：廣西大學，2021.

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

[7] ZHANG F，LI N，XUE T，et al. An improved dynamic window approach integrated global path planning [C]// 2019 IEEE International Conference on Robotics and Biomimetics（ROBIO）.Dali，China：IEEE，2019：2873-2878.

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

[9]卞永明，季鵬成，周怡和，等.基于改進型DWA的移動機器人避障路徑規(guī)劃[J].中國工程機械學報，2021，19（1）：44-49.

[10] LIU Tianyu，YAN Ruixin，WEI Guangrui，et al. Local path planning algorithm for blind-guiding robot based on improved DWA algorithm [C]// 2019 Chinese Control And Decision Conference （CCDC）. Nanchang，China：IEEE，2019：6169-6173.

作者簡介：陳靜夷（1998—），女，碩士，研究方向為移動機器人同步定位與建圖。