基于改进RRT 算法的六自由度机械臂路径规划

朱永浩 ，李 丹 ，龚 旭 ，金仁才

(安徽工业大学 电气与信息工程学院, 安徽 马鞍山 243032)

随着机器人技术的发展，机械臂具有工作空间大、操作简便、灵活性和自由度高等优点[1]，在焊缝焊接、制造加工、拆卸装配、喷漆和码垛等领域得到越来越广泛的应用[2-4]。与此同时，各项工作对机械臂的工作状态有更高的要求，路径规划作为机械臂工作的关键技术，受到越来越多的关注和研究。机械臂的路径规划和智能小车类似，均是以机器人当前位置为起点，规划出1 条运动到终点、无碰撞的最优路径[5]。但是，机械臂的运动规划主要用于多维空间，规划难度更大。

近年，机械臂路径规划算法主要分为基于图搜索、基于势场、基于采样等类别[6]。其中基于采样的算法无需预处理，也无需提前建图，在多维空间中得到更广泛的应用。快速扩展随机树(rapidly-exploring random tress，RRT)算法是机械臂路径规划中最常用的算法[7]，但该算法在复杂环境下会出现效率降低且无法得到最优路径的问题。基于RRT 的改进算法，Kuffner 等[8]提出让起点和终点同时生成随机树来进行双向拓展的RRT-connect 算法；Ferguson等[9]提出动态快速搜索树(dynamic-RRT，DRRT)算法，通过给定目标拓展方向，可较大程度地降低规划时陷入局部最优的情况；Karaman[10]提出改进的RRT*算法，通过添加图优化和剪枝理论，保留传统RRT 算法的完备性，同时兼具渐进最优性，但大大增加了搜索时间；Islam 等[11]提出RRT*-smart 算法，利用智能采样使搜索路径快速收敛；Klemm 等[12]结合RRT*的渐进最优和RRT-connect 的双向搜索，提出使搜索路径朝理论最优解收敛的RRT*-connect算法；朱宏辉等[13]通过加入规避步长延伸法改进RRT*算法，但改进算法用于复杂路径的规划时，难以获得最优解；陈肇星等[14]提出的DSSP-RRT 算法通过动态采样减少迭代次数，但存在可能陷入局部最优的情况。综上可看出：改进的RRT 算法均在一定程度上优化了路径规划性能，但依然存在一些不足，且在高维空间下的机械臂路径规划通常是不稳定的，需在不同层面上做出改进。鉴于此，结合RRT*和RRT-connect 算法的优点，提出1 种改进的RRT算法对六自由度机械臂进行路径规划，并分别利用Python 中的matplotlib 功能包以及ROS 平台中的动态规划库(open motion planning library，OMPL)，对睿尔曼RM-65 机械臂进行避障路径规划仿真实验，结果显示该算法规划出的路径更优且用时更少。

1 机械臂运动学分析

1.1 运动学模型

以六自由度机械臂为对象，对睿尔曼RM-65机械臂进行正运动学分析。机械臂可视为由关节和连杆组成，求解机械臂末端相对于基准坐标系的位姿，即为机械臂的正运动学建模[15]。机械臂结构尺寸如图1，其中d1=240.5 mm，d2=102.0 mm，d3=256.0 mm，d4=210.0 mm，d5=144.0 mm，d6=850.5 mm，d7=81.5 mm。

图1 RM-65 机械臂结构尺寸Fig.1 Structural dimensions of RM-65 robotic arm

基于改进的MD-H 参数建模，建立的连杆坐标系如图2。

图2 连杆坐标系示意图Fig.2 Schematic diagram of the connecting rod coordinate system

图3 Matlab 仿真结果示意图Fig.3 Schematic diagram of Matlab simulation results

x轴、y轴、z轴如图2 中标注所示，机械臂最底部的坐标系base_link 为机械臂的基坐标系，Link1～Link6 分别对应关节1～6 的坐标系。根据机械臂的尺寸得到MD-H 参数，如表1。

表1 RM-65 机械臂MD-H 参数Tab.1 RM-65 robotic arm MD-H parameter

根据MD-H 参数与各关节角度，确定机械臂末端执行器基于机械臂基座的空间位姿，矩阵ii-1T为连杆i坐标系到连杆i-1 坐标系的MD-H变换矩阵，该矩阵经以下四次运动变换得到，具体变换公式如下：

