基于BD-RRT算法的動態路徑規劃研究*

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

(江蘇大學機械工程學院,鎮江 212013)

0 引言

隨著自動化和機器人技術的發展[1],機器人在人們日常生活中扮演著越來越重要的角色,機器人的種類非常豐富,例如機械臂、移動機器人和服務機器人等。有了機器人的幫助,人們可以輕易完成從前無法單獨完成的任務,顯著減少了人力資源的浪費。機器人路徑規劃的優劣決定了其工作的穩定性和智能性[2],因此如何合理有效解決機器人的路徑規劃是當前科學界致力解決的關鍵問題。

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

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

1 基于BD-RRT算法的動態路徑規劃

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

傳統RRT算法是一種基于擴展隨機樹的采樣算法,其基本思想是從起點開始,隨機采樣生成一些節點,然后通過把這些節點連接成樹狀結構來搜索可行的路徑,示意圖如圖1所示。算法具體過程為:

圖1 傳統RRT算法示意圖

步驟1:確定擴展隨機樹的生長空間,將機器人的起點Xinit和目標點Xgoal添加至隨機空間;

步驟2:初始化根節點Xinit為擴展隨機樹的起點;

步驟3:在空間中生成一個隨機節點Xrand,遍歷現有節點,計算每個節點到Xrand距離并選擇距離最近的樹節點作為Xnearest;

步驟4:沿Xnearest向Xrand的方向以固定步長增量step生長一個新節點Xnew;

步驟5:根據運動學模型和環境約束,計算從節點Xnearest到達節點Xnew的路徑,并檢測該路徑是否與障礙物相交;

步驟6:如果路徑未發生碰撞,則將節點Xnew添加到樹結構中,并將其與最近的樹節點Xnearest連接,如果發生碰撞則重新生長節點Xnew;

步驟7:重復上述新節點生成流程,直到生成的新節點Xnew接近目標節點Xgoal;

步驟8:最后依次搜索并連接樹結構中從起點到目標節點的路徑。

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

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

1.2 BD-RRT算法的主要設計思路

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

1.3 動態RRT算法

動態RRT算法與傳統RRT算法有一個非常大的不同,那就是不完全放棄初始地圖中生成的擴展隨機樹的信息,而是在此基礎上重新規劃。

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

動態RRT算法的具體步驟如偽代碼表1所示。

表1 動態RRT算法偽代碼

首先,從初始地圖的RRT樹中復制節點信息,并使用障礙檢測函數check_collision和刪除節點函數remove_node來刪除與新地圖中新障礙物發生碰撞的節點。

然后,使用修剪后的擴展隨機樹作為起點來繼續生長。在每一次迭代中,先采樣一個新的隨機節點Xrand,找到最近的節點Xnearest,并通過steer函數從Xnearest到Xrand進行擴展。

最后,檢查擴展路徑是否與當前環境障礙物存在碰撞,并將新節點添加到更新后的樹中。如果新節點接近目標節點Xgoal,則將其添加到樹中并返回路徑。否則,我們根據新的地形重新計算障礙物位置,并刪除與新障礙物發生碰撞的所有節點。如果沒有找到可連接起點和終點的路徑,則返回None。

1.4 動態步長增長策略

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

針對這種現象,本文提出一種動態步長增長策略,具體步驟為:

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

步驟2:在擴展一個新節點時,計算新節點周圍障礙物的分布密度τ。可以通過隨機散布點來計算障礙物分布密度。將散點范圍設置為以新節點為圓心,初始步長為半徑的圓。隨機點總數定為n,落在障礙物中的點為b,則障礙物分布密度可以計算為:

(1)

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

(2)

步驟4:使用BD-RRT算法來選擇新節點的父節點,并連接新節點與父節點;

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

策略中閾值ω的選擇可以根據具體問題和應用場景進行調整。一般來說,需要考慮以下因素:

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

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

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

1.5 B-spline軌跡優化

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

J次的B樣條插值函數的表達式為:

(3)

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

(4)

(5)

基函數滿足微分方程為:

(6)

考慮到機器人工作在復雜實際環境下障礙物干擾較多,本文選用三次B樣條插值函數對路徑進行平滑處理。

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

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

1.6 BD-RRT算法

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

圖4 BD-RRT算法流程圖

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

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

ReachableNodesAndEdges:該函數分別從Xinit和Xgoal節點出發,搜索可以到達Gnew的有效節點和邊,存放到Ggoal和Ginit的列表中。

