基于改進RRT-Connect算法的全局路徑規劃*

朱建軍,王明森

(吉林化工學院信息與控制工程學院,吉林 132022)

0 引言

智能化的要求越來越高,路徑規劃作為移動機器人不可或缺的一部分,也在快速的發展[1]。路徑規劃的功能是讓機器人在存在障礙物的復雜環境中,在兩點之間規劃出一條安全、平滑的路徑,同時還要盡可能滿足計算量小、規劃時間短、路徑較優等要求[2]。路徑規劃算法主要由A*算法[3]、遺傳算法[4]、蟻群算法[5]組成。這些算法不僅耗時長、計算量大,而且在面對大量復雜障礙物時也容易陷入局部陷阱。

LAVALLE[6]提出快速擴展隨機樹算法(rapid exploring random tree,RRT),該算法既存在具有泛用性強,計算量小、尋跡時間短等特點,也存在隨機性大、收斂速度慢等問題。針對RRT算法存在的缺點,JR等[7]提出了RRT-Connect算法,該算法以雙樹的形式進行路徑規劃,減少算法的路徑代價和規劃時間;張順等[8]提出使用人工勢場法和RRT-Connect算法結合,以此加快搜索效率;CHEN等[9]提出在RRT-Connect算法中配置第3節點,增加了探索效率;張丹萌等[10]通過加入混沌序列來控制RRT-Connect算法的采樣范圍和采樣隨機性,減少了算法的收斂時間和無效采樣;杜傳勝等[11]在引入第3節點和引力RRT-Connect算法基礎上提出同根雙向連接,以此提升算法的收斂性、減少搜索時間。

本文基于RRT-Connect算法進行改進。首先,通過產生高質量隨機點來增強RRT-Connect算法中兩顆隨機樹的收斂性和探索性;其次,使用動態步長來加快連接速度,減少規劃時間和迭代次數;然后,引入正向尋優和逆向貪婪算法進行軌跡優化;最后,采用3次B樣條進行平滑處理,從而獲得一條質量更高、路徑更優的路徑。

1 RRT-Connect算法

RRT算法是基于采樣的路徑規劃算法,該算法以單棵隨機樹的形式生長[12]。RRT-Connect算法是由RRT算法改進而來的算法,該算法以雙樹的形式進行交替生長和直線連接,極大的減少了路徑規劃時間和規劃路徑的長度[13];RRT-Connect算法生長示意圖如圖1所示[14]。

圖1 RRT-Connect生長示意圖

步驟1:初始化狀態空間、起點、終點和障礙物坐標信息,以起點和終點生成T1和T2;

步驟2:以T1中的Xnearest為根節點向Xrand延伸生成Xnew;

步驟3:以T2中的Xprim_nearst為根節點向Xnew不斷延伸生成Xprim_new,直到Xprim_new不滿足狀態空間約束或者Xprim_new和Xnew連接。

2 改進RRT-Connect算法

相較于RRT,RRT-Connect算法雖然提升了規劃效率,但是隨機性較大、擴展性不足、拐點多和存在大量冗余節點等問題依然存在。本文針對RRT-Connect算法中存在的問題,將從引入高質量隨機點、引入動態步長、使用正弦尋優和逆向貪婪結合算法進行軌跡優化、3次B樣條進行平滑處理4個方面對RRT-Connect算法進行改進。

2.1 引入高質量隨機點

為了增加RRT-Connect算法的探索性和導向性,本文引入高質量隨機點。高質量隨機點的產生公式為:

(1)

式中:XH_rand為產生的高質量隨機點,Q為節點選擇概率,P為隨機概率,Xexp為探索性隨機點,Xcon為連接性隨機點。

2.1.1 探索性隨機點的生成

若隨機點到最近節點的最短歐氏距離大于當前的可生成范圍[15],說明該隨機點和隨機樹的距離足夠大,該隨機點具有高探索性。本文通過上述方式,判斷該隨機點是否生成輸出。探索性隨機點生成步驟:

步驟1:初始化R值,隨后進入循環;

步驟2:產生隨機點Xrand,并計算Xrand和樹的最短歐氏距離S;

步驟3:將最短距離S和R進行比較。如果R值較大,則返回步驟2重新生成Xrand;否則將生成的Xrand輸出。

