水果采摘机械臂避障运动规划优化算法

官金炫，林桂潮，张世昂，朱立学

（仲恺农业工程学院机电工程学院，广东 广州 510220）

0 引言

相关研究表明，水果种植生产过程中采摘环节需要耗费的劳动力占总体成本的35%～45%[1]。为了节省劳动力，提高水果生产效率，国内外对采摘机器人的研究日益增多。水果采摘环境比较复杂，采摘机器人作业时经常遇到树枝、藤条等障碍物，在自主采摘过程中，常因为障碍物的存在导致采摘不精准或采摘效率低，为了解决这一难题，国内外众多学者在避障运动规划算法上做了大量的相关研究，主要包含基于图搜索算法的A*算法、基于最优化理论的人工势场法和蚁群算法、快速随机树搜索算法（Rapidly-exploring Random Tree，简称RRT）和深度强化学习算法以及基于上述算法的改进算法。

1 A＊算法

A*算法继承了Dijkstra 算法可以在静态网络中利用图形遍历方法求解最短路径的优点[2]，首先在图向量中生成一个状态空间，并对每个需要搜索的位置进行评估，以获取最优位置点，再从该位置向目标点进行搜索路径的评估，采用最优评估的方法可以提高算法的搜索效率。位置和路径的评估常采用曼哈顿估价函数估计其曼哈顿距离。蔡健荣等[3]在水果采摘机器人机械臂的路径避障规划研究中引用了A*算法，通过使用机器人的位姿点密度作为算法的参数权重进行最优路径搜索，在图向量空间中，计算机器人的位姿并向目标点自由扩散，采用延迟碰撞检测的策略，降低了计算量，达到实时规划；陈善峰等[4]基于A*算法，提出一种适用于关节型机械手多关节角度计算和路径规划的方法，该方法首先需获取果实空间位置坐标和竖直类障碍物空间的位姿状态，进而求解出机械手的避障关节角度，并规划出机械手的路径。雷蕾等[5]基于改进的A*算法进行自动采摘机器人的路径规划，并采用栅格法建立机器人的移动路径，使得路径更加平滑，如图1所示。E.J.VAN HENTEN 等[6]采用A*算法计算黄瓜采摘机械臂的空间路径，所实现的采收机械中使用了六自由度的RV-E2 机械臂无碰撞运动，单次采摘时间只需要10 s。

图1 A★ 算法原理仿真示意图

基于图搜索的A*算法常结合其他算法，将立体空间路径规划问题转化为二维的平面路径规划和避障问题，简化了模型的参数量，并降低计算量。Dijkstra 算法是一种盲目的搜索算法，A* 算法是一种启发式搜索算法，在时间和空间效率的比较上看，当全局环境较大时，A*算法搜索的效率优于Dijkstra算法；当全局环境不大时，两者的执行效果相差不大[5]。

2 基于最优化理论的算法

2.1 人工势场法

人工势场法是通过障碍物的斥力场和目标位置的引力场共同作用形成一个虚拟的人工势场，再搜索一条势函数下降的方向，来寻找一条无碰撞的最优路径[7]。这是一种基于最优化理论的避障路径规划方法，其具体工作原理是：先以目标物为中心，建立一个引力场，以障碍物为中心，建立斥力场，如图2 所示，再通过最速下降法等梯度路径方法进行避障路径规划。该方法原理比较简单，且容易构建，但计算效率较低。

图2 人工势场法势场原理示意图

