基于改进RRT的采摘机械臂避障运动规划研究

陶 亮，陈成鹏，潘伟豪，汤先美，辜丽川，焦 俊

（1.安徽农业大学信息与计算机学院，合肥 230036；2.安徽中烟工业有限公司蚌埠卷烟厂，安徽蚌埠 233010）

机械臂路径规划是机器人技术的一个重要研究领域，主要为机械臂提供一条安全、有效、精确的路径，使机械臂能够完成预定的任务。在科学技术发展和国家政策的支持下，中国对果实采摘机器人研究也取得重大进展，其研发部门主要是一些科研院所和高校。[1-4]为了使机械臂能够完成这些任务，需要对机械臂的路径进行规划。由于机械臂具有多自由度和高度的路径灵活性，所以它的路径规划问题具有很高的复杂度，需要一些高效算法来解决。适合机械臂路径规划的算法有很多，如遗传算法[5]、粒子群算法[6]、A*算法[7]、人工势场法[8]、蚁群算法[9]、RRT 算法等。由于RRT 算法有很好的鲁棒性和适应性，故搜索效率高，不需要考虑算法运行的场景问题，即便有错综复杂的状态下，它都可以处理路径规划方面的问题，这使得该算法近年来在路径规划领域有很好的应用前途和价值。因此，选择RRT算法进行采摘机械臂路径规划实验研究具有一定的可行性。

路径规划时，机械臂必须避开障碍物，同时保证路径的精度和平稳性。在机械臂路径规划方面，相关论文在国内外研究中有很多：文献[10-13]对六自由度机械臂的构建，进行了路径学和工作空间研究分析，不足之处在于没有对机械臂路径规划仿真和理论分析；文献[14-16]提出了一种改进RRT*算法，RRT*算法，比传统RRT 算法在节点上增加了优化剪枝方法，而RRT*算法在随机树搜索过程中具有盲目性，无法找到最短搜索路径；文献[17-19]提出了一种RRT-Connect 算法，该算法从起点和终点位置进行检索并扩展搜索树，随着一次次的检索过程中，这2 棵搜索树可能会发生交汇，这样的话会结束检索并产生无碰撞轨迹。该算法缺点是随机树在搜索过程中产生扩展节点太多，影响算法的效率。

本文研究基于改进RRT 的采摘机械臂避障运动规划。首先，对其进行机械臂运动学分析与碰撞检测；然后，引入APF 算法和搜索树优化剪枝策略改进RRT算法模型，并进行实验和比较分析；最后，采用改进的RRT进行机械臂避障仿真实验。实验证明，改进RRT算法在农业采摘机械臂避障路径规划中寻优路线是有效的。

1 机械臂运动学分析与碰撞检测

1.1 机械臂运动学分析

本文以Aubo-i5 机械臂(见图1)为例，进行机械臂运动学分析，其连杆坐标系及相对应的D-H 参数表，如图2、表1所示。求出坐标系{i}相对于坐标系{i-1}的变换，相邻连杆之间变换的的位姿，进而求出相邻连杆坐标系的变换矩阵。

图1 Aubo-i5机械臂

图2 机械臂连杆坐标系

表1 D-H参数表

在表1 的连杆参数中，an表示连杆长度，dn表示关节偏置距离，αn表示连杆扭脚，关节转角θn为未知参数。

根据式（1）和表1，得到连杆坐标变换矩阵，通过计算机械臂连杆坐标变换矩阵的乘积，进而知道机械臂末端坐标系相对于基座坐标系的齐次变换矩阵，即为机械臂的总变换矩阵：

在式（2）中，机械臂的位资由m、n、k三个列向量表示，它的三维坐标由p所在的列向量表示。

1.2 碰撞检测

碰撞检测是路径规划中的一个不可缺少的步骤，通过检测机械臂和工作空间中障碍物的位置，计算出它们之间的距离大小。考虑到机械臂在路径规划中多发生在刚体间，进行简化处理，对于机械臂关节、连杆和障碍物间的采用包络法处理，因此能够将碰撞检测问题变换为球体和球体、柱体和球体的关系，由此计算量大幅减少，算法效率得到提升。

1.2.1 球体之间的包络法

简化的机械臂关节与球状障碍物之间的位置关系如图3所示。

图3 简化的关节与球状障碍物

在三维空间中O1=(x1,y1,z1)，O2=(x2,y2,z2)。O1与O2的为

在图3、（3）中，O1与O2分别表示机械臂关节与球状障碍物的球心，r1与r2分别表示机械臂关节与球状障碍物的半径，|O1O2|表示机械臂关节与球状障碍物的球心间的距离。

1）若|O1O2|≥r1+r2，则机械臂关节与球状障碍物没有发生碰撞；

2）若|O1O2|

1.2.2 柱体与球体之间的包络法

