改进的APF算法在采摘机械手运动规划中的应用

2023-12-20姚国林王合闯

机床与液压 2023年23期

姚国林，王合闯

(1.河南农业职业学院机电工程学院，河南郑州 451450；2.华北水利水电大学信息工程学院，河南郑州 450045)

0 前言

采摘机器人一直是智能农业装备领域的研究热点，由于机械手的可操作性和精度较高，一般采用机械手作为采摘机器人的主要执行部件[1]。采用机械手作为采摘机器人的主要执行部件，首先需要解决机械手的运动规划问题。运动规划是指机械手从初始构型移动到目标构型的过程中，各个关节的运动轨迹规划。在这个过程中，必须保证机械手不与工作空间内的任何障碍物或本身发生碰撞。

国内外研究学者对机器人的运动规划问题开展了大量研究，现有的运动规划算法包括快速随机搜索树(Rapidly Random Trees，RRT)[2]、概率路线图法(Probabilistic Roadmap Method，PRM)[3]、A*算法[4]、神经网络模型[5]、人工势场(Artificial Potential Field，APF)[6]。由于采摘机器人工作的环境是非结构化的、非常复杂的、障碍物很多的环境，机械手的C空间构造将花费极长的时间。从现有文献来看，RRT、PRM和A*算法需要构建C空间，因此无法满足拾取机器人的实时性要求。而神经网络算法不仅需要长时间的自我训练，对硬件的要求也更高，因此也无法用于拾取机器人的实时运动规划。

APF算法通过笛卡尔空间建立虚拟势场来规划运动，其中运动物体在从初始位置移动到目标位置的过程中会受到引力和斥力两种力的影响，并避免碰撞障碍物。与RRT、PRM等全局规划算法不同，APF是一种算法复杂度低、对计算机资源依赖性低、实时性强的局部运动规划算法。但是，APF存在局部最小缺陷(Local Minimum Trap，LMT)和目标不可达问题(Target Unreachable Problem，TNRP)等问题[7]。MATOUI 等[8]对APF算法进行了改进，设计了一种非最小速度算法，改进后的APF能有效避免出现LMT现象。彭鹏等人[9]提出一种改进的APF和RRT结合的机械臂避碰路径规划算法，该算法有效避免了机械臂运动规划陷入LMT问题。李卓等人[10]在传统的智能车运动规划方面，将速度和加速度等因素导入了引力势场，通过仿真表明该算法能有效避免TNRP问题。由于采摘机器人对运动规划算法的实时性要求较高，上述研究均耗时较长，无法满足采摘机器人的实时性要求。

在上述研究的基础上，本文作者分别对APF的势场和力的计算模型进行了改进，在规划机械手运动时直接计算机械手关节空间的吸引力矩，提出了改进的人工势场 (IAPF)优化算法，有效地减少机械手运动规划的计算时间，同时避免LMT和TPNR问题。最后通过实验结果，验证了所提优化算法在采摘机械手无碰撞运动规划中的实时性和准确性。

1 采摘机器人的结构模型

图1为本文作者所研究的智能采摘机器人的功能模型，主要由3个部件组成：负责果园内运动的自动移动车辆，并作为搭载所有其他部件的平台；用于采摘水果的六连杆机械手；安装在车辆上用于放置机器人控制柜的框架结构，包括水果贮藏箱、移动电源以及用于检测和定位水果的立体摄像机视觉系统。

图1 智能采摘机器人实物模型

2 采摘机械手的运动规划模型

2.1 采摘机械手的运动学模型

机械手运动学模型的建立是运动规划的前提工作，本文作者采用初等变换序列对六连杆机械手进行运动学建模[11]，如图2所示，di(i=1，2，…，6)表示连杆偏移量，αi(i=1，2，…，6)表示连杆扭转角，ai(i=1，2，…，6)表示连杆长度，θi(i=1，2，…，6)表示关节角度。从初等变换序列模型中，可得到机械手上每一点的雅可比矩阵。

图2 六连杆机械手的初等变换序列模型

图3所示为采摘水果的3个步骤：

图3 采摘机械手运动规划流程

步骤1，采摘机械手从初始位置移动到采摘位置，目标水果由末端执行器采摘分离。在规划运动时，同时设计和计算采摘构型。

步骤2，采摘机械手从采摘构型移动到释放构型，将水果放入存储箱中。

步骤3，采摘机械手从释放构型移动到初始构型，等待下一个采摘周期。在采摘实验中，初始构型和释放构型都是预先设定的，以进一步提高实时性。

2.2 机械手运动规划算法

图4所示为六连杆机械手的运动规划概念图。

图4 六连杆机械手的运动规划概念图

APF算法是通过建立虚拟势能场来规划运动轨迹，以避免与工作空间中的任何障碍物发生碰撞[12]。引力势场和引力的计算公式为

(1)

Fa=-∇Ua=kada∇da

(2)

