融合改進A*與DWA算法的機器人動態路徑規劃

劉建娟，薛禮啟，張會娟，劉忠璞

1.河南工業大學 電氣工程學院，鄭州 450000

2.河南工業大學 機電設備及測控技術研究所，鄭州 450000

目前，路徑規劃可分為局部路徑規劃和全局路徑規劃兩大類，用于局部路徑規劃的算法包括DWA算法[1-3]、粒子群算法[4-6]、蟻群算法[7-9]、遺傳算法[10-11]等；用于全局路徑規劃的算法包括D*算法[12-13]、A*算法[14-15]、快速擴展隨機樹[16-17]等。

A*算法廣泛應用于移動機器人路徑規劃，但是傳統A*算法在移動機器人的應用中存在拐點多、路徑不平滑等不足，不利于機器人的運行。針對這些不足，許多學者對傳統A*算法進行不同層面的改進，也取得了不錯的效果。文獻[18]利用JPS 算法對A*算法進行改進，優化搜索策略，同時利用貝塞爾曲線對路徑進行平滑優化，由實驗結果可知此算法明顯提升了路徑規劃效率，在大尺度環境中有較好的適應性，但是A*算法是在全局已知的前提下進行搜索和擴展的，因此無法實現隨機障礙物的動態避障。文獻[19]提出一種單邊矩形擴展A*算法，利用受迫擴展規則，簡化終止條件，提高搜索效率和路徑規劃質量，但是算法搜索地圖的不規則性和破碎化程度不斷加重，算法更適合在大尺度環境中的路徑規劃。DWA算法常用于移動機器人的局部路徑規劃，但是在大尺度復雜環境中由于缺少中間引導，無法得到最優路徑，甚至無法達到目標點。文獻[20]通過移動機器人的尺寸與障礙物之間的自由空間關系對DWA算法進行改進，由于對障礙物與機器人之間的距離進行限制，在復雜環境中出現的狹窄區域會造成機器人反復啟停，甚至停止運行。文獻[21]通過立體視覺進行避障，嚴格定義機器人的搜索空間得到機器人最優速度，雖然可以實現動態路徑規劃，但是由于缺乏中間指引，規劃路徑很難達到最優，而立體視覺又增加了時間和空間復雜性，對硬件設備性能提出更高的要求，經濟性會有所降低。目前來說，單一改進A*算法路徑規劃能得到從起點到終點的最優路徑，但是無法及時躲避局部障礙物；單一改進DWA算法雖然能有效躲避局部障礙物，但是不滿足全局最優路徑。因此，結合兩者的優勢，對兩種算法進行改進并實現兩種算法融合是當前研究的熱點。文獻[22]將搜索鄰域由3×3 擴展為7×7，優化了搜索角度，減少了路徑的轉折點，使得路徑更加平滑，然后將改進后的A*算法與DWA算法融合，雖然保證了全局最優，但是由于沒有對已知障礙物和未知動態障礙物進行區分，導致在動態路徑規劃時兩者相互影響，造成動態避障靈敏度低。文獻[23]通過判斷起點和終點的位置關系，將傳統的8搜索方向改為5搜索方向，設計新的啟發函數，實現無斜穿路徑，增加了路徑平滑度，實現算法融合。但是融合算法存在一定的不足，首先，由于沒有改進DWA 算法動態窗口評價函數，使得動態避障靈敏度不高，其次，將傳統8 搜索方向改為5 搜索方向時，沒有考慮到剩余5搜索方向都存在障礙物的情況并加以判斷，搜索可能會陷入死區，無法實現路徑規劃。文獻[24]通過關鍵點提取策略對傳統A*算法進行改進，并構建全局最優的動態窗口評價函數，將改進的算法進行融合，實現實時最優路徑規劃。算法雖然對動態窗口評價函數進行改進，增加機器人末端到終點的距離函數，其目的是不斷縮短與終點的距離，但是仍然沒有區分障礙物類型，既難以獲得全局最優路徑，又無法保證動態避障的靈敏度。

針對這些問題，將傳統A*算法的啟發函數中引入環境信息，實現對A*算法啟發函數的自適應調整；設計路徑平滑優化算法，刪除冗余節點、減少轉折、提高路徑平滑度；提取改進A*算法規劃路徑的關鍵點作為DWA算法的中間目標點做全局引導，在全局最優的基礎上實現改進A*與DWA算法融合。

