一種面向非結(jié)構(gòu)化環(huán)境的改進(jìn)跳點(diǎn)搜索路徑規(guī)劃算法

魏博聞, 嚴(yán) 華

(四川大學(xué)電子信息學(xué)院, 成都 610065)

近年來,智能機(jī)器人迅速發(fā)展,人們對(duì)智能機(jī)器人的研究取得了長(zhǎng)足的進(jìn)步。智能機(jī)器人是一個(gè)集環(huán)境感知、動(dòng)態(tài)決策與規(guī)劃、行為控制與執(zhí)行等多功能于一體的綜合系統(tǒng)。自主導(dǎo)航是移動(dòng)機(jī)器人的核心算法,為機(jī)器人提供基于感知的導(dǎo)航規(guī)劃能力,是學(xué)術(shù)與工業(yè)界的熱點(diǎn)。其具體定義可以做如下描述:根據(jù)相關(guān)性能指標(biāo)(滿足時(shí)間約束,滿足動(dòng)力學(xué)約束,滿足運(yùn)動(dòng)學(xué)約束),在運(yùn)動(dòng)空間中找到一條從起始狀態(tài)到目標(biāo)狀態(tài)且可以避開障礙物的最優(yōu)路徑[1-3]。

當(dāng)周圍環(huán)境的障礙物信息對(duì)智能體完全已知的情況下,這類規(guī)劃稱為全局路徑規(guī)劃[4]。全局路徑算法主要包括如下3類:智能仿真算法，如蟻群算法、遺傳算法、粒子群算法等[5]；基于圖搜索的算法，如A*、Dijkstra、HybridA*、D*、D*Lite等[6]基于采樣的算法,包括概率路圖法以及隨機(jī)搜索樹的各種變種算法[7]。

A*算法被廣泛使用于移動(dòng)機(jī)器人的自主路徑規(guī)劃中[8]。但是A*算法仍然存在問題。首先,全局路徑規(guī)劃只是智能機(jī)器人系統(tǒng)中的一步,通常系統(tǒng)只會(huì)分配小部分算力來進(jìn)行路徑規(guī)劃,在面對(duì)復(fù)雜的非結(jié)構(gòu)化地圖進(jìn)行路徑規(guī)劃時(shí),A*算法有計(jì)算量大、內(nèi)存消耗嚴(yán)重等缺陷。其次,由于智能機(jī)器人自身存在動(dòng)力學(xué)約束,A*算法給出的不平滑的路徑可能導(dǎo)致機(jī)器人無法直接執(zhí)行。

A*算法在擴(kuò)展過程會(huì)對(duì)當(dāng)前節(jié)點(diǎn)的每個(gè)鄰居節(jié)點(diǎn)做檢測(cè),所以該算法需要維護(hù)非常龐大的OpenList和CloseList表,這是導(dǎo)致內(nèi)存占用嚴(yán)重、計(jì)算時(shí)間長(zhǎng)的關(guān)鍵原因。A*算法在運(yùn)算過程中,每次從優(yōu)先隊(duì)列中選取代價(jià)值最小(優(yōu)先級(jí)最高)的節(jié)點(diǎn)作為下一個(gè)待遍歷的節(jié)點(diǎn),A*算法使用兩個(gè)集合來表示待遍歷的節(jié)點(diǎn)與已經(jīng)遍歷過的節(jié)點(diǎn),通常稱之為openset與closeset。針對(duì)A*運(yùn)算速度慢的問題,文獻(xiàn)[9]提出了跳點(diǎn)搜索算法(jump point search, JPS)進(jìn)行了改進(jìn),提高了運(yùn)算效率。但是,由于啟發(fā)函數(shù)設(shè)計(jì)過于簡(jiǎn)單,該方法在復(fù)雜不規(guī)則地圖中會(huì)無法保證最終路徑是全局最優(yōu)路徑。