由式(2)～(7)可得出机械臂末端坐标系相对于底端基坐标系的运动学位姿变换矩阵，即正运动学方程06T可表示为：

图中：RPY为矩阵60T中的旋转矩阵转化得到的欧拉角，q1～q6为对应6 个关节角度；取3 组关节角度，分别通过Robotic Toolbox 工具箱读取和建模得到正运动学方程计算，结果如表2。由表2 可看出：Robotic Toolbox 工具箱读取所得位姿与运动学方程计算所得位姿结果基本一致，表明由MD-H 参数法建模计算得到的正运动学方程正确。

表2 位姿对比结果Tab.2 Pose comparison results

逆运动学建模是通过已知的机械臂末端位姿来确定满足期望位姿的机械臂各关节的状态[16]，常用的解法有数值求解法和解析求解法。其中数值求解法采用非线性优化求解，适用范围较广但计算量较大、速度较慢；解析求解法速度快但往往会得出多组奇异解。因此，文中选择数值求解法计算得到若干组逆解，根据关节角度范围和碰撞关系的检测在逆解中找出最优解。

1.2 碰撞检测

机械臂各连杆的形状不规则，在进行碰撞检测时常使用包围盒法，利用简单几何体将机械臂包络起来。常用的包围盒法有轴对齐边界框包围盒(axis-aligned bounding box，AABB)、包围球、方向包围盒(oriented bounding box，OBB)等[17-18]。AABB 法采用平行于坐标轴的最小六面体包络对象，包围球法采用半径最小的球体包络对象，OBB 法采用最小长方体包络对象。李智[19]提出了子弹形的包围盒(bullet bounding box，BBB)，包围盒模型为圆柱体搭配半球体的组合。使用该方法对机械臂进行包络时，对于旋转不会影响相邻连杆模型的轴体，采用圆柱体来包络；对于旋转会影响相邻连杆的轴体，采用半球来包络，这样可使包络盒更紧凑，且不会大量增加后续计算量。

文中采用BBB 方法和包围球方法对机械臂包围盒进行包络，将机械臂与空间障碍物的碰撞检测简化为求解圆柱中心线与球体半径的关系问题以及圆柱中心线与AABB 包围盒各平面的距离问题[20]。根据机械臂连杆旋转是否会影响相邻连杆，对第一轴、第四轴和第六轴采用圆柱包络，对第二轴、第三轴和第五轴采用半球和球形包络。具体为：将第一轴、第一连杆和第二轴转换为子弹形包络，第二连杆和第三轴转换为球形包络，第三连杆、第四轴、第四连杆和第五轴转换形成子弹包络，第五连杆、第六轴和第六连杆视作1 个圆柱体，末端执行器视为1 个球体。使用包围盒法可对机械臂包络进行简化，根据简化的机械臂模型，对各包络体间距离进行求解，各包络体之间的最短距离可作为判断是否发生碰撞的指标，距离大于0 则未发生碰撞，否则判断为发生碰撞。

2 RRT 算法的改进

RRT 算法是1 种基于随机采样的快速路径规划算法，通过采样在工作空间全局搜索，以给定的起点为根节点，利用随机取点生成新的子节点使扩展树不断生长，具有出色的空间搜索能力，对高维空间的路径规划也具有出色的性能[21]。RRT 算法的主要步骤如图4，包括采样、寻找邻近节点、拓展根节点、生成候选节点、碰撞检测以及生成邻近节点。具体过程为：确定起点(S)与终点(F)，并明确起点与终点之间的障碍物信息；以起点为根节点，随机扩展采样，遍历树中所有节点，生成候选节点Tn，并以固定步长生成1 个邻近节点Tn+1，若Tn与Tn+1之间不存在障碍物，则Tn及Tn+1加入扩展树，否则放弃此次采样点并重新采样；循环上一步骤，直到采样得到1 个节点Tn+1为终点F或该节点距离终点F的距离小于设定的阈值，则规划路径成功并退出采样。

图4 RRT 算法示意图Fig.4 Schematic diagram of RRT algorithm

