改進(jìn)RRT 的復(fù)雜障礙物環(huán)境路徑規(guī)劃算法研究

牛旭，張志安

（南京理工大學(xué)機(jī)械工程學(xué)院，江蘇南京 210094）

路徑規(guī)劃，即根據(jù)規(guī)劃空間中的信息，規(guī)劃出一條最優(yōu)路徑[1]。對(duì)于復(fù)雜障礙物環(huán)境下或者高維空間下的路徑規(guī)劃問(wèn)題，常見(jiàn)的算法有RRT 算法和PRM 算法[2-4]。而對(duì)于多維度空間或復(fù)雜環(huán)境的路徑規(guī)劃，RRT 算法不需要規(guī)劃空間，其泛用性更好[5-6]。但是，基本的RRT 算法在規(guī)劃路徑時(shí)也存在一些缺點(diǎn)，由于采樣過(guò)程的隨機(jī)性，搜索路徑存在具有擴(kuò)展隨機(jī)性大，冗余節(jié)點(diǎn)多，收斂速度慢，導(dǎo)致搜索效率低等問(wèn)題。

在后續(xù)研究中，文獻(xiàn)[7]提出了漸進(jìn)最優(yōu)RRT*算法，通過(guò)重新選擇父節(jié)點(diǎn)和重布線思想來(lái)優(yōu)化路徑。文獻(xiàn)[8]提出了Quick-RRT*算法，通過(guò)擴(kuò)大RRT*算法動(dòng)態(tài)調(diào)整父節(jié)點(diǎn)的范圍，提高其搜索效率。文獻(xiàn)[9]在復(fù)雜環(huán)境下利用A*作為引導(dǎo)域限制RRT 的無(wú)效搜索提高其采樣效率。文獻(xiàn)[10]提出了一種Informed-RRT*的變體，利用目標(biāo)偏置引導(dǎo)策略搜索目標(biāo)點(diǎn)，利用橢球形子集細(xì)化路徑，從而找到最優(yōu)路徑。文獻(xiàn)[11]引入了權(quán)重系數(shù)，增加了樹(shù)向目標(biāo)點(diǎn)方向的擴(kuò)展分量，文獻(xiàn)[12]等在RRT 中引入了引力系數(shù)，增加樹(shù)目標(biāo)點(diǎn)的擴(kuò)展分量，引導(dǎo)擴(kuò)展偏向目標(biāo)點(diǎn)，提高其迭代效率。

但對(duì)于樹(shù)擴(kuò)展隨機(jī)性問(wèn)題的改進(jìn)研究，大都單一考慮了如何讓樹(shù)偏向目標(biāo)點(diǎn)方向進(jìn)行擴(kuò)展，而不考慮環(huán)境障礙所帶來(lái)的影響。對(duì)此，該文提出了一種改進(jìn)的RRT 算法：引入自適應(yīng)步長(zhǎng)目標(biāo)偏置，結(jié)合勢(shì)場(chǎng)力引導(dǎo)隨機(jī)樹(shù)向目標(biāo)點(diǎn)的生長(zhǎng)，借助區(qū)域定性降低障礙物附近的搜索區(qū)域，再對(duì)路徑關(guān)鍵點(diǎn)剪枝，利用B 樣條曲線完成對(duì)路徑平滑優(yōu)化，得到一條最優(yōu)路徑。

1 算法原理

1.1 RRT算法原理