简化的机械臂连杆与球状障碍物之间的位置关系如图4 所示，这里为了方便计算，将机械臂连杆与球状障碍物的碰撞检测转换成线段与球状障碍物的位置关系。

图4 简化的连杆与球状障碍物

设点O2到直线PQ存在最短距离dmin，连杆与球状障碍物的位置关系有以下几种情况：

1）若dmin≥r1+r2，则机械臂连杆与球状障碍物没有发生碰撞；

2）若dmin

①若|O2P|

图5 线段与球状障碍物的位置关系

②若|O2P|>r1+r2∧|O2Q|>r1+r2，则有以下2种情况存在：

(i)当点P、Q在点O2的两侧，机械臂连杆与球状障碍物发生碰撞，如图5（b）;

(ii)当点P、Q在点O2的同一侧，机械臂连杆与球状障碍物没有发生碰撞，如图5（c）。

2 改进RRT算法

由于传统RRT 算法采用了随机扩展方式，搜索空间比较大，随机扩展的节点数量较多，导致算法的时间和空间复杂度较高等问题。针对上述存在的不足，本文对传统RRT算法进行改进。将改进RRT算法的整体思路是在原有RRT 算法的基础上，通过引入新的算法策略、优化其节点扩展等方式，进一步提高其路径规划效率和质量。具体思路如下：

（1）引入新的机制：引入人工势场法，以减少整个搜索空间的大小，降低算法的时间复杂度和空间复杂度。

（2）优化节点扩展策略：对节点的扩展策略进行优化，引入广度优先搜索算法进行搜索树优化剪枝路径，再进行路径规划等，从而避免节点扩展过程中的冗余计算，提高算法的运行速度。

（3）改进算法流程：在原有RRT 算法的基础上，可以对其算法流程进行改进，将RRT 算法中的部分操作替换成其改进版，从而提高路径规划效率和质量。

2.1 引入APF算法

人工势场法（又称APF算法）在RRT算法中添加APF算法机制，APF算法的工作原理是在机械臂路径规划避障过程中会设置俩个势场，分别为引力场和斥力场。如果将APF算法添加到RRT算法中，引力场会将搜索树的节点逐步向目标节点扩展，减少算法迭代的时间；斥力场会将搜索树的节点在扩展搜索过程中尽早避开障碍物。这样的话，在引力场和斥力场存在的复合作用下，将使搜索树节点在没有发生碰撞的情况下更好的向目标节点扩展。搜索树在RRT算法中扩展过程的节点在人工势场下的受力情况，如图6所示。

图6 搜索树节点的受力情况

搜索树节点在引力场下所受到的引力为Fg，此时引力势函数可表示为：

在公式4中，kg表示引力常系数，dg表示目标点Xgoal距离搜索树节点Xnew的大小，dg′表示搜索树节点Xnew到目标点Xgoal方向上的方向导数。

搜索树节点在斥力场下所受到的斥力为Fr，此时斥力势函数可表示为：

在公式5中，kr表示斥力常系数，do表示障碍物距离搜索树节点Xnew的大小，p0表示斥力场的作用范围半径大小，p′0表示搜索树节点Xnew到障碍物方向上的方向导数。

故搜索树节点在引力场和斥力场下所受到的合力为Fs

引入APF 算法机制后，搜索树节点扩展过程中明显趋向于目标节点速度加快，但在路径规划过程中仍然存在较多搜索节点，这时候需要进行剪枝处理，接下来继续进行优化。

2.2 搜索树优化剪枝策略

传统的RRT 算法在搜索树节点扩展过程中，存在搜索节点无限扩展的劣势，这样会大量占用计算机内存，影响算法效率。下面将采用搜索树优化剪枝策略减少节点数量，减少RRT 算法迭代次数，进而提高算法性能。这里引入广度优先搜索算法进行搜索树优化剪枝路径，如图7 所示。其中红色节点Xstart表示起点，黄色节点Xgoal表示目标点，蓝色节点X2、X6、X8、表示冗余的点，黑色矩形、圆表示障碍物。

图7 搜索树节点优化剪枝方法

具体搜索树优化剪枝策略步骤如下：（1）将搜索树节点在扩展过程中形成的所有节点进行相互连接，若存在与障碍物冲突的搜索路径，进行删除操作，并且进行计算，将搜索树节点之间的搜索路径设置为带权路径长度，并统计记录最小的权重搜索节点；（2）及时迭代计算搜索树起始节点Xstart的最小的权重和最小权重的搜索树节点Xi，并继续进行搜索树循环扩展，直到扩展到目标点Xgoal变成最小的权重点；（3）根据图7 的优化剪枝策略方法，可以得到含有更少节点的搜索树路径，进而提高机械臂路径规划的效率。

2.3 改进RRT算法流程