RandomNode:此函數用來生成在區域范圍C內一個隨機節點。如果隨機數大于目標采樣率,就返回一個在區域內的隨機點Xrand,否則就返回終點。

Nearest:此函數使用歐氏距離和numpy模塊來計算和比較節點Xrand和Ginit的距離,返回一個節點列表中離Xrand最近的節點Xnearest_init。

DynamicStep:此函數采取動態步長策略通過以節點Xnearest_init為圓心生成隨機散布點來計算障礙物分布密度并計算對應障礙物復雜程度下應選用的生長步長Step。

GetNewNode:此函數中以Xnearest_init節點和Xrand節點的連線為導向,將Xnearest_init當作根節點向連線方向生長步長為Step的樹,生成的節點作為新節點并繼續循環。

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

SameNode:此函數用于判斷節點Xlast_node和Xnew_init是否為相同的點,若為相同則連接并返回Ggoal生成無碰撞的新路徑,否則重新開始循環。

UnionGraph:此函數用于合并拼接Ggoal和Ginit間的無碰撞節點和邊并將這些節點和邊的信息作為已經規劃完成的舊地圖節點存入Gold,同于繪制路徑。

GetFinalPath:此函數讀取Gold中的節點和邊的信息在地圖中由Xinit到Xgoal連接生成最終的無碰撞路徑。

B-spline:此函數以B樣條曲線對路徑進行優化。使用B-spline函數來生成B樣條曲線,使用numpy模塊中的linspace函數來生成均勻的采樣點,最后繪制優化后的無碰撞路徑。

2 實驗設置和結果分析

為了驗證BD-RRT算法的優越性和實用性,本文在Inter i7-8750h筆記本電腦上基于PyCharm Professional Edition 2022.2.2進行了不同環境下的仿真實驗。

分別設置簡單環境、復雜環境、動態環境,在不同環境中將本文算法與RRT、Bi-RRT、Extended-RRT[16]等算法進行實驗對比并分析結果。

2.1 簡單環境

設置環境1尺寸為50×40,起始點為(2,2),目標點為(49,35),構建簡單障礙物場景。目標采樣率0.08,路徑采樣率0.8,最大迭代次數500次,對比實驗結果如圖5所示。

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

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

對上述4種算法在簡單障礙物環境下進行100輪重復實驗,測試結果統計如圖6所示。圖中1號線為4種算法100輪測試的平均路徑規劃時間,2號線為100輪測試的平均路徑規劃的長度。圖中進行對比可以看出,本文BD-RRT算法在簡單環境規劃路徑花費的時間更短,且規劃路徑長度優于其余3種算法。

圖6 簡單環境下100輪實驗測試對比

2.2 復雜環境

設置環境2尺寸為100×100,起始點為(2,2),目標點為(98,98),構建多種形狀大小障礙物的復雜障礙物場景。目標采樣率為0.05,路徑采樣率為0.8,最大迭代次數為3000次,對比實驗結果如圖7所示。

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

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

對上述4種算法在復雜障礙物環境下進行100輪重復測試,測試結果統計如圖8所示。圖中1號線為4種算法100輪測試的平均路徑規劃時間,2號線為100輪實驗的平均路徑規劃的長度。基于圖中橫坐標進行對比可以看出,本文BD-RRT算法在復雜障礙物環境中規劃路徑的時間和長度也同樣優于其余3種算法,證明了本文算法的優越性;平均路徑規劃長度與時間分別為改進前的Bi-RRT算法的21.1%和95.3%,證明了改進的有效性。

圖8 復雜環境下100輪實驗測試對比

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

2.3 動態環境

在2.2節復雜障礙物環境的基礎上增加了隨機障礙物構成動態環境[17],對算法的動態避障性能進行實驗并與Extended-RRT算法對比。目標采樣率0.05,路徑采樣率0.8,最大迭代次數2000次。

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

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

基于以上環境對傳統動態規劃算法Extended-RRT和本文BD-RRT算法進行30輪重復實驗,實驗結果統計如表3所示。本文BD-RRT算法在動態障礙物環境中的平均重規劃時間和路徑長度都短于Extended-RRT,分別為0.09 s和161.42 mm,僅為Extended-RRT算法的5.2%和95.8%。此外BD-RRT算法的重規劃成功率也比Extended-RRT算法高,上述實驗結果證明了本文算法在動態避障方面的優越性和穩定性。

表3 30輪實驗對比

3 結束語

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

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