基于RRT 算法的逆正向?qū)?yōu)路徑規(guī)劃

朱道揚(yáng)

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

0 引言

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

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

1 RRT 算法基本原理

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

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

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

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

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

2 改進(jìn)RRT 算法

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

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

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

表1 逆正向?qū)?yōu)RRT algorithm

3 仿真驗(yàn)證

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

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

3.1 單個(gè)障礙物情況

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

3.2 單個(gè)狹窄通道

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

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

3.3 簡(jiǎn)單環(huán)境

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

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

3.4 復(fù)雜環(huán)境

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

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

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

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

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

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

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

4 結(jié)論

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

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

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

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

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