文獻(xiàn)[10]中提出了一種基于lattice采樣規(guī)劃方法,將障礙物和智能機(jī)器人本身轉(zhuǎn)化到frenet坐標(biāo)系中,在frenet坐標(biāo)系中使用lattice方式進(jìn)行規(guī)劃,將規(guī)劃路徑使用三次多項(xiàng)式進(jìn)行連接。雖然該方法能規(guī)劃一條免碰撞且符合車輛動(dòng)力學(xué)約束的路徑,但是其笛卡兒坐標(biāo)系與frenet坐標(biāo)系轉(zhuǎn)化計(jì)算量巨大,很難滿足路徑規(guī)劃模塊實(shí)時(shí)性要求,故該方法并未在實(shí)際中進(jìn)行推廣。

近年來,盡管已經(jīng)提出了很多優(yōu)秀的路徑規(guī)劃算法,但是生成一條滿足實(shí)時(shí)性的路徑仍是一項(xiàng)比較困難的工作。為了滿足路徑生成的實(shí)時(shí)性與安全性需解決以下問題:

(1)如何在保證全局最短路徑的前提下,降低算法的計(jì)算復(fù)雜性,降低算法運(yùn)算時(shí)間,進(jìn)而滿足路徑規(guī)劃算法實(shí)時(shí)性的要求。

(2)考慮到路徑點(diǎn)稀疏、不平滑的特征,為更好地控制機(jī)器人,如何構(gòu)造狀態(tài)空間中的一條軌跡(不考慮障礙物)連接任意給定的兩個(gè)狀態(tài),從而將稀疏的路徑點(diǎn)變成平滑的曲線或稠密的軌跡點(diǎn)。

針對(duì)當(dāng)前研究現(xiàn)狀中存在的問題,提出一種融合改進(jìn)跳點(diǎn)搜索算法和微分平坦法的全局路徑規(guī)劃算法,主要思路如下:

(1)基于跳點(diǎn)搜索的算法,設(shè)計(jì)一種更加符合智能駕駛規(guī)劃的啟發(fā)式函數(shù)。引入cross算子,實(shí)現(xiàn)啟發(fā)式函數(shù)的動(dòng)態(tài)調(diào)整,保證全局最短路徑的情況下,擴(kuò)展節(jié)點(diǎn)有限,用時(shí)較短。

(2)使用微分平坦法對(duì)改進(jìn)跳點(diǎn)搜索算法生成的路徑點(diǎn)進(jìn)行優(yōu)化,從而生成更適合底層控制的平滑軌跡。

1 算法設(shè)計(jì)

1.1 跳點(diǎn)搜索算法

跳點(diǎn)搜索算法整體的算法框架與A*搜索算法相同,其中跳點(diǎn)是指在全局搜索過程中可以直接跳躍的點(diǎn)。相比于A*在搜索過程中需要對(duì)路徑上每個(gè)節(jié)點(diǎn)的估價(jià)數(shù)值進(jìn)行比較,跳點(diǎn)搜索算法引入一套完整的擴(kuò)展結(jié)點(diǎn)策略。JPS算法通過尋找跳點(diǎn)的方式,排除了大量無關(guān)的點(diǎn),減少了openlist中搜索點(diǎn)的數(shù)量,從而提升搜索速度。整個(gè)尋找跳點(diǎn)的過程可以做如下解釋。

1.1.1 概念描述

強(qiáng)迫鄰居:如果節(jié)點(diǎn)n是x的鄰居,并且節(jié)點(diǎn)n的鄰居有阻擋(不可行走的格子),并且從parent(x)、x、n的路徑長(zhǎng)度比其他任何從parent(x)到n且不經(jīng)過x的路徑短,則n為x的強(qiáng)迫鄰居,x為n的跳點(diǎn),如圖1(a)左圖中,5號(hào)節(jié)點(diǎn)是x的強(qiáng)迫鄰居，在圖1(a)左圖中,1號(hào)節(jié)點(diǎn)是x的強(qiáng)迫鄰居。

