串番茄采摘机械臂避障路径规划算法研究*

张天成，李奇林，裴天朔

（江苏理工学院机械工程学院，江苏常州 213001）

0 引言

如今，伴随着我国科学技术水平和高端制造业能力的不断提高，机器人技术已经应用到生活的各方各面[1]。尤其是在农业领域，机器人正在逐渐展现出其巨大的作用[2]。其中，果蔬采摘农业机器人在进入21 世纪以来发展趋势尤为迅速，当中具有代表性的实例有中国农业大学的黄瓜采摘机器人、草莓收获机器人以及陆华明团队的林木球果大型采摘机器人等。这一种类的机器人凭借视觉或其他传感器输入作业环境各项参数，再由机器人根据输入参数去进行机械臂的运动规划[3-4]完成采摘。值得注意的是，当前串番茄作为近年来最为流行的果蔬之一，其相关的果蔬采摘机器人研究却远远不如黄瓜、茄子之类的同样热度高的蔬菜。

果蔬机器人实现采摘的重要前提之一是机械臂的运动规划任务，而运动规划并不是简单的起点和终点间的距离，而是一条复杂的路径[5]。尤其是在环境中存在各种障碍物的情况下，针对采摘机械臂进行避障碍路径规划到达采摘位置对于机器人能否实现果实采摘格外重要。在诸多学者的研究下，现在常用的适用于蔬果采摘机械臂的路径规划算法有A*算法、栅格法、RRT 系列算法和人工势场法[5]。其中，人工势场法作为局部路径规划算法，具备结构简单、计算量小的特点，在路径规划任务中应用比较广泛。人工势场法是在1986 年由著名学者O Khatib 在提出[6]，其主要思想是在机械臂的作业空间内构造一种人为设置的势场。将机械臂起始点作为势场内的起点，路径规划的终点作为势场内的对机械臂的引力场，障碍物则作为势场内作用于机械臂的斥力场，在引力场和斥力场的合力作用下控制机械臂从起点至终点的无碰撞运动。

传统的人工势场法[7]的引力势场表达式如式（1）所示：

传统的人工势场法的斥力势场表达如式（2）所示：

其中，ka、kr为斥力势场的系数；X=（x，y，z）为机械臂末端点在笛卡尔空间的坐标向量；X0=（x0，y0，z0）为障碍物在笛卡尔空间的坐标向量；Xg=（xg，yg，zg）为目标点在笛卡尔空间的坐标向量；d为障碍物产生的斥力场范围的半径值大小。于是有机械臂在斥力势场和引力势场的共同作用下往目标点方向移动。

值得注意的是，人工势场法属于局部路径规划算法，缺乏对应用空间环境因素的全局认识，所以采摘机械臂在人工势场法的引导下，会因为斥力与引力相抵消而陷入势场极值的状态，此时机械臂的运动状态会处于一个空间来回振荡。所以，在使用人工势场法对机械臂进行路径规划时，需要针对人工势场法的不足进行算法的改进。姬伟团队[8]通过人工势场法引导苹果采摘机械臂路径规划过程中，面对极值问题，他们通过提出设立虚拟目标点来增大机械臂从局部极值逃脱的概率。这种做法为本文对人工势场法的改进提供了理论依据。于是本文鉴于当前串番茄果实采摘人口数量不足，相关的机械采摘研究也很少见，以串番茄采摘机械臂为应用背景，机械臂避障路径规划算法为研究对象，就探讨如何解决采摘机械臂避开环境障碍物到达果实采摘位置这个机械采值问题，提出了一种基于机械臂避障路径规划的将人工势场法进行改进，然后与RRT*算法相结合的改进算法。

1 人工势场法的改进内容

1.1 结合RRT＊算法帮助机械臂逃离人工势场法极值

假设机械臂连杆A 的末端关节在迭代i次后的位置为Ai，在迭代到i第n次时构型的关节位置An。当|Ai-An|的值与差值μ满足|Ai-An|≤μ的关系时，说明在迭代n次后，机械臂关节角的变化不大，且Ai与期望位置Agoal间也不满足 |Agoal-Ai|≤δ，这可以表明机械臂陷入到了极值点。其中参数μ为定义的差值，下文设为0.3°，参数δ表示Agoal附近有效范围的大小。为了让机械臂以最快速度摆脱局部极小值的束缚，所以通过设立一个临时目标点让机械臂在RRT*算法的引导下运动到该点去逃离极值。临时目标点的设立过程如下所示。

