基于動態障礙物的機器人避障路徑規劃方法

郭翰卿，付麗霞，張 勇，毛劍琳

（昆明理工大學 信息工程與自動化學院，云南 昆明 650500）

0 引 言

隨著我國自動化技術的發展，自主移動機器人運用越來越廣泛。路徑規劃[1]是移動機器人的關鍵技術之一，路徑規劃問題可分為全局路徑規劃和局部路徑規劃[2-3]。當前，多因素復雜地形[4]和動態障礙物環境[5]下的避障路徑規劃是最具有挑戰性的課題之一，是研究發展的新動向。因此，研究一種能實現動態避障的路徑規劃算法很有必要。

對于靜態全局路徑規劃，常用的算法有A*算法[6]、蟻群算法[7]、神經網絡算法[8]等。A*算法由于其算法效率高而有著較為廣泛的應用，對于改進A*算法，文獻[9]對A*算法的搜索效率進一步提升，并且結合視覺分析，但是其還是基于靜態環境，并未考慮動態環境下的復雜因素。國內外對于障礙物信息不確定的動態局部路徑規劃也有所研究，文獻[10]在傳統動態窗口法中引進控制理念，使局部規劃更加精確，但該算法沒有考慮全局規劃。文獻[11]在動態窗口法中結合全局考慮，實現了一部分隨機避障，但是該算法還是在障礙物固定的環境下對機器人移動路徑進行規劃，并未考慮運動障礙物，算法有一定的局限性。

上述改進算法對路徑規劃問題做了優化，但是并未完全解決當前復雜環境尤其是動態障礙物環境下的路徑規劃需求。針對這個問題，設計一種融合障礙物信息的動態窗口法，并且引入改進A*算法融合提升路徑規劃的全局最優性。通過實驗證明，本文所提的算法能在保證全局最優的前提下實現移動機器人動態避障。

1 改進A*算法

1.1 傳統A*算法

A*算法是可以實現全局路徑規劃的啟發式算法，主要應用于柵格法環境建模的地圖中。其核心思想為：從路徑起點開始對周圍鄰域搜索，計算當前節點鄰域內節點到起始點的實際代價和到目標點的估值代價，選擇代價小的點作為下個待選節點，按照此搜索方式進行搜索，直至找到目標點。其估價函數為：

式中：F(k)是節點k的代價和，G(k)為機器人從節點k到達起點的實際代價，H(k)為機器人從節點k到達目標點的估值代價。其中，H(k)的計算方法有曼哈頓距離、切比雪夫距離、歐式距離[12]等，本文采用歐氏距離作為H(k)，具體公式為：

式中：(x1,y1)和(x2,y2)分別表示當前節點和目標節點的位置坐標。

1.2 改進A*算法

傳統A*算法規劃的路徑沒有考慮到與障礙物的位置關系，不利于機器人在動態障礙物環境下的避障規劃，因此本文對A*算法的實際代價函數G(k)進行改進，預設一個機器人與障礙物的距離函數，改進后的G(k)為：

式中：si為路段i的安全估值，si=k(1-di/r)，0＜di≤r；li是機器人在路段i的長度，ω是估值系數。由于傳統A*算法在路徑規劃時存在大量冗余節點且路徑轉折過多，本文采用一種雙向刪除轉折點算法，對傳統A*算法中的冗余轉折進行優化。該算法可有效減少路徑長度，使得規劃出的路徑更加平滑，減少轉折。具體操作如下。

（1）獲取傳統A*算法路徑節點G{Si,1≤i≤n}，并創建一個只有起點S1和終點Sn的集合P。

（2）遍歷路徑中所有節點，刪除路徑中的中間點，保留轉折點。

（3）從起始點開始，在在轉折點Si、Sj之間隔m取點，并判斷兩節點之間是否穿過障礙物。若沒有穿過障礙，選擇當前節點Si+m替代集合P中的點為路徑節點，反之不做選擇。

（4）從目標點反向遍歷，重復上一步判斷方法，并將所有取得的節點放入集合P中。

（5）依次連接P中所有節點，得到優化后的最優路徑。

路徑優化過程如圖1所示。

圖1 路徑優化過程示意圖

2 改進動態窗口法

2.1 傳統動態窗口法

