一種基于RRT-ConCon改進的路徑規劃算法

王 凡， 馮 楠， 胡 小 鵬

( 1.大連理工大學 電子信息與電氣工程學部, 遼寧 大連 116024；2.中國人民解放軍65066部隊， 遼寧 大連 116100 )

?

一種基于RRT-ConCon改進的路徑規劃算法

王 凡*1， 馮 楠1,2， 胡 小 鵬1

( 1.大連理工大學 電子信息與電氣工程學部, 遼寧 大連 116024；2.中國人民解放軍65066部隊， 遼寧 大連 116100 )

針對RRT算法缺乏穩定性和收斂速度慢的問題，基于RRT-ConCon算法和朝向目標搜索的策略，提出了一種改進的雙向搜索路徑規劃算法．該算法通過改變兩條搜索路徑的臨時擴展目標點，使搜索路徑不僅易于朝著目標點方向生長，而且提高了算法的穩定性，同時可以保證規劃的路徑接近最優解．改進的RRT-ConCon算法利用隨機節點生成函數，使朝著目標點生長的搜索路徑避免陷入局部極小值．同時，為了測試各種仿真實驗環境，還設計了一種仿真實驗環境平臺，實驗結果驗證了本算法的有效性和穩定性．

移動機器人；路徑規劃；快速擴展隨機樹(RRT)；雙向搜索樹(Bi-RRT)；RRT-ConCon算法

0 引 言

近年來移動機器人的研究和開發越來越受到各位學者的高度重視．在移動機器人相關技術的研究中，路徑規劃問題是其中的關鍵課題之一．所謂路徑規劃是指移動機器人按照某一性能指標(如距離、時間、能量等)搜索一條從起始狀態到目標狀態的最優或次優路徑[1]．傳統的路徑規劃算法主要有人工勢場算法、模糊規則算法、遺傳算法、人工神經網絡算法、模擬退火算法、蟻群優化算法、粒子群算法等[2-4]，這些路徑規劃算法在處理普通的路徑規劃問題時有一定的優越性，但是當機器人具有高自由度和工作環境更加復雜時，這些算法的計算復雜度會隨之提高，因此降低了求解速率[5-6]．

LaValle等提出了快速擴展隨機樹(rapidly- exploring random trees，RRT)算法[7-8]．由于該算法不需要對空間建模，而且搜索速度快，適合解決高維空間和復雜環境中機器人的路徑規劃問題[7-9]．近年來，RRT算法作為一種基于隨機采樣的單一查詢路徑規劃算法得到了廣泛的研究和應用，但是其隨機性導致其只能概率完備[8]，其本身包含一些缺點：(1)不穩定性，即對同一任務重復規劃時，會產生不同的路徑；(2)偏差性，即規劃出的路徑常常不是最優路徑或次優路徑；(3)無導向性，即搜索樹無任何偏向于目標的導向，收斂速度可能是緩慢的．

針對基本RRT算法的不足，國內外學者不斷對該算法進行改進以適應應用環境．如：為了提高搜索路徑的穩定性，一些學者提出了ERRT、DRRT、MP-RRT算法等[10-14]；為了提高搜索效率，一些學者提出了偏向搜索樹、雙向搜索樹(bidirectional rapidly-exploring random trees，Bi-RRT)算法等，以及其他的相關改進算法[15-19]．本文基于RRT-ConCon算法和朝向目標搜索策略，提出一種改進的雙向搜索路徑規劃算法，以保證規劃路徑接近最優路徑．

1 RRT-ConCon算法

1.1 基本的雙向搜索樹(Bi-RRT)算法

基本的雙向搜索樹(Bi-RRT)算法[20]的主要思想是：從初始點和目標點出發，并行構建兩棵搜索樹；在每次迭代過程中，兩棵搜索樹總是彼此朝著對方擴展，直至兩棵搜索樹相遇為止．基本的Bi-RRT構建過程如下：在每次迭代中，先擴展其中一棵搜索樹，然后嘗試將另一棵搜索樹擴展到當前搜索樹擴展的新節點；兩棵搜索樹Tinit和Tgoal交替擴展，直至兩棵搜索樹相遇為止．基本的Bi-RRT算法的偽代碼如下：

