王道威，朱明富，劉 慧

(華中科技大學(xué) 自動(dòng)化學(xué)院，湖北 武漢 430074)

動(dòng)態(tài)步長(zhǎng)的RRT路徑規(guī)劃算法

王道威，朱明富，劉 慧

(華中科技大學(xué) 自動(dòng)化學(xué)院，湖北 武漢 430074)

傳統(tǒng)的快速擴(kuò)展隨機(jī)樹(shù)(RRT)算法雖然有很多優(yōu)良特性，但是由于擴(kuò)展點(diǎn)的隨機(jī)選取，規(guī)劃出來(lái)的路徑具有很大的隨機(jī)性。文中在對(duì)RRT算法改進(jìn)的基礎(chǔ)上，提出了一種動(dòng)態(tài)步長(zhǎng)的RRT路徑規(guī)劃算法。其中步長(zhǎng)為RRT生長(zhǎng)的最小單位長(zhǎng)度。動(dòng)態(tài)步長(zhǎng)的RRT算法是在對(duì)傳統(tǒng)RRT算法的基礎(chǔ)上，添加了動(dòng)態(tài)步長(zhǎng)的特性，改善了快速擴(kuò)展隨機(jī)樹(shù)的不確定性，提高了避障能力，使得算法確定性和高避障能力兼?zhèn)洹７抡鎸?shí)驗(yàn)結(jié)果表明，該算法在路徑規(guī)劃中具有路徑確定、速度快和高避障能力的特點(diǎn)。

快速擴(kuò)展隨機(jī)樹(shù)；動(dòng)態(tài)步長(zhǎng)；路徑確定性；高避障能力

0 引 言

隨著無(wú)人機(jī)、無(wú)人汽車(chē)等無(wú)人智能設(shè)備的不斷應(yīng)用和發(fā)展，RRT路徑規(guī)劃算法[1-3]越來(lái)越多地被應(yīng)用。RRT算法是一種隨機(jī)搜索算法，它能在保證系統(tǒng)實(shí)時(shí)性的前提下，尋找到問(wèn)題的較優(yōu)解。但是RRT算法具有隨機(jī)性[4]，規(guī)劃出來(lái)的路徑偏離目標(biāo)點(diǎn)，與理想路徑相比有較大差異。

文中在經(jīng)典RRT算法的基礎(chǔ)上引入目標(biāo)引力函數(shù)，讓隨機(jī)樹(shù)朝著目標(biāo)點(diǎn)方向生長(zhǎng)，使得規(guī)劃出來(lái)的路徑更加接近理想路徑。但是這種添加了目標(biāo)引力函數(shù)的RRT算法雖然降低了路徑的隨機(jī)性,但同時(shí)也降低了算法的避障能力，起始點(diǎn)和目標(biāo)點(diǎn)障礙物比較多的情況下不能規(guī)劃出路徑。文中在RRT算法的基礎(chǔ)上引入了動(dòng)態(tài)步長(zhǎng)的概念。步長(zhǎng)即為RRT生長(zhǎng)的最小單位長(zhǎng)度。在RRT算法擴(kuò)展新節(jié)點(diǎn)時(shí)，當(dāng)遇到障礙物時(shí)自動(dòng)減小目標(biāo)點(diǎn)方向的步長(zhǎng)，沒(méi)遇到障礙物時(shí)再恢復(fù)原步長(zhǎng)。改進(jìn)后的RRT路徑規(guī)劃算法兼具路徑確定性和高避障能力的優(yōu)點(diǎn)。

1 RRT算法

快速隨機(jī)擴(kuò)展樹(shù)(RRT)算法由Steven M. LaValle于1998年在文獻(xiàn)[5]中首先提出。RRT算法是從空間中一個(gè)起始節(jié)點(diǎn)出發(fā)，通過(guò)隨機(jī)采樣，不斷增加新節(jié)點(diǎn)，生成一個(gè)隨機(jī)擴(kuò)展樹(shù)。當(dāng)增加的新節(jié)點(diǎn)中有目標(biāo)節(jié)點(diǎn)時(shí)，起始節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)就會(huì)至少有一條通路，稱為路徑。RRT算法已應(yīng)用于飛行器運(yùn)動(dòng)規(guī)劃[6]和移動(dòng)機(jī)器人路徑規(guī)劃中[7]。

1.1 經(jīng)典RRT算法