1 傳統A*與DWA算法

首先對現有文獻中傳統A*算法及DWA 算法的基本原理進行介紹。分析算法存在的不足及形成原因，為第2章中提出算法改進和算法融合提供理論依據。

1.1 傳統A*算法基本原理

A*算法廣泛應用于二維地圖的路徑規劃，是一種典型的啟發式搜索型路徑規劃算法，利用評價函數來指導節點的搜索與擴展，因此評價函數影響搜索空間的大小和算法的速度。傳統A*算法的評價函數如公式（1）所示：

在評價函數公式（1）中，n表示當前節點，F(n)表示從起點經由節點n到達目標點的評價函數；G(n)表示從起點到達節點n的實際代價；H(n)為估計值，表示從節點n到目標點估計代價，又叫啟發函數。由啟發函數H(n)的選取，可以控制節點的搜索與擴展，進而可以控制算法的速度和精度。傳統A*算法常用的啟發函數有曼哈頓距離、切比雪夫距離、歐幾里德距離三種，三種距離的示意圖如圖1所示。因為啟發函數H(n)與實際距離的關系會直接影響算法的速度和精度，為了提高算法的精度和靈活性，需要對啟發函數進行改進。

圖1 三種啟發函數距離示意圖Fig.1 Distance diagram of three heuristic functions

1.2 DWA算法基本原理

1.2.1 運動學模型

本文選用Turtlebot2移動機器人平臺作為實驗平臺進行實驗驗證。由于DWA算法要將位置控制轉化為速度控制，因此需要對Turtlebot2 的運動模型進行分析，Turtlebot2的兩輪差速運動的運動學模型如圖2所示。

圖2 Turtlebot2差速運動學模型Fig.2 Differential kinematics model of Turnlebot2

假設v(t)和ω(t)分別表示Turtlebot2 在世界坐標系下t時刻的線速度和角速度，在采樣周期Δt內，位移較小，近似做勻速直線運動，則此運動學模型的數學表達式可表示為公式（2）：

1.2.2 速度采樣

DWA算法將動態避障問題描述為速度空間中三個帶約束的優化問題，要根據實際情況對速度采樣范圍進行約束。

（1）移動機器人速度約束

機器人的采樣速度應控制在移動機器人最佳角速度和線速度區間內，則約束為公式（3）：

式中，vmax、vmin指機器人的最大、最小線速度，ωmax、ωmin指機器人最大、最小角速度。

（2）移動機器人電機加減速約束

機器人的采樣速度應考慮電機性能，速度應保持在當前速度在最大減速度和最大加速度條件下，速度可變范圍內，則約束為公式（4）：

式中，vc、ωc指當前時刻線速度和角速度，admax、aimax指機器人最大減速度和最大加速度，αdmax、αimax指機器人的最大角減速度和最大角加速度。

（3）移動機器人安全距離約束

機器人在進行局部路徑規劃時，為了保證其安全，在約束條件（2）最大減速度條件下，使得移動機器人到達障礙物之前線速度和角速度都降為零，則約束為公式（5）：

式中，dist(v,ω) 指模擬軌跡末端距離障礙物的最近距離，admax、αdmax指機器人最大減速度和最大加速度。

最終的動態窗口的速度為以上三個約束條件的交集，動態窗口速度Vw滿足公式（6）：

1.2.3 評價函數

DWA 算法的評價函數指標有偏向角、線速度及模擬軌跡末端與距障礙物的最近距離。由于這三個指標的度量單位不同，為避免某一個量的數值過大而占比過大，需要對每個指標進行歸一化處理，歸一化處理后得到的軌跡評價函數如公式（7）：

式中，α、β、γ是每一項的加權系數。Head(v,ω)為模擬軌跡終點方向和目標點之間方向角偏差；Vel(v,ω)用來評價移動機器人在當前估計中運行的速度大小；Dist(v,ω) 是指模擬軌跡終點與障礙物的最近距離。

對傳統DWA算法的原理介紹可知，傳統DWA算法存在兩點不足：第一，只有一個目標點，缺少中間指引，在大尺度環境中無法得到最優路徑；第二，未將已知障礙物和未知障礙物進行區分，導致動態避障靈敏度低。

2 算法改進與算法融合

