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

尹斌，卢文涛，魏文卿

（洛阳理工学院，河南洛阳，471000）

0 引言

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

为避免对障碍物的精确建模，有学者提出了基于随机采样的避障路径规划，这类方法不需要对机器人周边环境建模，只须知道机器人与障碍物是否碰撞的信息，依据此信息，连接无碰撞的采样点。随着采样点的增多，从而构建一张无碰撞的图，最终从图中搜索起始点到目标点的路径。其中，快速扩展随机树法（Rapidly-exploring Random Trees, RRT）[5]是这类算法的典型代表，它由LaValle 提出，是一种基于随机采样的步进式的算法。学者们从不同方面对基本的 RRT 算法进行了改善[6-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”表示扩展成功，到达目标构形。首先用初始构形 initx初始化图G的节点集V，边的集E为空集。RRT Planner 每次循环中，在自由空间中随机采样一个点randx，然后执行Extend函数：寻找xrand的最近节点xnearest，并连接xnearest和xrand得到点xnew，如图1 所示。如果xnearest和xnew连线的任意点与障碍物没有碰撞，则将新产生的点xnew和边(xnearest,xnew)添加到图G(V,E)中，并判断点xnew是否落在目标区域中，如果xnew落在目标区域中，则说明到达目标构形xgoal，停止循环，返回图G(V,E)；如果没有落在目标区域，则继续循环，直到xnew落在目标区域或达到循环次数的上限。

算法1RRT 算法的伪代码

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

图1 RRT 算法生成结点

1.2 RRT*算法原理

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

RRT*算法没有把新产生的点newx直接与它的最近节点xnearest相连，而是经过两个阶段的处理。第一阶段：从xnew的附近节点Xnear中，寻找使距离xnew代价最小的节点作为xnew的父节点，保证了新添加的节点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 所示。

图2 RRT 算法的扩展过程

图3 RRT*算法改进示意图

表1 UR5 机械臂的D-H 参数

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

图4 UR5 的仿真环境

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

图5 UR5 的起始位姿和目标位姿

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

图6 机械臂的运动轨迹

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

图7 路径代价的变化趋势

3 总结

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