假设采摘机械臂在迭代i次后的关节组合为Ai。通过将期望关节组合与当前关节组合相减，即Agoal-Ai得出一个方向向量α。得出相减的方向向量α后，用α去乘以一个步长λ，可以得到一个基础增量β1，并判断位姿（Ai+β1）时采摘机械臂是否会与障碍物发生碰撞。如果发生碰撞，则定义位姿（Ai+β1）失败，取消基础增量β1。接着用方向向量α去乘以两个步长λ得到基础增量β2，判断位姿（Ai+β2）时是否采摘机械臂会与障碍物发生碰撞。如果不发生碰撞，则保留该关节组合。按如上方法计算n个基础增量βn。

计算上一节得到的不与障碍物发生碰撞的关节组合（Ai+βn）的总势能，并找出最小势能的关节组合。在找出最小总势能的关节组合后，将其同搜寻路径中邻近的关节组进行比较，如果与邻近关节组角度大小变化不超过0.3°，则表示两个关节组相同，以此来判断找出的最小总势能关节组合是否为局部最小点，如果不是，则将其关节组合作为逃出极值的临时目标点。然后通过RRT*算法控制机械臂运动到该目标构型去摆脱陷入极值的状况。除此之外，如果上述方法得不出临时目标点，则将路径规划任务期望构型作为RRT*算法的临时目标点，使用RRT*算法控制机械臂运动到该目标构型。

在上述方法的指引下，机械臂能够在运行过程中及时发现潜在的极值。当判断极值产生时，能够自适应的计算临时目标点的位置，并控制机械臂运动到该构型以摆脱陷入极值的状况。此方法可在串番茄采摘环境中降低极值对于机械臂路径规划的影响，解决了机械臂受人工势场法引导而陷入极值无法完成避障路径规划的难题。

1.2 改进人工势场法斥力函数

传统的人工势场法规划的路径中，如果期望目标周围存在障碍物，可能会造成Ai无法进入Agoal的有效范围内，导致人工势场法的可靠性将大幅降低。其原因如图1 所示。其中，Ai表示机械臂连杆A 的末端关节在迭代i次后的位置，Aobs表示空间障碍物，D为障碍物斥力场的半径范围，Agoal为期望目标。如1所示，Agoal处于障碍物Aobs的斥力场半径范围D内。此时，Ai受引力朝着Agoal的方向前进，随着 |Agoal-Ai|的值减小，Agoal引力场的场强也在逐渐减小，但随着Ai进入Agoal的斥力场范围，其所受到的斥力场的场强逐渐变强。在某个位置，Ai所受的引力场和斥力场刚好平衡，此时机械臂将会停止运动或者在等势圈上震荡，无法逃离。而此时Ai与期望目标Agoal且无法抵达不同，导致路径规划算法失败。

图1 处于斥力场内的目标点

基于上述讨论，为了防止机械臂停止运动或者在等势圈上震荡，需要对传统势场的斥力函数或者引力函数加以改进。通过传统斥力场表达公式可知，传统斥力场的场强公式缺乏对Agoal和Ai差值大小的考虑，这使得不管机械臂在空间何处，斥力场场强都由 |Aobs-Ai|的大小来决定。所以在对传统势力场原有的表达式改进中加入了距离因素，改进结果如式（3）、（4）、（5）所示。

2 改进后算法搜索空间与整体流程

传统规划算法的搜索可以在笛卡尔空间或者关节空间中进行。在笛卡尔空间中的路径规划比较直观，考虑的影响因素相比关节空间较少。但在笛卡尔空间对机械臂进行路径规划时会增加逆运动学求解的次数[9]，可能会造成关节轴的变化不连续、出现奇异解和计算时间过长等问题，最终可能会导致机械臂采摘失败。所以本文决定在关节空间进行搜索。关节空间内搜索到的点对于机械臂都是可以到达的，这样减少了逆运动学出现无解的情况，增加了算法的计算效率，再通过正逆运动学的数学关系实现机械臂在笛卡尔空间和关节空间坐标的转换。

改进算法的流程如图2所示。

图2 改进算法流程

3 基于Matlab的仿真实验分析

本节通过Matlab软件进行改进算法在串番茄采摘环境下仿真实验，验证了改进算法在串番茄采摘环境下的鲁棒性，并对比改进前人工势场法，验证了改进算法的优越性。

3.1 仿真环境建立

仿真实验在搭载64位Windows 10系统的PC机上进行，PC机主要硬件配置为AMD Ryzen 7 4800H CPU @2.90 GHz 16 GB DDR4 RAM。本章通过运用Matlab 2019a 上Robitic toolbox 插件检验改进算法在模拟串番茄采摘环境下相比于传统人工势场法的优越性。