針對上文提到的傳統算法存在的不足，提出自己算法的改進和融合思路。首先，量化環境信息，將環境信息引入傳統A*算法的評價函數，實現了評價函數中啟發函數的自適應改變，提高了算法的靈活性，其次設計路徑平滑算法對路徑進行平滑優化，使得路徑更加適合機器人的運動。最后改進DWA 算法的評價函數，減少已知障礙物和未知障礙物的影響，并將改進后的兩種算法進行融合。

2.1 改進A*算法

2.1.1 量化環境信息

傳統A*算法的評價函數由實際代價函數G(n)和啟發函數H(n)組成。其中，啟發函數H(n)主導傳統A*算法的搜索性能。當起點與終點沒有障礙物時，選用歐幾里德距離公式估計的路徑與實際路徑相同，此時算法速度快、準確率高，但是實際應用中往往存在障礙物，導致搜索空間變大、效率降低、產生較多冗余節點。因此，當障礙物較少時，可以適當增大啟發函數H(n)權重，減小搜索范圍、提高搜索效率。當障礙物較多時，可以適當減小啟發函數H(n)權重，提高搜索范圍避免陷入局部最優。障礙物的存在使得產生多條可選路徑，最短路徑也會大于起點和終點之間的歐幾里德距離。為了估計最短路徑，引入環境障礙物比率K抽象反應環境復雜度。假設移動機器人起點為(xs,ys)終點為(xt,yt)，起點和終點組成的矩形柵格障礙物柵格數為N，那么障礙物的比率K可表示為公式（8）：

2.1.2 環境信息引入評價函數

環境障礙物比率K由障礙物柵格數與起點終點共同決定，K的大小隨起點終點的改變而改變。將環境障礙物比率K引入評價函數F(n)，改變啟發函數H(n)的權重，實現評價函數F(n)的自適應調整。啟發函數H(n)選取歐幾里德距離如公式（9）所示，改進后的評價函數F(n)如公式（10）所示。

由公式（10）可知，當環境中障礙物較少時，啟發函數H(n)的系數(1-lnK)大，算法縮小了搜索空間，提高了搜索效率。當環境中障礙物多時，啟發函數H(n)的系數(1-lnK)小，算法擴大了搜索空間，降低搜索速度，避免陷入局部最優。

2.1.3 路徑平滑優化

傳統A*算法路徑規劃是由連續柵格中心點連接組成，存在許多冗余節點，路徑轉折次數多，路徑不平滑。針對這些問題，基于Floyd 算法思想設計路徑平滑優化算法。

路徑平滑優化原理如圖3 所示，以此為例，傳統A*算法規劃的路徑規劃由柵格中心點的連線組成，優化前的路徑為（S，1，2，…，13，T），存在許多冗余節點。在考慮安全距離D的基礎上，對路徑進行平滑優化，使得路徑的選擇不再局限于過柵格的中心位置。優化后的路徑平滑度增加，路徑長度和轉折點減少，路徑平滑優化步驟如下：

圖3 路徑平滑優化原理圖Fig.3 Optimization of path smoothing

步驟1遍歷所有節點，刪除每一段路徑中間的冗余節點，保留起始點和拐點。刪除中間節點后剩下S，2，6，8，9，10，11，13，T五個節點。

步驟2遍歷起始點和拐點，從起點開始將每一個節點都與后面的節點相連作為備選路徑，判斷每條路徑與障礙物柵格的距離di與安全距離D的關系。若di≤D，則刪除路徑，若di>D，保留路徑，刪除路徑之間的拐點。刪除不必要拐點后剩余S，6，8，T三個節點。

步驟3提取剩余節點，輸出優化路徑，算法結束。

2.2 DWA算法評價函數優化

為了提高融合算法在復雜環境中的避障性能，需要對傳統DWA 算法的評價函數進行優化。在傳統DWA算法路徑規劃時，評價函數中Dist(v,ω)的權重，決定軌跡與障礙物的距離。若增大Dist(v,ω)系數，則生成路徑長度增大，達不到全局最優；若減小Dist(v,ω)系數，則遇到未知障礙物時，不能及時避障。為了減少全局已知障礙物與全局未知障礙物之間的相互干擾，將評價函數中Dist(v,ω)區分為模擬軌跡終點與已知障礙物的最近距離Dist_s(v,w)和模擬軌跡終點與未知障礙物的最近距離Dist_d(v,w)。則優化后的DWA 算法評價函數如公式（11）：公式（11）中，α、β、γ、φ是每一項的加權系數。Heading(v,w)為模擬軌跡終點和子目標點之間方向角偏差，隨著路徑的前進，按照提取的關鍵點順序不斷更新子目標點；Vel(v,ω) 用來評價移動機器人在當前估計中運行的速度大小；Dist_s(v,w)表示模擬軌跡終點與已知障礙物的最近距離，控制已知障礙物對局部路徑規劃的干擾；Dist_d(v,w)表示模擬軌跡終點與未知障礙物的最近距離，用來控制避障的靈敏度。