Algorithm1Bidirectional RRT Algorithm

Input:

T1: first RRT

T2: second RRT

l: number of attempts allowed to connectT1andT2

Output:

connected if the two RRTs are connected to each other; failure otherwise

1. fori=1 toldo

2.qrand← a randomly chosen free configuration

3.qnew,1← Extend RRT(T1,qrand)

4. ifqnew,1≠NIL then

5.qnew,2← Extend RRT(T2,qnew,1)

6. ifqnew,1=qnew,2then

7. returnPATH(T1,T2)

8. end if

9.SWAP(T1,T2)

10. end if

11. end for

12. return failure

Algorithm2Extend RRT Algorithm

Input:

T=(V,E): an RRT

q: a configuration toward which the treeTis grown

Output:

A new configurationqnewtowardq, or NIL in case of failure

1.qnear← closest neighbor ofqinT

2.qnew← progressqnearby step_size along the straight line betweenqnearandqrand

3. ifqnewis collision-free then

4.V←V∪{qnew}

5.E←E∪{(qnear,qnew)}

6. returnqnew

7. end if

8. return NIL

1.2 RRT-ConCon算法描述

基本的Bi-RRT算法比單棵RRT算法能產生更好的收斂性．為了進一步提高搜索效率，Kuffner和LaValle提出了Connect算法[21]，使得每次節點擴展操作更積極．Connect算法的偽代碼如下：

Algorithm3Connect Algorithm

Input:

T=(V,E): an RRT

q: a configuration toward which the treeTis grown

Output:

Connected ifqis connected toT; failure otherwise

1. repeat

2.S← Extend RRT(T,q)

3. until not (S=qnew)

4. returnS

分析可知，Connect算法是一種“貪婪算法”，通過迭代擴展，使當前節點一直朝著臨時目標點擴展．同時，Connect算法也是一種改進的擴展節點函數，它是通過重復調用擴展節點函數Extend來實現的．

如果將基本的Bi-RRT算法(Algorithm 1)視為RRT-ExtExt算法，那么通過用Connect函數替換Extend 函數衍生出RRT-ExtCon、RRT-ConCon 等多種算法．用Connect函數替換RRT-ExtExt算法中的第1個Extend函數，目的是使節點更積極地擴展到狀態空間．用Connect函數替換RRT-ExtExt算法中的第2個Extend函數，目的是在每次迭代中，算法能夠積極地嘗試連接兩棵搜索樹．RRT-ConCon算法就是將RRT-ExtExt 算法中的兩個Extend函數都用Connect函數替換．與其他變形的多種規劃算法相比，RRT-ConCon算法具有較高的規劃效率．圖1為RRT-ConCon構建過程，圖中，搜索樹Tgoal利用Connect函數擴展到隨機采樣點qtarget，同時將該點作為另一棵搜索樹Tinit擴展的子目標點；在搜索樹Tinit上找到離子目標點qtarget最近的節點qnear，然后通過Connect函數找到新的節點qnew添加到搜索樹S(xS,yS)上．兩棵搜索樹按照交換原則擴展．

圖1 RRT-ConCon構建過程

2 一種基于RRT-ConCon改進的路徑規劃算法

2.1 改進的路徑規劃算法描述

RRT-ConCon算法雖然比基本的Bi-RRT算法的收斂性能更好，但該算法生成的路徑仍具有隨機性和不穩定性．綜上，本文提出一種基于RRT-ConCon算法和朝向目標搜索的改進路徑規劃算法．改進的RRT-ConCon構建過程如圖2所示，虛線箭頭方向表示當前節點的擴展方向．改進的RRT-ConCon算法與原算法相比有兩處不同：一是在改進的算法中，搜索路徑Tinit的臨時目標點一直是終點qgoal，保證了算法的穩定性，搜索樹Tgoal的臨時目標點是另一條搜索路徑Tinit擴展得到的新節點；二是在改進的算法中，引入了隨機節點生成函數．