曹博等[8]提出一种改进人工势场法，通过建立冗余机械臂运动学模型，采用线段球体包络盒模型进行碰撞检测，在笛卡尔空间内建立末端引力势场和障碍物斥力势场，在关节空间内建立目标角度引力势场，所有势场共同作用引导机械臂逃离势场中的容易陷入局部极小值的路径点以完成避障。孙泾辉等[9]提出了人工势场法与A*算法相结合的路径规划方法，在机械臂陷入局部极小值时，采用A*算法控制其运动到虚拟目标点以逃离最小值，然后再转换回人工势场法继续搜索直到抵达终点。薛阳等[10]提出一种改进的人工势场法，用于六自由度的双机械臂路径避障规划，该方法改进了传统势场的势能函数，采用增加了虚拟吸引点的方法以避免机械臂陷入局部极小值。韩峰等[11]提出一种基于模糊人工势场法的机械臂避障控制算法，设计模糊控制器重构势场函数中的引力势场以及斥力势场权重系数来克服传统人工势场法的局部极小问题。姬伟等[12]针对自行研制的五自由度苹果采摘机器人的机械手实时避障问题，提出一种改进人工势场法的避障路径规划方法，在势场中增加虚拟目标点，使机械臂的搜索过程逃离局部极小值，从而避开障碍物。YOUCEF HARGAS 等[13]认为人工势场法可以为机械臂在复杂、已知有障碍物的环境中做避障运动规划，并采用RobuTER/ULM 机械手进行试验，将三维空间的机械手避障运动规划转换成二维的路径规划问题。S.LIU 等[14]提出一种利用面向构型的改进人工势场法在三维空间环境下实现空间机械臂无障碍物碰撞运动规划的方法，该方法与基于广义雅可比矩阵的逆运动学计算相结合，可以对整个空间机械臂的构型进行避障规划；DA SHEN 等[15]针对双六自由度机械臂的避障路径规划问题，提出一种改进的人工势场法，该方法将双机械臂划分为主臂和从臂，先对主臂进行运动规划，再将主臂当成从臂运动势场范围中的动态障碍物，进而规划从臂的避障运动路径，通过在主从臂的运动势场中选取新的组合势场函数，实现双机械臂的避障路径规划。SHENGNAN GAI等[16]在MA1440 工业机器人上利用基于人工势场法的六自由度工业机器人全手臂路径规划算法进行机械手避障路径规划，通过几何关系将笛卡尔空间障碍物转化为机械臂的C 空间，从而形成机械臂的自由运动空间，再根据障碍物斥力向量和目标点吸引向量，实现直观的排斥反应，主动避免机器人手臂的碰撞，最终到达目标。

人工势场法用于空间路径规划时，常遇到局部极小值问题，许多学者通过加入其他的算法，如A*算法、设置虚拟目标点、组建新的目标角度引力势场等方法进行规避，从而使机械手正确达到目标点。采摘机器人需要进行机械臂的运动控制较多，其缺点是需要计算的数据量较大，在路线规划的实时性上容易因局部极小值导致运动不在最优路径上，进而增大控制时间。

2.2 蚁群算法

蚁群算法是基于概率型的寻优路径算法，该算法提出的灵感源自于蚂蚁觅食过程中，给蚁群传递信号的行为，该算法本质上是一种启发式全局优化的算法，其主要特征包括分布计算、信息信号的正向反馈和启发式搜索[17]。但蚁群算法在求解高效率高精度的避障路径规划问题时，常出现局部最优、启发式搜索最优路径时间过长以及计算最优路径时收敛速度慢等不足[18]。