2.3 融合算法

改進A*算法能夠獲取全局路徑規劃中的最優解，在靜態工作環境中能很好地完成全局路徑規劃能力，但是對環境中出現的未知障礙物無法及時避障進行局部路徑規劃。DWA 算法路徑規劃缺少全局指引，只有目的地一個方向指引，在障礙物較多的環境中，容易陷入局部最優，導致全局路徑變大，甚至路徑規劃失敗。針對上述兩種算法的問題，首先提取改進A*算法規劃路徑上的關鍵點，然后將提取的關鍵點作為DWA 算法的中間目標點，引導局部路徑規劃，融合算法又將已知障礙物和未知障礙物進行分類，減少已知障礙物對路徑的影響，融合算法流程圖如圖4所示。

圖4 融合算法流程圖Fig.4 Flow chart of fusion algorithm

3 仿真驗證

3.1 改進A*算法仿真實驗

仿真實驗在MATLAB 2016b 環境下進行，為驗證算法的有效性和適應性，將對比實驗的地圖設計為迷宮式柵格地圖。在柵格地圖環境中，每個柵格設置為長為1 m的正方形，黑色柵格表示障礙物區域，白色柵格表示空白可移動區域，△表示機器人初始位置，○表示目標點。改進A*算法中障礙物比重參數K隨起點和終點的位置變化而變化，安全距離D設置為0.8 m。傳統A*算法與改進A*算法的仿真結果如圖5所示，性能比較如表1所示。

圖5 傳統A*算法與改進A*算法仿真結果對比圖Fig.5 Comparison of simulation results of traditional A* algorithm and improved A* algorithm

表1 傳統A*算法與改進A*算法性能指標對比Table 1 Performance comparison between traditional A*algorithm and improved A* algorithm

由圖5和表1可知，通過改進算法的評價函數，引入環境信息，在環境信息的引導下搜索更有方向性，使得路徑規劃時間減少0.8 s，轉彎次數減少48.0%，轉折角度減少了42.1%。通過借鑒Floyd算法思想對路徑進行平滑優化，使得路徑長度減少8.0%。由此可見改進算法不僅提高了算法的效率，而且降低了路徑長度，提高了平滑度。

3.2 融合算法仿真實驗

融合算法仿真實驗環境地圖在改進A*算法仿真實驗的基礎上做兩點改變：（1）在改進A*算法規劃路徑上隨機加上四個未知靜態障礙物；（2）在仿真環境中隨機加入一個未知動態障礙物。融合算法的評價函數的各項系數α=0.3、β=0.4、γ=0.2、φ=0.2，融合算法仿真實驗機器人運動學參數如表2所示。

表2 機器人運動學參數Table 2 Kinematic parameters of robot

融合算法仿真實驗結果如圖6所示，圖6（a）中粉色虛線為改進A*算法在靜態環境中的全局路徑規劃，路徑上的*為提取的關鍵點，作為融合算法的中間目標點。圖6（b）中紅色方塊用來模擬全局路徑上隨機加入的未知靜態障礙物，黃色方塊用來模擬環境中隨機出現的動態障礙物，且正在繞過第一個靜態障礙物。圖6（c）表示已繞過第二個靜態障礙物，而后與動態障礙物相遇開始局部動態路徑規劃。圖6（d）表示機器人在保證全局最優的基礎上完成了起點到終點的動態路徑規劃。在路徑規劃的過程中軌跡末端綠色短曲線為模擬軌跡，黑色虛線為動態障礙物的移動軌跡，藍色實線為融合算法規劃路徑。

圖6 融合算法路徑規劃仿真結果Fig.6 Simulation results of path planning based on fusion algorithm

由圖6融合算法仿真結果可知，提取改進A*算法路徑上的關鍵點，作為DWA算法的中間目標點，使得規劃路徑靠近改進A*算法靜態最優路徑；融合算法又將已知障礙物和未知障礙物進行分類，減少已知障礙物對路徑的影響，使得規劃路徑及時有效地避開未知動靜態障礙物。

