基于A*和動態窗口法的移動機器人路徑規劃?

王佳軍 孟令軍 薛志凌 李曉宇

（中北大學儀器與電子學院 太原 030000）

1 引言

隨著科技水平的快速提高，機器人由最初的結構簡單、功能單一發展為能夠適應多樣化環境、執行復雜任務的智能化機器人，廣泛應用于物流業、制造業、服務業等領域，極大地提高了社會的生產力水平。而路徑規劃是實現自主導航的關鍵一步，也是機器人智能化的關鍵技術之一，是機器人以及自動駕駛領域的研究熱點。目前，機器人路徑規劃主要集中在三類算法：1）基于圖的算法，如：Dijkstra、A*算法［1］，D*算法［2］，D*Lite算法［3］等；2）隨機采樣類算法，如：快速隨即搜索樹法（RRT）［4］、概率路圖法（PRM）等；3）智能優化算法：如蟻群算法、遺傳算法、神經網絡等，此外，還有人工勢場法［5］，動態窗口法［6］等。

Dijkstra 算法基于廣度優先遍歷，向四周盲目拓展，搜索節點過多。A*算法在Dijkstra 算法的基礎上引入啟發式函數，增強了搜索的目的性，在機器人全局路徑規劃中使用廣泛。但是該算法存在節點過于密集、軌跡不平滑等問題。文獻［7］利用跳點搜索理論，大大減小了搜索過程中節點的數量；文獻［8］引入電動車的能耗約束建立綜合估價函數，同時考慮中途充電站，規劃出能耗最優的行車路徑；文獻［9］將傳統A*算法八角度拓展節點優化為任意角度路線，減小了路徑長度，與蟻群算法進行融合實現多目標點路徑規劃。

動 態 窗 口 法（Dynamic Window Approach，DWA）對復雜環境具有很強的適應能力，實時結合機器人自身傳感器進行局部路徑規劃，能夠有效躲避障礙物且路徑為平滑曲線，但是該方法容易陷入局部最優解，無法抵達目標點。文獻［10］結合與全局路徑距離信息定義新的評價函數，能夠提前規避“C”型障礙物；文獻［11］結合自適應思想使得機器人在通過不同密度障礙物區域時，運動參數能夠自主變化，機器人的速度與路徑更加合理；文獻［12］利用動態窗口法對可選避障速度進行速度約束，同時，增大了移動機器人可選避障速度區域。

本文將A*算法與動態窗口法結合，首先對A*算法產生的路徑節點進行預處理，剔除冗余節點，得到路徑坐標點作為動態窗口算法的局部目標點，直至最終目標點，有效解決了單一算法在路徑規劃中存在的問題。

2 A*算法

2.1 環境構建

在機器人工作過程中，需要通過自身傳感器感知環境，構建一個機器人可以理解地表示所在環境的地圖模型。柵格法是經典的環境建模方法，簡單高效，將機器人構建的地圖劃分為大小相同、彼此相連的柵格，柵格帶有環境信息及機器人位置信息，機器人以柵格為基本單元進行移動和搜索，完成路徑規劃任務。如圖1所示，深色柵格表示環境中存在障礙物，機器人無法通過，淺色柵格表示允許搜索區域。

圖1 柵格地圖

2.2 A*算法基本原理

A*算法在搜索過程中假設環境不再改變，是求解最短路徑的有效方法。Dijkstra 盲目向四周搜索，而A*算法的改進在于結合當前位置與目標點的距離建立具有啟發性的搜索函數：

式中：g(n)為起始節點至當前節點的實際距離代價；?(n)為當前節點至目標點的估計距離代價。

代價函數f(n)來進行節點的擴展和搜索，g(n)代表搜索的完整性，g(n)越大越趨向于搜索的廣度，但會降低搜索的效率，h（n）具有啟發性，h（n）越大，搜索的效率高，但通常難以規劃到最優路徑。

A*算法沿著柵格地圖搜索的過程中，更新維護open list和close list兩個列表。Open存儲拓展點周圍節點，用于計算選擇最小代價節點作為下一個拓展點，close 存儲經過的節點以及障礙物。A*算法具體流程如下：

1）初始化環境信息，包括障礙物、起始點、目標點，以及open和close列表；

2）判斷open 列表是否為空或只有目標點，若為真，結束搜索；

3）選取open 列表中評價函數最小的節點作為父節點，調用拓展函數，計算g（n），h（n），將此節點放入close列表；

4）將拓展出來的節點放入open 列表中，保存父節點；

5）跳至步驟2），直至找到終點；

6）保存的父節點信息，即為路徑關鍵節點。

3 動態窗口法

動態窗口法由機器人運動學方程推導而來，在預測時間dt內，對機器人線速度和角速度采樣，形成速度空間(v,ω)，描繪出所有可能的運動軌跡，構建評價函數對軌跡進行評價，從中選擇最合理的路徑。