式中：Ua∈R为引力势场；Fa为引力；ka为引力系数；da为目标距离。斥力势场和斥力计算公式为

(3)

(4)

式中：Ur∈R为斥力势场；Fr为斥力；kr为斥力系数；dr为障碍物距离；d0为距离的阈值。合力势场Us和合力Fs计算公式为

Us=Ua+Ur

(5)

Fs=Fa+Fr

(6)

与移动车辆不同，APF的应用和机械手的无碰撞运动规划更加复杂。移动车辆可以看作是工作空间中的一个点进行运动规划，但对于机械手来说，需要考虑如何对整个机械手进行无碰撞运动规划，而不是将其作为单个点。机械手的路径规划可以在其工作空间或C空间中进行。由于C空间的计算量大，构建耗时长，所以通常采用工作空间中的路径规划。由于机械手上每个点的运动不能单独规划，因此必须在整体机械臂结构的背景下考虑机械手上的点。在此研究中，引力作用于所有初等变换序列坐标系的原点，斥力作用于每个初等变换序列线段上距离障碍物最近的点，如图5所示。通过引力作用，机械手可以从初始构型变为目标构型。通过斥力作用，可以避免在工作空间中移动时撞到任何障碍物。

图5 施加在机械手上的IAPF力示意

当工作空间中有多个障碍物时，合力势场的合力可以计算为

(7)

(8)

为了控制机械手，通过雅可比矩阵的转换，最终将合力映射到关节力矩上，计算关节力矩的公式为

(9)

2.3 运动规划优化策略

APF中由于存在局部最小缺陷(LMT)、目标位置不可达(TNRP)等问题，将直接导致两个问题，即Fa随着da的减小而线性减小，当da=0时，Fa减小到零，但当dr趋于零时Fr趋于增加，当dr=0时，Fr增大到无穷大。首先，如果在目标位置附近有障碍物，其斥力会远远大于引力。当物体接近目标位置时，会不断被斥力反弹，导致路径振荡甚至失效，这就是TNRP问题。其次，机械手关节所能承受的力矩有限，在运动规划过程中无法实现无限的斥力。为了解决上述两个问题，本文作者对势场和力模型进行了优化。

为了避免TNRP问题，需要在接近目标位置时仍然有较大的梯度，以确保Fa足够大，能够克服Fr的影响，连续引导物体到达目标位置。同时，为了使路径规划算法收敛，当da=0时，Fa也应该趋近于0。为此，IAPF算法在APF算法原有的引力势场计算模型的基础上，通过加入Sigmoid函数分量以提升算法的收敛性[13]，即IAPF算法的引力势场和引力计算为

(10)

(11)

式中：ka1和ka2为Sigmoid函数的控制因子，为避免Fr随dr减小而不可控的问题，IAPF算法改变了原有的斥力势场模型。APF算法的斥力势场模型是分段函数，不仅需要在分段接点处连续可微，而且当dr=0时还会发生梯度爆炸。为确保IAPF算法的斥力不会无限增加，并且随着dr的减小仍然是连续可微的，本文作者将IAPF算法的斥力变为一个反S的Sigmoid函数[14]，IAPF算法的斥力函数不仅在远离障碍物时趋近于零，而且随着dr的减小而增大，仍然是连续可微的。IAPF算法的Fr会逐渐增大，并趋于最大斥力，不会出现斥力爆炸的问题。IAPF算法的斥力势场和斥力计算为

(12)

(13)

式中：kr1和kr2为反S型Sigmoid函数的控制因子；kr3为Ur的补偿系数。为了消除LMT带来的影响，IAPF算法选择在合力势场梯度消失时，增加临时力Ft，打破斥力和引力的平衡。Ft不仅决定了IAPF算法结果的好坏，也决定了IAPF算法是否会再次陷入LMT。通过仿真实验，Ft同时垂直于Fa和Fr时效果较好。在文中，当IAPF算法即将进入LMT时，将出现Ft，其值与Fa相等，其方向如图6所示。

图6 避免LMT的临时力示意

通过雅可比矩阵的转换，将工作空间中的力直接映射到关节空间中的力矩，仍然需要大量的计算，因此为了进一步优化，IAPF算法直接计算关节空间中的引力矩，计算公式为

(14)

(15)

3 采摘机械手运动规划的仿真对比

文中的仿真都是在Windows 10操作系统下通过Python 3在Visual Studio Code中实现的。

3.1 势场面与力曲线的仿真

APF和IAPF算法的势场面和力曲线分别如图7和图8所示，表1给出了APF和IAPF算法的势场模型和力模型的具体参数，其中Pg为目标位置，Pc为当前位置，P0为障碍物位置。

表1 第3.1节仿真使用的具体参数

图7 APF和IAPF的势场面

图8 APF和IAPF的力曲线

