基于BD-RRT算法的動態(tài)路徑規(guī)劃研究*

左 宇,顧寄南,王文波,范天浩,盧寶勇,侯征輝

(江蘇大學(xué)機(jī)械工程學(xué)院,鎮(zhèn)江 212013)

0 引言

隨著自動化和機(jī)器人技術(shù)的發(fā)展[1],機(jī)器人在人們?nèi)粘Ｉ钪邪缪葜絹碓街匾慕巧?機(jī)器人的種類非常豐富,例如機(jī)械臂、移動機(jī)器人和服務(wù)機(jī)器人等。有了機(jī)器人的幫助,人們可以輕易完成從前無法單獨(dú)完成的任務(wù),顯著減少了人力資源的浪費(fèi)。機(jī)器人路徑規(guī)劃的優(yōu)劣決定了其工作的穩(wěn)定性和智能性[2],因此如何合理有效解決機(jī)器人的路徑規(guī)劃是當(dāng)前科學(xué)界致力解決的關(guān)鍵問題。

路徑規(guī)劃指的是機(jī)器人在連續(xù)空間中以最優(yōu)或者逼近最優(yōu)的路徑和時間為目標(biāo)進(jìn)行計(jì)算,輸出從起始點(diǎn)到目標(biāo)點(diǎn)無碰撞路徑的過程。在過去的幾十年,眾多學(xué)者設(shè)計(jì)改進(jìn)出了非常多的路徑規(guī)劃算法,目前最常用的主要分為以下3類:圖規(guī)劃算法、仿生智能算法和空間采樣算法。其中基于圖規(guī)劃的算法主要有A*、APF和D*等[3];基于仿生智能的算法主要有遺傳算法、粒子算法和蟻群算法等[4];基于空間采樣的算法主要有快速擴(kuò)展隨機(jī)樹(rapidly-exploring random trees,RRT)、RRT*、DWA等。基于采樣的RRT類算法可以在高維空間較好的完成規(guī)劃任務(wù),適合用于多自由度機(jī)器人在環(huán)境復(fù)雜空間的路徑規(guī)劃問題[5]。RRT算法在全局采取了隨機(jī)擴(kuò)展方式,通過固定步長生長擴(kuò)展隨機(jī)樹,在生成到達(dá)目標(biāo)點(diǎn)的無碰撞路徑時結(jié)束[6],該方法提高了機(jī)器人在高維空間的運(yùn)動能力,但存在路徑規(guī)劃時間過長,計(jì)算復(fù)雜度高,全局環(huán)境冗余等問題。為解決上述問題,LAVALLE等[7]改進(jìn)得到了Bi-RRT(bidirectional-RRT)算法,它通過雙向生長擴(kuò)展隨機(jī)樹并連接兩棵樹來獲得最優(yōu)路徑,當(dāng)端點(diǎn)位于非常狹窄的環(huán)境中時,該算法的效率會更加明顯,然而該算法仍然存在一些缺點(diǎn),如規(guī)劃路徑不平滑和無法對環(huán)境中新出現(xiàn)障礙物進(jìn)行重規(guī)劃避障等問題。

為了克服以上問題,本文提出了一種基于BD-RRT(bidirectional-dynamic-RRT)的動態(tài)路徑規(guī)劃算法,采用動態(tài)步長增長策略并結(jié)合動態(tài)RRT算法和B-spline軌跡優(yōu)化實(shí)現(xiàn)平滑的動態(tài)避障。不僅有效解決了傳統(tǒng)RRT算法規(guī)劃路徑時間長和路徑冗余曲折的問題,還為動態(tài)障礙場景中的避障問題提供了可行的解決方案,這對現(xiàn)實(shí)復(fù)雜工作環(huán)境中的機(jī)器人動態(tài)避障有著非常重要的參考價值。

1 基于BD-RRT算法的動態(tài)路徑規(guī)劃

1.1 RRT及Bi-RRT算法的基本原理

傳統(tǒng)RRT算法是一種基于擴(kuò)展隨機(jī)樹的采樣算法,其基本思想是從起點(diǎn)開始,隨機(jī)采樣生成一些節(jié)點(diǎn),然后通過把這些節(jié)點(diǎn)連接成樹狀結(jié)構(gòu)來搜索可行的路徑,示意圖如圖1所示。算法具體過程為:

圖1 傳統(tǒng)RRT算法示意圖