圖2 改進的RRT-ConCon構建過程

如果S(xS,yS)表示移動機器人初始點，G(xG,yG)表示移動機器人目標點，那么本文所提出的改進路徑規劃算法具體步驟如下：

Step1對搜索路徑PATH初始化，PATH開始只包含初始點S和目標點G．

Step2如果初始點S和目標點G的距離小于一個給定閾值T，則認為搜索路徑到達了目標點，返回搜索路徑PATH={(S,G)}；否則進入Step4．

Step3如果T1與T2相遇，則返回搜索路徑PATH={T1,T2}．

Step4對于搜索樹T1，調用擴展節點函數，朝著目標點方向擴展搜索樹，直至搜索路徑遇到障礙物或T1與T2相遇為止；將擴展節點函數的返回節點添加到搜索樹T1中．

Step5當T1遇到障礙物時，調用隨機節點生成函數，生成一個隨機節點，將該節點添加到搜索樹T1中．

Step6對于搜索樹T2，調用擴展節點函數，朝著T1擴展得到的新節點擴展，直至搜索路徑遇到障礙物或T1與T2相遇為止；將擴展節點函數的返回節點添加到搜索樹T2中．

Step7當T2遇到障礙物時，調用隨機節點生成函數，生成一個隨機節點，將該節點添加到搜索樹T2中．

Step8返回到Step3．

為了使該算法可控，在Step8中加入循環次數上限．如果在限制次數內搜索樹無法到達目標點或目標區域，則算法返回失敗．改進的RRT-ConCon算法的偽代碼如下：

Algorithm4Improved RRT-ConCon Algorithm

Input:

T1: first RRT

T2: second RRT

l: number of attempts allowed to connectT1andT2

Output:

Connected if the two RRTs are connected to each other; failure otherwise

1. fori=1 toldo

2.qnew,1← Connect (T1,G)

3.qnew,1← RandomNode (qnew,1,ρ)

4. ifqnew,1≠NIL then

5.qnew,2← Connect (T2,qnew,1)

6. ifqnew,1=qnew,2then

7. returnPATH(T1,T2)

8. elseqnew,2← RandomNode (qnew,2,ρ)

9. end if

10. end if

11. end for

12. return failure

2.2 改進的Connect函數

RRT-ConCon中調用擴展函數Connect (T,q)，其中T是搜索樹Tinit或搜索樹Tgoal，q是自由空間中選擇的臨時目標點．對于改進的RRT-ConCon算法，從初始點生成的搜索樹T1調用的擴展節點函數為Connect (T1,G)，其中G是目標點；從目標點生成的搜索樹T2調用的擴展節點函數為Connect (T2,qnew,1)，其中qnew,1是搜索樹T1擴展的一個新節點．

與Connect函數相同，改進的Connect函數也是通過重復調用擴展函數Extend來實現的．改進的Extend函數的偽代碼如下：

Algorithm5EXTEND Algorithm

Input:

P: a configuration in Route

Q: a free configuration

Output:

A new configurationqnewis obtained by movingPby step_size towardQ, or NIL in case of failure

1.qnew← progressPby step_size along the straight line betweenPandQ

2. ifqnewis collision-free then

3.V←V∪{qnew}

4.E←E∪{(P,qnew)}

5. returnqnew

6. end if

7. return NIL

對于輸入的當前節點P、臨時目標點Q，算法從當前節點P出發，朝著臨時目標點Q的方向嘗試擴展路徑．如果Extend函數返回值是一個新節點qnew，則將qnew添加到搜索路徑PATH中；如果Extend函數返回值是空，也就是說探測節點與障礙物發生碰撞，那么下一步將調用隨機節點生成函數來躲避障礙物．

2.3 隨機節點生成函數——RandomNode函數

由于在本文算法中，從初始點生成的搜索樹T1采用了朝向目標的擴展策略，雖然提高了算法的收斂速度，但也容易使搜索路徑陷入一個局部極小值．為了避免這個問題，本文在搜索樹遇到障礙物時，考慮隨機生成一個節點來躲避障礙物．

