改进RRT算法的机械臂路径规划

江 韩，晁永生，周江林，李纯艳

（新疆大学机械工程学院，新疆 乌鲁木齐 830047）

1 引言

近年来，随着科学技术的发展，机械臂应用的范围越来越广泛，如何让机械臂在工作环境中快速规划出一条无碰撞路径则是机械臂路径规划研究的重点。常用的路径搜索算法有A*算法［1］，人工势场法［2］，RRT算法［3］等，A*算法在不规则的障碍环境（如凹字形或工字形）中难以找到路径，人工势场法在路径搜索中容易陷入局部极小值从而导致路径规划失败，RRT算法能够有效地对高维空间进行搜索，并且无需对构型空间障碍物进行准确的描述，因此RRT算法被广泛应用于机械臂的路径规划中。针对传统RRT算法搜索路径随机性大、效率低的问题，许多学者对传统RRT算法进行了改进，文献［4］提出一种低振荡人工势场-自适应扩展随机树混合算法。文献［5］提出了一种改进的快速随机搜索树算法，该算法通过标准RRT算法获得可行路径，然后根据路径安全性和路径长度来计算路径代价。由文献［6］提出了一种改进RRT算法，并且采用双向同时剪枝取最优的策略来删除不必要的节点。文献［7］在仿人机器人中利用双向RRT算法进行抓取操作。

针对传统的RRT算法搜索效率低，随机性大的特点提出一种改进RRT算法。该算法通过双向扩展随机树、在随机采样中加入目标偏置、设置虚拟目标点和改变步长来提高算法的搜索效率，利用双向删除取最短路径的方法删除路径中的冗余节点，并使用三次B样条曲线对删除冗余节点后的无碰撞路径进行平滑处理。

2 算法原理

2.1 机械臂运动学模型建立

由于机械臂的每个关节都有自由旋转度与独立位置，因此需要借助数学模型来建立机械臂末端在世界坐标系中坐标与机械臂关节角度的联系［8］。以丹麦优傲公司的UR5机械臂作为研究对象，该机械臂具有六个自由度，机械臂的六个关节都为旋转关节。机械臂的实物，如图1（a）所示。运用D-H参数法建立机械臂运动模型，如图1（b）所示。

图1 机械臂模型Fig.1 Manipulator Model

2.2 障碍物碰撞检测

在实际的工作环境中障碍物是不规则的，机械臂与障碍物之间的关系判断非常复杂需要耗费大量的时间，也很难通过准确的数学公式进行表达。因此为了提高机械臂在工作空间中碰撞检测效率，对障碍物采用长方体包络，对机械臂连杆采用圆柱体包络，此方法虽然增加了障碍物和机械臂模型区域，但是提高了机械臂在工作环境中规划路径的效率，同时增加了机械臂在实际运行过程中的安全性。机械臂某个连杆和障碍物包络盒，如图2所示。

图2 机械臂某个连杆和障碍物包络盒示意图Fig.2 Schematic Diagram of a Simplified Link and Obstacle Envelope Box for Manipulator

把半径为r的机械臂连杆包络盒简化成线段AB，机械臂连杆包络盒和障碍物包络盒之间的碰撞关系可转换为线段AB和障碍物包络盒六个平面之间的碰撞关系。为便于计算把障碍物进行膨化处理，膨胀尺寸为r，建立障碍物包络盒坐标关系。

设线段AB的方程为f(x，y，z)，若方程f(x，y，z)=0的解集与上式（1）有交集则机械臂与障碍物产生碰撞，反之机械臂和障碍物不发生碰撞，对机械臂另外五个连杆的判断也采用同样的方法进行求解。

3 RRT算法研究

3.1 传统RRT算法

RRT算法是一种基于随机采样的搜索算法，由英国教授Steven M.La Valle在1998年首次提出来。该算法首先在机器人状态空间中从初始点开始构建随机树，通过随机采样的方法生成随机节点，然后从离该节点最近的随机树节点按照步长生成新节点，在生成新节点过程中，若发生碰撞，则舍去新节点，否则添加到随机树中。重复以上过程，直到生成的新节点为目标点。传统RRT算法的伪代码如下所示。