路徑規(guī)劃通常被視為在狀態(tài)空間Χ中搜索從初始狀態(tài)xinit到目標(biāo)區(qū)域xgoal∈X或目標(biāo)狀態(tài)xgoal的連續(xù)路徑[13]。該算法采用隨機(jī)擴(kuò)展的方法在狀態(tài)空間中構(gòu)造從初始節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的樹(shù)。以狀態(tài)空間的起點(diǎn)xinit作為根節(jié)點(diǎn)，在搜索空間中隨機(jī)選取一個(gè)點(diǎn)xrand。如果這個(gè)點(diǎn)在可行區(qū)域內(nèi)，它將遍歷T，在這個(gè)隨機(jī)點(diǎn)上尋找最接近的節(jié)點(diǎn)n。通過(guò)碰撞檢測(cè)，當(dāng)xrand與xnear之間的距離比設(shè)定的擴(kuò)展步長(zhǎng)小時(shí)，xrand將作為葉節(jié)點(diǎn)xnear加入到隨機(jī)樹(shù)中；而當(dāng)xrand與xnear之間的距離大于設(shè)定的擴(kuò)展步長(zhǎng)時(shí)，則在xrand與xnear之間的線段上，以xnear擴(kuò)展設(shè)定步長(zhǎng)的點(diǎn)xnew，將其加入到隨機(jī)展開(kāi)樹(shù)中，將與xnew最近的節(jié)點(diǎn)稱為xnew的父節(jié)點(diǎn)。重復(fù)上述迭代過(guò)程，直到目標(biāo)節(jié)點(diǎn)變?yōu)槿~節(jié)點(diǎn)或超過(guò)設(shè)定的迭代次數(shù)時(shí)則搜索結(jié)束。從目標(biāo)節(jié)點(diǎn)回溯到起點(diǎn)，即可獲得規(guī)劃的路徑。節(jié)點(diǎn)擴(kuò)展的原理如圖1 所示。

圖1 RRT算法節(jié)點(diǎn)擴(kuò)展原理圖

傳統(tǒng)RRT 算法偽代碼如下所示：

1.2 人工勢(shì)場(chǎng)法（APF）

APF 是一種局部路徑規(guī)劃算法[14-15]。假設(shè)機(jī)器人x在目標(biāo)引力分量和障礙物斥力分量的共同作用下運(yùn)動(dòng)。其總勢(shì)場(chǎng)U(x)可以表示為:

式中，Utarget(x)是目標(biāo)點(diǎn)作用于機(jī)器人x的引力勢(shì)場(chǎng)，Uobstacle(x)是障礙物對(duì)機(jī)器人的斥力勢(shì)場(chǎng)。

其勢(shì)場(chǎng)力F(x)可以表示為:

同樣地，在式（2）中，F(xiàn)target(x)是目標(biāo)點(diǎn)對(duì)于機(jī)器人x的引力作用力，F(xiàn)obstacle(x)是障礙物對(duì)機(jī)器人x的斥力作用力，而F(x)則是其合力。

但是人工勢(shì)場(chǎng)法在路徑規(guī)劃中存在以下的缺點(diǎn)：

1）當(dāng)運(yùn)動(dòng)空間障礙物較多時(shí)，路徑規(guī)劃的成功率會(huì)降低；

2）當(dāng)障礙物與目標(biāo)的距離小于一定閾值時(shí)，容易發(fā)生振蕩，使其無(wú)法到達(dá)目標(biāo)點(diǎn)；

3）當(dāng)F(x)所受合力在勢(shì)場(chǎng)中為0 時(shí)，將會(huì)陷入局部最小值，陷入局部振蕩階段。

2 改進(jìn)RRT路徑規(guī)劃算法

2.1 自適應(yīng)步長(zhǎng)目標(biāo)偏置

由于恒定步長(zhǎng)的設(shè)定，無(wú)效搜索路徑冗余繁多，使得算法的收斂速度慢；對(duì)狹窄區(qū)域或存在復(fù)雜障礙物區(qū)域的探索能力不足的情況，由此引入改進(jìn)的自適應(yīng)步長(zhǎng)策略。當(dāng)距離障礙物較遠(yuǎn)時(shí)，以大步長(zhǎng)擴(kuò)展，增加對(duì)所處未知環(huán)境的包容性，而當(dāng)其進(jìn)入障礙物影響域內(nèi)，為了增加其對(duì)復(fù)雜環(huán)境的探索能力，選擇以小步長(zhǎng)擴(kuò)展，如式（3）所示：