動態窗口方法（Dynamic Window Approach，DWA）基于機器人所載的傳感器實時檢測局部環境信息，結合機器人運動學和動力學進行速度空間采樣，實現路徑規劃。該算法對多組速度進行采樣來對機器人位置信息模擬，最后利用評價函數對軌跡進行評估，選出最優組，使得機器人能迅速到達目標點。傳統動態窗口法可最大限度地使機器人避開障礙物并以最快的速度移至目標點，評價函數的表達式為：

式中：head(v,w)是方位角評價函數，引導方位角始終朝向目標節點，評估軌跡朝向和目標的方位差；dist(v,w)是速度評價函數，vel(v,w)是距離評價函數，用于評價機器人與障礙物距離關系，如果沒有障礙物，則該參數為常數；α、β、γ為權重系數。

然而，傳統DWA算法缺少對全路徑的判斷，機器人容易陷入死鎖，導致路徑規劃失敗。并且當障礙物為運動障礙物時，由于機器人缺少對障礙物信息的實時判斷，機器人無法安全地避開障礙物。針對傳統動態窗口法的缺陷，設計一種基于動態障礙物信息的改進動態窗口法。

2.2 改進動態窗口法

2.2.1 融合運動障礙物分析

實際生活中的障礙物大多是類似汽車、動物等運動的障礙物。當人們遇見這類障礙物，會分析判斷其歷史運動軌跡，然后根據障礙物的意圖規劃下一步路線。基于以上原理，本文設計一種運動障礙物意圖分析法，對運動障礙物融合意圖分析，提高機器人避開運動障礙物的安全性，如圖2所示。

圖2 動態障礙物分析

機器人遇見運動障礙物時，首先對障礙物進行運動學分析，根據障礙物的歷史運動軌跡，預估運動障礙物的速度和方向，整體規劃將要避障的區域，選擇在所有可行的軌跡中時間最優的那一條作為機器人的最優路徑。同時，機器人在運動過程中根據障礙物位置實時調整運動方向來實現更好的避障效果。機器人動態避障方法如下。

（1）機器人根據傳感器實時采集運動障礙物信息，并確定運動障礙物歷史位置信息。

（2）對運動障礙物進行適當的膨脹處理，使得機器人在避障時更加安全。

（3）根據移動障礙物位置合理規劃將要行進的軌跡。

（4）機器人在行進過程中根據運動障礙物的位置變化合理地調整軌跡。

2.2.2 改進動態窗口評價函數

傳統動態窗口法對于障礙物固定的環境下規劃性能較好，在局部路徑規劃中有著較為廣泛的應用。但是當環境中存在運動障礙物時，由于傳統動態窗口法評價函數的局限性，機器人容易出現局部死鎖、無法及時避開障礙物等問題。為此，本文對傳統動態窗口法的評價函數進行合理的拓展，在距離評價函數后增加一項拓展距離函數spdist(v,w)。

拓展之后的評價函數可以使得機器人在檢測到障礙物時綜合速度差值與距離參數來選擇合適的運動軌跡，增強機器人對動態障礙物的避障能力。改進后的評價函數如下：

基于運動障礙物的不確定性，為了使機器人更加安全地避障，對于不同速度的障礙物應設定不同范圍的安全距離。運動速度快的障礙物，安全距離增大，反之減小。機器人在行駛過程中需要自主判斷運動障礙物的位置，在不影響障礙物的前提下運動，實行安全行進策略。改進后的算法流程如下。

（1）機器人在行進過程中判斷軌跡方向是否與運動障礙物的危險區域重疊。如果重疊，則進行下一步操作。如果沒有，就按照原定軌跡運動。

（2）判定機器人與運動障礙物即將相遇，計算各個速度下機器人與運動障礙物的位置信息，將預測與障礙物危險區域重疊的速度組舍棄。

（3）在其他不與運動障礙物危險區域重疊的速度組中判斷，選擇距離合適速度組，向左或向右移動機器人。

（4）根據機器人方向確定評價函數權重系數，并根據評價函數計算所有符合條件的軌跡，選出評估值最大的軌跡來最終確定機器人的移動軌跡。

改進后的動態窗口法能更加適應運動障礙物環境下的避障問題，使得機器人行進更加安全。

2.3 算法融合

動態窗口法（DWA）是基于機器人檢測到的局部速度信息來實時規劃路徑的，在局部路徑中有良好的性能。但是該方法缺乏對最優全局路徑的考慮，容易陷入局部最優解。因此本文將改進后的兩種算法融合，結合兩種算法的優點，從而實現機器人沿全局最優路徑行進時能實時避開運動的障礙物，獲得具有動態自主避障特性的全局最優路徑，并對動態窗口法評價函數進一步優化。優化后的DWA評價函數為：