为了真实模拟串番茄采摘环境，以甘肃省静远乡温室大棚串番茄种植地为虚拟样机模拟环境，取采摘环境如图3所示。

图3 串番茄采摘环境障碍物

串番茄的采摘环境的障碍物主要由串番茄植株茎秆、植株枝条和非本次规划采摘的其他串番茄果实串组成，在仿真过程中选择AABB 包围盒技术[10]对障碍物以不同形状的圆柱体进行包络，以此来代替不规则形状的障碍物进行避障仿真实验。

3.2 逃离局部极值对比检验

在本节中，将从算法的逃离极值方面的改进效果方面进行仿真实验，对比验证改进算法相对于传统人工势场法的优越性。在环境中各项输入参数如表1所示。

表1 不同环境参数

避障环境下机械臂在传统人工势场算法控制下和改进算法控制下的运动轨迹仿真结果分别如图4、5 所示。如图4 所示，在避障环境下，机械臂在传统人工势场算法的引导下从起始关节组合（10，0，30，40，50，60）出发，未能成功到达期望关节组合有效范围内，导致采摘路径规划失败。如图5 所示，在避障环境下，采摘机械臂在改进算法的引导下成功抵达目标。机械臂从起始关节组合出发，在改进的人工势场法的引导成功运动到期望关节组合。

图4 APF机械臂路径规划轨迹

图5 改进算法机械臂路径规划轨迹

为了进一步分析采摘机械臂路径规划的信息，机械臂在人工势场法和改进算法下的各关节所受总势能随步数变化趋势图如图6、图7所示。

图6 APF路径规划总势能变化

图7 改进算法路径规划总势能变化

由图6 可以发现采摘机械臂在传统人工势场法的引导下，在X=4步后总势能就呈现出振荡的现象，直接证明了此时采摘机械臂已经陷入极值。而如图7 所示，当采摘机械臂在改进算法的引导下，在运动到X=47 步时，机械臂陷入了极值，并在X=78步数时，总势能达到了路径规划过程中的最高值。这是因为采摘机械臂连杆在进入了障碍物斥力场的作用范围，随着机械臂连杆与障碍物的距离变小，导致总斥力势能也随之增大，最终出现斥力大于引力的情况，这就导致机械臂当前关节组合会远离期望的关节组合，回到上一个构型。但当机械臂处于上一个构型时，所受引力又大于斥力，机械臂继续运动到之前处于的构型，如此反复，形成震荡。此时停止改进人工势场法对机械臂的引导，该用自适应的方法去寻找临时可以逃离极值的关节组合，然后运用RRT*算法去控制机械臂运动到此构型逃离极值。

机械臂在人工势场法引导下和改进算法引导下关节角度随步数变化趋势如图8、9 所示。由图8可以明显看出，在传统人工势场法的引导下，关节1在避障环境中的路径规划的第4步处于振荡状态，并在随后的过程中角度一直在1°～4°之间振荡，进一步证明了采摘机械臂已经陷入极值。并经过计算，当前位置并不处于期望关节组合有效范围内，路径规划失败。由图9可以看出，采摘机械臂关节组合最终到达（140，29，30，2，30，0），到达期望关节组合（140，30，20，0，30，0）有效范围内（＜3°），路径规划成功。

图8 人工势场法关节角度变化趋势

图9 改进算法关节角度变化趋势

通过上述对比实验可看出本文改进算法在处理局部极值问题时具备可靠性，在仿真实验中陷入极值次数超过10 次，但都能成功引导机械臂逃离极值继续完成采摘机械臂的避障路径规划。

3.3 势场函数对比检验

在避障环境中各项输入参数如表2所示。

表2 避障环境输入参数

在上述环境下，分别用传统人工势场法和改进算法对串番茄采摘机械臂进行避障路径规划仿真，其仿真运动轨迹如图10、图11 所示。其中，采摘机械臂在传统人工势场法和改进算法引导下的各关节角度随步数变化趋势如图12、图13 所示。

图10 APF机械臂路径规划轨迹

图11 改进算法机械臂路径规划轨迹

图12 APF关节角度变化趋势

图13 改进算法关节角度变化趋势