RRT算法的作用是在度量空間X中，選擇一條從初始狀態(tài)點(diǎn)xinit到目標(biāo)狀態(tài)點(diǎn)xgoal或者到目標(biāo)狀態(tài)點(diǎn)區(qū)域xgoal∈X的路徑。通常選擇距離空間C作為度量空間，即X=C。在度量空間中路徑無(wú)法通過(guò)的區(qū)域稱為障礙區(qū)域，記為xobs∈X。障礙區(qū)域在度量空間中的補(bǔ)集稱為自由區(qū)域，記為xfree∈X。令xrand為每次擴(kuò)展RRT時(shí)在自由區(qū)域中隨機(jī)選取的點(diǎn)，xrand∈xfree。令xnear為每次擴(kuò)展RRT時(shí)在當(dāng)前RRT中與xrand距離最近的點(diǎn)。xnew為每次新增加的擴(kuò)展點(diǎn)。給定一個(gè)初始狀態(tài)xinit，一個(gè)隨機(jī)擴(kuò)展樹(shù)T，生成K個(gè)頂點(diǎn)的方法如下：

GENERATE_RRT(xinit,K,△t)

T.init(xinit);

Fork=1toKdo

xrand←RANDOM_STATE();

xnear←NEAREST_STATE(xrand,T);

u←SELECT_INPUT(xrand,xnear);

xnew←NEW_STATE(xnear,u,△t);

T.add_vertex(xnew);

T.add_edge(xnear,xnew,u);

ReturnT

其中,RANDOM_STATE()在自由空間中隨機(jī)選取一點(diǎn)xrand，NEAREST_STATE()在當(dāng)前RRT中選擇與xrand距離最近的點(diǎn)xnear，SELECT_INPUT()結(jié)合x(chóng)rand和xnear得到輸入u，NEW_STATE()得到新節(jié)xnew。

1.2 引入目標(biāo)引力思想的RRT算法

在文獻(xiàn)[1]中介紹了一種引入目標(biāo)引力思想的RRT算法，能有效減少RRT算法的隨機(jī)性，讓路徑朝著目標(biāo)方向擴(kuò)展。

在文獻(xiàn)[2]中介紹了一種目標(biāo)偏好RRT算法，該算法大部分?jǐn)U展過(guò)程使用隨機(jī)點(diǎn)，以一定小概率選取目標(biāo)點(diǎn)取代隨機(jī)點(diǎn)，將隨機(jī)樹(shù)向目標(biāo)區(qū)域擴(kuò)展。

文獻(xiàn)[3]中對(duì)目標(biāo)偏好RRT算法做了進(jìn)一步改進(jìn)，使它應(yīng)用于非完整微分約束中。

經(jīng)典的RRT算法隨機(jī)性比較明顯，規(guī)劃出來(lái)的路徑非常曲折，不適合實(shí)際使用中的路徑規(guī)劃。經(jīng)典RRT算法新節(jié)點(diǎn)位置的計(jì)算公式如式(1)所示。

(1)

其中,ρ為RRT生長(zhǎng)的最小單位長(zhǎng)度，稱為步長(zhǎng)。

加入目標(biāo)引力思想的RRT算法新節(jié)點(diǎn)位置的計(jì)算公式如式(2)所示。

(2)

其中:ρ1為隨機(jī)點(diǎn)方向的步長(zhǎng);ρ2為目標(biāo)點(diǎn)方向的步長(zhǎng)。

2 動(dòng)態(tài)步長(zhǎng)的RRT算法

路徑規(guī)劃是按照某一性能指標(biāo)搜索一條從起始狀態(tài)到目標(biāo)狀態(tài)的最優(yōu)或近似最優(yōu)的無(wú)碰路徑[8]。動(dòng)態(tài)步長(zhǎng)的RRT算法能夠規(guī)劃出比較理想的無(wú)碰路徑。

2.1 步長(zhǎng)對(duì)RRT算法的影響

首先考慮步長(zhǎng)在經(jīng)典RRT路徑規(guī)劃算法中的作用。經(jīng)典隨機(jī)擴(kuò)展樹(shù)生成算法擴(kuò)展一個(gè)新節(jié)點(diǎn)的過(guò)程為：首先在度量空間中隨機(jī)選取一點(diǎn)xrand，然后在當(dāng)前擴(kuò)展樹(shù)中(可能為空)找到與xrand距離最近的點(diǎn)xnear，在xnear到xrand的方向上擴(kuò)展一個(gè)步長(zhǎng)的距離得到新節(jié)點(diǎn)xnew加入擴(kuò)展樹(shù)中。

