融合A-Star與DWA雙優(yōu)化算法的自動引導車路徑規(guī)劃

董翼寧,曹景勝,李剛

(遼寧工業(yè)大學汽車與交通工程學院,錦州 121001)

隨著智能技術的蓬勃發(fā)展和不斷進步,需要自動引導車的場景越來越多,許多智能制造車間,封閉園區(qū)的貨物配送等都采用自動引導車,路徑規(guī)劃技術對自動引導車非常關鍵,它決定了自動引導車的工作效率。

在路徑規(guī)劃技術中,主要基于以下三大方案,一類是基于圖搜索的路徑規(guī)劃方案,如A-Star算法[1-2]、Dijkstra算法[3]、人工勢場法[4]等。姜辰凱等[5]針對自動引導車路徑規(guī)劃問題提出了一種改進Dijkstra算法,該算法可以避免規(guī)劃過程的碰撞沖突?？谆鄯嫉萚6]提出一種改進A-star算法,通過改進啟發(fā)函數(shù),引入轉彎正代價參數(shù),從而提高規(guī)劃效率和路徑平滑性。第二種是基于采樣的路徑規(guī)劃方案。如RRT[7-8]算法,賀伊琳等[9]提出了一種優(yōu)化RRT算法的無人駕駛路徑規(guī)劃,在傳統(tǒng)RRT算法的基礎上,限制采樣可行區(qū)域,基于目標偏向策略,以一定的概率將目標點作為隨機點進行隨機樹擴張,該方案改善了路徑規(guī)劃的效率。江洪等[10]針對RRT路徑規(guī)劃算法效率低且采樣隨機的問題提出了優(yōu)化算法,提出了一種偏向目標點的新節(jié)點擴展方法。該方法將目標點引力、障礙物斥力以及隨機點引力三力合一,并且添加了與障礙物距離相關的自適應函數(shù)。第三種方案是基于智能仿生的路徑規(guī)劃方案。如將蟻群算法[11]、遺傳算法[12]應用到路徑規(guī)劃中。郭蓬等[13]將蟻群算法應用到無人車路徑規(guī)劃中,通過蟻群算法設置始末位置、構造解空間、更新信息素、增加迭代次數(shù)、判斷路徑長度,最后輸出最短路線。針對傳統(tǒng)蟻群算法在路徑規(guī)劃中搜索速度慢,路徑規(guī)劃容易陷入局部最優(yōu)的問題,張儀夫等[14]提出一種雙向蟻群算法。設計相遇機制,引入獎懲因子,加快求解最優(yōu)路徑的速度。

以上路徑規(guī)劃方案在靜態(tài)環(huán)境中均有很好的效果。針對自動引導車路徑規(guī)劃中存在效率低,動態(tài)避障效果差的情況,現(xiàn)提出一種融合A-Star與DWA雙優(yōu)化路徑規(guī)劃算法。首先基于傳統(tǒng)的A-Star算法設計自適應啟發(fā)函數(shù),進行關鍵點的選取,刪除冗余點,之后融合優(yōu)化動態(tài)窗口算法,進行實時地動態(tài)避障。

1 A-Star算法

1.1 傳統(tǒng)A-Star算法

A-Star算法是一種傳統(tǒng)的路徑規(guī)劃算法,路徑規(guī)劃領域得到了廣泛的應用,該算法是由廣度優(yōu)先搜索算法(BFS)與Dijkstra算法進行融合,通過設置啟發(fā)函數(shù)做靜態(tài)全局路徑規(guī)劃。A-Star算法是靜態(tài)全局路徑規(guī)劃中最有效的算法,它能找到最短路徑。

A-Star算法本質上是廣度優(yōu)先搜索算法(BFS)的優(yōu)化,它是一種搜索算法,首先將目標區(qū)域地圖進行柵格化,在等效柵格化的地圖中設置起始點和目標點,遍歷起點周圍八領域柵格空間內的點,尋找最優(yōu)點,然后繼續(xù)遍歷該點周圍八領域柵格周圍內的最優(yōu)點,不斷更新最優(yōu)點,直到繞過障礙找到目標點。最優(yōu)點的獲取原則是基于路徑優(yōu)劣評價函數(shù),即

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

(1)

式(1)中:G(n)為從起點到達狀態(tài)點n的代價估計;H(n)為啟發(fā)函數(shù),是狀態(tài)點n到目標點的最佳路徑代價估計。

啟發(fā)函數(shù)H(n)是整個A-Star算法中最重要的部分,啟發(fā)函數(shù)H(n)的設計直接影響路徑規(guī)劃的效率以及能否找到最優(yōu)規(guī)劃路徑。傳統(tǒng)A-Star算法中啟發(fā)函數(shù)H(n)一般選用曼哈頓距離[式(2)]和歐幾里得距離[式(3)]。