引入自適應(yīng)步長(zhǎng)來(lái)消除遠(yuǎn)離目標(biāo)區(qū)域的無(wú)用搜索區(qū)域，引導(dǎo)隨機(jī)樹(shù)可能向目標(biāo)點(diǎn)生長(zhǎng)，大大減少了基本RRT 算法的迭代次數(shù)和節(jié)點(diǎn)數(shù)。

特殊情況下，針對(duì)xnear位于目標(biāo)點(diǎn)鄰域λ內(nèi)時(shí)，步長(zhǎng)，以目標(biāo)點(diǎn)偏置導(dǎo)向，高效迭代。

2.2 引入引力場(chǎng)與斥力場(chǎng)

2.2.1 引入引力分量

在自適應(yīng)步長(zhǎng)目標(biāo)偏置算法中引入人工勢(shì)場(chǎng)的概念，增加隨機(jī)節(jié)點(diǎn)的重力分量，控制隨機(jī)樹(shù)向目標(biāo)點(diǎn)生長(zhǎng)，降低隨機(jī)性。原理圖如圖2 所示。其主要的核心思想是在每個(gè)新生成的節(jié)點(diǎn)中加入引力分量。

圖2 添加引力分量構(gòu)造

在構(gòu)造時(shí)，引力函數(shù)G(n)在每個(gè)新節(jié)點(diǎn)n中引入，以改變生長(zhǎng)方向的合力。它可以表示為:

式中，F(xiàn)(n)代表隨機(jī)樹(shù)在搜索過(guò)程中確定新節(jié)點(diǎn)的函數(shù)，Rd(n)代表最新節(jié)點(diǎn)的隨機(jī)生長(zhǎng)函數(shù)，G(n)代表添加的目標(biāo)引力函數(shù)。

xnew節(jié)點(diǎn)的隨機(jī)生長(zhǎng)函數(shù)如式（5）所示:

目標(biāo)引力函數(shù)G(n)如式（6）所示：

式（5）、（6）中，ρ是步長(zhǎng)，g為引力增益系數(shù)，xgoal為目標(biāo)位置矢量，|xgoal-xnear|表示節(jié)點(diǎn)間集合距離的絕對(duì)值。

2.2.2 引入斥力分量

在自適應(yīng)目標(biāo)偏置RRT 算法中引入APF 中障礙物斥力場(chǎng)的概念，引導(dǎo)局部隨機(jī)樹(shù)朝向遠(yuǎn)離障礙物生長(zhǎng)。RRT 算法增加斥力分量的展開(kāi)圖如圖3 所示。其主要的核心思想是對(duì)局部隨機(jī)樹(shù)搜索展開(kāi)過(guò)程中產(chǎn)生的每個(gè)新節(jié)點(diǎn)引入斥力分量。

圖3 添加斥力分量構(gòu)造

在構(gòu)造時(shí)，障礙物排斥函數(shù)T(n)，在生成的新節(jié)點(diǎn)n處引入，可以表示為：

式中，F(xiàn)(n)代表隨機(jī)樹(shù)在搜索過(guò)程中確定新節(jié)點(diǎn)n的函數(shù)，Rd(n)代表最新節(jié)點(diǎn)的隨機(jī)生長(zhǎng)函數(shù)，和T(n)是添加的障礙物斥力函數(shù)。

障礙物斥力函數(shù)T(n)如下：

式中，krep為斥力增益系數(shù)，p(x)為隨機(jī)節(jié)點(diǎn)到障礙物的最短距離，r0為節(jié)點(diǎn)上障礙物的影響范圍，xobstacle為障礙物的斥力矢量。

2.3 改進(jìn)人工勢(shì)場(chǎng)法