可以看出：APF的引力势场梯度在目标位置附近逐渐消失，而IAPF算法的梯度并没有消失而是增加。因此，与APF相比，IAPF对TNRP的抵抗力更强，收敛速度更快。由于APF斥力势场中存在梯度爆炸现象，当APF靠近障碍物时，斥力势场的梯度远大于引力势场的坡度，导致合成势场的引力势场表面缺乏特征。然而，IAPF算法可以有效地克服以上问题，其合成势场表面具有引力势场面和斥力势场面的共同特征，这使得IAPF算法不仅具有更快的收敛速度，还具有较强的无碰撞能力。

3.2 验证TNRP的仿真对比

当目标位置附近有障碍物时，APF会出现TNRP问题。因此，为了验证IAPF算法能够有效避免TNRP问题，进行了下面的仿真实验。仿真区域为10×10矩形，具体参数如表2所示，各障碍物的动作阈值相等。仿真结果如图9所示，仿真曲线如图10所示。

表2 第3.2节仿真使用的具体参数

图9 TNRP的仿真结果

图10 TNRP的仿真曲线

从图9可以看出：当目标位置附近有障碍物时，机械手不断靠近目标位置，但被障碍物的斥力推开，来回摆动，无法通过APF到达目标位置，因此APF无法收敛。从图10可以看出：IAPF算法在第20次迭代附近接近目标位置，获得了增强的引力，避免了TNRP的发生。仿真结果表明，IAPF算法可以有效避免TNRP的发生，保证路径规划的成功率。

3.3 验证LMT的仿真对比

为了验证IAPF算法能够避免陷入LMT问题，设计了如下仿真实验：当目标与初始位置之间的直线上存在障碍物时，容易陷入LMT，因此在这种情况下，APF和IAPF算法同时进行路径规划。模拟区域为一个10×10的矩形，具体参数见表3，各障碍物的动作阈值相等，仿真结果如图11所示，仿真曲线如图12所示。

表3 第3.3节仿真使用的具体参数

图11 LMT的仿真结果

图12 LMT的仿真曲线

从仿真结果可以看出：APF迅速陷入LMT，并一直保持这种状态，直到路径规划失败；IAPF算法没有落入LMT，并迅速到达目标位置；IAPF算法在第22次迭代附近到达目标位置，而APF仍然停留在LMT中。仿真结果表明，IAPF算法能够有效克服LMT问题，保证路径规划的成功率。

3.4 机械手运动规划的仿真对比

采用APF和IAPF算法对机械臂进行路径规划。实验参数如表4所示，其中初始构型为Θi，目标构型为Θg，障碍物坐标为POi，障碍物半径均为120 mm。仿真结果如表5所示，机械手各初等变换序列线段与障碍物的关节角曲线和距离曲线如图13所示。

表4 第3.4节仿真使用的具体参数

表5 IAPF和APF算法的路径规划结果

图13 采摘机械手运动规划的实验结果

仿真结果表明：IAPF算法优于APF算法。IAPF算法的关节角曲线更平滑，变化率几乎相同；APF算法的关节角曲线变化比较明显，变化率相差较大，关节角θ3经过一个反向过程后开始收敛，而关节角θ2则出现波峰。APF算法的收敛速度非常慢，IAPF算法在第238次直接到达目标构型，而APF在第549次才收敛。与APF算法相比，IAPF算法在运行时间上减少了55.12%，在总关节角误差上减少了46.21%。

4 运动规划的实验验证

为进一步验证IAPF算法的有效性和可行性，对智能采摘机器人进行了运动规划实验，实验结果如表6所示。目标果树及其简化模型如图14所示，首先用部分球面包络(PSE)方法对目标果树进行建模，该方法类似于一个包围球算法[15-16]。PSE方法仅包含球面和线段模型，可以大大减少碰撞检测过程中的计算量，进一步保证IAPF算法能够满足智能采摘机器人的实时性要求。PSE通过球体包络果树的所有非采摘部分，通过直线分段对当前采摘部分的分支进行建模，智能采摘机器人的采摘模拟和轨迹曲线如图15所示，应用于智能采摘机器人的IAPF算法实验结果如图16所示。

表6 IAPF应用于智能采摘机器人的模拟实验结果

图14 目标果树(a)及其PSE模型(b)

图15 智能采摘机器人采摘仿真时的轨迹曲线

图16 应用于智能采摘机器人的IAPF实验结果

IAPF算法在智能采摘机器人采摘目标果实的路径规划过程中分别运动0.38、1.08、1.11 s，在采摘、释放和初始化阶段的关节总误差分别为1.29°、1.08°和1.21°。智能采摘机器人规划果实采摘路径耗时约2.56 s，由于机械手运动所花费的时间比路径规划所花费的时间要长得多，因此IAPF算法可以在智能采摘机器人采摘过程中充分规划下一个果实采摘路径。从实验结果可以得出结论：IAPF算法具有较快的求解速度和精度，能够满足智能采摘机器人的实时性要求。