RRT 算法在多维空间中适用性很强，但算法本身在搜索路径的过程中具有随机性，导致每次规划出的路径各不相同，规划出的是可行路径，并不是最优路径，且在复杂狭小环境中难以搜索到路径。为此，文中结合RRT*算法和RRT-connect算法的优点，在搜索过程中，应用目标采样思想对路径进行调整与重新布线，使每次采样趋近给定的扩展方向，改进RRT 算法。

2.1 目标采样

RRT 算法在随机采样的过程中，搜索树时常会扩展到离目标很远的地方，即“无用区域”。为提高算法的效率，应用目标采样的思想加强算法向目标点搜索的趋向性，减少无用搜索，即人为引导采样过程中随机点的生成，在生成1 个随机节点Tn时，以一定概率将节点生成在终点处，即Tn=TF。在提高算法效率的前提下，为保持搜索树对于未知空间的搜索能力，一般将概率p设定在0.05～0.10 之间。具体步骤为：生成1 个(0,1)的随机数rand，若rand≤p，则节点生成在终点处，即Tn=TF；若rand＞p，则新节点保持随机生成，图5 为加入目标采样前后的路径规划效果。

图5 加入目标采样前后的路径规划效果Fig.5 Path planning effect before and after adding target sampling

从图5 可看出：加入目标采样后的算法既能保证搜索树扩展时的随机性，又能一定程度上减免对“无用区域”搜索耗费的时间，在障碍物较少的空间效果较好。

2.2 贪婪思想

当随机采样到目标点附近区域时，受固定步长的限制，即使采样点与目标点之间无障碍物，搜索树也无法直接到达目标点，转而会继续进行无效搜索，使路径复杂、规划时长增加。针对这些采样点，引用贪婪思想，当搜索树生成的新节点与目标点间的距离在设定的阈值内时，判断新节点Tn1～Tn5与目标点F直接相连的可行性，如图6。当新节点与目标点之间存在障碍物时(如Tn1，Tn3，Tn4，Tn5)，该节点继续随机采样拓展；若之间不存在障碍物时(如Tn2)，则该节点停止随机采样，且不受固定步长的限制，将该节点Tn2与目标点F直接相连，并将两点间的路径放入最终规划出的路径中。

图6 引入贪婪思想的示意图Fig.6 Schematic diagram of introducing greedy thinking

2.3 路径平滑处理

在机械臂避障路径规划过程中，易在路径拐点出现急加速的情况，使规划过程的平稳性降低，故需对路径进行平滑处理。对于改进的RRT 算法，选取路径中的若干拐点，使用B 样条函数对拐点间的路径进行平滑处理。考虑到B 样条次数越高，平滑处理后的路径越平滑，为保证最终规划出的路径效果，文中选择五次B 样条函数进行平滑优化。根据需要平滑优化的拐点坐标，构造出1 条参数化的B 样条曲线，使用取值范围为[0，1]的参数t表示曲线上的点；将参数t映射到实际的自变量范围，根据拐点个数将自变量范围划分成若干区间，并对每个区间使用五次B 样条插值；根据插值结果可得到各区间的插值函数。位置表达式为

其中，将式(13)代入式(10)～(12)，解得：

2.4 改进RRT 算法的实现

改进的RRT 算法流程图如图7。

图7 改进的RRT 算法流程图Fig.7 Flowchart of improved RRT algorithm

由图7 可看出：采用改进的RRT 算法进行路径规划，其主要步骤如下：

1) 确定起点与终点，将起点和终点分别作为根节点向空间中扩展生成搜索树，2 棵树分别为TS，TF。

2) 加入目标采样，使搜索树TS按照给定方向扩展，采样得到1 个新节点Tr1，检查搜索树TS与Tr1最近节点Tn1之间是否存在障碍物。若不存在障碍物，则将节点Tn1储存进搜索树；若存在障碍物则重复步骤2)，直到找到1 个无碰撞的节点Tn1。

3) 以Tn1附近规定的半径范围内寻找路径代价更小的邻近节点作为父节点，并重新布线，调整经过新节点Tn1的路径。

4) 与步骤2)类似，使搜索树TF按照给定方向扩展，通过碰撞检测，采样得到1 个无碰撞的节点Tn2。

5) 以Tn2附近规定的半径范围内寻找路径代价更小的邻近节点作为父节点，并重新布线，调整经过节点Tn2的路径。

