基于RRT*算法的六自由度机械臂避障路径规划

摘要：本文采用一种基于随机采样的避障路径规划算法—RRT*算法，解决六自由度机械臂的避障路径规划问题，并在机器人操作系统中搭建了仿真平台，验证RRT*算法的路径规划和渐进优化能力。

关键词：避障路径规划;六自由度机械臂;

0 引言

六自由度机械臂因其工作空间大、灵活性强，在工业自动化领域应用很广泛，如在汽车领域用于搬运、焊接、喷涂等。工业现场中，机械臂周围经常会有其他设备、工件，甚至还需要与人协同工作，这些都需要机械臂具备自动规划路径、躲避障碍物的能力。机械臂的避障路径规划，就是机械臂自动规划一条从起始点到目标点的无碰撞路径。传统的避障路径规划算法：可视图法[1]、人工势场法[2]、自由空间法[3]、栅格法[4]等，这些算法都需要对障碍物进行准确的建模和描述，随着机器人自由度的增加和机器人所处环境的复杂性增大，这些算法实现的难度增大，执行效率也低。因此，这些算法不适合六自由度机械臂的避障路径规划。

为避免对障碍物的精确建模，有学者提出了基于随机采样的避障路径规划，这类方法不需要对机器人周边环境建模，只须知道机器人与障碍物是否碰撞的信息，依据此信息，连接无碰撞的采样点。随着采样点的增多，从而构建一张无碰撞的图，最终从图中搜索起始点到目标点的路径。其中，快速扩展随机树法(Rapidly-exploring Random Trees,RRT)[5]是这类算法的典型代表，它由LaValle 提出，是一种基于随机采样的步进式的算法。学者们从不同方面对基本的 RRT 算法进行了改善[6,7,8]，Sertac和Emilio提出一种RRT的改进方法—Rapidly-exploring Random Tree Star(RRT*)[9]，这种方法具有路径优化能力，而且具有概率完备性。本文将分析RRT*的原理，并在机器人操作系统(RobotOperating System，ROS)中搭建仿真平台，对比RRT和RRT*两种算法，验证RRT*算法的优势，解决六自由度机械臂的避障路径规划。

1 RRT*算法的引入

1.1 RRT算法原理

基本的RRT算法的伪代码如算法1所示。Extend函数有三个返回值，“Advanced”表示扩展成功，但未到达目标构形;“Trapped”表示发生碰撞，没有扩展成功;“Reached”表示扩展成功，到达目标构形。首先用初始构形xinit初始化图G的节点集V，边的集E为空集。RRT Planner每次循环中，在自由空间中随机采样一个点xrand，然后执行Extend函数：寻找xrand的最近节点xnearest，并连接xnearest和xrand得到点xnew，如图1所示。如果xnearest和xnew连线的任意点与障碍物没有碰撞，则将新产生的点xnew和边(xnearest,xnew)添加到图G(V,E)中，并判断点xnew是否落在目标区域中，如果xnew落在目标区域中，则说明到达目标构形x goal，停止循环，返回图G(V,E);如果没有落在目标区域，则继续循环，直到xnew落在目标区域或达到循环次数的上限。

算法1RRT算法的伪代码

图2是RRT算法在的平面无障碍区域中的扩展过程，分别给出了迭代次数为100、200、700的状态。从图中可以看出，扩展过程就像是以初始点为根的一棵树，不断有新的分支长出来，探索未知的空间[10]。

1.2 RRT*算法原理

RRT算法存在一个缺点，规划的路径不是最优的。为了改进规划路径的质量，Sertac和Emilio提出一种RRT的改进方法—RRT*，它具有渐进优化能力，即随着规划时间的增加，规划路径逐渐趋于最优值，并在理论上得到了论证。

RRT*算法没有把新产生的点xnew直接与它的最近节点xnearest相连，而是经过两个阶段的处理。第一阶段：从xnew的附近节点X near中，寻找使距离xnew代价最小的节点作为x new的父节点，保证了新添加的节点xnew对应的路径代价有局部最小值，图3(a)所示。第二阶段：如果经过节点xnew到附近节点xnear的路径代价小于节点xnear之前的路径代价，则将xnear的父节点改为节点xnew，如图3(b)所示。RRT*算法在这两点上改进了RRT算法，从而保证了RRT*算法的渐进优化能力。

2 在ROS中验证RRT*算法

为了验证RRT*算法的渐进优化能力，在机器人操作系统(ROS)中进行算法的验证。

计算机的CPU型号是：Intel Core i5-10210U，8G内存，运行Ubuntu 18.04操作系统，ROS版本为Kinetic。

六自由度机械臂采用的是优傲协作机器人UR5，臂展850mm，本体质量33.5kg，额定负载5kg，其D-H参数如表1所示。

搭建的仿真环境如图4所示，UR5机械臂前面放置了一个工作台，用于模拟机械臂的工作环境，同时也作为机械臂运动过程中的障碍物。

设定UR5机械臂初始位姿为零位，目标位置各个关节为[11.5°、-7°、 80.5°、 90°、 90°、 0]，机械臂的状态如图5所示。

基于上述的RRT*算法，编写了UR5机械臂的避障运动规划算法。运行该算法，成功规划出了从起始位姿到目标位姿的无碰撞路径。UR5机械臂的运动轨迹如图6所示，从图中可以看出，机械臂成功躲避了工作台，伸入到门字形挡板内，到达目标位姿。

选取UR5机械臂末端的笛卡尔路径作为路径代价，仿真中发现，随着迭代次数的增加，路径代价逐渐趋于最优值，如图7所示，这证明了RRT*算法具有渐进优化能力。

3 总结

本文采用了RRT*算法解决六自由度机械臂的避障路径规划问题，论述了RRT*算法的原理，并在ROS中搭建了UR5机械臂的仿真环境，基于RRT*算法UR5机械臂成功规划了避障路径，并验证了RRT*算法的渐进优化能力。但本文也存在有待完善的地方，比如RRT*算法的规划速度慢、成功率低，这是需要继续改进的地方。

参考文献

[1]J.M.Ahuactzin，K.K.Gupta.The kinematic roadmap：a motion planning based global approach for inverse kinematics of redundant robots.IEEE Transaction on Robotics and Automation.2010，15(4)：653-669.

[2] 黄献龙，梁斌，吴宏鑫.机器人避碰规划综述[J].航天控制，2002(1)：34-46.

[3]D.Zhu，J.C.Latombe.New heuristic for efficient hierarchical path planning.IEEE Transaction on Robotics and Automation,2010(7)：9-20.

[4]C.Suman，J.John.A methodology for intelligent path planning[C]// IEEE International Symposium on Intelligent Control and 13th Mediterranean Conference on Control and Automation.Limassol，2005：592-597.

[5]Steven M.LaValle,Rapidly-exploring random trees:a new tool for path planning[R].Ames:Iowa State University，1998.

[6]李洋，徐达.基于引力自适应步长RRT的双臂机器人协同路径规划[J].机器人，2020,42(05):606-616.

[7]王轸，常健，李斌，王聪，刘春，张良全.基于脊线模态法和RRT算法的蛇形机械臂避障控制研究[J].高技术通讯，2020,30(12):1274-1283.

[8]王兆光，高宏力，宋兴国.基于GB_RRT算法的机械臂路径规划[J].机械设计与制造，2019(07):1-4.

[9]SertacKaraman and Emilio Frazzoli.Sampling-based algorithms for optimal motion planning[J].International Journal of Robotics Research，2011，30(7)：846-894.

[10]尹斌.冗余机械臂运动学及避障路径规划研究[D].哈尔滨工业大学，2014.