跳點(diǎn):如果點(diǎn)x是起點(diǎn)或目標(biāo)點(diǎn),則x是跳點(diǎn)；如果x有強(qiáng)迫鄰居則x是跳點(diǎn)；如果parent(x)到x是水平或者垂直移動(dòng),并且x經(jīng)過水平或垂直方向移動(dòng)可以到達(dá)跳點(diǎn),則x是跳點(diǎn),例如圖1(b)左圖中,x是跳點(diǎn)；如果parent(x)到x是對(duì)角線移動(dòng),并且x經(jīng)過水平或垂直方向移動(dòng)可以到達(dá)跳點(diǎn),則x是跳點(diǎn),如圖1(b)右圖中,x是跳點(diǎn)。

1.1.2 在執(zhí)行算法的過程中,JPS的跳躍規(guī)則

(1)JPS搜索跳點(diǎn)的過程中,如果直線方向、對(duì)角線方向都可以移動(dòng),則首先在直線方向搜索跳點(diǎn),然后再在對(duì)角線方向搜索跳點(diǎn)。

(2)只有跳點(diǎn)才會(huì)加入openset,因?yàn)樘c(diǎn)會(huì)改變行走方向,而非跳點(diǎn)不會(huì)改變行走方向,最后尋找出來的路徑點(diǎn)也都是跳點(diǎn)。

JPS算法使用的啟發(fā)式函數(shù)與A*搜索算法相同,仍然是用傳統(tǒng)的歐氏距離或者曼哈頓距離做距離表達(dá)，即

(1)

hManhattan=dx+dy

(2)

式中：dx=|node.x-goal.x|；dy=|node.y-goal.y|。

做全局路徑規(guī)劃的時(shí)候,使用柵格地圖,默認(rèn)智能機(jī)器人可以8個(gè)方向前進(jìn)。面向這樣結(jié)構(gòu)化的設(shè)定,如果使用曼哈頓距離,算法無法保證最終輸出最優(yōu)路徑；使用歐氏距離,算法會(huì)拓展過多的無用節(jié)點(diǎn),只有在當(dāng)前節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的距離數(shù)值正好等于從當(dāng)前節(jié)點(diǎn)移動(dòng)到目標(biāo)代價(jià)的時(shí)候,算法效果最好[11]。依據(jù)這樣的思想,使用對(duì)角線距離表示兩點(diǎn)之間的距離來改進(jìn)啟發(fā)式函數(shù)的性能。

1.2 啟發(fā)函數(shù)的改進(jìn)

啟發(fā)式函數(shù)h(n)估計(jì)當(dāng)前點(diǎn)n到終點(diǎn)的代價(jià)。不同的啟發(fā)式函數(shù)的選擇對(duì)應(yīng)著不同算法的運(yùn)行效率。在啟發(fā)函數(shù)數(shù)值為0的時(shí)候,整個(gè)算法退化為迪杰斯特拉算法,能夠?qū)さ米顑?yōu)路徑,但是擴(kuò)展的節(jié)點(diǎn)特別多；當(dāng)啟發(fā)函數(shù)數(shù)值非常大時(shí),算法無法保證最終的路徑是最短路徑,這違背了全局路徑算法的目的。想要保證最終的搜索算法既能尋得最短路徑,又能在保證尋得最短路徑的前提下擴(kuò)展的節(jié)點(diǎn)有限,這需要選擇最合適的啟發(fā)式函數(shù)。

對(duì)啟發(fā)式函數(shù)做改進(jìn)的主要目的是尋找最短路徑。如圖2所示,其中Sstart表示搜索算法的起點(diǎn),Sgoal表示搜索算法的終點(diǎn),使用傳統(tǒng)的擴(kuò)展節(jié)點(diǎn)的方法,會(huì)生成很多無用的擴(kuò)展節(jié)點(diǎn)。