式中：SHead(v,w)是當前目標點方向和機器人當前軌跡方向的夾角，用來評價機器人在當前速度下到達終點時的方向和當前目標點之間的角度下差；α、β、γ和δ分別 為SHead(v,w)、dist(v,w)、vel(v,w)和spdist(v,w)的參數權重。

改進的評價函數可以使得動態窗口法在局部路徑規劃時避免死鎖，實現全局最優路徑，使得機器人能在復雜動態障礙物環境下規劃避障。具體算法流程如圖3所示。

圖3 融合算法流程圖

3 仿真驗證

為驗證本文提出方法的有效性，對算法進行仿真驗證。仿真環境為Matlab 2018b軟件、8 GB內存的64位Windows 10系統。

首先采用柵格法對實驗環境建模，在20×20的柵格地圖環境下實驗，如圖4所示。地圖中黑色為固定障礙物，白色是可行區域。實驗中，動態窗口法的參數設置為：最大線速度為1.5 m·s-1，最大角速度為20.0°·s-1，最大線加速度為0.2 m·s-2，最大角加速度為50.0°·s-2，采樣時間為0.1 s，評價函數參數α=0.05，β=0.2，γ=0.3，δ=0.3，ρ=0.4。

首先用傳統DWA算法進行路徑規劃。如圖4所示，以（2.5，2.5）為起點，以（13.5，18.5）為終點。因為地圖中存在有U型障礙物，通過傳統DWA算法進行路徑規劃，容易陷入局部最優，導致機器人死鎖無法前進。這種情況在地圖中存在半封閉障礙物時最為明顯，并且如果地圖中存在運動障礙物，傳統DWA算法也會因為評價函數的局限性，無法及時避開動態障礙物。

在同樣的地圖環境中加入動態障礙物，將對融合改進后算法進行路徑規劃。首先用改進A*算法進行全局路徑規劃，作為動態窗口法的全局最優序列指引。如圖5所示，機器人首先規劃出了一條虛線作為全局最優路徑，然后在全局最優路徑上加入運動障礙物和隨機出現的障礙物。其中虛線上的兩層方塊表示動態障礙物，兩層方塊表示對動態障礙物做了一定的膨脹處理，虛線中靠近終點的方塊表示隨機出現的障礙物。融合后的算法首先按照全局最優序列點行進，避免了傳統DWA算法的死鎖情況。由圖6、圖7可知，當機器人行進到運動障礙物附近開始進行障礙物軌跡歷史分析，結合改進動態窗口法綜合分析障礙物運動速度與機器人當前速度，決策出一條能安全快速的避開障礙物軌跡。由圖6可知，機器人遇見動態障礙物時會選擇一條既遠離運動障礙物、又遠離靜態障礙物且盡可能貼近全局最優路徑的軌跡，符合實際生活中機器人的安全行進規則。

圖5 融合算法起始規劃位置

圖6 避開動態障礙物

圖7 全局避障路徑規劃

對本文所提出算法和傳統動態窗口法進行多組仿真實驗，以證明實驗的準確性，得到機器人碰撞次數、路徑長度、運行時間的對比數據，如表1所示。由表1的數據可知，改進后的融合算法大幅度降低了碰撞次數，實現了機器人在運動障礙物環境下的避障規劃，同時也減少了機器人運動路徑長度，提升了機器人的運動效率。

表1 改進算法性能提升效果

4 結 語

本文針對傳統機器人路徑規劃沒有考慮到動態障礙物環境、不符合實際需求且容易陷入局部最優等問題，提出一種基于動態障礙物的機器人避障路徑規劃方法。首先通過對傳統A*算法的改進得到路徑更短且安全性更高的全局規劃路徑軌跡；其次融合運動障礙物軌跡分析，改進動態窗口法評價函數，使得動態窗口法能夠在存在動態障礙物的環境下實現避障；最后將改進后的動態窗口法和改進A*算法相融合，將改進A*算法的最優路徑序列點作為動態窗口法的目標，避免了DWA在局部路徑規劃容易陷入局部最優的問題，最終得到一種兼顧全局最優且能實時避開動態障礙物的路徑規劃算法。通過Matlab仿真實驗分析，結果表明該算法對動態復雜環境的適應度高，能夠滿足移動機器人的實際避障需求，具有一定的應用價值。