步驟1:確定擴(kuò)展隨機(jī)樹的生長空間,將機(jī)器人的起點(diǎn)Xinit和目標(biāo)點(diǎn)Xgoal添加至隨機(jī)空間;

步驟2:初始化根節(jié)點(diǎn)Xinit為擴(kuò)展隨機(jī)樹的起點(diǎn);

步驟3:在空間中生成一個隨機(jī)節(jié)點(diǎn)Xrand,遍歷現(xiàn)有節(jié)點(diǎn),計(jì)算每個節(jié)點(diǎn)到Xrand距離并選擇距離最近的樹節(jié)點(diǎn)作為Xnearest;

步驟4:沿Xnearest向Xrand的方向以固定步長增量step生長一個新節(jié)點(diǎn)Xnew;

步驟5:根據(jù)運(yùn)動學(xué)模型和環(huán)境約束,計(jì)算從節(jié)點(diǎn)Xnearest到達(dá)節(jié)點(diǎn)Xnew的路徑,并檢測該路徑是否與障礙物相交;

步驟6:如果路徑未發(fā)生碰撞,則將節(jié)點(diǎn)Xnew添加到樹結(jié)構(gòu)中,并將其與最近的樹節(jié)點(diǎn)Xnearest連接,如果發(fā)生碰撞則重新生長節(jié)點(diǎn)Xnew;

步驟7:重復(fù)上述新節(jié)點(diǎn)生成流程,直到生成的新節(jié)點(diǎn)Xnew接近目標(biāo)節(jié)點(diǎn)Xgoal;

步驟8:最后依次搜索并連接樹結(jié)構(gòu)中從起點(diǎn)到目標(biāo)節(jié)點(diǎn)的路徑。

Bi-RRT算法是在原始RRT的基礎(chǔ)上多加了一顆擴(kuò)展隨機(jī)樹,各自從起點(diǎn)和終點(diǎn)向外探索拓展,直到兩棵樹相遇時算法收斂[8]。Bi-RRT算法中的這兩顆隨機(jī)樹我們將其命名為a和b,a樹以Root_a為起點(diǎn)根節(jié)點(diǎn),b樹以Root_b為終點(diǎn)根節(jié)點(diǎn)。兩顆隨機(jī)樹的拓展方式和傳統(tǒng)RRT相同,都需要隨機(jī)采樣—步長限制—碰撞檢測這3個步驟,但不同的是Bi-RRT的擴(kuò)展隨機(jī)樹是交替生長的,若第一輪迭代中a樹生長,第二輪便切換為b樹生長,循環(huán)往復(fù)。在每輪迭代中,擴(kuò)展隨機(jī)樹除了向外生長外,還會遍歷另一顆擴(kuò)展隨機(jī)樹中的所有節(jié)點(diǎn),找出離New Node最近的節(jié)點(diǎn),用于判斷兩顆擴(kuò)展隨機(jī)樹是否相遇。

算法示意如圖2所示,在進(jìn)入某次迭代時以a樹拓展,算法成功拓展出a樹的新節(jié)點(diǎn)New Node_a,此時算法將遍歷b樹中的所有節(jié)點(diǎn),找出b樹中離New Node_a最近的節(jié)點(diǎn)Closest Node_b,接著判斷兩點(diǎn)是否滿足步長限制以及是否可以通過碰撞檢測,圖中橙色虛線為這兩節(jié)點(diǎn)的連線,明顯無法通過碰撞檢測,因此a樹和b樹在這一輪迭代中無法相遇。進(jìn)入下一輪迭代,切換為b樹拓展,此時算法拓展出新節(jié)點(diǎn)New Node_b,遍歷a樹后將上一輪的New Node_a作為Closest Node_a,判斷發(fā)現(xiàn)兩點(diǎn)滿足步長要求,圖中橙色實(shí)線為這兩節(jié)點(diǎn)的連線,明顯通過碰撞檢測,那么這時a樹和b樹判定為成功相遇,最后只需在兩棵樹的相遇處分別沿著父節(jié)點(diǎn)回溯便可以找出從起點(diǎn)到終點(diǎn)的有效路徑。這種改進(jìn)過的探索策略可以大幅度提高RRT的運(yùn)行效率。

1.2 BD-RRT算法的主要設(shè)計(jì)思路