圖2 基于搜索的算法在執(zhí)行過程中的擴(kuò)展點(diǎn)圖示Fig.2 The extension point of a search-based algorithm

針對(duì)JPS算法采用的傳統(tǒng)啟發(fā)式函數(shù)表達(dá),做如下改進(jìn)。

(1)使用對(duì)角線距離來表達(dá)當(dāng)前點(diǎn)與終點(diǎn)之間的距離，即

(3)

式(3)中：dx與dy的定義與式(2)中保持一致。diag(node)表示當(dāng)前節(jié)點(diǎn)node的對(duì)角線距離。具體可視化如圖3所示。

圖3 3種距離描述方法在二維空間中的圖示Fig.3 The representation of three distance description methods in two-dimensional space

(2)如果當(dāng)前節(jié)點(diǎn)代價(jià)函數(shù)f對(duì)應(yīng)著多條路徑時(shí),這些路徑都會(huì)被搜索,但其實(shí)只需要其中的一條路徑即可。這種情況在障礙物較少的地圖中出現(xiàn)非常頻繁。為解決該問題,在啟發(fā)式函數(shù)中添加附加值,大小為初始點(diǎn)指向目標(biāo)點(diǎn)向量和當(dāng)前點(diǎn)指向目標(biāo)向量的向量叉積,進(jìn)而改變啟發(fā)式函數(shù)h的值,使之在選擇路徑上更傾向從初始點(diǎn)到目標(biāo)點(diǎn)的連線。具體描述為

cross=Vc_e×Vs_e

(4)

式(4)中：cross表示初始點(diǎn)指向目標(biāo)點(diǎn)向量和當(dāng)前點(diǎn)指向目標(biāo)向量的向量叉積；向量下標(biāo)c表示當(dāng)前節(jié)點(diǎn)current,e表示目標(biāo)點(diǎn)end,s表示起始點(diǎn)start。

(3)考慮到實(shí)際運(yùn)行情況,路徑搜索算法在開始搜索時(shí),最重要的是迅速移動(dòng)到任意位置；而在搜索接近結(jié)束時(shí),最重要的是移動(dòng)到目標(biāo)點(diǎn)。所以在啟發(fā)式函數(shù)中設(shè)計(jì)權(quán)重w(w≥1),當(dāng)接近目標(biāo)時(shí),降低這個(gè)權(quán)值,從而降低啟發(fā)函數(shù)的重要性,同時(shí)增加了路徑真實(shí)代價(jià)的相對(duì)重要性。

綜上3點(diǎn)改進(jìn),可以將h函數(shù)表示為

(5)

使用改進(jìn)啟發(fā)式函數(shù)的JPS算法,稱之為帶權(quán)重的跳點(diǎn)搜索(weighted jump point search, WJPS)算法。

1.3 軌跡優(yōu)化

WJPS算法輸出的是面向柵格地圖的全局最優(yōu)路徑點(diǎn)。考慮到路徑點(diǎn)稀疏、不平滑的特征,為更好地控制機(jī)器人,需要構(gòu)造狀態(tài)空間中的一條軌跡(不考慮障礙物)連接任意給定的兩個(gè)狀態(tài),從而將稀疏的路徑點(diǎn)變成平滑的曲線或稠密的軌跡點(diǎn),再傳輸給底層驅(qū)動(dòng)做控制。

采用微分平坦方法[12],將軌跡表示為n階多項(xiàng)式。使用多項(xiàng)式來擬合最終的軌跡有如下優(yōu)點(diǎn):①因?yàn)槎囗?xiàng)式方程能夠多階求導(dǎo),所以可以確保整條軌跡滿足光滑性質(zhì)；②計(jì)算多項(xiàng)式參數(shù)的時(shí)候比較方便；③面向多維空間,多項(xiàng)式方程可以分離開,分別求解每個(gè)維度多項(xiàng)式方程的對(duì)應(yīng)參數(shù)。