H(n)=|nx-tx|+|ny-ty|

(2)

(3)

式中:nx為當前節(jié)點的橫坐標;ny為當前節(jié)點的縱坐標;tx為目標節(jié)點的橫坐標;ty為目標節(jié)點的縱坐標。

曼哈頓距離允許向上、下、左、右4個方向上進行移動,歐幾里得距離允許在上、下、左、右和斜向8個方向上進行移動。

1.2 優(yōu)化A-Star算法

啟發(fā)函數(shù)H(n)影響整個自動引導車路徑規(guī)劃的整體效率以及能否找到最優(yōu)路徑,啟發(fā)函數(shù)H(n)相較實際代價越小就越容易找到一條優(yōu)路徑,但是犧牲了路徑規(guī)劃的效率。如果啟發(fā)函數(shù)H(n)相較實際代價大,就會犧牲最優(yōu)路徑的選取,但是很大程度上縮短了自動引導車路徑規(guī)劃所需的時間。因此啟發(fā)函數(shù)H(n)的設計至關重要,啟發(fā)函數(shù)的設計要從全局出發(fā),全局中障礙物比率也對路徑規(guī)劃有著重要的影響,本文根據(jù)當前位置距目標點的距離以及障礙物比率提出一種自適應的啟發(fā)函數(shù),通過對啟發(fā)函數(shù)H(n)的優(yōu)化來提升自動引導車的路徑尋優(yōu)和規(guī)劃效率。由于車輛運動時在轉彎和避障的方向不確定,因此要減小方向約束,曼哈頓距離和歐幾里得距離均可以滿足自動引導車路徑規(guī)劃在方向上的要求,自適應啟發(fā)函數(shù)的設計選用歐幾里得距離公式進行設計。自適應啟發(fā)函數(shù)的設計為

(4)

Hadp(n)=(1+D)(1-lnK)H(n)

(5)

式中:sx為起始點的橫坐標;sy起始點的縱坐標;D為當前節(jié)點到目標點的距離與起始點到目標點距離的比值;K為障礙物占比。

A-Star算法中實現(xiàn)了靜態(tài)全局路徑規(guī)劃,但規(guī)劃路徑時存在冗余點過多,而且路徑不夠平滑的情況,冗余點對路徑規(guī)劃意義不大,而且增加了路徑規(guī)劃時的計算復雜度,本文研究針對這一問題提出了關鍵點選取策略,

在A-Star算法中冗余點很多是節(jié)點共線的情況,現(xiàn)提出一種去除冗余點的算法,其中,設路徑點A坐標為(x1,y1),從A點起的第2個路徑點的坐標為(x2,y2),從A點起的第3個路徑點坐標為(x3,y3)。

(6)

式(6)中:C為常數(shù)。

如果滿足式(6),則從路徑點A起的第2個點為冗余點。隨著A-Star算法不斷進行節(jié)點迭代更新,也不斷會生成新的路徑點,依次循環(huán)遍歷到目標點,去除所有滿足式(6)的路徑點。

利用插點法繼續(xù)進行關鍵點選取,平滑路徑。插點法的原理如圖1所示。

Obstacle為障礙物

假設1為子路徑點,4為目標路徑點,從點1～點4為最短規(guī)劃路徑,2、3為規(guī)劃路徑點,從點1到點2再到點4的距離和小于點1到點3再到點4的距離和,因此選擇路徑點1、2、4。依次在搜索范圍內遍歷路徑點進行選取。

對傳統(tǒng)A-Star算法進行啟發(fā)函數(shù)的優(yōu)化和關鍵點選取后,縮短了路徑的距離,平滑了路徑,減少了路徑規(guī)劃的時間,實現(xiàn)了最優(yōu)全局路徑規(guī)劃,優(yōu)化的A-Star算法可以在一定程度上提升自動引導車路徑規(guī)劃的效率。

2 優(yōu)化DWA算法

自動引導車在復雜的路面上行駛會遇到各種難以預料的障礙,靜態(tài)路徑規(guī)劃不能滿足自動引導車對動態(tài)障礙物的躲避,而動態(tài)窗口算法可以解決無法對動態(tài)障礙物實時避障的問題。DWA算法的基本原理是速度采樣,建立由線速度和角速度(v,ω)構成的速度采樣空間,對其進行實時采樣,得到自動引導車的運動軌跡,得到的軌跡有許多條,按照評價函數(shù)對這些軌跡進行擇優(yōu)篩選,得到最優(yōu)軌跡,根據(jù)最優(yōu)軌跡的速度給到自動引導車控制器使車到達目標地點。

2.1 自動引導車運動模型

