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

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

0 引言

隨著自動(dòng)化和機(jī)器人技術(shù)的發(fā)展[1],機(jī)器人在人們?nèi)粘Ｉ钪邪缪葜絹碓街匾慕巧?機(jī)器人的種類非常豐富,例如機(jī)械臂、移動(dòng)機(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)的路徑和時(shí)間為目標(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等?；诓蓸拥腞RT類算法可以在高維空間較好的完成規(guī)劃任務(wù),適合用于多自由度機(jī)器人在環(huán)境復(fù)雜空間的路徑規(guī)劃問題[5]。RRT算法在全局采取了隨機(jī)擴(kuò)展方式,通過固定步長生長擴(kuò)展隨機(jī)樹,在生成到達(dá)目標(biāo)點(diǎn)的無碰撞路徑時(shí)結(jié)束[6],該方法提高了機(jī)器人在高維空間的運(yùn)動(dòng)能力,但存在路徑規(guī)劃時(shí)間過長,計(jì)算復(fù)雜度高,全局環(huán)境冗余等問題。為解決上述問題,LAVALLE等[7]改進(jìn)得到了Bi-RRT(bidirectional-RRT)算法,它通過雙向生長擴(kuò)展隨機(jī)樹并連接兩棵樹來獲得最優(yōu)路徑,當(dāng)端點(diǎn)位于非常狹窄的環(huán)境中時(shí),該算法的效率會(huì)更加明顯,然而該算法仍然存在一些缺點(diǎn),如規(guī)劃路徑不平滑和無法對(duì)環(huán)境中新出現(xiàn)障礙物進(jìn)行重規(guī)劃避障等問題。

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

1 基于BD-RRT算法的動(dòng)態(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:在空間中生成一個(gè)隨機(jī)節(jié)點(diǎn)Xrand,遍歷現(xiàn)有節(jié)點(diǎn),計(jì)算每個(gè)節(jié)點(diǎn)到Xrand距離并選擇距離最近的樹節(jié)點(diǎn)作為Xnearest;

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

步驟5:根據(jù)運(yùn)動(dòng)學(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)向外探索拓展,直到兩棵樹相遇時(shí)算法收斂[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個(gè)步驟,但不同的是Bi-RRT的擴(kuò)展隨機(jī)樹是交替生長的,若第一輪迭代中a樹生長,第二輪便切換為b樹生長,循環(huán)往復(fù)。在每輪迭代中,擴(kuò)展隨機(jī)樹除了向外生長外,還會(huì)遍歷另一顆擴(kuò)展隨機(jī)樹中的所有節(jié)點(diǎn),找出離New Node最近的節(jié)點(diǎn),用于判斷兩顆擴(kuò)展隨機(jī)樹是否相遇。

算法示意如圖2所示,在進(jìn)入某次迭代時(shí)以a樹拓展,算法成功拓展出a樹的新節(jié)點(diǎn)New Node_a,此時(shí)算法將遍歷b樹中的所有節(jié)點(diǎn),找出b樹中離New Node_a最近的節(jié)點(diǎn)Closest Node_b,接著判斷兩點(diǎn)是否滿足步長限制以及是否可以通過碰撞檢測,圖中橙色虛線為這兩節(jié)點(diǎn)的連線,明顯無法通過碰撞檢測,因此a樹和b樹在這一輪迭代中無法相遇。進(jìn)入下一輪迭代,切換為b樹拓展,此時(shí)算法拓展出新節(jié)點(diǎn)New Node_b,遍歷a樹后將上一輪的New Node_a作為Closest Node_a,判斷發(fā)現(xiàn)兩點(diǎn)滿足步長要求,圖中橙色實(shí)線為這兩節(jié)點(diǎn)的連線,明顯通過碰撞檢測,那么這時(shí)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ā)生變化時(shí)進(jìn)行自動(dòng)重規(guī)劃路徑;②Bi-RRT算法與傳統(tǒng)RRT一樣是通過固定步長來生長擴(kuò)展隨機(jī)樹的,當(dāng)設(shè)置的步長過大時(shí)會(huì)增加隨機(jī)點(diǎn)被報(bào)廢的可能性,而當(dāng)過小時(shí)則會(huì)影響擴(kuò)展隨機(jī)樹的搜索效率;③Bi-RRT算法規(guī)劃得到的路徑不平滑,在實(shí)際工作中這種規(guī)劃方式容易對(duì)機(jī)器人舵機(jī)造成損壞且運(yùn)行效率較低。面對(duì)上述問題,本文提出BD-RRT(bidirectional-dynamic-RRT)算法,通過引入動(dòng)態(tài)RRT算法、動(dòng)態(tài)步長增長策略[9]、B樣條曲線優(yōu)化[10]等方法對(duì)Bi-RRT算法進(jìn)行改進(jìn)。

1.3 動(dòng)態(tài)RRT算法

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

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

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

表1 動(dòng)態(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ù)生長。在每一次迭代中,先采樣一個(gè)新的隨機(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 動(dòng)態(tài)步長增長策略

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

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

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

步驟2:在擴(kuò)展一個(gè)新節(jié)點(diǎn)時(shí),計(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ù)障礙物分布密度τ和閾值ω來決定動(dòng)態(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è)置得低一些,這樣算法會(huì)在障礙物密集區(qū)域使用更小的步長搜索可行路徑。相反的,如果障礙物分布較稀疏,那么閾值可以設(shè)置得稍高一些,以加快搜索速度。

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

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

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

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

首先,通過BD-RRT算法生成可到達(dá)目標(biāo)點(diǎn)的節(jié)點(diǎn),取兩節(jié)點(diǎn)間的4個(gè)點(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)化后生成的路徑,通過兩線段對(duì)比可以看出軌跡優(yōu)化效果明顯。

1.6 BD-RRT算法

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

圖4 BD-RRT算法流程圖

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

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

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

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

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

DynamicStep:此函數(shù)采取動(dòng)態(tài)步長策略通過以節(jié)點(diǎn)Xnearest_init為圓心生成隨機(jī)散布點(diǎn)來計(jì)算障礙物分布密度并計(jì)算對(duì)應(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:檢查兩個(gè)節(jié)點(diǎn)連結(jié)成的線段是否與障礙相交,若相交則重新生成節(jié)點(diǎn),否則將這兩個(gè)節(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樣條曲線對(duì)路徑進(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)境、動(dòng)態(tài)環(huán)境,在不同環(huán)境中將本文算法與RRT、Bi-RRT、Extended-RRT[16]等算法進(jìn)行實(shí)驗(yàn)對(duì)比并分析結(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次,對(duì)比實(shí)驗(yàn)結(jié)果如圖5所示。

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

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

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

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

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次,對(duì)比實(shí)驗(yàn)結(jié)果如圖7所示。

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

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

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

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

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

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

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

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

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

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

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

3 結(jié)束語

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

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