朱道揚

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

0 引言

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

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

1 RRT 算法基本原理

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

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

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

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

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

2 改進RRT 算法

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

圖1 逆正向路徑優(yōu)化

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

表1 逆正向尋優(yōu)RRT algorithm

3 仿真驗證

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

圖2 單個障礙物環(huán)境下的算法效果對比

3.1 單個障礙物情況

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

3.2 單個狹窄通道

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

圖3 單個狹窄通道環(huán)境下的算法效果對比

3.3 簡單環(huán)境

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

圖4 簡單環(huán)境下的算法效果對比

3.4 復雜環(huán)境

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

圖5 復雜環(huán)境下的算法效果對比

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

表2 規(guī)劃路徑平均長度

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

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

表3 規(guī)劃路徑平均節(jié)點數(shù)

4 結論

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

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

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

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

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