与传统RRT算法相比，改进后的RRT算法的流程图，如图8所示。

图8 改进RRT算法的流程图

3 仿真实验

3.1 二维环境下仿真实验

为了验证本文所提出的改进算法在二维环境下的可行性，本次实验设计了三种不同复杂度的地图环境，黑色部分表示静态障碍物，而空白区域表示可进入区域。设置地图大小为500x500，起始点位置坐标为[10，10]，目标点位置坐标为[490，490]，固定步长为10，阀值为10，分别进行300 次实验，并使用MATLAB仿真实验对不同算法进行了测试。

3.1.1 场景1简单环境

二维环境简单地图下，进行四种算法路径规划对比实验。其中，简单环境下4 种算法路径规划轨迹、实验数据结果分别如图9、表2所示。

图9 简单环境下四种算法的路径规划

表2 简单环境下四种算法的路径规划实验数据对比

由图9 可以看出，RRT 算法的的搜索树节点较多且路径较长，RRT*算法虽然减少了搜索书的节点，但是搜索路径明显更长，RRT-Connect算法使用2棵树同时进行搜索，虽然搜索时间有所提升，但是搜索树的节点数太多，搜索过程更为复杂，程序运行是需要占用更多的内存。结合图9和表2中的实验数据结果对比，可知本文算法在相同迭代次数环境下，其搜索路经长度更短，搜索树节点更少，收敛速度更快且搜索成功率达到了98%。

3.1.2 场景2复杂环境

跟场景1 相比，场景2的地图环境更为复杂，设置更多的障碍物是为了检验不同算法的对复杂环境的自适应性，更能突出改进算法的优势。复杂环境下4种算法路径规划轨迹、实验数据结果分别如图10、表3所示。

图10 复杂环境下四种算法的路径规划

表3 复杂环境下四种算法的路径规划实验数据对比

由图10 可以看出，相比于其他3 种基本算法，在复杂环境中本文算法能够进行更好的避障和适应性。从表3 中可以得出，本文算法在平均路经长度和平均运行时间方面，能够在更短的时间内获得低成本的路径长度。

3.1.3 场景3狭窄通道环境

跟场景1、场景2相比，在场景3的环境中有相同的起点和终点中间设置了三条狭窄通道，该设置的初衷是为了检测不同RRT算法在狭窄通道中的路劲搜索规划准确性。图11 和表4 展示了在狭窄通道环境下四种算法的路径规划和实验分析。

图11 狭窄通道环境下四种算法的路径规划

表4 狭窄通道环境下四种算法的路径规划实验数据对比

由图11 和表4 中可以看出，在狭窄通道环境中，RRT 算法和RRT*算法的检索路径较长，但RRT*算法由于优化剪枝的过程，故搜索树节点相对较少；RRT-Connect 算法与本文算法的检索路径短，检索时间少，但本文算法在搜索过程中的搜索树节点更少，可以得到较短的搜索路径，很好的体现出窄通道环境下剪枝策略的搜索优势。总的来看，在狭窄通道中，本文算法搜索路径、运行时间更短，成功率更高。

为了验证本研究所提出的改进算法的性能，在二维环境下构建了三种复杂度不同的地图，并进行了路径规划仿真实验。算法的收敛速度是衡量其效率的重要指标，通过对300 次实验数据的记录和统计分析，得到了四种算法的平均运行时间随着实验次数的变化曲线，该曲线的结果图如图12所示。实验证明本文算法的的收敛速度较快，效率高。

图12 四种算法平均运行时间对比

3.2 三维环境下机械臂避障抓取仿真实验

为了进一步验证本文提出的改进RRT 算法在复杂障碍环境下的适用性，设计了障碍物环境，在MATLAB 2021a中建立Aubo-i5机械臂模型，并将本文改进的RRT算法应用到采摘机械臂的避障路径规划中进行了验证。

如图13所示，其中绿色长方体表示障碍物，空白区域则代表着采摘机械臂可进入的区域。在障碍物环境下，进行了采摘机械臂的路径规划实验，机械臂从起始状态到达终点状态时并未发生碰撞，且路径是平滑的，说明本文算法在机械臂避障路径规划是可行的。

图13 障碍物环境下采摘机械臂避障路径规划

4 结论

本文针对传统RRT 算法在农业采摘机械臂避障路径规划过程中，存在的的搜索方向具有盲目性、搜索节点较多等问题，采用一种改进的RRT 算法设计进行解决。先对机械臂进行运动学分析和建模，然后引入APF 算法机制和广度优先搜索算法优化策略改进RRT算法，进行二维、三维环境下仿真避障实验，最后实验结果表明改进后的算法相对于传统算法收敛速度加快，盲目性、搜索节点较多问题也得到解决。对RRT算法的研究具有一定的参考价值，为进一步研究果实采摘机器人提供了实验依据。