基于RRT 算法的逆正向尋優路徑規劃

朱道揚

(武漢交通職業學院, 湖北 武漢 430065)

0 引言

根據對環境信息的把握程度可將路徑規劃劃分為全局路徑規劃和局部路徑規劃。 全局路徑規劃需要采集地圖信息來進行路徑規劃,傳統算法有A*算法[1]、Dijkstra 算法[2]、快速擴展隨機樹(RRT)算法[3-5]等,這類算法對復雜空間和障礙物進行建模的精度直接影響搜索效率。 基于智能算法有遺傳算法[6]、粒子群算法[7]、蟻群算法[8]等,此類算法雖然學習能力非常強,但實時性差、計算量大,需要占用大量的存儲空間。 其中,RRT算法具有結構簡單、概率完備性、強大的搜索擴展能力以及避免對復雜空間精確建模等優點被廣泛運用于機器人的路徑規劃,然而現有的RRT 算法有著搜索效率低、易陷入局部極小值、路徑不光滑等缺點。 北京工業大學的阮曉鋼等[9]提出了一種基于信息增益與RRT 相結合的機器人環境探索策略,增強機器人對未知環境的探索,降低傳統RRT 算法固有的盲目性,但是需要精確的空間建模且計算量較大。 廣東工業大學的何兆楚等[10]利用人工勢場法與RRT 算法結合避免陷入局部極小值,但是全局路徑不是最優。 譚波等[11]對搜索路徑的節點進行修剪,利用B 樣條曲線得到光順路徑,但是修剪節點常常不能取得滿意的結果,導致全局路徑不最優。

針對原始RRT 算法存在全局規劃路徑不最優、路徑平滑性差的問題,本文提出一種改進RRT 算法,該算法首先對原始RRT 算法的規劃路徑進行逆向尋優,減少路徑規劃的長度和節點數,然后再對該路徑進行正向尋優,避免規劃路徑陷入局部最優,實現規劃路徑的全局優化,最后通過在4 種地圖環境下對3 種算法進行100 次路徑規劃仿真實驗驗證本算法的可靠性和有效性。

1 RRT 算法基本原理

LaValle 于1999 年首次提出快速擴展隨機樹(RRT)算法, 優勢在于無需對系統進行建模,無需對搜索區域進行幾何劃分,搜索空間的覆蓋率高,搜索的范圍廣,可以盡可能地探索未知區域。在采樣空間內確定起點和目標點,通過隨機采樣擴展增加新節點,生成一個隨機擴展樹,當隨機樹中的新節點包含目標點或進入目標區域時,初始點到目標點間至少形成一條以隨機樹新節點組成的路徑。

從理論上看,RRT 算法是一種具有概率完備性的全局路徑規劃算法,只要迭代足夠多的次數就一定能保證找到一條從起點到目標點的路徑。 同時,RRT 算法不需要對環境進行具體的建模,只需要確定障礙物的位置坐標和抽象后的位姿信息,因此預處理的數據量相對較少。但是在實際應用中發現,基本的RRT 算法存在以下缺點。

1) 搜索效率低: 由于RRT 算法是對全局路徑進行規劃,雖然具有完備性的優點,但是針對目標的搜索效率較低,造成計算量較大。

2) 全局路徑不最優:搜索的路徑由眾多節點組成,其中許多節點為無效節點,造成規劃路徑蜿蜒曲折,因此不能保證每次規劃路徑最短。

3) 路徑平滑性不足:節點與節點之間通過直線連接,所規劃路徑的各段直線之間存在較多拐角,缺乏平滑性,難以滿足機器人運動的平穩性要求。

2 改進RRT 算法

為了實現RRT 算法規劃路徑的全局尋優,本文提出了一種逆正向尋優的方法來改進RRT 算法的規劃路徑,如圖1 所示。 逆正向尋優方法充分應用兩點之間線段最短的數學思想,達到縮短路徑長度的目的。 圖中X1為起點,X11為終點,原始的RRT 算法擴展產生的節點為Xi(i=2…11),黑色區域表示障礙物,藍色實線表示原始的RRT算法初步生成的搜索路徑,用X11到X1的黃色虛線表示逆向尋優路徑,用X1到X11的紅色點線表示在逆向尋優路徑后使用正向尋優產生的路徑。經過驗證,當節點數量太多,可以重復上述逆正向尋優算法,但是一次逆正向尋優可以避免多數規劃路徑陷入局部極值,多次逆正向尋優只會使路徑規劃質量略有提升,反而會使路徑尋優內存消耗顯著增加。

圖1 逆正向路徑優化