函數RandomNode (P,ρ)的作用是生成隨機節點．如圖3所示，灰色圓表示障礙物，圓點P表示機器人當前位置節點，虛線圓表示移動機器人下一步可能到達的位置軌跡，那么該圓的半徑為移動機器人的規劃步長；該虛線圓上的每一個無碰撞節點(如節點A、B、N)都是移動機器人下一步可能到達的節點，而虛線圓與障礙物發生碰撞的點(如節點C)都是移動機器人不能到達的節點．設節點N是移動機器人下一步將要到達的節點，那么節點N由函數RandomNode (P,ρ)隨機生成．隨機節點N(xN,yN)滿足以下3個條件：(1)節點N滿足方程(xN-xP)2+(yN-yP)2=ρ2；(2)節點N是無碰撞節點；(3)節點N能成功與節點P連接．隨機節點生成函數算法的偽代碼如下：

Algorithm6RandomNode Algorithm

Input:

P: a configuration in Route

ρ: step_size

Output:

A randomly chosen free configuration

1. while (connect(P,N)=0) do

2.N← a randomly chosen free configuration inO(P,ρ)

3. if connect (P,N) then

4. returnN

5. end if

對于輸入的當前節點P和機器人規劃步長ρ，在P為圓心、ρ為半徑的圓周上隨機地計算一個節點N．判斷N是否與環境中的障礙物發生碰撞并且判斷節點P和N是否成功連接，如果這兩項檢測結果都成功，那么返回節點N．

圖3 隨機節點生成過程

3 仿真實驗

3.1 仿真實驗環境平臺

為了建立各種障礙物環境，本文結合改進的RRT-ConCon算法，設計了一種仿真實驗環境平臺，該平臺的主要功能是繪制所需的仿真環境．仿真實驗環境平臺用Matlab開發，窗口界面主要包括參數配置和工作空間．在參數配置中可以選擇障礙物圖形(如矩形、圓形)和障礙物圖形半徑，可以設置起始點和目標點坐標．在工作空間中，環境設為30×30下的矩形區域．在設置仿真環境時，先配置好參數，然后在工作空間中單擊鼠標，便可生成一個障礙物．該平臺還提供了一個開放的算法接口，以便將仿真實驗環境應用于各類型的路徑規劃算法，拓展了平臺的使用范圍．圖4(a)所示為該仿真實驗環境平臺的窗口界面，圖4(b)所示為一個繪制的仿真實驗環境．

3.2 仿真實驗實例

為了驗證本文算法的有效性，實驗環境用Matlab開發，運行于PC機上，CPU主頻為2.4 GHz．仿真實驗環境基于上述實驗環境平臺建立，設置初始點坐標為(0,0)，目標點坐標為(30,30)．實驗目標為規劃一條從初始點到目標點的有效路徑．

圖5顯示了本文算法在不同環境(隨機環境、通道環境、柵格環境、狹窄環境)中的路徑規劃．圖5(a)中的障礙物是隨機設置的，圖5(b)～(d)中的障礙物是按需要設置的．

不失一般性，使用圖5(a)設置的隨機環境來測試本文算法的穩定性．如圖6所示，當重復規劃一條路徑時，本文算法基本保證了路徑的穩定性．

(a) 窗口界面

(b) 繪制實例

圖4 仿真實驗環境平臺

Fig.4 Simulation experimental environment platform

圖5 不同環境中的路徑規劃

表1為本文算法與基本RRT算法在圖6仿真實驗環境下，進行仿真實驗得到的數據比較．從表1中可以看出，相對于基本RRT算法來說，本文算法路徑更優，運行時間更短，并且可以保證規劃的路徑接近最優路徑．

圖6 重復規劃路徑比較

表1 仿真實驗環境下的數據比較

4 結 語

