刘学深，曹立佳,1b,2

（1.四川轻化工大学a.自动化与信息工程学院，b.计算机科学与工程学院，四川 宜宾 644000；2.企业信息化与物联网测控技术四川省高等学校重点实验室，四川 宜宾 644000）

引言

随着我国生产业从中国制造向中国智能制造转型，我国对工业机械臂提出了更高程度智能化与精度的要求。如何实现工业机械臂在有障碍物的狭小环境中高效安全地工作，成为机械臂研究中极其重要的课题。快速扩展随机树（Rapidly Expanding Random Tree，RRT）算法具有概率完备的特点，且无需对系统进行建模，可直接应用于非完整约束系统的规划，其算法复杂度小，十分适合解决高维空间中的避障规划问题[1]，经常被人们应用于机械臂、移动机器人和无人机等路径规划领域中。之后出现的RRT*算法能收敛到全局最优解，但是它往往需要很高的迭代次数[2]。目前已有众多学者对RRT*算法做出了一系列改进，且应用在了机械臂的路径规划中。

文献[3]针对RRT*算法随机性大和效率低的缺点，加入了自适应步长策略和节点拒绝策略，提高了RRT*搜索速度，但是该算法易陷入局部最优。文献[4]提出了节点扩展能力检测策略，减少了无效节点的生成，但存在计算量太大的缺陷。文献[5]加入了目标引力；文献[6]提出了柯西目标引力和节点拒绝策略，能够剔除不必要的采样节点；文献[7]提出了动态采样域策略，提高了拓展方向的确定性；文献[8]通过构建李势场改进了节点搜索方向及步长；文献[9]将启发函数应用在双向RRT 采样过程中；文献[10]增加了目标偏置策略；文献[11]改变了采样点生成机制，通过建立采样池来对采样点进行优选；文献[12]提出了高斯采样的方法，增加了对障碍物附近区域的采样次数；文献[13]采用了覆盖剔除机制，减少了重复采样；文献[14]采用了椭球子集约束采样，提高了算法收敛速度。

可以看出以上文献都是从以下3种方式中选取1～2 种方式去提升算法效率：(a)改变扩展方向（设计思想：为了直接减小耗时）；(b)删除冗余节点（设计思想：为了减小内存，间接减小耗时）；(c)改变步长（设计思想：为了优化路径，进而加快收敛）。这3种方式都是从不同的维度去减小算法耗时，但是以上文献算法要么计算量太大不适用于复杂环境，要么耗时减小得不够明显，而且在复杂环境中单独采取一种方式的效果往往并不是最优的，因此本文将联合以上3种方式，从不同维度出发，提出一种方法简明、占用内存小且耗时短的改进RRT*算法，以提升算法综合性能。

1 机械臂运动学模型的建立

以某品牌的6 轴串联机械臂为研究对象，首先运用DH 参数法建立6 轴机械臂运动学模型。该方法是在每个连杆上建立一个坐标系，通过齐次变换来实现两个连杆上的坐标变换[15]，模型如图1所示。

图1 机械臂坐标系

DH 法的4 个参数可以表达相邻两个关节之间的相对位置，进而可以构建出6 轴机械臂运动学模型。图1 中i（i=1,2，…，6）表示连杆，ai-1表示转轴i和转轴（i-1）之间的连杆长度；di表示转轴i的连杆偏距，即ai-1与ai两条直线与转轴i交点的距离；αi-1表示转轴（i-1）相对于转轴i的连杆转角，θi表示关节角度。

对全部连杆规定坐标系后，由两个旋转和两个平移来建立相邻两连杆（i-1）与i之间的相对关系，且可由4 个齐次变换来描述，可表示为矩阵T[16-17]，如式（1）所示：

RRT*算法应用在机械臂路径规划的流程如图2 所示。首先输入机械臂的初始状态，然后经过采样和生成新节点的过程，再进行碰撞检测，然后再进行父节点重选和路径剪枝过程对路径进行优化，以减小路径代价。重复以上的流程，当达到最大的迭代次数时，最终输出路径点信息。

