基于改进渐进最优的双向快速扩展随机树的移动机器人路径规划算法

王坤 曾国辉 鲁敦科 黄勃 李晓斌

摘 要：针对带启发式的快速扩展随机树（RRTConnect）算法路径生成的随机性以及渐进最优的双向快速扩展随机树（BRRT*）算法收敛速度的缓慢性，提出了一种基于BRRT*改进的高效路径规划算法（EBRRT*）。首先引入一种智能采样函数，使随机树的扩展更具方向性，从而减少寻路时间，并提高路径的平滑性;其次在BRRT*算法的基础上，在EBRRT*算法中加入了一种快速扩展策略，使改进后的算法在自由空间中使用RRTConnect算法的扩展方式进行快速扩展，而在障碍物空间则使用改进的渐进最优的快速扩展随机树（RRT*）算法进行扩展，在提高扩展效率的同时避免算法陷入局部最优。将EBRRT*算法分别与快速扩展随机树（RRT）、RRTConnect、RRT* 和BRRT*算法进行仿真对比，仿真结果表明，改进后的算法在路径规划效率及路径平滑性方面均明显优于其他算法;且相对于BRRT*算法，其在路径规划时间上降低了68.3%，在迭代次数上减少了48.6%。

关键词：移动机器人;路径规划;快速扩展随机树;带启发式的快速扩展随机树算法;渐进最优的双向快速扩展随机树算法

中图分类号：TP242.6

文献标志码：A

Abstract： To overcome the randomness of RRTConnect and slow convergence of BRRT*（asymptoticallyoptimal Bidirectional Rapidlyexploring Random Tree） in path generation， an efficient path planning algorithm based on BRRT*， abbreviated as EBRRT*， was proposed. Firstly， an intelligent sampling function was intriduced to achieve more directional expansion of random tree， which could improve the smoothness of path and reduce the seek time. A rapidlyexploring strategy was also added in EBRRT* by which RRTConnect exploration mode was adopted to ensure rapidly expanding in the free space and improved asymptoticallyoptimal Rapidlyexploring Random Tree （RRT*） algorithm was adopted to prevent trapped in local optimum in the obstacle space. Finally， EBRRT* algorithm was compared with Rapidlyexploring Random Tree （RRT）， RRTConnect， RRT* and BRRT* algorithms. The simulation results show that the improved algorithm is superior to other algorithms in the efficiency and smoothness of path planning. It reduced the path planning time by 68.3% and the number of iterations by 48.6% compared with BRRT* algorithm.

英文關键词Key words： mobile robot; path planning; Rapidlyexploring Random Tree （RRT）; RRTConnect algorithm; asymptoticallyoptimal Bidirectional Rapidlyexploring Random Tree （BRRT*） algorithm

0 引言

随着人工智能等新兴领域的不断崛起，移动机器人的发展速度也获得飞速提升。目前，诸如家庭、工厂以及医院等公共场所对移动机器人的需求正在不断增加，而移动机器人作为生产工具，需要经常从一个位置移动至另一个位置，所以如何规划出一条即无障碍物又可以最小化消耗的路径成为了该领域的一项热门研究。国内外的学者纷纷对此进行了相关的研究，并提出了许多可行的路径规划理论及方法， 其中包括蚁群算法[1]、粒子群优化算法[2]、遗传算法[3]、人工势场法[4]和A*[5]算法等。这些算法在简单环境下可以实现快速收敛到最优路径，但在处理复杂环境及高维空间时一方面收敛速度会急剧下降，另一方面其所生成的路径并未考虑移动机器人的运动学约束，导致规划出的路径并不能被机器人所执行。

为解决高维环境下路径规划问题，基于采样的路径规划算法被提出， 其中包括概率路图法（Probability Roadmap Method， PRM）[6]和快速扩展随机树（Rapidlyexploring Random Tree， RRT）算法[7]。PRM算法作为最早的基于采样的路径规划算法，其在路径搜索的实时性与最优性上可以满足要求，但仍未考虑到移动机器人的非完整约束[8]，且当环境中存在未知障碍物时会严重影响其规划效率。由于PRM算法存在的不足，LaValle等提出了一种基于采样的RRT算法，该算法具有概率完备且收敛速度快，可应用在未知障碍物等优点，但其也存在一些缺陷[9-10]：1）由于算法采用随机采样状态空间进行扩展的方式，使得随机树的扩展没有方向性，导致最终生成的路径并非最优路径;2）由于算法需要探索整个未知空间，并进行反复迭代以找到一条可行路径，使得算法在运行中需要消耗大量的内存;3）由于算法的随机性，导致最终生成的路径较为粗糙，不适合移动机器人直接采用。