加入目標(biāo)引力思想后的RRT算法能有效解決路徑隨機(jī)性強(qiáng)的問(wèn)題。引入目標(biāo)引力思想后的RRT算法與經(jīng)典RRT算法相比添加了xgoal方向的分量。加入目標(biāo)引力思想后，步長(zhǎng)的選擇對(duì)新節(jié)點(diǎn)的位置會(huì)有影響。此時(shí)新節(jié)點(diǎn)不再在xnear到xrand的連線上了。當(dāng)ρ2>ρ1時(shí)，新節(jié)點(diǎn)偏向于xgoal，由于xgoal的位置通常是固定的，擴(kuò)展樹(shù)朝著xgoal方向擴(kuò)展。當(dāng)ρ2<ρ1時(shí)，新節(jié)點(diǎn)偏向于xrand，由于xrand的位置是隨機(jī)選取的，此時(shí)擴(kuò)展樹(shù)的擴(kuò)展方向接近RRT經(jīng)典算法。

引入目標(biāo)引力思想后的隨機(jī)擴(kuò)展樹(shù)朝著目標(biāo)點(diǎn)方向發(fā)展，規(guī)劃出來(lái)的路徑更加接近理想路徑。ρ2相對(duì)于ρ1取的越大，擴(kuò)展樹(shù)越朝著xgoal方向擴(kuò)展，規(guī)劃出來(lái)的路徑越接近理想路徑。

以上所討論的都在無(wú)障礙空間中規(guī)劃路徑，在實(shí)際的路徑規(guī)劃應(yīng)用中，起始點(diǎn)和目標(biāo)點(diǎn)之間都或多或少存在很多障礙，很難找到一個(gè)有效的路徑規(guī)劃方法來(lái)應(yīng)對(duì)各種復(fù)雜環(huán)境[9]。規(guī)劃出的路徑需要避開(kāi)所有障礙到達(dá)目標(biāo)點(diǎn)。經(jīng)典RRT算法新節(jié)點(diǎn)的添加都是隨機(jī)方向的，能夠有效地避開(kāi)障礙。引入目標(biāo)引力的RRT算法常常為了能夠使規(guī)劃出的路徑接近理想路徑，目標(biāo)點(diǎn)方向的步長(zhǎng)要較大于隨機(jī)點(diǎn)方向的步長(zhǎng)。此時(shí)如果起始點(diǎn)和目標(biāo)點(diǎn)障礙物較多，那么擴(kuò)展樹(shù)無(wú)法繞開(kāi)障礙物到達(dá)目標(biāo)點(diǎn)。

可以得出結(jié)論：路徑理想和避障能力是相互矛盾的。為了解決這對(duì)矛盾，在目標(biāo)引力RRT算法基礎(chǔ)上添加了動(dòng)態(tài)步長(zhǎng)的概念。當(dāng)沒(méi)有遇到障礙物時(shí)取ρ2>ρ1，使擴(kuò)展樹(shù)盡可能朝著目標(biāo)點(diǎn)方向擴(kuò)展。當(dāng)遇到障礙物時(shí)取ρ2<ρ1，使擴(kuò)展樹(shù)朝著隨機(jī)點(diǎn)方向擴(kuò)展，因?yàn)殡S機(jī)點(diǎn)的隨機(jī)性，新節(jié)點(diǎn)也具有隨機(jī)性，從而可以高效地避開(kāi)障礙物。

2.2 動(dòng)態(tài)步長(zhǎng)RRT算法的優(yōu)點(diǎn)

經(jīng)典RRT算法的隨機(jī)性很強(qiáng)，規(guī)劃出來(lái)的路徑與理想路徑相差很大，但由于擴(kuò)展節(jié)點(diǎn)的隨機(jī)性，經(jīng)典RRT算法具有很強(qiáng)的避障能力。

為了規(guī)劃出更加理想的路徑，在經(jīng)典RRT算法中引入目標(biāo)引力思想，讓新節(jié)點(diǎn)朝著目標(biāo)節(jié)點(diǎn)的方向擴(kuò)展。但是這種算法的避障能力很弱，當(dāng)起始點(diǎn)和目標(biāo)點(diǎn)間有很多障礙的時(shí)候不能規(guī)劃出路徑。

動(dòng)態(tài)步長(zhǎng)RRT算法具有以上兩種算法的優(yōu)點(diǎn)，不僅能朝著目標(biāo)節(jié)點(diǎn)擴(kuò)展新節(jié)點(diǎn)，而且具有和經(jīng)典RRT算法一樣的避障能力。動(dòng)態(tài)步長(zhǎng)RRT算法在路徑規(guī)劃中更加實(shí)用。和經(jīng)典RRT算法一樣，該算法具有高效的搜索特性，使其適合解決高維空間多自由度的復(fù)雜約束下的運(yùn)動(dòng)規(guī)劃問(wèn)題，能直接應(yīng)用到非完整性約束或非完整性動(dòng)力學(xué)約束規(guī)劃中[10]。