6) 循环步骤2)到5)，直到采样得到的节点Tn1与Tn2重合或2 个节点间的距离小于设定的阈值，则规划路径成功并退出采样，得到的TS至Tn1的路径、TF至Tn2的路径加上Tn1至Tn2的路径即为最终规划出的路径。

3 仿真实验与结果分析

为验证改进RRT 算法的路径规划效果，利用Python 中的matplotlib 功能包以及ROS 平台的动态规划库(open motion planning，OMPL)，对改进RRT算法与传统RRT 算法进行路径规划仿真实验。

3.1 三维空间仿真实验与结果分析

三维空间地图范围单位大小为100×100×100，设置路径规划的起点坐标为(10,10,10)，终点坐标为(95,95,95)，如图8。图中冗余线段为拓展树的树结构，从起点连接到终点的线段为传统RRT 算法和改进RRT 算法最终规划出的路线。地图中的障碍物为8 块正方体，初始设定采样长度为5、步长为5，当采样次数超过1 000 次时认定此次路径规划失败，放弃采样。

图8 三维空间仿真实验示意图Fig.8 Schematic diagram of three-dimensional space simulation experiment

在相同仿真环境下，分别使用传统和改进的RRT 算法进行50 次路径规划实验，对比2 种算法规划的平均路径长度、计算时长和采样点数，结果如表3。

表3 2 种算法的路径规划实验结果Tab.3 Experimental results of path planning for two algorithms

由表3 可看出：与传统RRT 算法相比，改进RRT 算法平均采样点数略高，但规划出的单位平均路径长度从292.6 减少至206.2，即规划出的路径代价大幅减少，且平均计算时长从1.264 s 减至1.156 s，改进RRT 算法规划出的路径更优且用时更少。

3.2 ROS 仿真实验与结果分析

为验证改进的RRT 算法在加入真实机械臂后的路径规划效果，使用六自由度睿尔曼RM-65 机械臂在ROS 平台下进行仿真实验，操作系统为Ubuntu18.04。使用Moveit! Setup Assistant 对机械臂进行配置，具体流程如图9。

图9 Moveit! Setup Assistant 配置流程图Fig.9 Flowchart of Moveit! Setup Assistant configuration

在ROS 平台下的动态规划库(open motion planning library，OMPL)中进行算法匹配，配置完成利用Moveit!及其插件接口实现机械臂在仿真环境下的避障运动规划。如图10 所示绿色机械臂为机械臂起始位姿，橙色机械臂为目标位姿，绿色长方体为障碍物，机械臂从起始位姿开始经过路径规划并避开障碍物最终到达目标位姿视为实验成功。

图10 避障运动规划示意图Fig.10 Schematic diagram of obstacle avoidance movement planning

使用传统RRT 算法和改进的RRT 算法分别进行30 次实验，2 种算法的运动规划过程如图11。在选用轨迹可视化功能后，图中连续出现的机械臂即为运动规划过程中的路径轨迹，机械臂轨迹越多说明规划时间越长、路径越复杂。

图11 ROS 平台下避障运动规划示意图Fig.11 Schematic diagram of obstacle avoidance motion planning on ROS platform

从图11 可看出：在相同实验环境和运动参数下，相比于传统RRT 算法，改进的RRT 算法规划时间和更短、规划的路径更优，且路径规划成功率提高了10 个百分点，2 种算法平均规划时间和规划成功率统计如表4。

表4 避障运动规划实验结果Tab.4 Experimental results of obstacle avoidance movement planning

4 结论

提出1 种改进的RRT 算法对六自由度机械臂进行路径规划，结合RRT*和RRT-connect 算法的优点，应用目标采样的思想加强算法向目标点搜索的趋向性，引入贪婪思想提高算法效率，结合五次B样条插值对路径平滑优化，使机械臂可在较短时间规划出更优路径。利用Python 中的matplotlib 功能包统计传统与改进RRT 算法规划路径所需的时间、采样点数量与路径长度，且在ROS 平台中利用动态规划库对所提改进与传统RRT 算法进行仿真实验。结果表明：采用所提改进算法对六自由度机械臂进行路径规划，可规划出路径代价更小的路径且用时更少，最终路径也更为平滑，验证了该算法在实际机械臂避障路径规划中的可行性。