傳統(tǒng)的勢(shì)場(chǎng)為了充分利用勢(shì)場(chǎng)的優(yōu)勢(shì)，其斥力系數(shù)遠(yuǎn)遠(yuǎn)大于引力系數(shù)，會(huì)出現(xiàn)在目標(biāo)位置附近由于障礙物的斥力過(guò)大而無(wú)法到達(dá)目標(biāo)位置的問(wèn)題。針對(duì)上述問(wèn)題，如圖4 所示，利用多區(qū)域的劃分，重新構(gòu)造目標(biāo)斥力函數(shù)為：

式中，rsta為障礙物的安全距離（障礙物膨脹后）。

既要保障斥力勢(shì)場(chǎng)的效果明顯，又要兼顧附近目標(biāo)位置的需求，利用劃分定性，在R0(R0=2r0)內(nèi)的點(diǎn)所受斥力作用為無(wú)窮大，而當(dāng)其處于Rsta(Rsta=2rsta)臨界區(qū)時(shí)，斥力由斥力系數(shù)定性。

2.4 復(fù)雜障礙物環(huán)境下逃離局部最小值

機(jī)器人在整個(gè)勢(shì)場(chǎng)中的運(yùn)動(dòng)是隨機(jī)的，當(dāng)其處于復(fù)雜障礙物環(huán)境下時(shí)，由于引入勢(shì)場(chǎng)的概念，勢(shì)場(chǎng)引導(dǎo)會(huì)出現(xiàn)合勢(shì)場(chǎng)為0 的點(diǎn)，使得節(jié)點(diǎn)擴(kuò)展處于緩慢，振蕩循環(huán)的狀態(tài)；通過(guò)連續(xù)五次相鄰路徑節(jié)點(diǎn)間的角度變化均處于±5°誤差范圍內(nèi)，判定其處于局部振蕩階段；

由圖5 可知，處于復(fù)雜環(huán)境（三個(gè)近距離障礙物1，2，3）的勢(shì)場(chǎng)，具體實(shí)現(xiàn)：從點(diǎn)A擴(kuò)展到點(diǎn)I結(jié)束，當(dāng)計(jì)算第i個(gè)夾角時(shí)，需要利用i-1 和i+1 的節(jié)點(diǎn)坐標(biāo)，用余弦定理進(jìn)行角度計(jì)算；求出點(diǎn)A、B和點(diǎn)C之間的角度，以及點(diǎn)B、C和點(diǎn)D之間的角度，以此類推，其計(jì)算公式如式（10）所示：

為了引導(dǎo)其逃離局部最小值，該文引入局部定向剪枝概念，對(duì)路徑關(guān)鍵點(diǎn)進(jìn)一步擇優(yōu)剪枝選取。如圖5 所示，當(dāng)機(jī)器人從點(diǎn)A開(kāi)始陷入局部振蕩時(shí)，則連接點(diǎn)A與點(diǎn)B、C、D、E、F、G、H、I之間的線段進(jìn)行碰撞判定，直到完成路徑迭代：A→H→I。

2.5 B樣條曲線的擬合優(yōu)化

B 樣條是樣條曲線的一種特殊表現(xiàn)形式，同時(shí)也是貝塞爾曲線的一般形式，其具有凸包性、幾何不變性、局部支撐性、非負(fù)性等優(yōu)點(diǎn)[16]。n次B 樣條曲線表達(dá)式為：

式中，Pi為第i段控制點(diǎn)的方程，F(xiàn)i,n為n次B 樣條的基函數(shù)，其公式為：

而常見(jiàn)的B 樣條曲線的是二次和三次B 樣條曲線，因?yàn)槎蜝 樣條曲線同樣不需要經(jīng)過(guò)首尾的節(jié)點(diǎn)，所以B 樣條曲線基函數(shù)選擇為三次樣條曲線。

為了保證沿路徑運(yùn)動(dòng)時(shí)的速度平穩(wěn)性，減少突變處，對(duì)圖5 進(jìn)行三次樣條擬合優(yōu)化，從a到目標(biāo)點(diǎn)的平滑曲線如圖6 所示。

圖6 三次樣條曲線擬合優(yōu)化

3 仿真實(shí)驗(yàn)與結(jié)果分析