首先將軌跡建模為多項(xiàng)式方程模型,具體為

(6)

式(6)中：p為多項(xiàng)式軌跡參數(shù),可以將之設(shè)成參數(shù)向量:

p=[p0,p1,…,pn]T

(7)

進(jìn)一步將上述軌跡表述寫成向量形式:

p(t)=[1,t,t2,…,tn]p

(8)

接下來,軌跡需要根據(jù)具體情況判斷使用的多項(xiàng)式的次數(shù)。在微分平坦空間中,有多項(xiàng)式次數(shù)與機(jī)器人的平移、旋轉(zhuǎn)與推力的一一對(duì)應(yīng)關(guān)系,具體如表1所示。

表1 多項(xiàng)式次數(shù)對(duì)應(yīng)的物理意義Table 1 The physical meaning of the degree of polynomial

根據(jù)表1,在通常軌跡優(yōu)化手段中,有如下兩種最小化原則:

(1)minimum jerk,本質(zhì)上是在最小化角加速度。通常無人機(jī)在初始化的時(shí)候,需要緩慢移動(dòng)攝像頭,使得視差的變化最小,所以這種方法通常在無人機(jī)上為了實(shí)現(xiàn)更好的視覺跟蹤而被采用。

(2)minimum snap,這種方法實(shí)際是在最小化推力的導(dǎo)數(shù),也就是說要使得推力的變化盡可能平緩,從而可以達(dá)到節(jié)省能耗的目的。

根據(jù)上述公式,對(duì)于任意時(shí)刻t,可以根據(jù)參數(shù)計(jì)算出軌跡的位置、速度、加速度、jerk和snap,對(duì)應(yīng)的方程為

v(t)=p′(t)=[0,1,2t,3t2,…,ntn-1]p

(9)

a(t)=p″(t)=[0,0,2,6,12t2,…,n(n-1)tn-2]p

(10)

(11)

(12)

因?yàn)闊o法由一個(gè)多項(xiàng)式將整條運(yùn)動(dòng)軌跡表述,這部分將移動(dòng)速度固定,從而根據(jù)每段運(yùn)動(dòng)軌跡的長(zhǎng)度,將時(shí)間分成多段,每一段用一條多項(xiàng)式表示為

(13)

式(13)中：k為軌跡的段數(shù)。那么第i段軌跡的參數(shù)向量就可以表示為

pi=[pi0,pi1,…,pin]T

(14)

接下來公式表述主要以x軸為例,其他坐標(biāo)方向以此類推。至此,已經(jīng)完成了全部軌跡建模的工作。出于效率與軌跡光滑的綜合考量,采用三次樣條插值的方法,來對(duì)前文生成的路徑點(diǎn)進(jìn)行平滑處理,最終求得每一段軌跡的系數(shù),進(jìn)而使得整體軌跡滿足起始點(diǎn)、終止點(diǎn)、中間點(diǎn)的約束以及整條軌跡的光滑連續(xù)約束。

如圖4所示,x、y分別表示地圖的兩個(gè)垂直方向。藍(lán)色軌跡是直接連接離散的路徑點(diǎn)的結(jié)果，紅色曲線是對(duì)綠色離散點(diǎn)做曲線擬合后生成的軌跡。

圖4 軌跡優(yōu)化后的結(jié)果Fig.4 Trajectory optimization results

2 實(shí)驗(yàn)驗(yàn)證

實(shí)驗(yàn)驗(yàn)證分為3部分。第1部分面向JPS算法改進(jìn)的WJPS算法,在選取豐富復(fù)雜的地圖后,橫向?qū)Ρ華*、JPS和WJPS 3種算法,分別從運(yùn)行時(shí)間、路徑長(zhǎng)度以及擴(kuò)展節(jié)點(diǎn)數(shù)3方面做考量,定量評(píng)估3種方法的優(yōu)缺點(diǎn)。第2部分是針對(duì)第1部分WJPS算法輸出的路徑點(diǎn),做可視化的軌跡優(yōu)化展示,并就實(shí)驗(yàn)結(jié)果做具體分析。所有的實(shí)驗(yàn),均在python3.7環(huán)境下實(shí)現(xiàn)。