針對RRT算法缺乏穩定性和收斂速度慢的問題，本文提出了一種基于RRT-ConCon算法和朝向目標搜索的雙向搜索路徑規劃算法．改進的RRT-ConCon算法通過改變一條搜索路徑(Tinit)的臨時擴展目標點提高了算法的穩定性，同時可以保證規劃的路徑接近最優解．由于本文算法結合了朝向目標搜索的策略，使得搜索路徑易于陷入局部極小值的問題能夠通過隨機節點生成函數得到解決．同時，為了測試各種環境，本文設計了一個仿真實驗環境平臺，可以簡單地繪制所需的仿真實驗環境．通過與本文算法結合的仿真實驗結果，驗證了本文所提出的改進算法在穩定性、導向性及獲得最優路徑方面都優于RRT算法．但本文中仍存在一些有待完善的地方，下一步，嘗試改進隨機節點生成函數，以便確定一條更優的路徑，同時將該算法應用于實際工程領域以更好地展示其有效性．

[1] LaValle S M.PlanningAlgorithms[M]. Cambridge:Cambridge University Press, 2006.

[2] 康 亮,趙春霞,郭劍輝. 未知環境下改進的基于RRT算法的移動機器人路徑規劃[J]. 模式識別與人工智能, 2009,22(3):337-343.

KANG Liang, ZHAO Chun-xia, GUO Jian-hui. Improved path planning based on rapidly-exploring random tree for mobile robot in unknown environment [J].PR&AI, 2009,22(3):337-343. (in Chinese)

[3] 付 濤,王大鎮,弓清忠,等. 改進神經網絡自適應滑模控制的機器人軌跡跟蹤控制[J]. 大連理工大學學報, 2014,54(5):523-530.

FU Tao, WANG Da-zhen, GONG Qing-zhong,etal. Robot trajectory tracking control of improved neural network adaptive sliding mode control [J].JournalofDalianUniversityofTechnology, 2014,54(5):523-530. (in Chinese)

[4] 徐望寶,陳雪波,趙 杰. 個體機器人局部路徑規劃的人工力矩方法[J]. 大連理工大學學報, 2012,52(3):418-425.

XU Wang-bao, CHEN Xue-bo, ZHAO Jie. Artificial moment method for local path planning of single robot [J].JournalofDalianUniversityofTechnology, 2012,52(3):418-425. (in Chinese)

[5] 王 勇,蔡自興,周育人,等. 約束優化進化算法[J]. 軟件學報, 2009,20(1):11-29.

WANG Yong, CAI Zi-xing, ZHOU Yu-ren,etal. Constrained optimization evolutionary algorithms [J].JournalofSoftware, 2009,20(1):11-29. (in Chinese)

[6] 王 濱,金明河,謝宗武,等. 基于啟發式的快速擴展隨機樹路徑規劃算法[J]. 機械制造, 2007,45(12):1-4.

WANG Bin, JIN Ming-he, XIE Zong-wu,etal. Algorithm of path planning of rapidly-exploring random tree based on heuristics [J].Machinery, 2007,45(12):1-4. (in Chinese)

[7] LaValle S M. Rapidly-exploring random trees:A new tool for path planning, TR98-11 [R]. Ames:Department of Computer Science, Iowa State University, 1998.

[8] LaValle S M, Kuffner J J Jr. Rapidly-exploring random trees:Progress and prospects [C] //TheFourthInternationalWorkshoponAlgorithmicFoundationsofRobotics(WAFR). Natick:A. K. Peters, 2000:293-308.

[9] Mezura-Montes E, Coello Coello C A. A simple multimembered evolution strategy to solve constrained optimization problems [J].IEEETransactionsonEvolutionaryComputation, 2005,9(1):1-17.

[10] 張國亮. 動態環境中移動機器人路徑規劃研究綜述[J]. 機床與液壓, 2013,41(1):157-162.

ZHANG Guo-liang. Survey on path planning for mobile robot under dynamic environment [J].MachineTool&Hydraulics, 2013,41(1):157-162. (in Chinese)

