董翼寧,曹景勝,李剛

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

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

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

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

1 A-Star算法

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

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

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

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

(1)

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

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

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

(2)

(3)

式中:nx為當(dāng)前節(jié)點(diǎn)的橫坐標(biāo);ny為當(dāng)前節(jié)點(diǎn)的縱坐標(biāo);tx為目標(biāo)節(jié)點(diǎn)的橫坐標(biāo);ty為目標(biāo)節(jié)點(diǎn)的縱坐標(biāo)。

曼哈頓距離允許向上、下、左、右4個(gè)方向上進(jìn)行移動(dòng),歐幾里得距離允許在上、下、左、右和斜向8個(gè)方向上進(jìn)行移動(dòng)。

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

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

(4)

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

(5)

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

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

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

(6)

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

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

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

Obstacle為障礙物

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

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

2 優(yōu)化DWA算法

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

2.1 自動(dòng)引導(dǎo)車運(yùn)動(dòng)模型

圖2為離散化的自動(dòng)引導(dǎo)車運(yùn)動(dòng)學(xué)模型,得到車輛位姿(x,y,θ)通過(guò)線速度和角速度的變化來(lái)反映引導(dǎo)車的運(yùn)動(dòng),通過(guò)采樣速度空間得到的一組(vt,ωt)代表一條所預(yù)測(cè)的軌跡。

圖2 自動(dòng)引導(dǎo)車運(yùn)動(dòng)模型

當(dāng)采樣時(shí)間間隔Δt比較小時(shí),車輛的軌跡可認(rèn)為是由一段短的線段連接而成。車輛的微分約束可以寫(xiě)成離散化的形式,即

(7)

2.2 速度空間采樣

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

(1)最大速度約束為

(8)

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

(9)

(3)距離約束為

(10)

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

2.3 評(píng)價(jià)函數(shù)

評(píng)價(jià)函數(shù)是DWA算法中關(guān)鍵的一個(gè)步驟,傳統(tǒng)的DWA算法的評(píng)價(jià)函數(shù)考慮了方位角、當(dāng)前距障礙物的距離、當(dāng)前速度這3個(gè)指標(biāo)。

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

(11)

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

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

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

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

(12)

為了避免某項(xiàng)標(biāo)準(zhǔn)在評(píng)價(jià)函數(shù)中太占優(yōu)勢(shì),減小評(píng)價(jià)標(biāo)準(zhǔn)對(duì)評(píng)價(jià)函數(shù)最終結(jié)果的影響,對(duì)評(píng)價(jià)函數(shù)中的權(quán)重項(xiàng)進(jìn)行平滑處理,進(jìn)行歸一化操作,如式(13)所示,進(jìn)行平滑操作后是得自動(dòng)引導(dǎo)車避開(kāi)障礙朝著目標(biāo)地點(diǎn)以一定的速度行駛。

(13)

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

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

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

圖3 融合算法流程圖

4 仿真分析

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

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

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

Δ為路徑規(guī)劃的起始點(diǎn);Ο為目標(biāo)點(diǎn)

Δ為路徑規(guī)劃的起始點(diǎn);Ο為目標(biāo)點(diǎn)

圖6 環(huán)境3

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

表1 不同算法性能對(duì)比

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

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

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

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

Δ為規(guī)劃起點(diǎn);Ο為目標(biāo)點(diǎn)

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

4.3 融合算法仿真分析

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

表3 融合算法性能對(duì)比

圖8 環(huán)境1

圖9 環(huán)境2

圖10 環(huán)境3

5 結(jié)論

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