為了驗(yàn)證改進(jìn)后的算法能否在空間的起始點(diǎn)更高效地搜索目標(biāo)點(diǎn)并生成路徑，利用Matlab 搭建仿真環(huán)境，完成改進(jìn)RRT 算法的仿真實(shí)驗(yàn)。在該研究中，只考慮二維空間中的情況，模擬地圖的尺寸為1 000×1 000（無(wú)量綱）。為了方便比較，統(tǒng)一了各算法的仿真參數(shù)。具體參數(shù)如下：起始節(jié)點(diǎn)坐標(biāo)為(0,0)，目標(biāo)節(jié)點(diǎn)坐標(biāo)為(999,999)，起始點(diǎn)和目標(biāo)點(diǎn)分別用不同形狀標(biāo)記，擴(kuò)展步長(zhǎng)為30。

3.1 標(biāo)準(zhǔn)RRT 與自適應(yīng)步長(zhǎng)（AS-RRT）算法實(shí)驗(yàn)結(jié)果分析

標(biāo)準(zhǔn)RRT 與自適應(yīng)步長(zhǎng)(AS-RRT)算法實(shí)驗(yàn)結(jié)果分別如圖7、8 所示。

圖7 標(biāo)準(zhǔn)RRT實(shí)驗(yàn)結(jié)果圖

圖8 自適應(yīng)步長(zhǎng)目標(biāo)偏置RRT實(shí)驗(yàn)結(jié)果圖

從圖7-8 可以看出，標(biāo)準(zhǔn)RRT 算法在搜索過(guò)程中沒(méi)有自適應(yīng)步長(zhǎng)偏置引導(dǎo)，路徑是隨機(jī)生成的。而對(duì)于改進(jìn)的自適應(yīng)步長(zhǎng)偏置算法，引入大步長(zhǎng)加大在無(wú)障礙物影響的搜索區(qū)域的范圍，而引導(dǎo)隨機(jī)樹(shù)在多障礙物影響域引入小步長(zhǎng)增加環(huán)境遍歷包容性；大大減少了基本RRT 算法在多障礙物域內(nèi)搜索的迭代次數(shù)和擴(kuò)展的節(jié)點(diǎn)數(shù)。

3.2 APF-RRT和APF-ASRRT算法實(shí)驗(yàn)結(jié)果分析

APE-RRT 和APF-ASRRT 算法實(shí)驗(yàn)結(jié)果分別如圖9、10 所示。觀察兩圖發(fā)現(xiàn)，通過(guò)引入引力系數(shù)kp，其值為0.000 01，路徑可以更嚴(yán)格地限制在起始節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)之間的區(qū)域內(nèi)，與自適應(yīng)步長(zhǎng)RRT 算法相比，減小了搜索面積。由于障礙物斥力場(chǎng)的存在（斥力系數(shù)krep為200 000），對(duì)距離障礙物不同距離范圍內(nèi)的斥力定性，保障了障礙物附近有效點(diǎn)的搜索面積。因此，可以發(fā)現(xiàn)多障礙物之間的路徑幾乎是一條線，不僅使搜索路徑與障礙物保持適當(dāng)?shù)木嚯x，而且保證多障礙物環(huán)境定性區(qū)域搜索的完備性；但是由于勢(shì)場(chǎng)的缺陷，路徑會(huì)出現(xiàn)局部振蕩階段。

圖9 APF-RRT實(shí)驗(yàn)結(jié)果圖

圖10 APF-ASRRT實(shí)驗(yàn)結(jié)果圖

3.3 剪枝優(yōu)化和三次樣條曲線結(jié)果分析

APF-ASRRT 算法的剪枝優(yōu)化和三次樣條擬合實(shí)驗(yàn)結(jié)果分別如圖11、12 所示。

圖11 APF-ASRRT算法剪枝優(yōu)化實(shí)驗(yàn)結(jié)果圖

