融合改進的A*算法和動態窗口法的機器人路徑規劃

豐雪艷,李振璧,2*

(1. 安徽理工大學 電氣與信息工程學院,安徽 淮南 232000;2. 亳州學院 電子與信息工程系,安徽 亳州 236000)

0 引言

連接起點位置和終點位置的序列點或曲線稱之為路徑,構成路徑的策略稱之為路徑規劃[1].根據障礙物是否完全已知,路徑規劃又可分為全局路徑規劃和局部路徑規劃.

全局路徑規劃算法有基于圖搜索的A*算法、D*算法,也有智能仿生算法如粒子群算法、人工魚群算法等[2].其中具有啟發式思想的A*算法被認為是進行靜態全局路徑規劃最有效的方法,但它規劃出的路徑存在拐角過大、擴展節點較多等缺點.申瑞等[3]提出的改進A*算法采用精簡搜索策略、安全保護策略、路徑平滑策略可以快速搜索出一條安全平滑的路徑,提高了搜索效率和安全性.王迪等[4]將A*算法的鄰域節點進行分級,根據節點的優先級確定要擴展的方向,然后利用Floyd算法剔除所規劃路徑中的多余節點,提高規劃路徑的平滑度.汪四新等[5]采用雙向A*算法進行路徑規劃,并根據人工勢場法的思想設置虛擬目標點和距離衰減系數,使得雙向搜索能在中間區域相遇,在規劃路徑的基礎上提高了算法的收斂速度.

局部路徑規劃算法有DWA算法[5]和人工勢場法[6](Artificial Potential Field,APF).DWA算法可用于未知環境下的動態避障,可以較好地躲避障礙物,其優點是計算復雜度低,同時考慮了移動機器人的動力學特性,可以規劃出一條安全無碰撞的路徑[7-8].Mai等[9]提出的改進DWA算法可以提前感知障礙物的分布情況,使機器人在行駛時安全地避開障礙物密集的區域.姚進鑫等[10]將曼哈頓和歐氏距離進行結合作為優化A*算法的評價函數,在獲得全局路徑后以DWA算法為核心規劃出一條平滑度較高的全局最優路徑,有效解決了優化A*算法規劃路徑轉折曲率不連續的問題.

綜上所述,本文提出一種融合改進A*算法和DWA算法的機器人路徑規劃算法.該算法首先在傳統A*算法啟發函數前引入動態的權重系數,實現自適應調整以提高算法的搜索效率,然后采用關鍵點選取策略剔除路徑上的冗余節點,規劃出一條只含有關鍵點的全局路徑,最后將改進A*算法所規劃的路徑上的關鍵點作為DWA算法的中間目標點,使局部路徑遵循全局路徑,并實現動態避障.

1 改進的A*算法

1.1 傳統A*算法

A*算法是全局路徑規劃中常用的一種圖搜索算法,其結合了Dijkstra算法和廣度優先算法(Breadth-First-Search,BFS)的優點,提升了算法的搜索效率[12].代價函數為:

F(n)=G(n)+H(n),

(1)

式中:n為當前節點;F(n)為總的代價值;G(n)為起點到當前點所花費的實際代價;H(n)是啟發函數,為當前點到終點的預估代價值.傳統A*算法常用的啟發函數有曼哈頓距離、切比雪夫距離和歐幾里德距離3種[13],本文選取歐幾里得距離作為啟發函數.

1.2 優化評價函數

本文對A*算法進行優化,結合標準正態分布引入隨機器人位置而變化的動態權重系數w(x),當前點離終點較遠時w(x)較大,此時啟發函數在代價函數中所占的比重增加,提高了算法的搜索速度;隨著移動機器人逐漸向終點靠近,w(x)也隨之減小,此時啟發函數在代價函數中所占的比重減少,擴大了算法的搜索范圍,使其能夠搜索出最短路徑.具體計算公式為:

F(n)=G(n)+w(x)H(n).

(2)

(3)

(4)

式中:r為起點到當前點的距離;R為當前點到終點的距離.

1.3 關鍵點選取策略

傳統A*算法規劃的路徑存在一些共線的冗余點和多余的轉折點,若機器人以傳統A*算法所規劃的路徑行駛,則需要在一些不必要的轉折點改變其航向角,這些多余的轉折會影響機器人的正常行駛.在實際工作中,機器人運動路徑的平滑程度會影響機器人工作的完成度,路徑越平滑,工作時產生誤差的幾率越小,工作的完成度就越好.因此,本文引入了關鍵點選取策略,即在節點(m-1,m,m+1)中,連接不相鄰的兩點(m-1,m+1),如果連線經過障礙物區域,則m為關鍵點,否則為冗余點.通過關鍵點選取策略剔除冗余點,得到一條包含關鍵點,起點和終點的優化路徑.關鍵點選取策略示如圖1所示,優化前的路徑為s-1-2-3-4-5-g;優化后的路徑為s-4-g,其中4為選取的關鍵點.

圖1 關鍵點選取策略

2 DWA算法

DWA算法是具有避障能力的局部路徑規劃算法,其考慮了機器人動力學約束問題[14].DWA算法在機器人運動過程中對其線速度v與角速度w進行采樣,以此來模擬機器人在一定時間間隔Δt內可能出現的運動軌跡,并利用評價函數對該運動軌跡進行評價,選取出評價最高的軌跡,將該軌跡對應的線速度v和角速度w作為機器人下一周期運動的速度.

2.1 機器人運動學模型

在對機器人的運動軌跡進行模擬前,需要根據機器人的線速度v和角速度w建立機器人的運動學模型,對于兩輪差速模型,機器人t時刻的線速度為vt、角速度為wt、航向角為θt,設機器人在較短時間間隔Δt內做勻速運動,則其運動軌跡近似為一條直線,運動學模型表示為[15]:

(5)

2.2 速度空間

(1)受機器人硬件性能以及環境等因素的影響,機器人速度約束表示為:

(6)

式中,vmin、vmax分別表示線速度的最大值和最小值;wmin、wmax分別表示角速度的最小值和最大值.

(2)受機器人電機性能的影響,機器人需要在電機力矩可以承受的范圍內進行加減速,機器人電機加減速約束表示為:

(7)

式中,vc、wc分別表示當前的線速度和角速度;va、wa分別表示機器人的最大線加速度和角加速度;vb、wb分別表示機器人的最大線減速度和角減速度.

(3)為了避免機器人與動態障礙物發生碰撞,需要保證機器人與障礙物之間有一定的安全距離,因此需要對機器人的速度進行約束,速度約束表示為:

(8)

式中,dist(v,w)為軌跡與障礙物的距離.

機器人的速度范圍vr滿足以上3種約束,即:vr∈vm∩vd∩vs.

2.3 評價函數

根據機器人的運動學模型和速度范圍可以模擬出多條采樣軌跡,需要采用評價函數對多條采樣軌跡進行評價,選取一條評價最高的軌跡作為機器人下一周期的運動軌跡,評價函數為:

(9)

式中:σ為平滑系數;α、β、γ為各子函數的加權系數;head(v,w)為當前速度下軌跡末端點與目標點的方位角偏差;dist(v,w)為軌跡與障礙物的最近距離;vel(v,w)為當前速度大小的評價函數.

3 融合算法路徑規劃

改進后的A*算法雖然能尋找到一條最優路徑,但無法躲避環境中出現的動態障礙物.DWA算法雖然可以躲避動態障礙物,但缺少全局指引,在障礙物較多的復雜環境中易陷入局部最優或無法規劃出一條到達目標點的路徑.因此本文對兩種算法進行融合,將改進A*算法所規劃路徑的關鍵點作為DWA算法的當前目標點,通過融合算法規劃的路徑,可以在保證全局最優的基礎上實現動態避障.融合算法流程如圖2所示.

圖2 融合算法流程

4 仿真實驗及分析

為了驗證融合算法的可行性,在Matlab2022a中進行仿真實驗.仿真實驗地圖為20 m×20 m、40 m×40 m兩種,黑色區域表示已知障礙物,灰色區域表示未知障礙物,白色區域表示可移動區域,S表示起始點,T表示目標點.

4.1 改進的A*算法仿真對比實驗

改進的A*算法在傳統A*算法的基礎上對啟發函數進行了優化,并利用關鍵點選取策略剔除冗余點.在20 m×20 m、40 m×40 m的柵格地圖上,分別使用傳統A*算法和本文A*算法進行對比實驗,在20 m×20 m柵格地圖中,起始點為S(1.5,1.5),目標點為T(19.5,17.5);在40 m×40 m柵格地圖中,起始點為S(1.5,1.5),目標點為T(39.5,37.5).兩種算法的性能指標如表1所列,仿真結果如圖3和圖4所示.

表1 改進前后算法性能指標對比

(a)傳統的A*算法 (b)改進的A*算法圖3 20 m×20 m柵格地圖下的對比實驗

(a)傳統A*算法 (b)改進A*算法圖4 40 m×40 m柵格地圖下的對比實驗

根據表1可知,在20 m×20 m的柵格地圖中,改進的A*算法所規劃的路徑長度比傳統A*算法所規劃的路徑長度減少了4.89%,其規劃路徑所用時間比傳統A*算法減少了18.18%,其規劃路徑的轉折次數比傳統A*算法減少了57.14%,路徑的轉折角度也比傳統A*算法減少了63.31%;在40 m×40 m的柵格地圖中,改進A*算法所規劃的路徑長度比傳統A*算法所規劃的路徑長度減少了4.33%,其規劃路徑所用時間比傳統A*算法減少了31.48%,其規劃路徑的轉折次數比傳統A*算法減少了53.33%,路徑的轉折角度也比傳統A*算法減少了58.06%.結合圖3、圖4可知,改進的A*算法不僅剔除了共線的冗余點和多余的轉折點,其規劃的路徑長度和所用時間相比傳統A*算法也有所減少,提高了搜索效率和路徑的平滑度.

4.2 融合算法對比實驗

為驗證融合算法的路徑規劃及動態避障能力,在20 m×20 m的柵格地圖中進行實驗,在改進的A*算法所規劃的全局路徑上添加隨機障礙物,其在圖中用灰色方格表示,融合算法的仿真實驗結果如圖5所示.

(a)躲避未知障礙物 (b)到達目標點圖5 融合算法規劃路徑

虛線代表改進的A*算法所規劃的路徑,實線代表融合算法所規劃的路徑,*代表融合算法的當前目標點.隨著機器人的運動,下一個關鍵點將會成為新的當前目標點.

實驗結果表明,提取改進A*算法所規劃路徑的關鍵點,將其依次作為DWA算法進行局部路徑規劃的當前目標點,可在全局路徑的基礎上規劃出一條能夠躲避未知障礙物的局部路徑,實現了機器人導航的全局路徑最優與動態避障功能.

5 結論

針對傳統的A*算法存在搜索效率低、規劃路徑平滑度差以及不能躲避未知障礙物等問題,本文提出一種融合改進的A*算法和DWA算法的機器人路徑規劃算法,與傳統A*算法相比,該算法所規劃的路徑更加平滑,且能躲避環境中的未知障礙物;與DWA算法相比,該算法所規劃的路徑更優,且不易陷入局部最優.實驗結果表明,融合算法實現了機器人導航路徑全局最優以及動態避障功能,具有一定的可行性.