赵华东等[19]针对空间六自由度机械臂路径规划问题，提出了一种基于改进蚁群算法的路径规划方法，该方法在机械臂末端建立可视化的搜索空间，并将其运动分解成x、y、z 3 个轴方向的直线运动以降低搜索空间的复杂程度，并对信息浓度设置上、下限标准，避免算法出现早熟和局部最优问题，增加了算法的正反馈作用，以ABB 公司的1410 型六自由度工业机械臂为研究对象，使用改进后的蚁群算法进行路径规划，把机械臂末端的运动轨迹看作是依次经过规划路径点的集合，进而求出机械臂运动时的关节转角，实现其避障路径规划。袁路路[20]利用势场蚁群算法，提出一种基于手势识别的远程控制方案，将基于视觉的手势识别与远程控制机械手相结合，通过深度相机采集机械手的手势图像并提取姿态特征，经算法模型转换为机械手舵机的运动控制命令，并通过无线传输的方式发送至采摘机器人的控制单元，实现视觉手势对机器人的远程控制。YONG DAI 等[21]提出了一种基于螺旋理论（Screw Theory）和蚁群算法的串联机械手抓取工作时避障路径规划问题的求解方法，利用螺旋理论将串联机器人路径规划问题转化为多维目标函数极大值问题，并对目标函数的自变量进行网格化处理，利用蚁群算法在函数的多维网格域上进行路径规划，提高了算法的收敛性、准确性和启发性。MOHD MURTADHA MOHAMAD 等[22]采用蚁群算法与概率路径规划算法相结合解决六自由度关节机械臂运动路径规划问题，该算法的重点是应用蚁群行为在机械臂的c -空间，以SBL-PRM 多目标运动规划为基准，减少算法起始位置和目标位置之间的参数配置量，进而实现快速的路径避障规划。DONGSHENG ZHOU 等[23]提出了一种改进的蚁群算法来解决空间机械臂末端执行器的避障规划问题，使用蚁群算法可以避免空间机械臂的运动学模型路径规划函数陷入局部最优解，进而找到最佳避障运动路径。

蚁群算法作为一种新型的仿生启发式算法，是一种具备并行分布式计算、较强鲁棒性和易于二次开发等优点的算法[18]；在机器人的最优路径规划问题中取得了显著的应用效果，由二维问题发展而来的机械臂的空间避障运动规划等三维问题的应用也受到了关注。但是蚁群算法运用在机械臂的避障运动规划上，存在容易进入局部最优解，算法计算搜索时间过长的问题。国内外研究者基于上述问题，提出了简化信息素浓度、建立可视域搜索空间、结合人工势场法、螺旋理论、与概率路径规划算法相结合等方法进行规避。

3 快速随机树搜索算法

快速随机树搜索算法（RRT）由STEVEN M.LAVALLE 和JAMES J.KUFFNER JR.于1998 年提出，是一种为具有状态约束的非线性系统生成开环轨迹的技术[24]。RRT 在多维空间中具有较高的寻优效率，它采用随机采样的方式在初始点处增加叶子节点，进而生成空间中的随机扩展树，当叶子节点包含目标点或者进入目标区域时，便开始寻找一条初始点到目标点的非线性最优路径[25]。RRT 在避障运动规划领域取得较大的进展，但该算法在搜索空间中生成随机扩展树时，常因为节点的利用率低而导致计算出的路径不稳定，现有研究者针对RRT 算法提出了对应的改进，主要从单向随机树扩展、多向随机树扩展及其他方面展开[25]。马慧丽等[26]针对机械臂避障的难题，采用圆柱体包络法进行碰撞检测，采用RRT*的改进算法进行路径搜索规划，RRT*在RRT 的基础上改进了父节点选择的方式，加入代价函数保证解的渐进最优性，并加入目标的引力，减少路径搜索的随机性。王兆光等[27]针对五自由度机械臂路径规划问题，提出一种基于RRT 算法的优化算法，采用高斯采样的方法进行启发式采样，结合贪婪扩展算法来提高随机树的局部扩展速度，同时采用双向同时剪枝取最优的策略来删除不必要的采样节点，路径扩展的示意如图3 所示。仲崇迪[28]提出了一种改进的RRT-connect 算法对苹果采摘机械臂的避障运动路径规划进行研究，基于D-H 参数法进行机器人数学建模，实现机器人末端姿态的正解和工作空间的计算，并利用八叉树算法，对采摘环境下的视觉点云数据进行障碍模型的建立，采用改进的RRT-connect 算法，结合八叉树算法进行路径寻优。马冀桐[29]提出一种基于构型空间先验知识引导点的RRT-connect（IGPRRT-connect）算法，结合主干网络为 ResNet-152 的 Mask RCNN-152 深度神经网络，解决柑橘采摘机械臂深入树冠内部作业避障运动规划的问题，采用一种将六维构型空间降维成三维的构型空间降维方法以降低计算参数量，机械臂的避障运动规划的综合成功率为 90.70%，平均规划时间为 1.26 s。