圖2為離散化的自動引導車運動學模型,得到車輛位姿(x,y,θ)通過線速度和角速度的變化來反映引導車的運動,通過采樣速度空間得到的一組(vt,ωt)代表一條所預測的軌跡。

圖2 自動引導車運動模型

當采樣時間間隔Δt比較小時,車輛的軌跡可認為是由一段短的線段連接而成。車輛的微分約束可以寫成離散化的形式,即

(7)

2.2 速度空間采樣

在速度空間中采樣得到多組速度對,根據(jù)自動引導車的自身要求和其他環(huán)境的要求,對采樣的條件進行限制。采樣空間被限制在一個動態(tài)窗口中,根據(jù)自動引導車的自身要求,需要對其最大行駛速度、加減速的能力等進行約束。

(1)最大速度約束為

(8)

(2)車輛加減速能力約束為

(9)

(3)距離約束為

(10)

得到的動態(tài)窗口,分別對v、ω等時間間隔t采樣,得到vi、ωi,將其組合得到多對(v,ω)i。dist(v,ω)表示軌跡上與障礙物最近的距離。

2.3 評價函數(shù)

評價函數(shù)是DWA算法中關鍵的一個步驟,傳統(tǒng)的DWA算法的評價函數(shù)考慮了方位角、當前距障礙物的距離、當前速度這3個指標。

G(v,ω)=σ[αheading(v,ω)+βdist(v,ω)+γvelocity(v,ω)]

(11)

式(11)中:heading(v,ω)為方位角評價函數(shù),用來評價自動引導車在當前設定的采樣速度下,達到模擬軌跡末端時的朝向和目標之間的角度差距,其值越大,就說明此時模擬軌跡越朝向目標點,是全局搜索能力的代表;dist(v,ω)為自動引導車在當前軌跡上與障礙物之間的最近距離,是一項安全閾值;Velocity(v,ω)用來評價當前軌跡的速度大小,Velocity(v,ω)的值越大表明當前引導車線速度大,角速度小,沿著軌跡直線快速行駛。3個評價指標對最終路徑規(guī)劃和避障能力有著重要的影響,而全局障礙物比率K也對規(guī)劃有著一定影響,引入障礙物比率關聯(lián)3個評價指標,重新設計評價函數(shù)。優(yōu)化后的評價函數(shù)為

G(v,ω)=σ[(1+k)αheading(v,ω)+

0.76(1-k)βdist(v,ω)+

(2-k)γvelocity(v,ω)]

(12)

為了避免某項標準在評價函數(shù)中太占優(yōu)勢,減小評價標準對評價函數(shù)最終結果的影響,對評價函數(shù)中的權重項進行平滑處理,進行歸一化操作,如式(13)所示,進行平滑操作后是得自動引導車避開障礙朝著目標地點以一定的速度行駛。

(13)

式(13)中:normal_head(i)、normal_dist(i)、normal_velocity(i)分別為歸一化后的方位角、距離以及速度評價函數(shù)。

3 融合優(yōu)化A-Star與優(yōu)化DWA算法

傳統(tǒng)的A-Star路徑規(guī)劃算法應用范圍廣,適應性好,能夠找到全局最優(yōu)路徑,但搜索效率不可控,優(yōu)化A-Star算法對小車進行了高效靜態(tài)的全局路徑規(guī)劃,找到了靜態(tài)狀態(tài)下的最優(yōu)全局路徑,但自動引導車在實際工作環(huán)境中不只需要對靜態(tài)障礙物進行避障,道路環(huán)境中還有其他如行人等動態(tài)障礙物,DWA算法通過對自動引導車速度空間進行采樣,約束車輛的運動狀態(tài),達到局部路徑規(guī)劃最終實現(xiàn)動態(tài)避障的效果。優(yōu)化A-Star算法與DWA算法融合充分發(fā)揮了兩個算法的優(yōu)勢。優(yōu)化A-Star算法與優(yōu)化DWA算法的融合算法流程如圖3所示。

圖3 融合算法流程圖

4 仿真分析

為了驗證本文優(yōu)化的A-Star算法靜態(tài)全局路徑規(guī)劃和優(yōu)化DWA算法的性能以及優(yōu)化A-Star算法和優(yōu)化DWA算法的融合算法在動態(tài)避障局部路徑規(guī)劃的有效性,本文研究在windows10系統(tǒng),i7-8550U CPU @ 1.80 GHz的環(huán)境下利用MATLAB2021a對算法進行仿真分析。

4.1 優(yōu)化A-Star算法對比分析

文獻[15]對A-Star算法進行了優(yōu)化,提出了一種新型的啟發(fā)函數(shù),能夠找到最佳路徑,并在搜索效率上針對傳統(tǒng)算法有一定提升,為了驗證優(yōu)化后的A-Star算法的魯棒性和泛化性,選取3個不同環(huán)境進行傳統(tǒng)算法中的Dijkstra算法、文獻[15]算法和優(yōu)化后的A-Star算法對比。如圖4～圖6所示。