Bi-RRT與傳統(tǒng)RRT相比提高了算法的收斂速度,解決了在復(fù)雜環(huán)境下路徑規(guī)劃較慢,全局環(huán)境冗余等問題,但仍有需要改進(jìn)的缺陷:①Bi-RRT的改進(jìn)是基于增加擴(kuò)展隨機(jī)樹和添加貪心策略來實(shí)現(xiàn)的,并不能在障礙物環(huán)境發(fā)生變化時進(jìn)行自動重規(guī)劃路徑;②Bi-RRT算法與傳統(tǒng)RRT一樣是通過固定步長來生長擴(kuò)展隨機(jī)樹的,當(dāng)設(shè)置的步長過大時會增加隨機(jī)點(diǎn)被報廢的可能性,而當(dāng)過小時則會影響擴(kuò)展隨機(jī)樹的搜索效率;③Bi-RRT算法規(guī)劃得到的路徑不平滑,在實(shí)際工作中這種規(guī)劃方式容易對機(jī)器人舵機(jī)造成損壞且運(yùn)行效率較低。面對上述問題,本文提出BD-RRT(bidirectional-dynamic-RRT)算法,通過引入動態(tài)RRT算法、動態(tài)步長增長策略[9]、B樣條曲線優(yōu)化[10]等方法對Bi-RRT算法進(jìn)行改進(jìn)。

1.3 動態(tài)RRT算法

動態(tài)RRT算法與傳統(tǒng)RRT算法有一個非常大的不同,那就是不完全放棄初始地圖中生成的擴(kuò)展隨機(jī)樹的信息,而是在此基礎(chǔ)上重新規(guī)劃。

在初始地圖中先用RRT算法生成一個擴(kuò)展隨機(jī)樹,將該樹的節(jié)點(diǎn)信息(坐標(biāo)、父節(jié)點(diǎn))存儲在一個節(jié)點(diǎn)集合中。地圖發(fā)生改變之后,先檢測新地圖中比初始地圖多出的障礙物,然后,以碰撞檢測為判斷依據(jù),刪除老節(jié)點(diǎn)集合中與新障礙物發(fā)生碰撞無法通過檢測的邊和節(jié)點(diǎn)。最終得到一顆修建過后的與新地圖無碰撞的擴(kuò)展隨機(jī)樹,再在這顆修剪后的擴(kuò)展隨機(jī)樹的基礎(chǔ)上,繼續(xù)生長這棵樹[11],直到這棵樹連接起點(diǎn)和終點(diǎn),然后回溯路徑,得出新的路徑。

動態(tài)RRT算法的具體步驟如偽代碼表1所示。

表1 動態(tài)RRT算法偽代碼

首先,從初始地圖的RRT樹中復(fù)制節(jié)點(diǎn)信息,并使用障礙檢測函數(shù)check_collision和刪除節(jié)點(diǎn)函數(shù)remove_node來刪除與新地圖中新障礙物發(fā)生碰撞的節(jié)點(diǎn)。

然后,使用修剪后的擴(kuò)展隨機(jī)樹作為起點(diǎn)來繼續(xù)生長。在每一次迭代中,先采樣一個新的隨機(jī)節(jié)點(diǎn)Xrand,找到最近的節(jié)點(diǎn)Xnearest,并通過steer函數(shù)從Xnearest到Xrand進(jìn)行擴(kuò)展。

最后,檢查擴(kuò)展路徑是否與當(dāng)前環(huán)境障礙物存在碰撞,并將新節(jié)點(diǎn)添加到更新后的樹中。如果新節(jié)點(diǎn)接近目標(biāo)節(jié)點(diǎn)Xgoal,則將其添加到樹中并返回路徑。否則,我們根據(jù)新的地形重新計(jì)算障礙物位置,并刪除與新障礙物發(fā)生碰撞的所有節(jié)點(diǎn)。如果沒有找到可連接起點(diǎn)和終點(diǎn)的路徑,則返回None。

1.4 動態(tài)步長增長策略

現(xiàn)實(shí)環(huán)境一般比較復(fù)雜,障礙物的分布隨機(jī)性太大,通過實(shí)驗(yàn)發(fā)現(xiàn)增大步長可以加快算法在障礙物分布稀疏的地方的收斂速度,但是在障礙物分布密度大的地方增大步長反而會令算法忽略部分可行路徑[12]。

針對這種現(xiàn)象,本文提出一種動態(tài)步長增長策略,具體步驟為:

步驟1:將初始步長設(shè)置為一個較大的值step_init;

步驟2:在擴(kuò)展一個新節(jié)點(diǎn)時,計(jì)算新節(jié)點(diǎn)周圍障礙物的分布密度τ?？梢酝ㄟ^隨機(jī)散布點(diǎn)來計(jì)算障礙物分布密度。將散點(diǎn)范圍設(shè)置為以新節(jié)點(diǎn)為圓心,初始步長為半徑的圓。隨機(jī)點(diǎn)總數(shù)定為n,落在障礙物中的點(diǎn)為b,則障礙物分布密度可以計(jì)算為:

(1)

步驟3:根據(jù)障礙物分布密度τ和閾值ω來決定動態(tài)增長的步長,即:

(2)

步驟4:使用BD-RRT算法來選擇新節(jié)點(diǎn)的父節(jié)點(diǎn),并連接新節(jié)點(diǎn)與父節(jié)點(diǎn);

步驟5:重復(fù)上述步驟,直到找到有效路徑。

策略中閾值ω的選擇可以根據(jù)具體問題和應(yīng)用場景進(jìn)行調(diào)整。一般來說,需要考慮以下因素:

(1)障礙物的分布情況:如果環(huán)境中障礙物比較密集,那么閾值可以設(shè)置得低一些,這樣算法會在障礙物密集區(qū)域使用更小的步長搜索可行路徑。相反的,如果障礙物分布較稀疏,那么閾值可以設(shè)置得稍高一些,以加快搜索速度。

(2)搜索效率和搜索質(zhì)量的平衡:如果希望搜索速度盡可能快,可以將閾值設(shè)置得較高;如果更關(guān)注搜索質(zhì)量而可以接受稍慢的搜索速度,可以將閾值設(shè)置得較低。

(3)算法的性能要求:如果算法對搜索效率的要求比較高,適當(dāng)提高閾值以減少計(jì)算時間;若算法對搜索質(zhì)量的要求比較高,則應(yīng)該選擇較低的閾值以獲得更好的搜索結(jié)果。

1.5 B-spline軌跡優(yōu)化

Bi-RRT算法在生成最終路徑時是將相關(guān)的節(jié)點(diǎn)直接進(jìn)行簡單連接,生成的路徑曲折變化很大,未考慮實(shí)際工作場景下機(jī)器人運(yùn)動時出現(xiàn)的運(yùn)動學(xué)問題,有可能會干擾機(jī)器人的穩(wěn)定運(yùn)動,因此需要對生成的折線進(jìn)行平滑處理。針對這一問題,對BD-RRT算法采取B樣條方法對最后的軌跡點(diǎn)進(jìn)行曲線擬合優(yōu)化,B樣條是一種普適化的貝塞爾曲線插值和逼近方法,可以用于優(yōu)化機(jī)器人的運(yùn)動路徑[13]。

J次的B樣條插值函數(shù)的表達(dá)式為:

(3)

式中:di,i=0,1,2,3…代表控制點(diǎn),Ni,j(u)代表J次的B樣條插值函數(shù)的基函數(shù),也叫J次規(guī)范B樣條基函數(shù)[14]。下式為其Cox-de Boor遞推公式:

(4)

(5)

基函數(shù)滿足微分方程為:

(6)

考慮到機(jī)器人工作在復(fù)雜實(shí)際環(huán)境下障礙物干擾較多,本文選用三次B樣條插值函數(shù)對路徑進(jìn)行平滑處理。

首先,通過BD-RRT算法生成可到達(dá)目標(biāo)點(diǎn)的節(jié)點(diǎn),取兩節(jié)點(diǎn)間的4個點(diǎn)作為控制點(diǎn)代入式(7)和式(8)求解3次B樣條的基函數(shù),再代入式(6)可以得到3次的B樣條插值函數(shù)并生成經(jīng)過優(yōu)化后的平滑路徑。將生成的平滑路徑作為新的起點(diǎn)和終點(diǎn),利用BD-RRT算法在新的起點(diǎn)和終點(diǎn)之間生成一條路徑;最后,重復(fù)上述步驟,直到最終生成的路徑到達(dá)目標(biāo)點(diǎn)為止。

具體的優(yōu)化效果如圖3所示,其中粉色線段為初始路徑,紅色線段為B-spline軌跡優(yōu)化后生成的路徑,通過兩線段對比可以看出軌跡優(yōu)化效果明顯。