[11] Bruce J, Veloso M. Real-time randomized path planning for robot navigation [C] //IEEEInternationalConferenceonIntelligentRobotsandSystems(IROS). Piscataway:IEEE, 2002:2383-2388.

[12] Ferguson D, Kalra N, Stentz A. Replanning with RRTs [C] //Proceedings2006IEEEInternationalConferenceonRoboticsandAutomation,ICRA2006. Piscataway:IEEE, 2006:1243-1248.

[13] Zucker M, Kuffner J, Branicky M. Multipartite RRTs for rapid replanning in dynamic environments [C] //2007IEEEInternationalConferenceonRoboticsandAutomation,ICRA′07. Piscataway:IEEE, 2007:1603-1609.

[14] FENG Lin, JIA Jing-hui. Improved algorithm of RRT path planning based on comparison optimization [J].ComputerEngineeringandApplications, 2011,47(3):210-213.

[15] Shi K, Denny J, Amato N M. Spark PRM:Using RRTs within PRMs to efficiently explore narrow passages [C] //2014IEEEInternationalConferenceonRoboticsandAutomation,ICRA2014. Piscataway:IEEE, 2014:4659-4666.

[16] Denny J, Morales M, Rodriguez S,etal. Adapting RRT growth for heterogeneous environments [C] //IEEEInternationalConferenceonIntelligentRobotsandSystems(IROS). Piscataway:IEEE, 2013:1-7.

[17] Lee J, Kwon O, ZHANG Liang-jun,etal. SR-RRT:Selective retraction-based RRT planner [C] //2012IEEEInternationalConferenceonRoboticsandAutomation,ICRA2012. Piscataway:IEEE, 2012:2543-2550.

[18] Karaman S, Frazzoli E. Sampling-based algorithms for optimal motion planning [J].InternationalJournalofRoboticsResearch, 2011,30(7):846-894.

[19] PAN Jia, ZHANG Liang-jun, Manocha D. Retraction-based RRT planner for articulated models [C] //2010IEEEInternationalConferenceonRoboticsandAutomation,ICRA2010. Piscataway:IEEE, 2010:2529-2536.

[20] Laumond J P, Sekhavat S, Lamiraux F.GuidelinesinNonholonomicMotionPlanningforMobileRobots[M]. Berlin:Springer Berlin Heidelberg, 1998.

[21] Kuffner J J Jr, LaValle S M. RRT-Connect:An efficient approach to single query path planning [J].Proceedings-IEEEInternationalConferenceonRoboticsandAutomation, 2000,2:995-1001.

AnimprovedpathplanningalgorithmbasedonRRT-ConCon

WANG Fan*1, FENG Nan1,2, HU Xiao-peng1

( 1.Faculty of Electronic Information and Electrical Engineering, Dalian University of Technology, Dalian 116024, China；2.Troops 65066, The Chinese People′s Liberation Army, Dalian 116100, China )

Aiming at the lack of stability and slow convergence for RRT algorithm, based on RRT-ConCon algorithm and towards goal search strategy, an improved bidirectional search path planning algorithm is proposed. By changing the temporary extension target for two search paths, the algorithm not only can make the search path grow easily towards the direction of target, but also can improve the stability of the algorithm, at the same time can guarantee the planning path close to the optimal solution. The improved RRT-ConCon algorithm uses random node generating function to avoid the search path growing towards the target point falling into local minimum. Meanwhile, in order to test a variety of simulation experimental environments, a simulation experimental environment platform is designed, and experimental results demonstrate the effectiveness and stability of the proposed algorithm.

mobile robot; path planning; rapidly-exploring random trees (RRT); bidirectional rapidly-exploring random trees (Bi-RRT); RRT-ConCon algorithm

1000-8608(2014)06-0637-07

2014-03-07；

: 2014-06-12．

國家自然科學基金資助項目(61272523).

王 凡*(1975-)，女，博士，碩士生導師，E-mail:wangfan@dlut.edu.cn； 馮 楠(1981-)，女，碩士生，E-mail:fengnanbangbang@163.com.

TP24

：Adoi:10.7511/dllgxb201406007