图3 RRT原理扩展过程

国外西班牙维尔瓦大学的GOMEZ-BRAVO F等[30]将RRT算法运用在机械臂的避障规划问题上，利用该算法提供一组可行轨迹函数，再根据适应度函数优化原始解，得到避障路径。比利时鲁文大学NGUYEN T T等[31]对九自由度的苹果采摘机械臂的实时避障控制进行多种算法的试验验证，发现基于RRT改进的RRT-connect算法效果最好，基本能在3 s左右完成路径规划任务并指导机械臂运动至目标位置。PAULLIN等[32]针对葡萄采摘机器人的避障路径规划问题，对比了8种基于采样的路径规划算法，上述研究证明，RRT算法具有高效的随机扩展性，可快速生成可行的机械臂避障路径，为高维且复杂的机器人路径规划问题提供了一种新的解决方案。RRT及其改进算法在计算时间、搜索路径规划上具有较好的表现，但仍未达到理论的最优效果[25]。

4 深度强化学习算法

深度强化学习（Deep Q-learning，简称DQN）结合了深度学习的感知能力和强化学习的决策能力，是一种模拟人类思维方式的智能方法，可以根据输入的图像进行识别并给出决策策略，算法模型如图4 所示，智能体与环境进行直接交互[33]。基于DQN方法的机械臂控制问题中，将机械臂作为智能体Agent，与采摘环境进行交互，将产生的回报信息通过DQN 算法模型进行计算，并输出智能体下一步的动作决策，采用基于 DQN 方法进行机械臂的运动控制，有利于机械臂的作业更加智能化，同时也具备一定程度人脑思维的自主学习方式，从而使得机械臂的控制更加精准，且稳定性、实用性可得到更好的保障[34]。

图4 强化学习算法模型示意图

王鸿涛[34]研究了基于确定性策略及随机性策略的机械臂控制，即基于 DDPG 算法的机械臂控制与基于 PPO 算法的机械臂控制，并在仿真环境 Gazebo中实现了基于该算法的机械臂抓取特定区域内目标物体的任务。郑钧天[35]提出了基于动态步长与分区奖励的机械臂轨迹规划方法来解决深度强化学习算法运用于机械臂轨迹规划时出现的效果不佳的问题，并通过提供少量的示教信息，利用算法对示教进行自动增广，同时存储进记忆库供机械臂进行模仿学习，有效降低了训练初期的难度，获得了更加优秀的性能。陈建华[36]基于 MuJoCo 仿真平台搭建了无障碍物和有障碍物仿真环境，在仿真环境中，通过设定的奖励函数，采用改进深度强化学习算法，即深度确定性策略梯度算法进行了机械臂路径规划控制策略的自主学习训练，实现了机械臂由输入到输出的端对端控制，完成了机械臂避障的路径规划。李鹤宇等[37]针对现有空间机械臂控制方法在实际应用中调试时间长、稳定性差的问题，提出一种基于深度强化学习的控制算法，构建仿真环境用于产生数据，通过状态变量实现仿真环境与深度强化学习算法的交互，通过奖励函数实现对神经网络参数的训练，最终实现使用近端策略优化算法(Proximal Policy Optimization，PPO) 控制空间机械臂将抓手移动至物体下方特定位置的目的。刘钱源[38]针对双臂机器人运动规划时容易碰撞的问题，提出了基于深度强化学习的DADDPG（Dual-Arm Deep Deterministic Policy Gradient）算法，使用了ROS 仿真平台将Kinect、两个UR3 机械臂、DADDPG 算法、Mask-RCNN 整合到一个分布式架构中，降低了各模块之间的耦合性，使双臂机器人成功地完成了抓取物体任务。LEITNER 等[39]针对三自由度机械臂的避障路径规划问题，以视觉感知作为状态输入，再通过基于DQN 算法模型实现对机械臂的避障控制。FINN 等[40]基于深度强化学习算法，结合逆最优控制方法，实现了机械臂在现实场景下的运动控制。KNEPPER 等[41]基于深度强化学习算法的模型预测控制在线、实时的指导机械臂完成加工食品的作业。SATHEESHBABU 等[42]基于深度强化学习，提出了一种用于软空间机械臂开环位置控制的无模型方法控制气动驱动的机械臂，并通过单向弯曲和双向扭转变形的组合来获得机械臂工作空间。SATHER[43]提出一种基于深度强化学习的算法来实现草莓自主采摘，该算法将相机采集的图像数据进行深度感知，采用强化学习的采摘决策能力，对比视觉伺服的基准采摘策略，采摘速度提高了8%。