2.1 WJPS算法實(shí)驗(yàn)結(jié)果

根據(jù)文獻(xiàn)[13]專門設(shè)計(jì)了9張地圖,包括復(fù)雜巷道、復(fù)雜室內(nèi)場(chǎng)景、復(fù)雜停車場(chǎng)、復(fù)雜商業(yè)區(qū),以及迷宮場(chǎng)景,力求覆蓋盡可能多的非結(jié)構(gòu)化地圖場(chǎng)景。然后在每張地圖上分別應(yīng)用3種算法進(jìn)行路徑規(guī)劃結(jié)果,可視化結(jié)果如圖5～圖13所示,其中x、y分別表示地圖的兩個(gè)垂直方向。

圖5 地圖1(巷道)中3種算法的可視化對(duì)比Fig.5 Comparison of the three algorithms in map 1 (roadway)

圖6 地圖2(復(fù)雜巷道)中3種算法的可視化對(duì)比Fig.6 Comparison of the three algorithms in map 2 (complex roadway)

圖7 地圖3(迷宮)中3種算法的可視化對(duì)比Fig.7 Comparison of the three algorithms in map 3 (labyrinth)

圖8 地圖4(復(fù)雜迷宮)中3種算法的可視化對(duì)比Fig.8 Comparison of the three algorithms in map 4 (complex labyrinth)

圖9 地圖5(商場(chǎng))中3種算法的可視化對(duì)比Fig.9 Comparison of the three algorithms in map 5 (shopping)

圖10 地圖6(復(fù)雜商場(chǎng))中3種算法的可視化對(duì)比Fig.10 Comparison of the three algorithms in map 6 (complex shopping)

圖11 地圖7(復(fù)雜迷宮)中3種算法的可視化對(duì)比Fig.11 Comparison of the three algorithms in map 7(complex labyrinth)

圖12 地圖8(復(fù)雜停車場(chǎng))中3種算法的可視化對(duì)比Fig.12 Comparison of the three algorithms in map 8 (complex park)

圖13 地圖9(復(fù)雜居民區(qū))中3種算法的可視化對(duì)比Fig.13 Comparison of the three algorithms in map 9 (complex residential area)

9張地圖中,除了地圖1巷道和地圖5商場(chǎng)外,其余都是非常復(fù)雜的環(huán)境,包括迷宮、居民區(qū)等。觀察上述路徑規(guī)劃結(jié)果,可以做如下分析:在最終路徑長(zhǎng)度上,WJPS算法無論在復(fù)雜環(huán)境還是簡(jiǎn)單環(huán)境,都是路徑最短的算法,而JPS算法的啟發(fā)式函數(shù)設(shè)計(jì)簡(jiǎn)單,導(dǎo)致尋路效果過于激進(jìn),在極其復(fù)雜的場(chǎng)景，如地圖4、地圖8、地圖9中甚至出現(xiàn)打彎現(xiàn)象,這是全局路徑規(guī)劃一定要避免的情況。對(duì)于全局路徑規(guī)劃來說,一條全局最短的路徑是最重要的,因?yàn)楹罄m(xù)的工作都要基于這個(gè)全局路徑來進(jìn)行。3種方法在多個(gè)地圖運(yùn)行10次的平均數(shù)據(jù)如表2所示。

在表2最重要的路徑長(zhǎng)度項(xiàng)中,可以清晰得知,WJPS算法無論在簡(jiǎn)單環(huán)境還是在復(fù)雜環(huán)境中都能保證生成路徑是最短的。