图2 RRT*机械臂路径规划算法流程

2 RRT*算法原理

RRT 是一种基于采样的路径规划算法，该算法具有概率完备性。首先在搜索空间中随机采样一点，进而确定扩展方向，然后向该方向以某一固定步长生成新节点，重复这些操作直到节点靠近目标点。该算法搜索能力强，只要不限运行时间和迭代次数，总能在存在障碍物的复杂空间中找到一条无碰撞的可行路径[18]。

为了解决RRT 算法规划出来的路径不是最优的问题，RRT*在原始RRT 算法每次迭代生成新节点后加入了父节点重选[19]和路径剪枝的过程，然后不断迭代，最终可以让规划路径趋于渐进最优。其中RRT*父节点重选的过程如图3所示，图中小圆圈中的数字是节点编号，而直线中间的数字代表路径代价。

图3 父节点重选

图3 中1 号点为Xint，首先由传统RRT 算法生成的6 号节点Xnew的父节点为3 号节点，该路径为1-2-3-6，整体路径代价为5+4+4=13。然后RRT*算法将以6号节点为中心以自己定义的半径范围内找出“近邻”节点2、3、4、5，并且依次与6号节点相连计算出相应的路径代价，如果路径代价小于原来的13，则替换6号节点的父节点3。

6号节点的“近邻”节点有2、4、5，选择4号节点为父节点的路径为1-2-3-4-6，路径代价为5+4+3+6=18，由于18 ＞13，所以不替换原来的父节点；选择2号节点为父节点的路径为1-2-6，路径代价为5+5=10，由于10 ＜13，显然这条路径更好，所以替换6号节点的父节点3号点为2号节点，路径长度明显缩短。

尽管RRT*优化了搜索路径，但是RRT*规划出的路径质量与迭代次数有关，往往需要设定较大的迭代次数才能搜索出最优路径，所以为提高RRT*的算法效率，本文算法进行改进。

3 RRT*算法改进

3.1 改进近邻节点选取范围

由于RRT*是在定义的半径内寻找近邻节点，使得剪枝和父节点重连策略的优化程度不充分，算法效率低[20]。针对这些问题，本文将扩大近邻节点的选取方式。具体思路是考虑缩短从起点到Xnew的路径长度，找出Xnew第n层的祖先节点O2和第n+1层的祖先节点O3与原本近邻节点O1组成一个新的近邻节点集合A，然后依次与Xnew相连进行比较，从中选取使得Xnew路径代价最小的节点作为Xnew的父节点，如图4（以n=1为例）所示。

图4 Xnew的父节点

重新选择父节点后，路径将会得到缩短，求得的各个集合分别为：

3.2 全局采样方式优化

由于RRT*是在整个搜索空间内随机均匀采样，随机性大，为了减小冗余搜索，缩短路径搜索时间[21]，将缩小采样空间：

1）以一定概率a在搜索空间中均匀随机采样；

2）以一定概率b采取目标偏置采样，令Xrand=Xgoal；

3）如图5 所示，当生成新节点坐标Xnew时，会判断Xnew是否处于目标点Xgoal的邻域内。如果是，则将Xnew作为Xgoal的父节点，但是步长ρ可能会设置得比Xgoal邻域半径R大，导致随机树在Xgoal扩展时会发生振荡，导致算法收敛速度降低。

图5 随机树在目标点附近振荡

为了避免这种情况，更快地得到最优路径，所以将以一定概率c，不进行采样，而是遍历所有随机树节点，找出代价Cost最小的节点Xk如式(2)所示：

其中，Ck为从起点Xint到Xk的实际路径代价大小，d(Xk,Xgoal)为节点Xk到目标点Xgoal的欧式距离。连接Xk和Xgoal，对Xk到Xgoal的这段路径进行碰撞检测，如果没发生碰撞，则直接将Xk作为Xgoal的父节点，把Xgoal加入随机树中，生成路径；如果发生碰撞，则改为均匀随机采样，其中a+b+c=1。