動(dòng)態(tài)步長(zhǎng)RRT算法時(shí)間復(fù)雜度低。在文獻(xiàn)[11-13]中介紹的滾動(dòng)規(guī)劃算法，每次在一個(gè)窗口中規(guī)劃出最優(yōu)路徑，時(shí)間復(fù)雜度很高。動(dòng)態(tài)步長(zhǎng)RRT算法在擴(kuò)展每一個(gè)新節(jié)點(diǎn)時(shí)，只需計(jì)算一個(gè)式(2)的值。很多類似文獻(xiàn)[4]中提出的RRT改進(jìn)算法，在擴(kuò)展一個(gè)新節(jié)點(diǎn)時(shí)計(jì)算若干個(gè)隨機(jī)點(diǎn)的估價(jià)函數(shù)，選取估價(jià)函數(shù)值最小的作為新節(jié)點(diǎn)。這些做法使算法時(shí)間復(fù)雜度成倍增加。因?yàn)镽RT擴(kuò)展樹(shù)一般都具有非常多的節(jié)點(diǎn)，所以保持算法的較低的時(shí)間復(fù)雜度非常重要。

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

實(shí)驗(yàn)環(huán)境：Windows Embedded 8.1, Intel? CoreTMi5-3317U CPU @ 1.70 GHz, 內(nèi)存8 G。

編譯工具：Visual Studio 2013。

圖1～5中左下角為起始節(jié)點(diǎn)，右上角為目標(biāo)節(jié)點(diǎn)。圖中空心圓環(huán)為障礙物，距障礙物環(huán)心40個(gè)單位長(zhǎng)度內(nèi)的區(qū)域?yàn)檎系K區(qū)域。圖中黑色粗線和黑色細(xì)線共同構(gòu)成隨機(jī)擴(kuò)展樹(shù)，黑色粗線連接起始點(diǎn)和目標(biāo)點(diǎn)，被選為規(guī)劃出的路徑。

圖1 經(jīng)典RRT算法(無(wú)障礙)

圖2 經(jīng)典RRT算法(有障礙)

在無(wú)障礙的情況下，如圖1、3、4所示，都能規(guī)劃出路徑。由圖1可見(jiàn)，經(jīng)典RRT算法的隨機(jī)性較強(qiáng)，路徑比較曲折。由圖3、4可見(jiàn)，目標(biāo)引力RRT算法和動(dòng)態(tài)步長(zhǎng)RRT算法在無(wú)障礙情況下效果相同，規(guī)劃出的路徑比較接近理想路徑。

圖3 目標(biāo)引力RRT算法(無(wú)障礙)

圖4 動(dòng)態(tài)步長(zhǎng)RRT(無(wú)障礙)

圖5 動(dòng)態(tài)步長(zhǎng)RRT(有障礙)

在有障礙的情況下，如圖2、5所示，目標(biāo)引力RRT算法無(wú)法規(guī)劃出路徑，其他兩種算法都能規(guī)劃出路徑。由圖2可見(jiàn)，經(jīng)典RRT算法能有效避開(kāi)障礙物，但是隨機(jī)性較強(qiáng)。目標(biāo)引力RRT算法在有障礙的情況下，由于起始點(diǎn)和目標(biāo)點(diǎn)障礙物過(guò)多，隨機(jī)樹(shù)無(wú)法繞開(kāi)障礙物到達(dá)目標(biāo)節(jié)點(diǎn)，導(dǎo)致運(yùn)行程序無(wú)響應(yīng)。由圖5可見(jiàn)，動(dòng)態(tài)步長(zhǎng)RRT算法能有效避開(kāi)障礙物，而且沒(méi)有經(jīng)典RRT算法的隨機(jī)性。

4 結(jié)束語(yǔ)

經(jīng)典RRT算法具有隨機(jī)搜索特性[14]，因此具有很強(qiáng)的避障能力，而且生成的路徑不夠光滑，與理想路徑相差很遠(yuǎn)。目標(biāo)引力思想RRT算法犧牲了部分隨機(jī)性，規(guī)劃出的路徑更加光滑，但是減弱了避障能力，很多情況下無(wú)法規(guī)劃出路徑。動(dòng)態(tài)步長(zhǎng)的RRT算法不僅能夠規(guī)劃出接近理想路徑的路徑，而且具有很強(qiáng)的避障能力，算法時(shí)間復(fù)雜度低，能夠高效廣泛地應(yīng)用在實(shí)際路徑規(guī)劃中。