改進后的RRT 算法偽代碼如表1 所示,具體優化過程如下。 首先在起點X1與終點X11直接生產RRT 的規劃路徑,然后逆向尋優規劃路徑,從終點X11搜索并連接最近的父節點X9、X8、X7……X1,依次形成逆向規劃路徑X11—X9、X11—X8、X11—X7、X11—X6、……X11—X1,只要檢測到逆向規劃路徑有障礙物存在,就把該路徑節點的上一父節點存儲起來。 例如,在檢測逆向規劃路徑X11—X9沒有障礙物,但是在檢測逆向規劃路徑X11—X8有障礙物,則把X11、X9作為父節點存儲起來,接著從X9繼續形成逆向規劃路徑X9—X8、X9—X7、X9—X6、X9—X5、……X9—X1,并檢測是否有障礙物,同樣把碰撞點的前一個父節點存儲起來,因此,下圖中逆向尋優路徑可以找到父節點X11、X9、X6、X4、X3、X2、X1。 可以看出逆向尋優雖然可以對規劃路徑全局尋優,但是仍然有部分節點(X4、X3、X2、X1)陷入局部極值,因此,在逆向尋優路徑后進行一次正向尋優,可以解決上述問題。從起點X1出發尋找碰撞點的前一個父節點,可以找到父節點X1、X4、X6、X9、X11。 當整個路徑完成優化時,得到如圖1 所示的正向尋優路徑,即算法結束。

表1 逆正向尋優RRT algorithm

3 仿真驗證

將原始的RRT 算法、逆向尋優RRT 算法、逆正向尋優RRT 算法在MATLAB 中做仿真實驗。在二維仿真柵格地圖環境下進行全局路徑規劃對比驗證,仿真地圖分為單個障礙物、單個狹窄通道、簡單環境和復雜環境4 個不同的場景,仿真地圖尺寸均為1000×1000 像素的格柵表示,左上起點坐標為(100,-100),右下目標點坐標(900,-900),目標點閾值為50,搜索允許擴展最大步長為30。 在4 種環境下每種算法進行200 次重復實驗,并統計算法各個指標的平均值。 如圖2—5 所示,圖中左上角紅點表示起點,右下角綠色點表示目標點,紅色樹狀分支表示擴展節點,藍色實線表示RRT 算法規劃路徑,綠色虛線表示逆向尋優規劃路徑,黑色點線表示正向尋優規劃路徑。

圖2 單個障礙物環境下的算法效果對比

3.1 單個障礙物情況

在單個障礙情況下,3 種算法路徑規劃效果如圖2 所示。 原始RRT 算法規劃出的路徑節點較多,光滑性很差,路徑長度不是最優或者次優;逆向尋優后的規劃路徑大大減少了RRT 算法節點數量,路徑節點數目為3 個,路徑長度縮短,路徑質量顯著提高;對逆向尋優后的規劃路徑再次進行正向尋優,其效果基本和逆向尋優效果相同,說明逆向尋優后的RRT 算法足以滿足簡單場景下的路徑規劃。

3.2 單個狹窄通道

在單個狹窄通道情況下,3 種算法路徑規劃效果如圖3 所示。 原始RRT 算法依然可以找到規劃路徑,規劃出的路徑節點很多,光滑性很差,路徑長度不是最優;逆向尋優后的RRT 算法路徑節點數量明顯減少,多次實驗得到路徑節點數目為4 個,路徑規劃質量有所改善;對逆向尋優后的規劃路徑再次進行正向尋優,其效果基本和逆向尋優效果相同,說明逆向尋優后的RRT 算法同樣可以滿足此類場景下的路徑規劃。

圖3 單個狹窄通道環境下的算法效果對比

3.3 簡單環境

在簡單環境情況下,3 種算法路徑規劃效果如圖4 所示。 由于RRT 算法的全概率采樣特點,在此種障礙物較多的情況下,依然可以找到規劃路徑,其路徑光滑性也較差;逆向尋優后的RRT算法改變了父節點的選取,規劃出的路徑長度有所降低,路徑節點數目為7,光滑性相對較好,但是仍存在局部路徑節點選取不是最優的情況,如圖中左下角路徑節點未合理優化;正向尋優可以避免逆向尋優陷入局部最優的問題,實現規劃路徑的全局尋優,其路徑節點只有6 個,路徑長度減少,路徑光滑性會有所提高。

圖4 簡單環境下的算法效果對比

3.4 復雜環境