1.6 BD-RRT算法

BD-RRT算法相較于Bi-RRT算法做出了4個改進(jìn),算法流程如圖4所示,增加了動態(tài)步長策略計(jì)算步長;增加了障礙物動態(tài)出現(xiàn)場景下的重規(guī)劃避障能力;重規(guī)劃避障時對碰撞路徑進(jìn)行剪枝[15],并以原有無碰撞路徑為基礎(chǔ)重規(guī)劃,提高了動態(tài)避障效率;對生成的路徑進(jìn)行B-spline優(yōu)化。

圖4 BD-RRT算法流程圖

具體步驟如算法偽代碼表2所示,各函數(shù)功能為:

ClearCollisionNode:該函數(shù)遍歷舊地圖中所有節(jié)點(diǎn)和邊的信息Gold,如果節(jié)點(diǎn)與新地圖中的障礙物發(fā)生了碰撞,或者邊的兩個端點(diǎn)有任何一個與障礙物相交則判定為無效,最后因清空與障礙物相交而無效的點(diǎn)和邊。

ReachableNodesAndEdges:該函數(shù)分別從Xinit和Xgoal節(jié)點(diǎn)出發(fā),搜索可以到達(dá)Gnew的有效節(jié)點(diǎn)和邊,存放到Ggoal和Ginit的列表中。

RandomNode:此函數(shù)用來生成在區(qū)域范圍C內(nèi)一個隨機(jī)節(jié)點(diǎn)。如果隨機(jī)數(shù)大于目標(biāo)采樣率,就返回一個在區(qū)域內(nèi)的隨機(jī)點(diǎn)Xrand,否則就返回終點(diǎn)。

Nearest:此函數(shù)使用歐氏距離和numpy模塊來計(jì)算和比較節(jié)點(diǎn)Xrand和Ginit的距離,返回一個節(jié)點(diǎn)列表中離Xrand最近的節(jié)點(diǎn)Xnearest_init。

DynamicStep:此函數(shù)采取動態(tài)步長策略通過以節(jié)點(diǎn)Xnearest_init為圓心生成隨機(jī)散布點(diǎn)來計(jì)算障礙物分布密度并計(jì)算對應(yīng)障礙物復(fù)雜程度下應(yīng)選用的生長步長Step。

GetNewNode:此函數(shù)中以Xnearest_init節(jié)點(diǎn)和Xrand節(jié)點(diǎn)的連線為導(dǎo)向,將Xnearest_init當(dāng)作根節(jié)點(diǎn)向連線方向生長步長為Step的樹,生成的節(jié)點(diǎn)作為新節(jié)點(diǎn)并繼續(xù)循環(huán)。

CheckCollision:檢查兩個節(jié)點(diǎn)連結(jié)成的線段是否與障礙相交,若相交則重新生成節(jié)點(diǎn),否則將這兩個節(jié)點(diǎn)信息添加到Ggoal的列表中并繼續(xù)循環(huán)。

SameNode:此函數(shù)用于判斷節(jié)點(diǎn)Xlast_node和Xnew_init是否為相同的點(diǎn),若為相同則連接并返回Ggoal生成無碰撞的新路徑,否則重新開始循環(huán)。

UnionGraph:此函數(shù)用于合并拼接Ggoal和Ginit間的無碰撞節(jié)點(diǎn)和邊并將這些節(jié)點(diǎn)和邊的信息作為已經(jīng)規(guī)劃完成的舊地圖節(jié)點(diǎn)存入Gold,同于繪制路徑。

GetFinalPath:此函數(shù)讀取Gold中的節(jié)點(diǎn)和邊的信息在地圖中由Xinit到Xgoal連接生成最終的無碰撞路徑。

B-spline:此函數(shù)以B樣條曲線對路徑進(jìn)行優(yōu)化。使用B-spline函數(shù)來生成B樣條曲線,使用numpy模塊中的linspace函數(shù)來生成均勻的采樣點(diǎn),最后繪制優(yōu)化后的無碰撞路徑。

2 實(shí)驗(yàn)設(shè)置和結(jié)果分析

為了驗(yàn)證BD-RRT算法的優(yōu)越性和實(shí)用性,本文在Inter i7-8750h筆記本電腦上基于PyCharm Professional Edition 2022.2.2進(jìn)行了不同環(huán)境下的仿真實(shí)驗(yàn)。