深度强化学习算法，本质上是一种无监督的智能体自我学习算法，在机械臂的控制研究中，许多研究的流程都是先搭建仿真模拟环境，提供算法训练的空间，降低了训练的成本，在算法迭代更新到满足所需要的精度时，再部署到实际机器上进行运动控制。深度强化学习，是一种模拟人类思维的方法，可以根据环境奖励进行自我更新驱动的优点，在果园非结构化、复杂多变的机械臂采摘作业研究中，具有重要的运用前景。

5 结语

水果采摘机械臂的避障运动控制主流算法包括基于图搜索的A*算法、基于最优化理论的人工势场法和蚁群算法、快速随机树搜索算法和深度强化学习算法。以上几种算法的比较如表1 所示。基于图搜索的A* 算法采用启发式搜索算法，在一定程度上提高了整体的搜索效率，但需要对全局环境进行完整的网格建模工作，难以满足高速性要求的多自由度机械臂的避障搜索问题。人工势场法构造简单，但是计算效率低，国内使用方法主要是在C 空间上进行势场构建，并不能保证其他关节的避障问题。国外一些学者采用关节空间构建势场，再通过雅可比矩阵进行避障路径规划，但是计算雅可比矩阵带来了计算效率的降低，该方法无法应用于高维空间。蚁群算法虽完备且最优，但收敛速度慢，导致计算的效率较低，未得到广泛应用。RRT 是一种纯粹的随机搜索算法，对环境类型不敏感，也是目前采摘机器人避障运动规划中用得较多的一种。RRT 作为一种适用于高维空间的避障方法，因为其具有状态约束的非线性系统生成开环轨迹的特性，在计算效率上高于图搜索算法和基于最优化理论的人工势场法和蚁群算法；但是当C-空间中包含大量障碍物或狭窄通道约束时，该算法的收敛速度也会变慢，效率会大幅下降，难以在有狭窄通道的环境中找到路径。深度强化学习将深度学习感知能力和强化学习决策能力相结合，为复杂系统的感知决策问题提供了解决思路。

表1 几种主流算法在采摘机器人避障运用中的比较

近几年，随着大数据的发展和计算机运算能力的提升，深度学习在农业机器人视觉感知的研究上取得了巨大的进展，但以机器视觉为闭环的深度学习智能采摘机器人仍缺乏决策能力，尤其对于复杂多变的果园采摘环境，机械臂遇到障碍物时，无法智能决策；基于图搜索和最优化理论的算法因为计算效率低的问题，难以达到实时避障的要求。深度强化学习结合了深度学习的感知能力和强化学习的决策能力，在保证计算效率的同时，可以实现复杂环境的智能避障，有望推动农业机器人实现智能采摘取得进一步发展。