对于RRT算法存在的不足，国内外学者对其作了一定程度的改进，其中国外较为典型的改进有带启发式的快速扩展随机树（RRTConnect）算法[11]、渐进最优的快速扩展随机树（asymptoticallyoptimal Rapidlyexploring Random Tree， RRT*）算法[12]、渐进最优的双向快速扩展随机树（asymptoticallyoptimal Bidirectional Rapidlyexploring Random Tree， BRRT*）算法[13]和IBRRT*（Intelligent Bidirectional RRT*）算法[14]。RRTConnect算法由Kuffner等[11]于2000年提出，该算法通过并行生成两棵随机树的方式来提高寻找路径的速度。RRT*算法由Karaman等于2010年提出，该算法的提出很好地解决了RRT算法生成路径非最优问题，但由于在探索中计算量的增加，使其路径生成效率大幅降低[15]。Jordan等通过借鉴RRTConnect算法思想，于2013年提出了双向扩展的RRT*算法，即BRRT*，同时通过改进RRTConnect算法的连接函数，从而保证算法所连接的两棵树可以生成一条最优路径。Qureshi等[14]于2015年提出了IBRRT*算法，在BRRT*算法中引入一种智能样本插入函数，从而提高算法收敛到最优路径的速度。国内对RRT算法的研究较少，目前较早的研究为康亮等[16]于2010年提出了一种将RRT算法与滚动窗口相结合的改进算法，该算法克服了RRT算法只能在环境已知的条件下进行路径规划的限制;冯楠[17]于2014年提出一种改进RRTConCon的算法，提高了RRT算法的稳定性，使其更易朝着目标点扩展;潘思宇等[18]于2017年提出一种RRT*的改进算法，引入一种节点启发式采样函数，提高路径规划的速度与质量。

本文主要针对BRRT*算法采样的随机性和扩展方式的缓慢性，提出一种基于BRRT*的改进算法EBRRT*（Efficient Bidirectional RRT*），该算法引入智能采样函数代替原始算法中的随机采样函数，通过在起始点与目标点之间采样最优固定点作为两棵树的目标点，使随机树在扩展中具有方向性，同时使用快速扩展策略，将随机树的扩展分成两个阶段：当探索无障碍物空间时，算法采用RRTConnect算法的策略进行快速扩展;当检测到扩展方向有障碍物时，算法启动改进后的RRT*进行扩展，从而保证算法不会陷入局部最优。通过仿真实验验证了改进后的算法在有效性、实时性和优越性上具有明显优势。

4 结语

本文以BRRT*算法为基础，通过将原始算法中的随机采样函数替换为智能采样函数，并考虑RRTConnect算法的扩展方式从而引入快速扩展策略，使算法的扩展更高效，生成路径更平滑。通过对改进后的算法在不同地图中进行仿真，其结果表明改进后的算法在路径生成速度、迭代次数及平滑性上相较于其他算法具有明显优势。不过改进后的算法仍存在一些不足之处，比如在扩展方式切换时路径会出现较大弯曲，下一步将尝试在扩展中加入平滑处理函数，使扩展方式改变时路径衔接更平滑。

参考文献 （References）

[1] DORIGO M， GAMBARELLA L M. Ant colony system： a cooperative learning approach to the traveling salesman problem[J]. IEEE Transactions on Evolutionary Computation， 1997， 1（1）： 53-66.

[2] 韩明，劉教民，吴朔媚，等. 粒子群优化的移动机器人路径规划算法[J]. 计算机应用， 2017， 37（8）： 2258-2263.（HAN M， LIU J M， WU S M， et al. Path planning algorithm of mobile robot based on particle swarm optimization[J]. Journal of Computer Applications， 2017， 37（8）： 2258-2263.）

[3] OZDIKIA O. Genetic algorithm with random coordinates for route planning on a 3D terrain[C]// Proceedings of the 2011 5th International Conference on Genetic and Evolutionary Computing. Washington， DC： IEEE Computer Society， 2011： 146-149.

[4] CHEN T B， ZHANG Q. Robot motion planning based on improved artificial potential field[C]// Proceedings of the 2013 3rd International Conference on Computer Science and Network Technology. Piscataway， NJ： IEEE， 2013： 1208-1211.

[5] LIN M， YUAN K， SHI C， et al. Path planning of mobile robot based on improved A* algorithm[C]// Proceedings of the 2017 29th Chinese Control and Decision Conference. Piscataway， NJ： IEEE， 2017： 3570-3576.