分別設(shè)置簡單環(huán)境、復(fù)雜環(huán)境、動態(tài)環(huán)境,在不同環(huán)境中將本文算法與RRT、Bi-RRT、Extended-RRT[16]等算法進(jìn)行實(shí)驗(yàn)對比并分析結(jié)果。

2.1 簡單環(huán)境

設(shè)置環(huán)境1尺寸為50×40,起始點(diǎn)為(2,2),目標(biāo)點(diǎn)為(49,35),構(gòu)建簡單障礙物場景。目標(biāo)采樣率0.08,路徑采樣率0.8,最大迭代次數(shù)500次,對比實(shí)驗(yàn)結(jié)果如圖5所示。

(a) RRT算法 (b) Bi-RRT算法

圖5中,灰色線段為擴(kuò)展隨機(jī)樹的生長路徑,黑色線段為最終連接生成的路徑。圖中可以看出,本文的BD-RRT算法在添加動態(tài)步長生長策略,動態(tài)RRT方法及B-spline軌跡優(yōu)化后,在生成擴(kuò)展隨機(jī)樹時的冗余路徑更少,路徑簡單明了,擴(kuò)展隨機(jī)樹對目標(biāo)點(diǎn)生長的指向性更強(qiáng),路徑更加平滑。此外,本文算法保留了原始Bi-RRT算法較好的避障能力和雙向生長擴(kuò)展隨機(jī)樹加速收斂的能力。

對上述4種算法在簡單障礙物環(huán)境下進(jìn)行100輪重復(fù)實(shí)驗(yàn),測試結(jié)果統(tǒng)計(jì)如圖6所示。圖中1號線為4種算法100輪測試的平均路徑規(guī)劃時間,2號線為100輪測試的平均路徑規(guī)劃的長度。圖中進(jìn)行對比可以看出,本文BD-RRT算法在簡單環(huán)境規(guī)劃路徑花費(fèi)的時間更短,且規(guī)劃路徑長度優(yōu)于其余3種算法。

圖6 簡單環(huán)境下100輪實(shí)驗(yàn)測試對比

2.2 復(fù)雜環(huán)境

設(shè)置環(huán)境2尺寸為100×100,起始點(diǎn)為(2,2),目標(biāo)點(diǎn)為(98,98),構(gòu)建多種形狀大小障礙物的復(fù)雜障礙物場景。目標(biāo)采樣率為0.05,路徑采樣率為0.8,最大迭代次數(shù)為3000次,對比實(shí)驗(yàn)結(jié)果如圖7所示。

(a) RRT算法 (b) Bi-RRT算法

圖7中,灰色線段為擴(kuò)展隨機(jī)樹的生長路徑,黑色線段為最終連接生成的路徑。圖中可以看出,本文的BD-RRT算法在復(fù)雜環(huán)境下也可以較好的通過狹窄通道,完成路徑規(guī)劃和避障任務(wù),生成路徑的長度短于其他算法且路徑柔順,顯著降低了在執(zhí)行任務(wù)時損壞機(jī)器人的概率。

對上述4種算法在復(fù)雜障礙物環(huán)境下進(jìn)行100輪重復(fù)測試,測試結(jié)果統(tǒng)計(jì)如圖8所示。圖中1號線為4種算法100輪測試的平均路徑規(guī)劃時間,2號線為100輪實(shí)驗(yàn)的平均路徑規(guī)劃的長度?；趫D中橫坐標(biāo)進(jìn)行對比可以看出,本文BD-RRT算法在復(fù)雜障礙物環(huán)境中規(guī)劃路徑的時間和長度也同樣優(yōu)于其余3種算法,證明了本文算法的優(yōu)越性;平均路徑規(guī)劃長度與時間分別為改進(jìn)前的Bi-RRT算法的21.1%和95.3%,證明了改進(jìn)的有效性。

圖8 復(fù)雜環(huán)境下100輪實(shí)驗(yàn)測試對比

第2.1節(jié)是在簡單場景中規(guī)劃路徑,因此不需要考慮規(guī)劃成功率的問題。但是在本節(jié)復(fù)雜場景的測試中,由于限制了迭代次數(shù)且環(huán)境中障礙物的密度較高,需要考慮規(guī)劃失敗的問題。圖9為復(fù)雜場景下100輪實(shí)驗(yàn)后4種算法的規(guī)劃成功率,觀察對比柱狀圖可以看出本文的BD-RRT算法在規(guī)劃任務(wù)中成功率最高,為100%,相較于其余的算法更優(yōu)秀。