R為隨機點的可生成范圍的半徑,計算公式為:

R=G(n)*Acol_len

(2)

式中:Acol_len為碰距值,G(n)為無碰撞系數,其計算公式為:

(3)

式中:Gin為碰撞增值,Gde為碰撞降值,G為無碰撞系數的預設值。探索性隨機點生成示意圖如圖2所示。

圖2 探索性隨機點生成示意圖

2.1.2 連接性隨機點

RRT-Connect算法以雙樹形式進行拓展。若兩棵樹的末端節點連接,則表示路徑規劃成功。故本文提出以兩棵樹各自的末端節點作為連接性隨機點引導雙樹的連接,連接性隨機點數據list的更新步驟為:

步驟1:生成新節點Xnew;

步驟2:在連接性隨機點數據list中添加新節點Xnew。若Xnew的父節點Xnear在list中,刪除父節點Xnear;

步驟3:對list的數據長度進行刪減,使其維持在設定數量內。

2.2 動態步長探索

RRT-Connect算法兩棵樹彼此生長時,往往存在冗余節點較多、容易陷入局部最優的問題。針對上述問題,本文提出采用動態步長來代替固定步長進行生長延伸。動態步長生長示意圖如圖3所示。

圖3 動態步長生長示意圖

由圖可知,當T1生成新節點Xnew之后,在T2根據歐氏距離尋找距離Xnew最近的父節點Xprim_new。隨后以父節點與隨機點的連線方向為T2的連接方向,L*為動態步長拓展T2樹,L*的計算公式為:

L*=N*L

(4)

式中:L為設定步長,N為動態系數,由是否滿足空間約束決定。N的表達式為:

(5)

引入高質量隨機點和動態步長后的改進RRT-Connect算法流程圖如表1所示。

表1 改進RRT-Connect算法

3 軌跡優化、平滑處理

改進RRT-Connect算法很大程度上優化了原算法探索性和導向性弱、冗余節點多的問題,但拐點較多、路徑代價較大、不滿足機器人運動學規律的問題依舊存在。針對上述問題,本文提出了一種正向尋優和逆向貪婪結合的算法來進行軌跡優化,并對優化后的路徑使用3次B樣條優化處理,以此得到拐點較少、路徑代價較小且更加平滑的路徑。

3.1 正向尋優算法:

正向尋優算法示意圖如圖4所示,Xstart為起點,Xgoal為終點,Xn為已知路徑點。首先,連接Xstart和Xgoal,如果兩點之間沒有障礙物,則由兩點之間直線最短的原則,將兩點作為最終路徑輸出。若兩點之間存在障礙物,則判斷Xstart和終點前一個有效路徑點X9之間是否存在障礙物,若兩點之間不存在障礙物,則重復上述判斷,直到得到到滿足條件的X6。連接X6和Xstart,然后以X6作為起點重復上述操作,直到滿足新的起點和終點可以直接連接,輸出路徑。

圖4 正向尋優算法示意圖

3.2 “分裂化”處理

由于改進RRT-Connect規劃出的路徑經過正向尋優之后變為幾個關鍵位置的點,在其連線上沒有點的存在,無法使用逆向貪婪算法優化。故使用逆向貪婪算法之前,需要對已有路徑進行“分裂化”處理:將原有路徑下的每兩個點之間分裂為若干個點,讓原本只存在幾個關鍵點路徑重新變成一條有眾多冗余節點、可以使用逆向貪婪優化的路徑。

3.3 逆向貪婪算法

使用逆向貪婪算法對“分裂化”處理的路徑進行軌跡優化。節點處理后的路徑如圖5所示。

圖5 逆向貪婪算法

3.4 B樣條平滑處理

為了保證路徑的平滑性,本文使用3次B樣條曲線方程對軌跡進行平滑處理。為了保證平滑處理后的軌跡滿足空間狀態,需要在路徑點周圍產生臨近節點。以路徑點Xn前后節點Xn-1和Xn+1為方向,step為步長產生新的臨近節點,臨近節點如圖6中灰色節點所示。

圖6 產生臨近節點

3次B樣條路徑點計算公式為:

P(t)=P0G0,3(t)+P1G1,3(t)+P2G2,3(t)+P3G3,3(t)

(6)

式中:P(t)為路徑點,P0、P1、P2、P3為控制曲線的特征點,特征點的基函數Gi,n(t)在3次B樣條中的公式為:

