改进Informed-RRT*算法的柑橘采摘机械臂运动路径规划

刘 顿，王 毅

(重庆理工大学 机械工程学院， 重庆 400054)

近年来，随着工业机器人的快速发展和我国乡村振兴战略推进，利用机器人提高农业生产效率成为一个新的研究方向，果蔬产品的收获采摘是农业生产的重要组成部分，占到其生产过程作业量的40%，利用机器人来完成果蔬采摘收获是农业自动化、智能化发展的重点研发领域[1-4]。机器人在采摘空间合理的运动路径是保证采摘成功率和缩短采摘时间的关键因素。在自然环境中，果蔬生长位置随机，大多被枝干、枝叶等障碍物包围，在实际采摘中带有末端执行器的机械臂很难通过一次规划来完成采摘，目前大多机器人都需要多次规划路径并满足合理的采摘姿态才能保证采摘顺利进行。2017年，Lehnert等[5]研制了6轴机械臂与带有吸盘切割刀片的末端执行器组合的甜椒主动收获机器人，该型收获机器人在室外实验的成功率为46%～58%，并且成功收获果实的机械臂路径规划尝试次数都在2次以上。2019年，Xiong等[6]设计了带有障碍物分离算法的草莓自动采摘机器人，该机器人在尝试采摘草莓时，最多含有5段行程，包括机械臂前移、采摘预备、采摘控制、花梗剪切位置调整、返回，采摘成功率在50%～90%之间波动。2019年，王毅[7]提出了一种基于预采摘点的采摘方法来提高采摘成功率，机械臂分为两段运动，先运动到中间点再调整姿态完成采摘，室外单果采摘成功率为87.5%。

运动路径的最优性也很关键，这样不仅可以节约采摘时间，提高采摘效率，还可以减少机械臂的运动幅度，减小与外界发生碰撞的可能，这就对机械臂的路径规划算法提出了新的要求。基于采样的路径规划算法，基于采样的快速扩展随机树算法(rapid-exploration random tree,RRT)，以增量方法在工作空间中随机采样，能快速有效地探索高维空间，在机械臂运动规划中运用广泛。但是该算法存在收敛速度慢，生成路径非最优、不平滑的问题。Karaman等[8]提出RRT*，通过重新选取父节点和重新布线找到最优路径，但是当环境较为复杂，需要节点较多时，算法的内存消耗和计算时间会成倍增长。Gammell等[9]提出Informed-RRT*，通过生成超椭球子集然后对齐集中采样，让搜索更加集中，对规划问题的维度和域的依赖性更小，能够更快更好地找到不同的路径;但是该算法的生成超椭圆子集的大小严重依赖初始路径的优劣，并且当超椭圆子集体大于规划问题的范围时，该算法将不再适用[10]。

路径规划算法在采摘机械臂的运用方面,Ademovic等[11]在RRT基础上，利用障碍点扩展体积信息，提出了构型空间中机械臂搜寻无碰撞路径的方法，提高了机械臂高维空间的探索能力。2019年，马冀桐等[12]针对采摘机械臂需要深入树冠内部进行采摘的问题，提出了一种基于构型空间先验知识引导点的RRT改进算法，通过增加引导点提高了机械臂通过树冠内众多枝干构成的窄通道的能力。2021年，张勤等[13]在RRT算法基础上，引入启发式采样和节点拒绝策略，改善了路径成本，提升了收敛速度。有研究报道，当前大多数机器人系统执行轨迹要比规划路径花费更多时间[14]。但是目前很少有采摘机械臂规划算法以减少采摘机械臂规划和运动次数来提高采摘效率，减少采摘耗时的报道。

本文针对目前采摘机械臂完成采摘存在多次行程规划效率低、耗时长、路径非最优等问题，在渐进最优算法Informed-RRT*基础上，结合采摘环境与采摘动作，在机械臂构型空间中定义了预采摘点，提出了四颗随机树同时探索方法，同时加入自适应步长函数和启发式的节点优化策略。可一次性规划出满足采摘的机械臂运动路径，并且保证了采摘路径质量，降低路径规划的时间。

1 算法提出

1.1 基于预采摘引导点的四颗随机树并行探索

为了保证采摘顺利进行，预采摘点的设置至关重要。王燕[15]采用两段摆线运动组合的轨迹规划方法使机械手的末端执行器在采摘果实时始终保持为水平状态，机械臂保持水平采摘运动也会缩小机械臂的采摘距离。因此，本文设置的预采摘点位置为与果实水平且相距X=25 cm的位置，如图1所示。