4 實驗驗證

實驗選取Turtlebot2移動機器人平臺進行物理平臺實驗驗證。為滿足實驗需要，除Turtlebot2 自身所帶的傳感器外，還配備了思嵐A2激光雷達和Kinect v1深度相機來感知周圍環境。為了方便調試和機器人的自主導航，在Turtlebot2上配備一臺小型工控機作為主機，個人PC作為從機，主機和從機都安裝64位ubuntu16.04操作系統和機器人操作系統ROS（Kinetic）并在局域網內完成主機與從機的網絡配置，通過IP 實現個人PC 對Turtlebot2 的遠程控制。為了驗證算法的可行性，用Turtlebot2 移動機器人，在同一環境中對三種算法進行對比驗證。Turtlebot2 移動機器人的實驗參數設置如下：最大線速度0.5 m/s，最大角速度0.25 rad/s，線速度采樣個數為10，角速度采樣個數為20，采樣周期0.2 s。實驗場地如圖7所示。

圖7 實驗場地地圖Fig.7 Experimental site map

通過從機打開從機終端，啟動底盤，啟動Gmapping構圖算法。打開從機終端，啟動鍵盤節點，打開Rviz可視化工具，通過鍵盤控制節點控制機器人運動，構建環境地圖如圖8所示。

圖8 實驗場地建圖Fig.8 Construction of experimental site

4.1 傳統A*算法實驗驗證

在ROS 系統下通過主機遠程控制軟件登錄，實現對Turtlebot2 的遠程控制。啟動底盤控制節點，啟動定位并選定地圖，打開Rviz可視化界面，啟動傳統A*算法路徑規劃，選定起點和終點。運行結果如圖9所示。

圖9 傳統A*算法路徑規劃Fig.9 Traditional A* algorithm for path planning

傳統A*算法實驗結果由圖9，圖中紅色箭頭擺動幅度反應拐彎的角度。由實驗結果可以看出，傳統A*算可以實現在靜態環境中的全局路徑規劃，但是轉彎的折點多，路徑不夠平滑，不利于機器人的行駛。

4.2 改進A*算法實驗驗證

保持起點和終點位置不變，啟動改進A*算法路徑規劃，實驗結果如圖10所示。

圖10 改進A*算法路徑規劃Fig.10 Improved A* algorithm for path planning

改進A*算法實驗結果由圖10，圖中紅色箭頭擺動幅度反應拐彎的角度。由實驗結果可知，相比于傳統A*算法，改進A*算法的路徑規劃算法運動更加平滑，但是改進A*算法是全局靜態路徑規劃，無法及時躲避環境中出現的障礙物。

4.3 融合算法實驗驗證

實驗場景和設備與改進A*算法實驗保持一致，在實驗環境中站立三個人靜止不動，作為靜態未知障礙物，在小車運行過程中走動拍攝者作為動態未知障礙物。保持起點和終點位置不變，啟動融合算法路徑規劃，實驗結果如圖11所示。

圖11 融合算法路徑規劃Fig.11 Path planning based on fusion algorithm

改進A*算法實驗結果由圖11，圖中紅色箭頭擺動幅度反應拐彎的角度。由實驗結果可知，融合算法在全局最優的基礎上，又能及時躲避環境中出現的動靜態障礙物，提高了移動機器人在復雜環境中的適應能力。

5 結束語

傳統A*算法路徑規劃存在效率低、路徑規劃不平滑等缺點，不能滿足機器人的實際要求。這篇文章將環境信息引入傳統A*算法的評價函數，優化搜索策略，使得路徑的搜索隨著環境復雜度的變化而自適應調整，提高了算法的效率和靈活性；基于Floyd 算法思想設計路徑平滑算法，刪除冗余節點和不必要的拐點，使得路徑更加平滑，實驗結果表明，改進A*算法在大尺度復雜環境中不僅能生成最優路徑，效率也有所提高。提取改進A*算法的關鍵點作為DWA算法的中間目標點，在全局最優的基礎上實現了算法融合。實驗結果表明，融合算法規劃路徑圍繞全局最優路徑輕微浮動，且成功對比全局路徑上出現的障礙物及環境中隨機出現的動態障礙物。通過仿真驗證和實驗驗證，結果表明了融合算法在機器人路徑規劃中的可行性。