如图10～11 所示，可以看出采摘机械臂在传统人工势场法引导下并未到达期望目标关节有效范围内。结合图12、图13所示，发现采摘机械臂在人工势场法的引导下，于X=60 步后各关节停止转动，出现此情况是因为采摘机械臂在该位置附近受到的叠加势场刚好平衡，即此时采摘机械臂受到障碍物对其斥力和期望坐标对其引力大小相等，方向相反，达到了一个平衡位置，使得机械臂停滞不前，造成目标不可达的结果，导致路径规划的失败。而采摘机械臂在改进算法的引导下，于X=78 步靠近密集障碍物区域，该位置附近受到的叠加势场平衡，使得机械臂停滞不前，虽然机械臂可以通过RRT*算法来进行引导，但由于目标坐标处于障碍物斥力范围内，所以机械臂无论如何逃离，都不会接近到目标坐标（除非将虚拟目标构型定为期望关节构型，在接下来的规划中全程使用RRT*算法去引导采摘机械臂，但这回导致计算量陡然上升，并且机械臂采摘效率会严重下降）。在对势场函数进行改进后，有效抑制随着机械臂靠近障碍物而斥力势能无限增大的趋势，使其能在引力势能下顺利运动到目标坐标，此时斥力势能和引力势能都将为0。如上所分析，采摘机械臂在改进算法引导下有效逃离平衡势场影响，并在X=123 步时到达关节组合（142，15，20，0.3，30，0），处于期望关节组合（140，15，20，0，30，0）（＜3°）有效范围内，表明路径规划成功。

上述实验结果表明，在算法的斥力势函数中加入关节空间中当前采摘机械臂构型与期望构型的距离影响因素和当前采摘机械臂构型与会受到障碍物斥力影响的构型的距离影响因素，可以有效的约束斥力场的增长，从而逃离平衡势场，使得机械臂路径规划成功。

3.4 鲁棒性检验

通过上节的对比实验验证了本文改进算法的优越性，考虑到上文实验只在三障碍物障碍的环境下进行了实验，不足以证明改进算法的鲁棒性。于是，本节将改进算法用于不同障碍物数量的作业环境，以更好地证明改进算法的鲁棒性。

在本节中，将额外构造两种障碍物数量不同的机械臂采摘作业空间，障碍物的数量为1 个和5 个。在环境中各项输入参数如表3所示。

表3 避障环境输入参数

作业环境1 中有一个障碍物。采摘机械臂在作业环境1 中受改进算法引导下的路径规划效果如图14 所示，关节组合及末端坐标的变化如图15所示。

图14 作业环境1规划效果

图15 作业环境1关节组合变化

由图14、15可知，采摘机械臂在作业环境1下，受改进算法引导在X=88 步到达关节组合（138，16，20，0，30，1），成功到达期望关节组合（140，15，20，0，30，0）有效范围内（＜3°），路径规划成功。

在作业环境2 有5 个障碍物。采摘机械臂在作业环境2 中受改进算法引导下的路径规划效果如图16 所示，关节组合及末端坐标的变化如图17所示。

图16 作业环境4规划效果

图17 作业环境2 关节组合及末端坐标的变化

由图17可知，采摘机械臂在作业环境2下，受改进算法引导在X=163步到达关节组合（139，16，19，1，30，0），成功到达期望关节组合（140，15，20，0，30，0）有效范围内（＜3°），路径规划成功。

如上文所述，采摘机械臂在3 种不同的作业场景中（包括对比验证的三障碍物环境），改进算法都可以引导采摘机械臂做出有效的避障路径规划，进一步验证了改进的人工势场法与RRT*算法结合算法在应用于穿番茄采摘机械臂在面对不同采摘环境进行路径规划的鲁棒性。

4 结束语

本文通过提出将人工势场法进行改进，然后与RRT*算法结合的路径规划算法，解决了如何去引导在串番茄采摘环境下机械臂在串番茄采摘环境下进行避障路径规划的机械采摘问题。改进结果如下。

（1）在逃离局部极值对比检验中，人工势场法引导下的机械臂在步数为4 步时陷入极值状况，关节1 角度在1°～4°之间来回振荡，路径规划失败。机械臂在改进算法的引导下，机械臂在步数为47 步时陷入极值，并由RRT*算法引导机械臂运动到临时目标构型摆脱极值困境，该实验中机械臂前后一共超过10次陷入极值状态，但都成功逃离。

（2）在势场函数对比检验中，在传统人工势场法的引导下，机械臂在步数为60 步时各关节停止运动，导致目标不可达现象，是的机械臂路径规划失败；在改进算法的引导下，机械臂在步数为123 步时成功抵达期望构型，完成路径规划任务。以上验证了改进算法相对于人工势场法的优越性。

（3）在鲁棒性检验中，当环境内障碍物为一个时，机械臂于步数为88 步时顺利抵达期望构型；当环境内障碍物为五个时，机械臂于步数为163 步时顺利抵达期望构型且不发生碰撞，证明了改进算法在不同环境下的鲁棒性。

以上仿真的结果为改进算法应用到实际串番茄采摘机器人避障路径规划中提供了理论依据。