在圖11-12 兩幅圖中，從圖11 看出針對(duì)局部振蕩階段的震蕩缺陷，剪枝擇優(yōu)選取路徑關(guān)鍵點(diǎn)，綠色表示算法在二維空間中剪枝后的路徑。圖12 中黑色路徑即是該三次樣條曲線擬合出最優(yōu)可行路徑。從這兩幅圖中可以看出，剪枝優(yōu)化與曲線擬合路徑保障環(huán)境的實(shí)施可行性。

圖12 APF-ASRRT算法三次樣條擬合實(shí)驗(yàn)結(jié)果圖

3.4 復(fù)雜環(huán)境RRT算法和改進(jìn)RRT算法實(shí)驗(yàn)結(jié)果分析

復(fù)雜環(huán)境標(biāo)準(zhǔn)RRT 算法和改進(jìn)RRT 算法實(shí)驗(yàn)結(jié)果分別如圖13、14 所示。

圖13 復(fù)雜環(huán)境標(biāo)準(zhǔn)RRT實(shí)驗(yàn)結(jié)果圖

為了增加對(duì)復(fù)雜障礙物環(huán)境的對(duì)比，將算法部署到多障礙物復(fù)雜環(huán)境，從圖13 和圖14 中可以看出，該算法仍能保證對(duì)路徑的搜索穩(wěn)定性。

圖14 復(fù)雜環(huán)境改進(jìn)RRT結(jié)果圖

3.5 RRT算法改進(jìn)實(shí)驗(yàn)系列對(duì)比分析

研究中，改進(jìn)的AS-RRT 算法、改進(jìn)的APF RRT算法和基本算法在相同系數(shù)的前提下，對(duì)50 次仿真的迭代次數(shù)、搜索時(shí)間和路徑長(zhǎng)度進(jìn)行了對(duì)比，計(jì)算模擬實(shí)際的平均搜索時(shí)間、平均路徑長(zhǎng)度和平均迭代次數(shù)，如表1 所示。

表1 仿真實(shí)驗(yàn)結(jié)果對(duì)比

從表1 中可以看出，AS-RRT 算法與標(biāo)準(zhǔn)RRT 算法相比，路徑搜索時(shí)間減少了10.16%，路徑長(zhǎng)度減少了9.15%。而改進(jìn)的APF-RRT 算法進(jìn)一步減少了52.1%的迭代次數(shù)和92.1%的計(jì)算時(shí)間。在路徑長(zhǎng)度上，由于斥力場(chǎng)的限制，APF-RRT 改進(jìn)算法的路徑長(zhǎng)度并沒(méi)有減少。在多障礙物環(huán)境下，由改進(jìn)的APF-ASRRT 算法結(jié)果可以看出，該算法在保證了對(duì)同一路徑搜索的同時(shí)，搜索時(shí)間和迭代次數(shù)分別顯著減少68.5%和20%。

4 結(jié)論

該文研究基于RRT 算法結(jié)合人工勢(shì)場(chǎng)的概念改進(jìn)算法，提出一種多障礙物路徑尋優(yōu)的策略。仿真實(shí)驗(yàn)證明，該混合算法明顯改善了基本RRT 算法的不足。通過(guò)引入自適應(yīng)步長(zhǎng)偏置策略和人工勢(shì)場(chǎng)概念，有效地減少了路徑搜索過(guò)程中的迭代次數(shù)和運(yùn)行時(shí)間，減少了路徑的隨機(jī)性和偏差，并且對(duì)初步規(guī)劃出的路徑進(jìn)行剪枝擇優(yōu)和三次樣條B 曲線平滑處理。該算法對(duì)RRT 進(jìn)行多處改進(jìn)，對(duì)于多障礙物環(huán)境的路徑規(guī)劃，具有一定的參考意義。

雖然在對(duì)多障礙物的表面做規(guī)則量化處理，但在實(shí)際的環(huán)境中，往往是不規(guī)則的多邊形，因此未來(lái)的研究可針對(duì)不規(guī)則的障礙物進(jìn)行研究。