表2 3種方法在多個(gè)地圖運(yùn)行10次的平均數(shù)據(jù)Table 2 Three method’s average data on multimaps 10 times

分析表2中的尋路時(shí)間可以得知, 3種算法在尋路時(shí)間上都是毫秒級(jí)別,其中JPS由于擴(kuò)展節(jié)點(diǎn)少,在尋路時(shí)間上往往更快速,但在復(fù)雜環(huán)境中并未尋得最短路徑；A*算法由于其算法本身擴(kuò)展過多無用節(jié)點(diǎn)的原因,通常是最慢的一種。WJPS算法因?yàn)槔肑PS跳點(diǎn)算法中尋找拓展點(diǎn)的策略,同樣能夠?qū)崿F(xiàn)毫秒級(jí)別的規(guī)劃,算法效率能夠滿足智能體對(duì)路徑規(guī)劃層的要求。

所以本文的結(jié)論是:如果場(chǎng)景足夠簡(jiǎn)單,機(jī)器人系統(tǒng)對(duì)運(yùn)算速度有特別強(qiáng)的需求的時(shí)候,JPS算法是一種比較好的選擇；但是當(dāng)場(chǎng)景比較復(fù)雜,WJPS算法能夠?qū)さ米疃搪窂健？紤]到后續(xù)的軌跡優(yōu)化以及控制層執(zhí)行軌跡的時(shí)候都要依賴全局路徑規(guī)劃的結(jié)果,在時(shí)間都是毫秒級(jí)別的情況下,全局路徑規(guī)劃最重要的指標(biāo)是輸出一條全局最短的軌跡點(diǎn),所以認(rèn)為WJPS算法更適合非結(jié)構(gòu)化復(fù)雜場(chǎng)景。

2.2 路徑擬合實(shí)驗(yàn)結(jié)果

面向WJPS算法生成的路徑點(diǎn)做曲線擬合,生成一條光滑的軌跡。具體曲線擬合的效果如圖14所示,每一個(gè)小圖表示對(duì)應(yīng)離散地圖點(diǎn)做擬合后的結(jié)果，其中紅色曲線為軌跡優(yōu)化的結(jié)果,綠色節(jié)點(diǎn)為WJPS算法生成的路徑點(diǎn),藍(lán)色線段為WJPS算法生成的折線路徑。

圖14 任意6張地圖中對(duì)生成路徑點(diǎn)做優(yōu)化的結(jié)果Fig.14 The results of optimizing the generated path points in any 6 maps

對(duì)軌跡優(yōu)化算法進(jìn)行實(shí)驗(yàn)驗(yàn)證。使用的三次樣條曲線對(duì)WJPS算法輸出的路徑點(diǎn)做擬合,最終將稀疏的路徑點(diǎn)連接成為平滑的曲線或稠密的軌跡點(diǎn),更適合機(jī)器人底層做具體控制。

3 結(jié)論

針對(duì)非結(jié)構(gòu)化復(fù)雜場(chǎng)景下的路徑規(guī)劃問題,提出了一種改進(jìn)路徑規(guī)劃算法WJPS算法,并對(duì)其生成的路徑點(diǎn)做軌跡優(yōu)化。經(jīng)過詳細(xì)的對(duì)比實(shí)驗(yàn)得到以下結(jié)論：

(1)與傳統(tǒng)A*算法以及基于A*改進(jìn)的JPS算法相比,WJPS算法可以在復(fù)雜的非結(jié)構(gòu)化的環(huán)境中找到長(zhǎng)度最短的路徑。

(2)WJPS算法的算法執(zhí)行時(shí)間是毫秒級(jí)別,完全滿足工業(yè)中對(duì)路徑規(guī)劃層的實(shí)時(shí)性要求。

(3)使用微分平坦法對(duì)路徑點(diǎn)做擬合,將稀疏的路徑點(diǎn)連接成為平滑的曲線或稠密的軌跡點(diǎn),保證了最終路徑連續(xù)平滑。