Δ為路徑規(guī)劃的起始點;Ο為目標點

Δ為路徑規(guī)劃的起始點;Ο為目標點

圖6 環(huán)境3

從3種不同的環(huán)境圖上分析Dijkstra算法、文獻[15]算法和優(yōu)化A-Star算法上的性能上看,由圖4～圖6可以看出,優(yōu)化后的A-Star算法規(guī)劃的路徑更加平滑。圖中灰色陰影區(qū)域代表算法在路徑規(guī)劃過程中的搜索區(qū)域,可以明顯看到在環(huán)境1、2、3中優(yōu)化后的A-Star算法相較Dijkstra算法和文獻[15]算法極大地減少了路徑規(guī)劃過程中的搜索范圍,減少了計算路徑規(guī)劃系統(tǒng)的計算量,提高了工作效率。仿真結果如表1所示,從表1中可以看出,優(yōu)化后的A-Star算法在環(huán)境1、2、3中路徑規(guī)劃距離相較Dijkstra和文獻[15]算法有所減小,而且規(guī)劃時間也大幅減小。關鍵點選取算法的使用減少了傳統(tǒng)算法在路徑規(guī)劃過程中轉折點過多的問題,減少了路徑規(guī)劃過程中路徑點的數(shù)量。優(yōu)化后的A-Star算法極大地提高了路徑規(guī)劃效率。

表1 不同算法性能對比

從仿真結果上來看,優(yōu)化后的A-Star算法無論在路徑規(guī)劃最短距離、規(guī)劃時間、搜索節(jié)點數(shù)、路徑規(guī)劃點上都比傳統(tǒng)Dijkstra、文獻[15]算法上有提升,優(yōu)化后的A-Star算法達到了路徑全局最優(yōu)規(guī)劃的目的,而且提高了效率。

4.2 優(yōu)化DWA算法對比分析

對優(yōu)化后的DWA算法與傳統(tǒng)DWA算法進行仿真,對比分析兩種算法的性能。優(yōu)化評價函數(shù),引入障礙物比率,將評價函數(shù)中heading(v,ω)、dist(v,ω)和velocity(v,ω)3個權重項關聯(lián)。算法仿真如圖7所示。算法性能對比如表2所示。

表2 優(yōu)化DWA算法對比

Δ為規(guī)劃起點;Ο為目標點

從該組實驗結果上來看在同樣的規(guī)劃路徑下,優(yōu)化算法在動態(tài)規(guī)劃時間上比傳統(tǒng)算法減少了5.343%。提升了路徑規(guī)劃效率。

4.3 融合算法仿真分析

DWA算法的融入是為了保證自動引導車在運行過程中對動態(tài)障礙物的躲避,DWA算法實現(xiàn)了局部的動態(tài)避障規(guī)劃。文獻[15]優(yōu)化了A-Star算法的啟發(fā)函數(shù)并融入傳統(tǒng)DWA算法實現(xiàn)了全局最優(yōu)規(guī)劃和實時避障的效果。為了驗證本文提出融合算法的魯棒性和泛化性,在3種不同環(huán)境與文獻[15進行對比仿真,如圖8～圖10所示。表3為兩種融合算法的性能對比分析,分別采用移動時間和移動距離對兩種算法進行比較。從表3可以看到,本文的融合算法相較其他算法在效率上有很大提升,在3種環(huán)境下平均提升了19.24%,移動距離在簡單環(huán)境下提升相對較小但是在復雜環(huán)境上有一定提升,提升了11.07%。

表3 融合算法性能對比

圖8 環(huán)境1

圖9 環(huán)境2

圖10 環(huán)境3

5 結論

針對自動引導車路徑規(guī)劃問題提出了一種靜態(tài)全局最優(yōu)規(guī)劃加局部動態(tài)避障規(guī)劃的路徑規(guī)劃方案,滿足自動引導車在工作過程中對路徑規(guī)劃算法所要求的全局最優(yōu)和動態(tài)實時避障的要求。靜態(tài)全局最優(yōu)規(guī)劃優(yōu)化傳統(tǒng)A-Star算法,優(yōu)化后的算法不僅能得到最優(yōu)路徑而且規(guī)劃出的路徑與傳統(tǒng)算法相比,較平滑,減少了轉折點的數(shù)量,減少算法計算量,提高了規(guī)劃效率。針對實時避障的要求,融合優(yōu)化DWA算法。融合算法既達到了全局最優(yōu)路徑規(guī)劃又做到了對動態(tài)障礙物的實時躲避。提高了路徑規(guī)劃效率和安全性。