图1 水平预采摘位置设置

同时将采摘引导点纳入到规划算法中，在构型空间探索中，以预采摘点为第3点实现四颗树同时扩展。假设在构型空间中，果实目标点的坐标为(x，y，z)，则预采摘点的坐标为(x1，y1，z1)，如图2所示。Informed-RRT*算法采用原始的RRT算法生成一条初始路径Cbest，而初始路径的好坏直接影响该算法效率。原始的RRT算法存在的一些缺陷，双向搜索是一种常用的改进策略，已经被证明可以明显提高基于采样的运动规划算法的成功率和收敛速度。并且为了加快随机到达目标点的速度，在随机树的每次生长过程中，加入随机概率P(0.0-1.0)的函数ChoseNode来选择生长方向是目标点还是随机点。规划器在双树随机采样扩展的同时，以P概率向终点目标运动，如图3所示。

图2 构型空间中预采摘点位置

图3 加入概率P函数的双树探索

函数:ChoseNode(qnew, P)1 if Rand≤P2 qrand=qgoal else3 qrand=Rand

1.2 自适应动态步长

原始的Informed-RRT*算法中，在对未知空间探索时步长是不变的，这种定值步长在探索无障碍空间时会影响随机树的扩展速度。本文在算法中加入了动态步长的调节函数AdustableSteer，引入调节系数k，具体为：随机树以初始步长ε向目标探索，在扩展新节点时，如在节点处没有发生碰撞且未与另一颗树连接，则在下一次扩展中，步长为ε=ε+kε，依次叠加。步长越大，算法扩展速度就越强，收敛速度也越快，但是在环境复杂、障碍物较多时，机械臂的局部路径也会变长，与环境发生碰撞的可能性也会增加，因此在遇到障碍物碰撞之后，步长变为原始步长，调节系数由正变负，依次叠加，直到脱离局部路径。

1.3 启发式节点优化策略

在 Informed-RRT*算法的路径优化过程采用RRT*类似的方法，即对潜在最优父节点的搜索对象是由路径规划过程中构成的节点数。潜在的最优父节点的数量只有少数几个，而检测的过程中需要检测所有节点，存在检测多余节点而造成路径规划效率低的问题。

改进算法在Informed-RRT*基础上，加入启发式的节点优化策略NodeOptimazation函数，在路径优化时，以更少的迭代次数和更少的节点来优化路径，提高效率。在节点qnew扩展前，计算出该节点到起始点路径的代价α，再通过先验知识得到该节点到目标点的估计代价β。如果条件成立即加入该点的估计代价小于当前路径代价，那么该节点就有可能改善当前路径，从而将该点纳入扩展树中。

αstar-to-q+βq-to-goal≤Cbest

(1)

式中:αstar-to-q为扩展节点qnew到起始点路径的代价;βq-to-goal为qnew到目标点的直线距离;Cbest为当前最低的路径代价。

1.4 采摘算法的实现

图4 算法节点扩展过程示意图

2 实验验证与结果分析

2.1 二维环境仿真实验与讨论

在Matlab软件中进行二维算法模拟,设置了3种模拟环境：无障碍环境、简单障碍环境以及二维采摘模拟环境。简单障碍在目标点之间分布3个固定障碍物；二维采摘模拟环境是对实际采摘环境进行二维映射，果实目标点被枝干等障碍物包围。起始点为(130，130)，目标点为(710，520)。二维仿真实验中，概率P设为0.1，步长设置为20，动态步长调节系数K为0.1，最大步长设置为30，搜索半径为120，最大迭代次数为1 000。仿真环境的硬件信息为：CPU为i7-6700,主频3.4 GHz，显卡为GTX1060，24 G内存，1 TB硬盘，操作系统为Windows 10专业版。

基于上述的3种模拟环境分别进行50次模拟实验，比较Informed-RRT*与本文提出的改进算法的路径规划，验证算法的有效性。仿真过程如图4所示，红色为起始点，绿色为目标点，紫色为预采摘点，黑色为障碍物；绿色曲线为节点重新布线，蓝色曲线为规划算法生成的初始的路径，黑色曲线为从算法优化更新后的路径，黄色曲线为最终生成的路径。

比较图5、6算法路径规划图，可以看出2种算法都具有明显的渐进最优性，改进Inform-RRT*算法在进行路径规划时，随机扩展的节点和绿色曲线明显减少，最终的路径成本也更小，表明改进算法提高了节点的利用率，减少了路径探索的盲目性，提高了路径规划的效率，降低了路径成本。