3.2 改进RRT算法

针对传统RRT 算法搜索效率低的问题，文献［9］提出了双向RRT算法，该算法从初始点和目标点同时生成两棵随机树，这样虽然能够提高随机树搜索的效率，但还是存在搜索随机性大的问题。文献［10］在随机树搜索过程中引入了目标偏置的思想，减小了在全局状态空间均匀采样耗费的代价，但避免不了算法的低效扩展。在上述算法的基础上通过设置虚拟目标点和变步长来对算法进行改进。

3.2.1 设置虚拟目标点

为了提高算法的搜索效率，在双向RRT算法扩展时设置虚拟目标点，使随机树在目标偏置过程中把虚拟目标点当作目标点进行扩展。虚拟目标点产生的步骤如下：

（1）当初始点和目标点的连线和障碍物没有发生碰撞，则取初始点和目标点连线的中点作为虚拟目标点。

（2）当初始点和目标的之间的连线和障碍物发生碰撞时，设初始点的坐标为(x1，y1，z1)，目标点的坐标为(x2，y2，z2)，直线L1的方程为：

过初始点和目标点的中点A(xm，ym，zm) 作直线L1的垂线L2和L3，直线L2和L3分别平行于水平面和铅锤面。设直线L2和L3的方向向量分别为(n1，m1，p1)和(n2，m2，p2)，L2和L3的方程分别为：

以中点A(xm/2，ym/2，zm/2)为球心，以r为半径，则球的方程为：

联立式（3）～式（5）可求得球O和直线L2、L3的交点，若球O和直线L2、L3的交点都在障碍物内，则以球半径增大球的半径，再进行计算，直到球和直线L2、L3至少有一个交点不在障碍物内为止。若只有一个交点在障碍物外时，则把该点作为虚拟目标点，若有多个交点都在障碍物外时，则把距离障碍物远的点作为虚拟目标点。

3.2.2 改变步长

为了进一步提高算法的搜索效率，在障碍物周围设置一个半径为R的球形安全区域，在新节点生成时，先判断该节点是否在安全区域内，若在安全区域内时，则改变步长生成新的节点且不进行碰撞检测。若不在安全区域内时，则以原来的步长生成新节点并进行碰撞检测。变步长生成新节点的过程，如图3所示。

图3 变步长生成新节点过程Fig.3 Variable Step Size to Generate New Node Process

生成新节点的具体过程如下。

以Xnearst为父节点生成的新节点Xnew1在安全区域内，则改变步长重新生成新节点Xnew2，保留Xnew2，舍去Xnew1。生成的新节点Xnew3在非安全区域时，则连接Xnew3Xnearst进行碰撞检测，若发生碰撞，则舍去Xnew3，否则保留Xnew3。Xnew重新生成，如式（6）所示。

式中：p1—步长增加系数。

3.3 缩短路径

虽然在改进RRT算法中加入添加了目标偏置点和变步长，但搜索到的避障路径还是存在着冗余节点多的问题。针对这一问题，对避障路径中的冗余节点进行双向删除取最优的处理，分别从初始点和目标点对路径中的冗余节点进行删除，比较两次删除冗余节点后路径的长度，保留较短路径的节点。这样虽然不能保证路径最优，但还是能够有效的缩短路径。删除冗余节点原理，如图4所示。

图4 删除冗余节点原理Fig.4 Principle of Deleting Redundant Nodes

其详细步骤如下：

（1）通过改进RRT 算法得到从初始点到目标点的一系列无碰撞路径点集M1。

（2）首先从初始点连接路径点1，如果初始点和路径点1之间未发生碰撞，则初始点再连接路径点2，依次下去，直到两点之间发生碰撞，则将发生碰撞的路径点的前一个点保留，并且以该节点为父节点再次执行上述操作，直到到达目标点。

（3）从目标点开始连接前一个节点，重复步骤（2），直到到达初始点。

（4）分别比较从初始点和目标点删除冗余节点后路径的长度，保留较短路径的节点，连接保留的路径节点。