2.3 動態(tài)環(huán)境

在2.2節(jié)復(fù)雜障礙物環(huán)境的基礎(chǔ)上增加了隨機(jī)障礙物構(gòu)成動態(tài)環(huán)境[17],對算法的動態(tài)避障性能進(jìn)行實(shí)驗(yàn)并與Extended-RRT算法對比。目標(biāo)采樣率0.05,路徑采樣率0.8,最大迭代次數(shù)2000次。

圖10為本文BD-RRT算法得到的動態(tài)避障圖。圖10a為首次規(guī)劃生成的路徑,其中灰色線段為擴(kuò)展隨機(jī)樹的生長路徑,黑色的線段為最終連接生成的路徑;圖10b為動態(tài)增加障礙物1后再規(guī)劃生成的路徑,障礙物1坐標(biāo)為(40 mm,54 mm),黃色線段為在無碰撞擴(kuò)展隨機(jī)樹的基礎(chǔ)上繼續(xù)生長的路徑,紫色的線段為最終連接生成的再規(guī)劃路徑;圖10c為動態(tài)添加障礙物2后生成的再規(guī)劃路徑,障礙物2坐標(biāo)為(58 mm,66 mm);圖10d為動態(tài)添加障礙物3后生成的再規(guī)劃路徑,障礙物3坐標(biāo)為(8 mm,10 mm)。對比上述4張圖可以發(fā)現(xiàn),在障礙物發(fā)生動態(tài)增加后,本文算法可以有效的利用上一次規(guī)劃生成的無碰撞路徑,并在其基礎(chǔ)上繼續(xù)避開新的障礙物,顯著提高了路徑節(jié)點(diǎn)的利用效率,可以實(shí)現(xiàn)更快速的動態(tài)避障。

(a) 首次規(guī)劃生成的路徑 (b) 動態(tài)增加障礙物1后再規(guī)劃生成的路徑

基于以上環(huán)境對傳統(tǒng)動態(tài)規(guī)劃算法Extended-RRT和本文BD-RRT算法進(jìn)行30輪重復(fù)實(shí)驗(yàn),實(shí)驗(yàn)結(jié)果統(tǒng)計(jì)如表3所示。本文BD-RRT算法在動態(tài)障礙物環(huán)境中的平均重規(guī)劃時間和路徑長度都短于Extended-RRT,分別為0.09 s和161.42 mm,僅為Extended-RRT算法的5.2%和95.8%。此外BD-RRT算法的重規(guī)劃成功率也比Extended-RRT算法高,上述實(shí)驗(yàn)結(jié)果證明了本文算法在動態(tài)避障方面的優(yōu)越性和穩(wěn)定性。

表3 30輪實(shí)驗(yàn)對比

3 結(jié)束語

本研究針對Bi-RRT算法在路徑規(guī)劃時的不足,提出了一種基于BD-RRT(bidirectional-dynamic-RRT)的動態(tài)路徑規(guī)劃算法,以解決實(shí)際場景中障礙物變化時的避障問題。通過動態(tài)步長增長策略提高對障礙物附近點(diǎn)的采樣精度,減少采樣點(diǎn)并增強(qiáng)算法探索能力,加速收斂;引入動態(tài)RRT算法,可以在出現(xiàn)新的障礙物后重新計(jì)算碰撞并修剪首次規(guī)劃路徑中有碰撞的節(jié)點(diǎn),在無碰撞的節(jié)點(diǎn)的基礎(chǔ)上繼續(xù)規(guī)劃新路徑;采取B樣條曲線平滑生成的路徑以達(dá)到提高機(jī)器人運(yùn)行穩(wěn)定性的目的。

通過多個環(huán)境下的仿真實(shí)驗(yàn)對比,相較于其它傳統(tǒng)路徑規(guī)劃算法,在靜態(tài)環(huán)境中,本文算法能生成更簡短有效的路徑,路徑規(guī)劃的時間更短,生成的路徑更加平滑;在動態(tài)環(huán)境下,本算法也能通過重規(guī)劃避開新的障礙物,比一般的動態(tài)規(guī)劃算法生成路徑的速度更塊且路徑更短更平滑,有著更好的動態(tài)環(huán)境實(shí)用性。