记录二维模拟环境中20次重复实验的相关数据，如表1所示。由于Informed-RRT*无法一次规划出两段路径，因此在同一张地图下，进行2次原始Informed-RRT*算法仿真，将2次的算法规划相关数据相加。相较于原始Informed-RRT*算法，改进的算法在初始路径和最终路径都有明显的改进，在初始路径阶段加入了四树探索、P概率采样以及动态步长，大大加速了路径探索的速度；在生成初始路径阶段，改进后的算法路径使成本减少了12%，时间缩短了78%，节点数缩短了24%，平均迭代次数下降了60%。通过节点优化策略剔除了不能改善当前路径的节点，大大提高了路径优化的效率。在路径优化阶段，改进后的算法路径使成本减少了6%，时间缩短了88%，节点数缩短了42%，平均迭代次数下降了64%。

图5 改进Informed-RRT*算法路径规划

图6 原始Informed-RRT*算法路径规划

表1 算法二维仿真结果数据

2.2 三维采摘仿真实验与结果分析

2.2.1 实验设计

利用本团队研发的在Linux系统下基于ROS的采摘机器人平台[5]验证改进算法，该机器人由机械臂、视觉系统及移动底盘等构成。机械臂为北京遨博公司生产的协作机械臂aubo_i5，机械臂的采摘范围是0～2 m，如图7所示。

为了更好地验证改进算法在采摘环境下的性能，在windows下安装的Ubuntu 虚拟机，分配4GB的内存大小，GPU为GTX1060，搭建基于ROS的柑橘采摘机器人仿真控制平台，如图7所示。

1.末端执行器; 2.aubo机械臂; 3.双目摄像头; 4.履带底盘; 5.机器人外壳; 6.果实盛放口

为了验证本文所提出的轨迹规划算法是否能在高维环境中实现采摘路径的一次性规划。利用ROS机器人系统中的moveit插件和Rivz虚拟仿真环境进行了仿真实验。在Rviz环境中，为减小碰撞检测的计算量，对柑橘采摘机器人模型进行简化，使用简单的长方体包围盒代替复杂的底座三维形状，如图8所示，在ROS系统的rviz仿真环境中以绿色圆柱体外包络线表示枝干，橘黄色的球体表示柑橘，重建了4棵柑橘树，进行了仿真环境下的柑橘单果采摘避障规划实验。将设计的改进算法添加到moveit的开源运动规划库OMPL中，并与RRI-connect、RRT*、Informed-RRT*这3种基于采样的路径规划算法进行比较。实验中使用其他算法时，采用2次规划两段路径来实现采摘。采摘姿态均采用水平姿态采摘。采摘机械臂的初始姿态均为(0，282，234,0,0,0)，如图9所示。在不同的采摘环境下，每种算法进行20次重复采摘规划实验。改进算法的为黄色路径，其他算法路径为蓝色，如图10所示。

图8 机器人本体模型 图9 机械臂的初始位姿

图10 路径规划仿真结果

2.2.2实验过程和分析

仿真实验结果表明：改进后的算法可以实现复杂采摘环境的避障和采摘。由表2数据可以看出，基于预采摘点的改进算法显著提高了在复杂障碍采摘环境下采摘路径规划的规划时间和成功率，在不同的采摘环境下,改进算法的平均规划时间约为0.81 s，规划成功率约为96%。原始的Informed-RRT*算法的路径比RRT-connect和RRT*算法有明显改善。相较于使用Informed-RRT*算法来采摘规划，改进算法规划时间减少了约46%，规划成功率提高约11%，路径成本优化了约16%。

表2 三维仿真采摘环境下路径 规划算法数据比较

续表(表2)

3 结论

针对目前采摘机械臂完成采摘存在多次行程规划效率低、路径成本高、优化不足的问题，提出了基于Informed-RRT*改进的规划算法。仿真实验表明：基于Informed-RRT*改进的采摘算法通过在构型空间内加入预采摘引导点可实现一次就规划出满足采摘需求的机械臂运动路径，提高了机械臂采摘效率。

基于Informed-RRT*改进的采摘算法通过引入预采摘点，在初始点与预采摘点和预采摘点与目标点之间四树并行扩展；用P概率采样取代随机采样改善采样的盲目性；通过动态步长生成新的节点，提高了算法探索未知空间的速度和灵活性；节约了采摘路径规划时间。并且加入了启发性的节点优化策略，提高了算法路径渐进优化效率。实验表明：相较于原始的Informed-RRT*算法，规划耗时减少46%，规划成功率提高了11%。