基于改進A*算法融合DWA 算法的機器人路徑規劃

王海瑤，安天洋

（沈陽航空航天大學 電子信息工程學院，遼寧沈陽，110000）

0 引言

自主移動機器人[1]作為自動化技術的應用之一，在新能源汽車，智能家居，商場智能向導，餐廳自動送餐等應用領域作用極大，而其路徑規劃[2]是機器人自主導航中的關鍵問題，目標是搜尋出一條無障礙且最優先的路徑。目前生產生活實際應用中普遍使用的定位建圖方式有粒子濾波法[3]，Gmapping[4]等，建圖之后，考慮到醫院等地形的復雜性[5]以及算法效率問題，被廣泛使用的路徑規劃算法有A*[6]，蟻群[7]，粒子群算法[8]，Dijkstra 算法[9]，動態窗口法[10]等。普通路徑規劃算法不能夠同時實現全局路徑規劃[11]與局部路徑規劃[12]，為實現實際需求，本文提出了一種改進傳統A*算法[13]以此提升效率，接著融合DWA 算法[14]使路徑在全局正確的前提下能夠做到局部避障規劃且更加平滑化的融合性算法。本文章主要論述路徑規劃算法的創新想法與實用性分析，從結果可以看出，在有障礙物的環境下，提出的將改進 A *算法與動態窗口法 DWA 算法相結合的算法提高了移動機器人在復雜環境中規劃路徑的效率，縮短了路徑規劃時間及軌跡距離，更適于應用在實際中，以實現移動機器人的路徑規劃和及時避障，快速準確地找出最佳路徑。

1 傳統A*算法

傳統A*算法同時結合了廣度優先搜索、Dijkstra 算法、最佳優先搜索這三種算法的特點于一身，是一種常用的路徑規劃和圖形遍歷算法。廣度優先搜索是以廣度為優先級進行，這種算法會遍歷圖中的每一個節點，像洪水一樣，實際對于大部分情況來說是沒有必要的；Dijkstra 算法是1956 年由計算機科學家Edsger W.Dijkstra 提出的一種用來尋找節點之間最短路徑的方法，考慮到相鄰節點之間移動代價的不同，列出一個優先隊列，將每次移動時計算到的從當前節點到起點的總移動代價最小的路徑放入優先隊列，直到遍歷到終點結束；最佳優先搜索與Dijkstra 算法類似，同樣列出一個優先隊列，將當前節點到終點的距離作為優先級，最終選取離終點最近的路徑，但是同時這種算法也存在缺陷即沒有考慮到中間有障礙物的情況。

通過上述可知，A*是一種結合評價函數使用的、通過不斷尋找節點的方式最終組成一條完整路徑的啟發式算法，傳統A*算法以從當前節點到下一個節點的實際成本（或稱代價)和估計成本之和作為評價標準，評價函數如下：

其中f(n)是節點的綜合優先級，h(n)和g(n)分別表示到目標點的實際和估計代價。h(n)的選擇將直接影響到算法成功率、速率和準確率，h(n)的值與準確值間的誤差越小，搜索效率越高。極端情況下，當h(n)=0 時，f(n)完全由g(n)決定，此時算法退化成Dijkstra 算法；當h(n)始終小于等于節點到終點的代價時，一定可以找到最短路徑，但隨著h(n)值的減小，被遍歷到的節點數會越多，算法整體速率變慢；當h(n)完全等于節點到終點的代價時，此時的路徑為最佳路徑并且速率很快，但這種情況為理想狀態，很難實現；當h(n)大于節點到終點的代價時，不能保證可以找到最短路徑，但此時算法速率很快；另一種極端情況下，當h(n)遠大于g(n)時，此時只有h(n)發揮作用，此時算法退化成最佳優先搜索。

對于網格形式的圖形，設定兩個距離dx、dy：

如果節點只允許朝上下左右四個方向移動的話，h(n)可以使用曼哈頓距離，一般情況下兩個相鄰節點之間的移動代價為一個固定的常數，記作D，曼哈頓距離的公式：

如果節點允許朝斜著的方向移動即朝著八個方向移動，則h(n)可以使用對角距離。D2 指的是兩個斜著相鄰節點之間的移動代價。如果所有節點都正方形，其值是：

對角距離的公式：

如果節點允許朝任何方向移動，h(n)可以使用歐幾里得距離，由于本系統采用雙輪差速驅動，機器人的移動方位不受機械結構限制，故采用可以直接反映兩點距離的歐幾里得距離模型作為實際代價函數：

2 優化A*算法

■2.1 代價函數的優化

在傳統A*算法中，代價評價函數中的h(n)和g(n)權重相同，而在實際問題中，當距離目標點距離較大時，實際代價遠大于估計代價，這將使得系統遍歷許多無效節點，使算法效率降低。而在接近目標點時，估計代價逐漸增大并接近于估計代價。針對以上問題，可以給h(n)賦予動態權重w(n)，使系統能根據自身狀態自適應調整h(n)的影響力，以此提高算法效率。賦予動態權重的原則是在開始尋找路徑后，快速地到達終點比重占的更大；尋找結束后，獲得最佳路徑比重占的更大。w(n)與h(n)呈線性變化關系，當h(n)較大時，A*算法搜索速率增大，很快地向終點擴散但會錯過最佳路徑；當h(n)較小時，A*算法會降低搜索速率而更傾向于尋找最佳路徑

此時，再對當前節點與目標點的長度dn、起始點與目標點間的長度ds加以考慮。則優化后的代價函數：

■2.2 剔除冗余節點