首先，我們建立機器人的運動模型，機器人的運動軌跡由一系列的圓弧組成，在dt時間內由速度對(v,ω)決定，運動模型為

依據上述機器人運動模型，在機器人的實際工作中，具有無窮多(v,ω)，需要根據環境和工作狀態添加約束，選擇出朝向目標、躲避障礙物的(v,ω)。

出于安全考慮，對機器人行駛的速度范圍進行約束，定義Vs為機器人最大線速度和角速度，所以動態窗口所能搜索的最大范圍為

由于機器人電機所提供轉矩的限制，機器人在dt時間內所能達到的實際速度受到一定的約束，即：

為了保證機器人在行駛過程中避免與障礙物發生碰撞，當機器人檢測到環境中障礙物時，能夠以最大減速度停下來，即：

最終，由以上三個速度構成動態窗口速度集合Vr：

經過速度采樣后得到速度空間，如何在該空間內選擇一條最優的軌跡，需要我們設計適宜的評價函數，使得機器人朝向目標快速前進，局部避開障礙物。評價函數設計為

式中：σ為平滑系數，α,β,γ為各子函數加權系數；heading（v，ω）表示當前位置與全局之間的角度偏差，使機器人朝向目標點方向；dist（v，ω）表示生成的速度軌跡至障礙物的最近距離，防止機器人發生碰撞；vel（v，ω）表示采樣軌跡中的速度大小，盡快抵達。

4 優化與改進

A*算法根據評價函數，遍歷整個柵格地圖，得到一系列路徑點，導致路徑點過于密集，曲率不連續，關鍵拐點選取策略，剔除不相關的冗余節點，提升路徑的平滑度。設A→B→C 為路徑中三個節點的行駛路線，若A 與B 共線，B 與C 共線，則認為B節點為冗余節點，刪去該結點。

傳統評價函數從方向性、安全性、效率性方面充分評價速度空間內最合理的速度。向方向子函數加入全局路徑信息約束：全局路徑距離當前位置最近的節點之間的角度，新的方向函數保證機器人不斷跟隨新的目標點前進。該改進方法有效地解決了動態窗口法中單一目標點容易陷入局部最優的缺陷，規劃出的路徑即為全局最佳路徑，改進后算法具體執行步驟如下：

1）根據機器人傳感器構建柵格地圖；

2）初始化A*及DWA參數；

3）A*算法進行搜索，獲取關鍵全局路徑點信息；

4）根據機器人運動模型，計算當前動態窗口Vr；

5）根據機器人運動模型，計算預測時間dt內運動軌跡點，同時根據評價函數對各軌跡點打分，分數最高者為下一個dt時間機器人的運動速度；

6）判斷機器人是否到達目標點，若抵達，尋路成功，否則，返回第4）步，繼續搜索。

5 仿真與分析

為了驗證本文算法的有效性，在Win10 系統Matlab R2016a 環境下構建柵格地圖進行仿真實驗。根據文獻［13］，機器人運動參數設置為Vmax=1.0m/s，ωmax=20°/s，V.max=0.2m/s2，ω?max=50°/s，速度分辨率和角速度分辨率分別為0.01m/s，1°/s。各評價子函數系數分別為0.05，0.4，0.1。起始點、目標點分別為（1.5，2.5），（10.5，9.5）。

傳統A*算法規劃路徑如圖2所示，當搜索鄰域由4鄰域變為8鄰域時，路徑節點數、路徑長度和轉彎角度有了明顯改善。實驗數據如表1所示。可見，隨著搜索鄰域的增加，比如：24 鄰域［14］，48 鄰域［15］，運行時間沒有明顯增加，轉彎角度降低，路徑更加平滑。同時，坐標（3，3），（6，5），（8，9）處距離障礙物距離過近，導致路徑的安全性降低。

表1 仿真實驗數據

圖2 全局最優路徑及節點

圖3 全局最優路徑及關鍵節點

由圖4 可以看出，傳統DWA 算法規劃路徑時會陷入局部最優，尋路失敗；由圖5可知，與A*算法融合改進后得到新的路徑為平滑曲線，距離障礙物距離更加合理，提高機器人行駛的安全性。本文得到運動學參數結果圖6所示，速度和角速度連續變化，整體符合要求。在迭代次數為300 次通過稠密障礙物時，機器人方向出現小幅度震蕩，在今后的研究中還需要改進。

圖4 DWA路徑

圖5 改進型DWA路徑

圖6 速度曲線

6 結語

為了提高機器人在復雜動態環境下的表現，結合A*算法全局路徑最優的特點和動態窗口法局部最優特點，提出一種新的算法。仿真結果表明，相較于單一算法，混合算法克服了局部最優問題，平滑度上有了巨大改進。但是在經過稠密障礙物時，還需進一步改進。本文僅僅做了初步的研究和實驗，在今后還將在多目標、實時性方面做更多的探索。