基于改进式RRT*的手术机器人在线避障运动规划

胡孟慧，王怀震，梁凤强，孟超

(1.山东大学 临床医学院，山东 济南 250011;2.山东新一代信息产业技术研究院有限公司,山东 济南 250011;3.临沂供电公司，山东 临沂 276003;4.临沂正信工程勘察设计有限公司，山东 临沂 276005 ）

0 引言

运动规划是机器人研究的基本问题，针对移动机器人的路径规划方法已经非常丰富，目前该类方法主要有Dijkstra 算法、A*算法、随机采样法（PRM）、快速随机搜索树（RRT）、人工势场法、智能算法等。但是，针对机械臂的多输入多输出、非线性、强耦合的高维复杂系统，上述方法很难得到应用。为保证机械臂安全工作，应该满足其在线避障要求。因此，机械臂在线避障运动规划是机械臂控制的一个关键问题。

为了将移动机器人路径规划方法应用到机械臂系统上，学者们对这些规划方法提出了相应改进。祁若龙等[1]对理想轨迹进行分段描述，将空间机械臂轨迹规划问题转变为多目标优化求解问题，建立适应度评价函数，用遗传算法进行求解。理论上该算法可以推广到任意自由度和任意多的障碍物，但是参数量增加导致计算成本增加，计算时间也随之成倍增长。杜爽等[2]使用双向RRT 算法的规划方法，得到仿人机器人双腿稳定位形和抓手位姿序列，实现了机器人的全身运动规划。杨一波等[3]对人工势场法进行了改进，对障碍物建立排斥势场，对目标建立引力势场，针对其容易陷入局部极小点的问题，引入了与目标点相关的因子，能够避开障碍物到达目标点。刘成菊等[4]对RRT进行了改进，引入了引力场的概念，使得随机树的生长向终点偏移，引入路径缓存方法和动态扩展随机树的方法，适合于移动障碍物环境中。贾庆轩等[5]利用运动学方法将空间障碍物转换到了关节空间，再根据连杆与障碍物的碰撞条件，确定出了机械臂的自由运动空间，利用A*算法在自由运动空间进行路径搜索，实现了无碰撞路径规划。何兆楚等[6]将RRT 和人工势场法结合起来，利用人工势场法进行局部路径规划，一旦陷入局部极小值，就使用RRT 选择临时目标点，使其跳出局部最小值，能够实现避障，最终到达目标点，但其只考虑了机械臂末端的避障，未曾考虑机械臂杆件的避障。这些改进方法都是开环的，不能应对动态环境，国外有学者关于实时避障做了一些研究[7-8]，可以适应于动态环境，但其主要针对的是移动机器人，关于机械臂实时或在线避障的内容较少。

本文对RRT*进行改进并应用到机械臂空间，根据目标位置变化更新搜索路径形成闭环，实现在线避障运动规划。

1 机械臂在线避障运动规划

在学者们改进的基础上，结合机械臂系统高维度的特点，本文对RRT*算法进行了改进使其适用于机械臂在线避障运动规划。为降低运动学逆解所带来的时间损耗，机械臂运动规划一般在关节空间内进行。而目标点通常是基于笛卡尔空间的，通过逆解转换到关节空间。

在线避障运动规划包括两个阶段：预规划和重规划。预规划阶段从起始点开始搜索，同时将搜索到的路径点送至机械臂控制器，直至搜索到目标点进入重规划阶段。重规划阶段在包含目标节点的路径节点附近采样以优化路径，利用摄像机或其它设备监控目标点，当目标点发生移动时，以当前机械臂位置作为起始点重新规划，实现闭环控制，直至机械臂移动到目标节点。

1.1 预规划

预规划的主要任务是对整个自由空间进行探索，使得节点布满解空间，同时找到目标点。Algorithm1 给出了预规划的伪代码，此算法在RRT*的基础上做了改进。在此过程中，机械臂始终处于运动状态，当机械臂qa运动至与根节点q0之间的距离小于设定阈值时，从规划的路径中选取前k个节点输入给机械臂控制器（17 行），控制机械臂按照这k个节点进行运动，将根节点q0更新为（14—16 行），保证了在线运动。

Algorithm2 给出了规划包含k个节点路径的伪代码。算法使用成本函数式（1）选择节点，当路径规划进行到叶节点时，标记该节点并返回规划路径（3—5 行）。

其中，cost(qi)为起始点到qi的成本，H(qi)为qi到目标点的成本，计算方法如式（2）所示。当qi标记时，表明qi已经访问过，当通过重新布线或添加新节点将未标记节点添加为其子节点时，其祖先节点都有机会再次访问，即所有祖先节点去除标记。当规划路径比最佳路径更接近目标点时，则更新最佳路径（6 行）。

1.1.1qrand的选取和qnew的计算

qrand采用带有目标偏向的随机采样，如式（3）所示，qnew计算方式如式（4）所示。该方法以概率α向目标点方向采样，概率1—α在自由空间内随机采样，利于搜索树向未知空间探索，又保证朝目标点进行搜索，兼顾了随机性和搜索目标的快速性[9]。α的设置根据障碍物确定，当障碍物较少时，α设为较大值，障碍物较多时，α设为较小值。

其中，Pr∈[0,1]是一个随机数，α为自定义常数，qrand=[θ1,θ2,...,θ6]为关节空间内6 个关节角组成的点。

1.1.2 成本计算

路径的总成本为节点之间的距离总和，一般用欧式距离来定义[10]。关节空间的欧式距离定义为式（5），笛卡尔空间位置和姿态的欧式距离定义为式（6）和式（7），r，p，y分别为绕z，y，x轴的转角。

对机械臂来讲，两组相差较小的关节角对应的末端位姿可能相差甚远，另外由于逆解的多解性，两组完全不同的关节角对应的末端位姿也可能完全一致，因此可用三者的加权值来定义距离成本，如式（8）所示。

1.2 重规划

由于目标位置的变化可能会导致规划好的路径不再适用，所以需要对路径进行重新搜索和优化[11]。Algorithm3 给出了重规划的伪代码描述。4—5 行在路径周围进行采样和重新布线，如果找到更优的路径则更新path（6 行）。7—10 行与Algorithm1 对应代码作用相同。当目标点移动导致选择的目标点与障碍物发生碰撞时，需重新选择目标点（11—12 行），以机械臂当前点作为起始点重新规划，形成闭环在线控制，直至机械臂到达目标点。

1.2.1 扩展与重新布线

RRT*-smart[12]在二维空间扩展与重新布线示意图如图1 所示，对比二维空间，机械臂空间也有类似的性质。扩展采样方式如式（9）所示，该方法以概率α在以信标qbeacon为圆心，r为半径的圆内随机采样（左图），概率1—α在自由空间内随机采样。信标qbeacon是路径的节点，包含路径的“拐点”信息，因此在信标周围采样能优化这些“拐弯”处的路径。右图展示了如何重新布线优化路径，根据三角形定理“两边之和大于第三边”可知去掉中间点会使得成本更小，据此对整条路径重新布线以优化路径。

图1 扩展与重新布线

1.2.2 查找临近点

预规划阶段对全局进行搜索，存储了大量的节点，优化路径时需要进行查找，查找速度决定算法的运行速度，因此需要合适的存储结构减少查找时间。本文使用KD 树[13]（k-dimension tree）存储节点，KD 树是一种对K 维空间中的实例进行存储以便对其进行快速检索的数据结构。

算法在执行过程中不断扩展节点，将这些节点按照“左小右大”的原则动态添加进树中。在查找近邻点时，从根节点出发，递归地向下遍历，当目标点当前维小于切分点的坐标值时，移动到左子树，否则移动到右子节点，直到叶节点为止。KD 树的引入将查找的时间复杂度从O(N)降低到了O(logN)。

2 仿真实验

为了验证本文提出的算法，利用MATLAB在三维空间内进行两组测试。设置7 个障碍物模型，初始点为{10,10,10}，目标点为{90,90,90}，步长为5，最大迭代次数为2 000。最后在实际机械臂上对算法进行了验证。运动平台选用的计算机主频为2.3 GHz，内存为8 GB。

2.1 算法对比

针对规划速度而言，RRT 系列算法的速度远高于其它规划算法，因此本文与RRT 系列算法进行对比。在初始点、目标点、障碍物等变量相同的条件下，分别对RRT、RRT*和Algorithm1 进行了10 次对比，对比结果见表1，迭代次数、运行时间和路径长度取成功平均值。

表1 算法性能比对

可以看出，针对7 个障碍物的复杂环境，三种算法都有着较高的成功率。RRT*由于对父节点的选择做了改进，路径长度小于RRT，但运行时间较长，迭代次数更多。Algorithm1在RRT*的基础上改进采样方式，同时将前k个节点送给机械臂控制器，使其路径长度更短，成功率更高，迭代次数也最短，而运行时间与RRT 相比变化不大。

三种算法搜索结果如图2 所示，可以看出，RRT 搜索轨迹不够光滑，RRT*的搜索点几乎布满了整个空间，很多都是没必要的，Algorithm1 以概率α向目标点进行搜索，轨迹较为光滑，同时在自由空间也有一些搜索点，便于重规划进行扩展和重新布线。

图2 三种算法搜索结果比较

2.2 规划对比

预规划与重规划的对比如图3 所示，左图是预规划给出的路径，对中间障碍物进行膨胀操作后与规划路径发生冲突，此时重规划以当前机械臂位置为起点重新规划后面的路径，如右图所示，依然可以顺利完成避障。表明该算法在环境发生变化时，能够在线重新规划后面的路径，适应动态环境以完成避障任务。

图3 规划对比

2.3 实物模拟

根据本文提出的算法，利用ROS（Robot Operating System）中的运动规划库MoveIt!控制Jaco2 机械臂进行避障运动规划验证。实际环境下避障过程如图4 所示。图4 中长方体纸箱为障碍物，黄色方块位置为起始点，将其移动至障碍物的另一侧，在规划的同时，将得到的路径点送至机械臂控制器，实现在线规划任务。图中可见本文算法能够完成避障任务，验证了仿真结果。

图4 实际环境下避障过程

3 结语

本文从机械臂的作业需求出发，提出了一种在线避障规划算法。规划算法分为两阶段：预规划和重规划。

预规划阶段，在关节空间内以概率α向目标点搜索，对节点进行碰撞检测以确定是否添加进搜索树中，同时将规划出的前k个节点送给机械臂控制器，实现规划与运动并发进行。

重规划阶段，在路径点附近采样，对之前生成的路径进行“剪枝”和“重连接”操作，优化路径，减少成本。当目标节点移动时，以机械臂当前位置为起始点重新规划路径，能够实现动态避障和跟随目标。

在仿真环境中添加了7 个障碍物进行了算法验证，证明该算法能够在复杂环境中规划出一条避障路径，当环境变化导致规划路径不可行时，算法能及时调整路径以适应新的环境，满足机械臂在线避障要求。