(7)

式中:t為時間參數,取值為t∈[0,1]。

4 仿真校驗

為了驗證算法的可行性,本文使用PyCharm平臺分別在少障礙物環境、狹窄通道環境、多障礙物環境和H型障礙物環境中對RRT-Connect算法和本文提出的改進RRT-Connect算法進行仿真對比實驗。地圖大小均為50*30,其中少障礙物環境、多障礙物環境和狹窄通道環境的起點坐標為(2,2),終點坐標為(49,24);H型陷阱環境的起點為(15,15),終點坐標為(30,15),起點和終點分別使用灰色點和黑色點表示,節點擴展步長為0.8,黑色方塊和黑色圓形代表障礙物和邊界。灰色線條代表生成的隨機樹,黑色線條代表規劃出的最終路徑。為了減小算法本身存在的隨機性較大的問題,每次實驗的結果為20次實驗數據的平均值。

4.1 實驗結果與分析

本文使用RRT-Connect算法和改進RRT-Connect算法分別在少障礙物環境、狹窄通道環境、多障礙物環境和H型障礙物環境中進行仿真實驗,仿真結果如圖7～圖10所示,其中圖7a～圖10a為RRT-Connect算法的規劃結果,圖7b～圖10b為改進RRT-Connect算法的規劃結果。

(a) RRT-Connect算法 (b) 改進RRT-Connect算法

(a) RRT-Connect算法 (b) 改進RRT-Connect算法

由圖7可知,RRT-Connect算法在少障礙物環境需要產生大量節點來探索路徑,而改進RRT-Connect算法通過探索性隨機點增強了對未知區域的探索,減少了迭代次數和冗余節點數量。

由圖8可知,RRT-Connect算法在狹窄通道環境中存在盲目搜索的問題,而改進RRT-Connect算法通過引入連接性節點,增加了算法的導向性,減少了算法的隨機性和規劃時間。

由圖9可知,RRT-Connect算法在多障礙物環境存在隨機性較大的問題,而改進RRT-Connect算法通過引入高質量隨機點,增加算法的探索性和導向性,提升了算法的規劃效率。

(a) RRT-Connect算法 (b) 改進RRT-Connect算法

由圖10可知,RRT-Connect算法在H型陷阱障礙物中存在容易陷入局部陷阱的問題,而改進RRT-Connect算法通過引入高質量隨機點和動態步長,改善了算法容易陷入局部陷阱的問題,減少冗余節點的數量,提升了算法的規劃質量。

(a) RRT-Connect算法 (b) 改進RRT-Connect算法

4.2 算法數據分析

使用兩種算法在4種環境下進行20次實驗,每次試驗為20次實驗的平均值,共400次實驗。在實驗后對規劃時間、路徑長度和節點個數進行整理統計。400次實驗的數據結果如表2所示。每20次路徑規劃的平均時間、平均路徑長度、平均節點個數的單位分別為秒、米、個。

表2 不同環境下算法數據對比

由表2可知,相比于RRT-Connect算法,改進RRT-Connect算法在少障礙物環境、狹窄通道環境、多障礙物環境和H型障礙物環境中進行400次實驗后的平均時間分別減少了28.57%、35.64%、16.41%、25.01%;平均路徑長度分別減少了21.65%、15.09%、14.02%、25.45%;平均節點個數分別減少了42.19%、51.07%、29.26%、45.1%。由上述數據可知,在不同環境中,改進RRT-Connect算法相比于RRT-Connect算法規劃效率更高、路徑代價更小、冗余節點數量更少。

5 結論

本文在RRT-Connect算法的基礎上,提出了一種基于高質量隨機點的動態步長改進RRT-Connect算法,改進算法在基礎RRT-Connect的基礎上通過生成高質量隨機點,進而引導隨機樹的生成、同時引入動態步長來減少冗余節點的數量,隨后使用正向尋優和逆向貪婪對軌跡進行優化,最后使用3次B樣條進行平滑處理。通過兩種算法在4種不同環境下的仿真實驗,驗證了改進算法相比于基礎RRT-Connect效率更高、路徑質量更優,從而表明了改進算法的可行性。未來的研究中會考慮在實際運行中加入局部路徑規劃,以此來提高算法的實用性。