[6] KAVRAKI L E， SVESTKA P， LATOMBE J C， et al. Probabilistic roadmaps for path planning in highdimensional configuration spaces [J]. IEEE Transactions on Robotics and Automation， 1996， 12（4）： 566-580.

[7] LAVALLE S M， KUFFNER J J. Randomized Kinodynamic planning [C]// Proceedings of the 1999 IEEE International Conference on Robotics and Automation. Piscataway， NJ： IEEE， 1999： 473-479.

[8] 宋金澤，戴斌，单恩忠，等. 一种改进的RRT路径规划算法[J]. 电子学报， 2010， 38（Z1）： 225-228.（SONG J Z， DAI B， SHAN E Z， et al. An improved RRT path planning algorithm[J]. Acta Electronica Sinica， 2010， 38（Z1）： 225-228.）

[9] GAMMELL J D， SRINIVASA S S， BARFOOT T D. Informed RRT*： optimal samplingbased path planning focused via direct sampling of an admissible ellipsoidal heuristic[C]// Proceedings of the 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway， NJ： IEEE， 2014： 2997-3004.

[10] DOSHI A A， POSTULA A J， FLETCHER A， et al. Development of microUAV with integrated motion planning for opencut mining surveillance [J]. Microprocessors and Microsystems， 2015， 39（8）： 829-835.

[11]KUFFNER J J， LAVALLE S M. RRTconnect： an efficient approach to singlequery path planning[C]// Proceedings of the 2000 IEEE International Conference on Robotics and Automation. Piscataway， NJ： IEEE， 2000： 995-1001.

莫栋成，刘国栋. 改进的RRTConnect双足机器人路径规划算法[J].计算机应用， 2013， 33（8）： 2289-2292.（MO D C， LIU G D. Improved RRTConnect path planning algorithm for biped robot [J]. Journal of Computer Applications， 2013， 33（8）： 2289-2292.）

[12] KARAMAN S， FRAZZOLI E. Samplingbased algorithms for optimal motion planning[J]. The International Journal of Robotics Research， 2011， 30（7）： 846-894.

[13] JORDAN M， PEREZ A. Optimal bidirectional rapidlyexploring random trees [EB/OL].[2018-03-20]. https：//dspace.mit.edu/bitstream/handle/1721.1/79884/MITCSAILTR-2013021.pdf.

[14] QURESHI A H， AYAZ Y. Intelligent bidirectional rapidlyexploring random trees for optimal motion planning in complex cluttered environments*[J]. Robotics and Autonomous Systems， 2015， 68： 1-11.

[15] OLZHAS A， HUSEYIN A V. A novel RRT*based algorithm for motion planning in dynamic environments[C]// Proceedings of the 2017 IEEE International Conference on Mechatronics and Automation. Piscataway， NJ： IEEE， 2017： 1416-1421.

[16] 康亮，赵春霞，郭剑辉. 基于模糊滚动RRT算法的移动机器人路径规划[J]. 南京理工大学学报（自然科学版）， 2010， 34（5）： 642-648. （KANG L， ZHAO C X， GUO J H. Path planning based on fuzzy rolling rapidlyexploring random tree for mobile robot[J]. Journal of Nanjing University of Science and Technology （Natural Science）， 2010， 34（5）： 642-648.

[17] 馮楠. 自主移动机器人路径规划的RRT算法研究[D]. 大连： 大连理工大学， 2014. （FENG N. Research on RRT algorithm of path planning for autonomous mobile robot[D]. Dalian： Dalian University of Technology， 2014.

[18] 潘思宇，徐向荣. 基于改进RRT*的移动机器人运动规划算法[J]. 山西大学学报（自然科学版）， 2017， 40（2）： 244-254.（PAN S Y， XU X R. Improved RRT*based motion planning algorithm for mobile robot [J]. Journal of Shanxi University （Natural Science Edition）， 2017， 40（2）： 224-254.）

[19] ZAID T， AHMED H Q， YASAR A， et al. Potentially guided bidirectionalized RRT* for fast optimal path planning in cluttered environments[J]. Robotics and Autonomous Systems， 2018， 108： 13-27.

[20] NOREEN I， KHAN A， HABIB Z. Optimal path planning using RRT* based approaches： a survey and future directions[J]. International Journal of Advanced Computer Science & Application， 2016， 7（11）： 97-107.

[21] IRAM N， AMNA K， HYEJEONG R， et al. Optimal path planning in cluttered environment using RRT*AB[J]. Intelligent Service Robotics， 2017， 11（1）： 41-52.