3.3 节点拒绝策略

为了避免随机树向没必要探索的空间扩展，且满足机器人运动学约束，本文将引入节点拒绝策略，具体方法如图6 所示。在每一次随机树扩展生成新节点Xnew时，判断Xnear与Xnew的连线与起点Xint和目标点Xgoal的连线夹角是否大于某一个角度值，如果夹角太大，大于σ（本文设置为60°），则删除Xnew，θ的计算公式如式(3)所示。

图6 夹角θ

3.4 算法仿真实验验证

为验证改进RRT*算法，本文将使用Matlab 仿真平台，在3 个二维环境中每种算法分别进行50 次实验，然后取平均值把改进RRT*和传统RRT*算法作对比。起点设置为[5,5]，终点为[95,95]，步长ρ为2，近邻节点选取半径为5。

3.4.1 算法在不同地图中仿真效果

传统RRT*及其改进算法在地图1 中路径优化仿真结果如图7 所示。从图7 中可以看出，相比于传统RRT*算法（图7(a)），加入改进点1 和2 后（图7(b)），路径更短、更平滑，并且随机树的分支趋向同一节点，加入改进点3 后的RRT*算法（图7(c)）避免了向多余空间探索，进一步提高了算法效率。传统RRT*及其改进算法在地图1 中仿真结果见表1。从表1中可以看出，在扩展节点数大幅减少的情况下，平均时间减少了84.1%，路径长度减少了7.4%。

表1 地图1仿真结果

图7 传统RRT*及其改进算法在地图1中仿真效果

传统RRT*及其改进算法在地图2 中路径优化仿真结果如图8和表2所示。在地图2中改进RRT*算法每次运行平均时间减少了约72.2%，路径长度减少了约7.8%。

表2 地图2仿真结果

图8 传统RRT*及其改进算法在地图2中仿真效果

传统RRT*及其改进算法在地图3 中路径优化仿真结果如图9 和表3 所示。在地图3 这样的复杂环境中相比于传统RRT*算法（图9(a)），加入改进点3 后的RRT*算法（图9(c)）的扩展节点数大幅减少，每次运行的平均时间减少了81%，路径长度减少了约7.2%。

表3 地图3仿真结果

图9 传统RRT*及其改进算法在地图3中仿真效果

以上的仿真实验验证了改进方法的可行性，接下来将会把改进RRT*算法、文献[5]中的目标引力RRT*算法和文献[10]中的目标偏置RRT*算法在地图3 中分别进行50 次仿真实验验证，然后对各个评价指标（时间、路径长度、扩展节点数、成功率）取平均值进行对比。

3.4.2 不同算法在地图中仿真对比

不同算法在地图中仿真对比将进行两轮实验，第一轮将偏置RRT*算法和目标引力RRT*算法的采样次数都设置为3000，将改进RRT*算法的采样次数设置为2500，仿真结果如图10和表4 所示。

表4 第一轮对比实验结果

图10 第一轮对比实验

第二轮将偏置RRT*算法和目标引力RRT*算法的采样次数都设置为2000，将改进RRT*算法的采样次数设置为1500，仿真结果如表5和图11所示。

表5 第二轮对比实验结果

图11 第二轮对比实验

两轮的实验结果表明，尽管改进RRT*在迭代次数较少的情况下成功率略低，但是相比于另外两种算法耗时更少，规划出来的路径质量更优，同时随机树节点占用的内存远小于另外两种算法。

4 仿真实验验证

将改进的RRT*算法应用于机械臂的路径规划，采样点是笛卡尔坐标系中的三维矢量，如式（4）所示：

设随机树中的每个节点为9 维向量，如式(5)所示：

其中，Xi为随机树中的第i个节点，px、py、pz为路径规划算法得到的机械臂末端在三维空间中的一点Xnew的坐标，θ1、θ2、θ3、θ4、θ5、θ6为相应的机械臂6 个关节角度值，两者的关系如式(6)所示：