在復雜環境情況下,3 種算法路徑規劃效果如圖5 所示。 RRT 算法依然可以找到規劃路徑,其路徑同樣較為曲折,光滑性也非常差;逆向尋優后的RRT 算法改變了父節點的選取,尋優后的規劃路徑平均長度顯著降低,規劃路徑平均節點數目為23,光滑性相對較好,但是仍存在局部路徑節點選取不是最優的情況,如圖中左上角有兩處路徑節點未合理優化;再經過正向尋優后規劃路徑平均節點只有20 個,路徑拐角減少,路徑光滑性會有所提高。

圖5 復雜環境下的算法效果對比

為了便于進一步和原始RRT 算法的相關數據進行比較,驗證逆向尋優RRT 算法、逆正向尋優RRT 算法有效性與可靠性,在相同條件下對以上4 個不同的場景重復進行100 次路徑規劃實驗,計算3 種算法的規劃路徑平均路徑長度和平均節點數目,結果如表2、3 所示。

表2 規劃路徑平均長度

由表2 可知,在單個障礙物環境下,原始RRT算法路徑平均長度為1523.5 mm,后兩種算法的規劃路徑平均長度分別為1215.2 mm、1204.4 mm,比原始RRT 算法在規劃路徑平均長度上分別減少20.24%、20.95%;在單個狹窄通道環境下,原始RRT 算法規劃路徑平均長度為1536.6 mm,后兩種算法的規劃路徑平均長度分別為1181.9 mm、1176.6 mm,比原始RRT 算法的規劃路徑平均長度分別減少23.08%、23.43%;在簡單環境下,原始RRT 算法規劃路徑平均長度為2159.3 mm,后兩種算法的規劃路徑平均長度分別為1636.7 mm、1596.4 mm,比原始RRT 算法規劃路徑平均長度分別減少24.20%、26.07%;在復雜環境下,原始RRT 算法規劃路徑平均長度為2983.2 mm,后兩種算法的規劃路徑平均長度比原始RRT 算法規劃路徑平均長度分別減少了19.16%、20.20%。 因此,在4 中環境下逆正向尋優后的RRT 算法在規劃路徑尋優上性能最佳。

由表3 可知,在單個障礙物環境下,原始RRT算法規劃路徑平徑節點數為51.48,后兩種算法的規劃路徑平均節點數分別為3.08、3.04,比原始RRT 算法的規劃路徑平均節點數減少94.02%、94.09%;在單個狹窄通道環境下,原始RRT 算法規劃路徑平均節點數為51.95,后兩種算法的規劃路徑平均節點數分別為4.04、4.02,比原始RRT 算法的規劃路徑平均節點數減少92.22%、92.26%;在簡單環境下,原始RRT 算法規劃路徑平均節點數為76.68,后兩種算法的規劃路徑平均節點數分別為7.55、6.35,比原始RRT 算法的規劃路徑平均節點數減少90.15%、91.72%;在復雜環境下,原始RRT 算法規劃路徑平均節點數為100.3,后兩種算法比原始RRT 算法的規劃路徑平均節點數減少77.57%、79.66%,在4 種環境中,依然是逆正向尋優后的RRT 算法性能最佳。

表3 規劃路徑平均節點數

4 結論

針對原始RRT 算法存在全局規劃路徑不最優、路徑平滑性差的問題,本文提出了一種改進的RRT 算法,該算法首先對原始RRT 算法的規劃路徑進行逆向尋優,然后再對該路徑進行正向尋優,通過在4 種地圖環境下對3 種算法進行100 次路徑規劃實驗。 仿真結果表明,與原始RRT 算法相比,改進的算法的有效性和可靠性主要體現在以下幾個方面。

1) 與原始RRT 算法相比,逆正向尋優后的RRT 算法進行路徑規劃所需的平均路徑最短,路徑長度減少了19%以上。 通過對比上述4 種環境,逆正向尋優后的RRT 算法性能提升幅度都是最大。

2) 與原始RRT 算法相比,逆正向尋優后的RRT 算法進行路徑規劃所需的平均節點數最少,節點數減少了70%以上。 通過對比上述4 種環境,逆正向尋優后的RRT 算法性能提升幅度都是最大,且隨著環境復雜程度的增加,逆正向尋優后的RRT 算法性能會依次降低。

3) 與原始RRT 算法相比,逆正向尋優后的RRT 算法規劃路徑的平滑性有顯著改善,規劃路徑的拐角減少,其能夠更好地滿足機器人運動的要求。

4) 本文所提出的改進RRT 算法雖然可以搜索路徑的全局尋優,但是無法得到最短路徑;規劃路徑拐角的平滑性還需進一步優化;規劃路徑太過靠近障礙物等,上述缺點也是今后的重點研究方向。