[1] 郝利波，侯媛彬.基于一種改進(jìn)RRT算法的足球機(jī)器人路徑規(guī)劃[J].西安科技大學(xué)學(xué)報(bào)，2011，31(1)：81-85.

[2] Urmson C,Simmons R.Approaches for heuristically biasingRRTgrowth[C]//ProcofIROS.[s.l.]:[s.n.],2003:1178-1183.

[3] 徐 娜，陳 雄，孔慶生,等.非完整約束下的機(jī)器人運(yùn)動(dòng)規(guī)劃算法[J].機(jī)器人,2011，33(6)：666-672.

[4] 王 濱，金明河，謝宗武,等.基于啟發(fā)式的快速擴(kuò)展隨機(jī)樹(shù)路徑規(guī)劃算法[J].機(jī)械制造，2007，45(12)：1-4.

[5]LaValleSM.Rapidly-exploringrandomtrees:anewtoolforpathplanning[R].Ames:IowaStateUniversity,1998.

[6]AminJN,BoskovicJD,MehraRK.Afastandefficientapproachtopathplanningforunmannedvehicles[C]//ProcofAIAAguidance,navigation,andcontrolconference.Keystone,Colorado:AIAA,2006:1-9.

[7] 樊曉平，李雙艷.帶滾動(dòng)約束輪移式機(jī)器人動(dòng)態(tài)規(guī)劃的研究[J].控制與決策，2005，20(7)：786-788.

[8] 李 磊，葉 濤，譚 民，等.移動(dòng)機(jī)器人技術(shù)研究現(xiàn)狀與未來(lái)[J].機(jī)器人，2002，24(5)：475-480.

[9] 宋金澤，戴 斌，單恩忠，等.一種改進(jìn)的RRT路徑規(guī)劃算法[J].電子學(xué)報(bào)，2010，38(2A)：225-228.

[10]JouandeauN.Rapidly-exploringsortedrandomtree:aselfadaptiverandommotionplanningalgorithm[J].LectureNotesinElectricalEngineering,2009,24(5):63-73.

[11] 席裕庚.動(dòng)態(tài)不確定環(huán)境下廣義控制問(wèn)題的預(yù)測(cè)控制[J].控制理論與應(yīng)用,2007，17(5)：665-670.

[12] 趙春霞，唐振民，陸建峰，等.面向自主車(chē)輛的局部路徑規(guī)劃仿真系統(tǒng)[J].南京理工大學(xué)學(xué)報(bào):自然科學(xué)版，2002,26(6):570-574.

[13] 張純剛，席裕庚.全局環(huán)境未知時(shí)基于滾動(dòng)窗口的機(jī)器人路徑規(guī)劃[J].中國(guó)科學(xué):E輯，2001,31(1)：51-58.

[14] 康 亮，趙春霞，郭劍輝.未知環(huán)境下改進(jìn)的基于RRT算法的移動(dòng)機(jī)器人路徑規(guī)劃[J].模式識(shí)別與人工智能，2009，22(3)：337-343.

Rapidly-exploring Random Tree Algorithm Based on Dynamic Step

WANG Dao-wei，ZHU Ming-fu，LIU Hui

(School of Automation,Huazhong University of Science and Technology,Wuhan 430074,China)

Although the traditional Rapidly-exploring Random Tree (RRT) algorithm has many good features,there is a lot of randomness in path planning of RRT because of the random selection of the vertex.Based on the improvement of RRT algorithm,a new RRT path planning algorithm of dynamic step size is proposed in this paper.The step size is the minimum unit length when RRT exploring.Based on traditional RRT,the dynamic step size is added,avoiding the uncertainty,and the obstacle avoidance capability is improved,thus the path planning of RRT algorithm has both obstacle avoidance ability and high certainty.The results of simulation experiments show that the algorithm has the features of avoiding the uncertainty,fast speed and obstacle avoidance in path planning.

RRT;dynamic step size;path planning certainty;obstacle avoidance

2015-06-30

2015-09-30

時(shí)間：2016-02-18

國(guó)家自然科學(xué)基金資助項(xiàng)目(61403154)

王道威(1989-)，男，碩士研究生，研究方向?yàn)槎嘀悄荏w控制、圖像處理。

http://www.cnki.net/kcms/detail/61.1450.TP.20160218.1638.086.html

TP301.6

A

1673-629X(2016)03-0105-03

10.3969/j.issn.1673-629X.2016.03.025