其中，f为机械臂逆运动学求解函数，具体求解算法见文献[22]，pitch、yaw、roll为机械臂末端执行器在整个路径规划过程中根据需求自行设定的相对于机械臂基座坐标系的抓取姿态参数，pitch为俯仰角（绕基坐标y轴的转角），yaw为航向角（绕基坐标x轴的转角），roll为横滚角（绕基坐标z轴的转角）。由于在某些工业应用时，为了安全起见，需要机械臂保持一个稳定的抓取姿态去完成抓取任务，所以将roll与pitch设置为0°，yaw设置为180°，使得机械臂始终保持一个夹爪掌心朝下的抓取姿态，如图12所示。

图12 抓取姿态示例

现将改进RRT*算法应用到机械臂路径规划中，改进的RRT*算法步骤如下：

1）确定起点、目标点位置和机械臂抓取姿态参数。

2）采样一点Xrand，从随机树中找到距离Xrand最近的节点，记作Xnear，从节点Xnear沿着Xnear与Xrand的连线方向生成一点Xnew，如图13 所示。设置步长为ρ，Xnew的计算公式如式(7)所示：

图13 RRT*节点扩展图示例

3）在Xnew处进行机械臂的逆运动学求解（见式(6)），得到各个关节角度。

4）通过正运动学求解得到各个关节在三维空间的位置，进而确定每个连杆的直线方程，然后检测每个连杆是否和障碍物相交[23]，如果相交则发生碰撞。

5）若没发生碰撞，则对Xnew进行父节点重选，在将Xnew加入随机树成为新的节点后，再进行剪枝。

6）若发生碰撞，则删除扩展节点Xnew，并返回步骤2）。

7）如果达到最大迭代次数，则输出路径信息，否则返回步骤2）。

本文采用Matlab 搭建仿真平台，其中的机械臂运动学模型由Matlab 机器人工具箱搭建[24]，DH 参数见表6。

表6 DH参数

改进的RRT*迭代次数设置为5000，球体和圆柱体为障碍物，起点设置为[0,400,200]，目标点设置为[300,10,450]，机械臂的基座坐标设置为[0,0,0]，步长设置为15，近邻节点选取范围为40。

改进RRT*算法的仿真结果如图14，球体和柱体为障碍物，蓝色曲线为改进RRT*优化路径。

图14 机械臂末端轨迹

将改进RRT*规划出来的关节角度数据进行线性插补2000 次，得到数据Q，然后在Matlab 命令窗口输入指令“robot.plot(Q)”就能让机械臂模型沿着图中的蓝色轨迹进行运动，如图15所示。图15(a)所示为机械臂的起点位置，图15(b)-图15(e)所示为机械臂运动过程的中间位置，图15(f)所示为机械臂的目标点位置。机械臂关节角度变化如图16所示，从图中可以看到机械臂能有效避开障碍物到达目标点。

图15 机械臂路径规划过程

图16 关节角度变化曲线

综上所述，无论是路径规划时间、长度、还是占用内存，改进RRT*算法都要优于传统RRT*，能够在较短的时间内搜索出接近最优的最短期望路径。改进RRT*在三维空间中规划出较优路径的情况下，关节角度变化曲线整体较为平滑，没有突变（见图16），这表明改进RRT*算法可以很好地应用到机械臂路径规划中，能够让机械臂更加安全高效地工作。

5 结束语

本文采用了改进RRT*算法来解决工业机械臂在狭窄障碍物空间中运动规划的难题。改进算法改变了父节点重选的方式去优化路径；然后通过改进RRT*算法的采样方式，提高了算法收敛速度；为满足机器人运动学规律，引入节点拒绝策略，消除了路径中过大的转弯角。采用MATLAB 进行了一系列仿真实验，证明了改进RRT*算法的可行性和有效性，可以快速地对机械臂进行路径规划，对机器人路径规划具有一定的参考价值。