傳統A*算法選取的節點位于柵格中心位置，可能導致原本可以互相直達的兩個節點之間存在第三個冗余節點，這種冗余節點每存在一個就會使機器人做出兩次多余的轉向動作，將促進系統誤差的累積，縮短機械系統壽命，降低路徑效率，本文嘗試通過計算向量角和作線段的方式消除冗余節點：設第一個節點為P1，后一個節點為P2，P2后一個節點P3，而后計算∠P3P2P1，若∠P3P2P1=180°，則P2為冗余節點，剔除；若∠P3P2P1≠180°，則作線段L(P3P1)，若L(P3P1)經過障礙物，則保留P2；若L(P3P1)不經過障礙物，則P2為冗余節點，剔除。接著前往下一個節點直至終點。此時得出的路徑對比傳統A*算法，轉彎次數和軌跡總長都會有所下降。

■2.3 局部路徑規劃

在建圖成功的前提下，進行優化代價函數和剔除冗余節點后的A*算法（以下統稱改進A*算法)可以規劃出全局路徑，但是當有各種各樣移動中的未知障礙物時，僅靠改進A*算法是無法及時避開未知移動障礙的，于是引入動態窗口法（DWA)來進行局部路徑規劃，在DWA 中，機器人在速度空間內采樣，生成模擬軌跡，最后根據評價函數選擇并沿著最合理的軌跡移動。

(1)根據硬件條件與周圍具體環境條件將機器人的速度空間控制在：

機器人在模擬運行的一定時間間隔?t內，機器人所能達到的速度被加速度限制在：

由于機器人減速性能有限，為確保機器人不會撞到障礙物，于是將速度限制在：

于是，得出機器人的有效速度采樣區間：

其中，vmin,vmax為最小，最大線速度；ωmin,ωmax為最小，最大角速度；vc，ωc為機器人線速度，角速度；，為機器人最小，最大線加速度；，為機器人最小，最大旋轉角加速度；dist(v,ω)為機器人與最近障礙物間的距離。

機器人在有效速度采樣區間采樣后，就可以利用評價函數來評價所采樣的速度對應的軌跡：

其中，α,β,γ是權重因子；heading(v,ω)表示機器人采樣區間使，所在軌跡方向與目標角度值θ的函數；vel(v,ω)表示運動軌跡速度大小。

■2.4 全局路徑規劃與局部路徑規劃的融合

改進A*算法可以計算出兩點之間的最佳軌跡，DWA 算法可以利用傳感器及時躲避事先未知的障礙物，進行局部路徑規劃。故將兩者結合，便可以得到可在實際醫院環境中使用的融合路徑規劃算法，流程如圖1 所示。

圖1 路徑規劃流程

■2.5 仿真實驗

根據計劃元件組裝簡易機器人，安裝系統后，放置在模擬的走廊和儲物室環境中，運行地圖構建算法節點，打開rviz，可獲得如圖2，圖3 兩種模擬環境的室內環境效果圖。

圖2 走廊環境

圖3 儲物室環境

如圖2，圖3 所示，使用計劃系統的硬件和算法構建出的地圖能準確表現出墻體、路面以及障礙物的位置，存在些許傾斜、重影誤差但不會影響到后續的自主路徑規劃工作。為驗證算法可行性，配置測試環境，運行系統為Windows11X64，平臺為Matlab R2018a，利用Matlab 中的colormap 函數搭建20*20 柵格地圖，其中白色為空區域，黑色為障礙物，左上角為三角形為起點，右 下角圓形為目標點，設定表1 參數，運行算法，觀察軌跡。

表1 機器人及算法參數

首先進行傳統A*算法與改進A*算法的對照實驗。

圖4 中黑色實線顯示的是傳統A*算法計算得出的軌跡，圖5 中黑色實線顯示的是改進A*算法計算得出的軌跡，統計轉彎次數，總長度，計算時間得表2。

圖4 傳統A*算法

圖5 改進A*算法

根據表2 數據可輕易看出，改進A*算法可以顯著減少機器人的轉彎次數，同時能夠減少軌跡長度，降低計算時間，在系統整體性能方面對比傳統A*算法有顯著提升。

為驗證DWA 算法的融合是否能對改進A*算法起到輔助躲避未知障礙的作用，在柵格地圖的改進A*算法的軌跡上放置灰色塊作為事先未知的障礙物（如圖6)，后使用改進A*與DWA 融合后的算法（以下統稱融合算法)再次運行，得出圖7 所示軌跡。

圖6 放置未知障礙

圖7 融合算法軌跡

如圖7 可見，融合算法得出的路徑在避開了所有臨時加入的障礙的基礎上對比改進A*算法的路徑更加平滑，更符合普通雙輪差速機械結構的運動特性，綜上所述，最終的融合算法得出了一條可在實際問題中使用的優質軌跡。

3 結語

本文提出了一種針對A*路徑規劃算法的改進方法，首先優化評價函數使實際成本值在系統運行過程中具有動態影響力，減少節點數，提升了算法效率，通過剔除冗余節點的方式保留了重要節點，刪除了非必要節點，最大程度上提升算法效率，并減少了機器人轉彎總次數，縮短路徑，使機械誤差的累計值達到最小。同時引入DWA（動態窗口)算法，解決了運行過程中躲避未知障礙的問題，同時增加了軌跡平滑度。該融合算法經過仿真實驗后得知具備有效的可行性，可以提高機器人的自動路徑規劃效率與準確度，通過更快更優的路徑規劃，一定程度上提高了全局路徑規劃中躲避障礙的效果，可以實現動態路徑規劃中的全局避障。本文僅介紹了簡單二維環境下的自主導航處理辦法，針對更加復雜的上下寬窄不一的障礙物或高度障礙問題未做論述，未來還需進一步的針對性研究。