3.4 路径平滑处理

删除避障路径中冗余节点虽然缩短了路径的长度，但路径在转折点处存在着不平滑问题，针对这一问题，采用三次B样条曲线对避障路径进行平滑处理。B样条曲线的方程如下：

式中：Ni，k(u)(i=0，1，…，n)—k次B样条基函数；

Pi(i=0，1，…，n)—控制顶点，决定B样条的控制多边形。

k次B样条基函数的递推公式为：

式中：k—B样条函数的阶数；i—B样条函数的序列号；记0/0=0。

通过关节空间中对应的位置—时间序列点，将其作为型值点反算控制顶点，再利用三次B样条插值法进行轨迹的拟合，保证了运动轨迹经过所对应的工作空间位置，其第i段曲线的函数表达式为：

4 实验结果与讨论

为了验证改进算法和对改进算法搜索到的避障路径优化的有效性，分别在MATLAB平台和ROS平台上进行仿真验证。

4.1 MATLAB仿真实验

在MATLAB 仿真实验中，机械臂的工作空间为（100×100×100）cm，初始点坐标为（3，34，40），目标点坐标为（74，34，40），路径的搜索步长为1cm，设定随机树产生随机点最大个数为2500，如果在设定的产生最大随机点数范围内找到路径则认为路径搜索成功，反之则认为路径搜索失败。

在同等参数设置下分别对双向RRT算法，目标偏置RRT算法以及改进的RRT算法进行20次实验，双向RRT算法机械臂末端路径搜索图，如图5（a）所示。偏置RRT算法机械臂末端路径搜索图，如图5（b）所示。改进RRT算法机械臂末端路径搜索，如图5（c）所示。

图5 三种算法机械臂末端路径搜索图Fig.5 Three Algorithm Manipulator End Path Search Diagram

取这20次实验的平均值，三种算法的实验对比数据，如表1所示。改进RRT 算法相对比双向RRT 算法和目标偏置RRT 算法，路径长度分别缩短了约31%和12%，路径搜索时间和采样点个数分别为双向RRT 算法的5%和15%，目标偏置RRT 算法的47%和33%。改进RRT算法相对比双向RRT算法在路径搜索成功率上也有了提高。为了进一步优化改进RRT算法搜索到的避障路径，对避障路径进行双向删除取最短路径的处理，最后利用三次B样条曲线对删除冗余节点后的避障路径平滑处理。删除冗余节点和用三次B样条曲线优化路径，如图6所示。图6中折线为双向删除冗余节点取最优得到的路径图，曲线为三次B样条曲线对删除冗余节点后避障路径平滑处理图。

表1 三种算法实验数据对比Tab.1 Comparison of Experimental Data of Three Algorithms

图6 删除冗余节点和三次B样条曲线优化路径图Fig.6 Delete Redundant Nodes and Cubic B-Spline Curve to Optimize the Path Diagram

4.2 ROS机械臂路径规划实验

在ROS仿真平台上，对UR5机械臂进行路径规划实验，把三次B样条曲线优化后的路径节点通过KDL（The Kinematic and Dynamics Library）求解器逆解到关节空间，来实现对机械臂的控制。机械臂初始状态，如图7（a）所示。机械臂运动轨迹，如图7（b）所示。机械臂到达目标点，如图7（c）所示。

图7 机械臂运动状态Fig.7 Manipulator Movement State

机械臂通过反解三次B样条曲线优化后的路径节点顺利地避开障碍物且到达目标点。

5 结论

结合双向RRT算法双树扩展和目标偏置RRT算法目标偏置的优点，并在其中设置虚拟目标点和变步长来对RRT算法进行改进。MATLAB仿真实验表明，改进RRT算法在保证路径搜索成功率的同时不仅对路径搜索效率有了较大的提升，路径长度也有明显的缩短。考虑到机械臂实际运行情况，采用双向删除取最优的策略删除避障路径中的冗余节点，并利用三次B样条曲线对删除冗余节点后的避障路径进行平滑处理。ROS平台仿真实验表明，机械臂反解优化后的路径节点能顺利的避开障碍物到达目标点。