一种基于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