一种类球形果实的采摘顺序动态规划方法

摘要

本发明公开了一种类球形果实的采摘顺序动态规划方法，设计了将感知点与采摘点一起进行规划的动态收获感知方法。机械臂每次采摘或感知完成后经过判断进行权值更新，添加了果实深度信息的权值更新规则。本发明所提出的动态收获感知采摘顺序规划方法在动态规划方法的基础上将采摘的感知点与采摘点相结合，与传统的基于经验的采摘顺序规划方法、全局感知方法和批量感知等方法相比，所规划的采摘顺序不仅采摘路径短，且实时性强，提高了采摘机器人的采摘效率和采摘成功率。对所设计的动态收获感知采摘顺序规划方法进行了改进，加入了基于三维空间的前后果实采摘顺序规划，避免了因采摘顺序导致的果实破坏，降低了果实采摘破坏率。

说明书

技术领域

本发明属于机器人果实采摘领域，特别涉及类球形果实的采摘，是用于植物大棚、植物工厂等果实量大、果实分布不均匀，需要采摘机器人独立完成采摘任务的领域。

背景技术

随着植物大棚、植物工厂的规模庞大化以及人力成本的提高，果蔬采摘与收获的智能化成为各国农业的迫切需求。伴随着计算机、图像处理等领域的技术突破，果蔬采摘机器人得到了迅速的发展。如美国弗罗里达大学开发的柑橘采摘机器人、以色列内盖夫本哈古里安大学开发的应用于商业温室中的甜椒采摘机器人、上海交通大学开发的双臂采摘机器人等。采摘机器人技术日趋成熟并逐渐向商用方向发展。

但是各国高校和研究所都致力于采摘机器人的果实识别和机械臂避障等方面的研究，鲜有对于果实采摘顺序的研究，采摘机器人几乎都是采用不进行任何优化策略的固定顺序采摘，效率低且易损伤果实。仅有的一些研究也都存在着许多弊端，如专利公开号为CN105773614B的“一种基于双目视觉的空间降维果实采摘顺序规划系统及方法”，该专利所述方法需要双目相机旋转多个角度才能获取全局信息，检测效率和采摘效率低；采摘顺序使用全局规划，实时性差，采摘机器人在执行采摘时果实位置可能已经发生了变化，采摘成功率低；对采摘目标进行了降维处理，其采摘顺序并不是最优；采摘顺序未考虑果实深度信息，可能对果实造成破坏。

发明内容

为了克服目前常用的基于经验规定的从上到下、从左到右的采摘方法采摘效率低、易造成果实损坏的问题，本发明从提高整体果实收获速度、提高果实采摘成功率、降低果实破坏率的角度考虑，提出了一种类球形果实的采摘顺序动态规划方法。

所述采摘顺序规划方法应用于采摘苹果、西红柿等类球形果实的采摘机器人。所述采摘机器人采用移动轮式机器人平台安装六自由度采摘机械臂、采摘机械臂末端安装深度相机的形式进行采摘。所述采摘机器人夹爪下方安装有与收获筐相连的软管，采摘机器人在每次采摘之后果实会直接沿着软管进入收获筐，采摘机器人不需要往返于采摘点与收获筐。移动机器人到达采摘区域后，深度相机进行环境感知，采集环境的RGBD图像，并利用深度学习算法对环境中的果实进行识别，将识别到的果实的中心点位置信息进行发布。下文中将深度相机进行感知的位置称为感知点，移动机器人每移动一次为一个新的地点。

由于采摘环境狭小，采摘机械臂与植物之间距离较近，相机拍摄范围有限，同一个地点深度相机进行一次感知无法获取该地点所有的环境信息，果实也会因为枝叶的遮挡无法在同一个感知点感知到所有的果实。因此在同一个采摘地点设置机械臂末端移动到不同的位置使深度相机能够在不同的位置进行多次感知。感知点的个数根据采摘环境的不同进行调整，直到能够使深度相机感知此采摘点的所有环境信息。

采摘顺序最优，即采摘机器人在保证采摘成功率的情况下完成整体采摘任务的采摘消耗总时间最短或采摘消耗总能量最小的果实采摘顺序。为了使整体采摘时间最短，只需让采摘机器人在每个地点的采摘时间最短。采摘机器人完成一个地点的所有果实的采摘时间可以由式(1) 表示。

T

式中：T

由式(1)可以看出，采摘机器人进行采摘消耗的总时间由机械臂移动到各采摘点和感知点花费的时间、采摘动作花费的时间和感知花费的时间组成。其中采摘动作花费的时间和感知花费的时间是固定的，采用任何采摘顺序均不会产生变化。因此要使整体采摘时间最短，只需使机械臂移动到各采摘点和感知点花费的时间T

第一步，在第一感知点进行感知；

采摘机器人到达采摘位置后，此时采摘机械臂处于初始位置，双目相机在第一感知点即初始位置进行环境感知，双目相机将采集到的环境RGBD图像通过ROS进行发布，利用深度学习yolov3算法对双目相机采集到的RGBD图像进行订阅并处理；使用yolov3算法对环境RGBD 图像进行识别，将识别到的果实i的中心点三维位置坐标(x

第二步，检测果实重叠情况；

提取果实位置坐标的x-y平面位置信息，所述x-y平面即垂直于双目相机法线，双目相机拍摄的RGB图像对应的平面；提取后的果实i中心点平面坐标为(x

式中(x

根据式(3)判断两个果实在x-y平面的重叠情况，当两个果实间的距离l

第三步，果实间权重的计算及动态更新；

令权重c

其中i，j为集合V中的点，根据式(4)计算集合V中所有点两两之间的权重；

权重的计算：根据式(4)计算第一步中检测到的所有果实位置和感知点集合V中所有点两两之间的权重。

权重的动态更新：对在步骤二中判定为重叠的果实集合V

当遮挡果实i被采摘，则判定先前被遮挡果实j不再被遮挡，即式(3)不再成立，则更新 j果实的权重，使j果实的权重值再次以式(4)计算得到；

除了遮挡果实的权重更新外，机械臂在新的感知位置感知到了新的果实后，也需要对集合 V中所有点进行权重更新。

第四步，采用动态收获感知方法对采摘机器人采摘路径进行规划；将感知点和采摘点放到一起进行动态规划，即对集合V中所有点进行动态路径规划。令位置集合V

1)当移动位置集合V

2)当移动位置集合V

d(m，V)＝min(c

其中d(k,V

综合1)和2)两种情况，机械臂最短路径动态规划方程如式(7)所示：

通过1)和2)的不断求解，对所有点依次迭代，求出所有路径的权重之和，则权重之和最小的路径即为所求的最短路径；最终求得采摘机械臂末端走完集合V中所有点的最短路径；

第五步，采摘机器人按照步骤四所求得的最短采摘路径进行采摘或感知，在每一次采摘或感知之后进行判断，如果存在剩余的果实或感知点，则采摘机器人再次进行第二步，判断果实重叠情况有无变化或有无新增的果实，如果有，则继续依次执行第三步-第五步；如果判断没有新增果实且果实重叠情况没有发生变化，则继续执行步骤五，直到判断此区域没有剩余的果实或感知点，则采摘机器人进入下一采摘地点进行采摘。

上述动态规划方法解决了采摘机器人在采摘点都已知的情况下的最短路径规划问题。采摘机器人在一个感知点无法感知全部的环境信息，需要在多个感知点进行感知获取全部的环境信息，以免发生果实遗漏的情况。感知点的规划对采摘顺序有着很大的影响，常见的规划方法有全局感知和批量感知方法等。全局感知即采摘机械臂先到所有的感知点将环境信息全部获取然后进行整体规划；批量感知方法先在感知点进行感知并将感知到的所有果实进行规划采摘，然后依次到后边的感知点进行感知和采摘；本发明所提出的动态收获感知方法，将所有的感知点与初始位置感知点感知到的果实放在一起进行动态路径规划，在每个感知点感知到新的果实后再次进行动态规划。动态收获感知方法保证了每一次采摘时的路径最优，将感知点也纳入整体路径规划，使得机械臂不需花费多余的时间在感知点之间运动，提高了整体采摘效率。

本发明添加了对重叠果实或者位置相近的前后生长的果实的采摘顺序规划。自然生长的果实常常出现果实重叠生长的现象，两个果实位置相近，一前一后生长，如果此时不对果实采摘顺序做任何处理，采摘机器人按照从左到右或从上到下顺序依次采摘，那么采摘机器人就有可能先采后边的果实，从而对前边的果实造成破坏。为了解决此问题，对所述的动态规划方法进行了改进，当检测到两个果实在x-y平面的位置相近，判断依据如式(3)所示，x-y平面为与深度相机深度信息垂直的平面。如果2个果实的x-y平面距离小于2r，则会发生重叠，给与多余的0.5r是给机械臂提供抓取的余量。如果检测到两个果实在x-y平面的距离较近，则比较两个果实的深度信息，d

本发明提出了针对采摘需要进行多次感知的问题，设计了将感知点与采摘点一起进行规划的动态收获感知方法。机械臂每次采摘或感知完成后经过判断进行权值更新，添加了果实深度信息的权值更新规则。

本发明的有益效果是所提出的动态收获感知采摘顺序规划方法在动态规划方法的基础上将采摘的感知点与采摘点相结合，与传统的基于经验的采摘顺序规划方法、全局感知方法和批量感知等方法相比，所规划的采摘顺序不仅采摘路径短，且实时性强，提高了采摘机器人的采摘效率和采摘成功率。对所设计的动态收获感知采摘顺序规划方法进行了改进，加入了基于三维空间的前后果实采摘顺序规划，避免了因采摘顺序导致的果实破坏，降低了果实采摘破坏率。

附图说明

图1为基于动态收获感知规划的采摘机器人采摘流程图；

图2为全局感知方法规划的采摘顺序；

图3为批量感知方法规划的采摘顺序；

图4为动态收获感知方法规划的采摘顺序；

图5为重叠果实权重更新说明图。

具体实施方式

下面结合附图，对本发明进行详细的说明。

实施例1

应用基于动态收获感知方法进行采摘的采摘机器人采摘流程图如图1所示。采摘机器人在到达采摘位置后，先在默认位置，即第一感知点进行感知，双目相机对环境信息进行采集并使用深度学习算法对果实进行识别和定位。将采集到的果实位置信息进行处理，对比所有果实位置，如果发现有果实位置相近，则对其果实权重进行更新，避免后方果实先采导致前方果实被破坏的情况。在果实权重更新后，利用动态收获感知规划方法对果实采摘顺序进行规划，机械臂按照规划路径进行采摘。采摘机器人在完成一个果实采摘完毕或完成一个感知点的感知后，判断是否有剩余果实或者感知点，如果有，则返回果实重叠情况判断，依次进行判断和循环，直到所有的果实和感知点均已完成，则采摘机器人进入下一地点进行采摘。

感知点在采摘顺序中的规划对采摘效率有着很大的影响，结合常见的两种规划方法：全局感知方法和批量感知方法，对比说明本发明所设计的动态收获感知方法的优点。图2所示为全局感知方法规划的采摘顺序，采摘机器人在第一感知点感知之后，对感知到的果实位置信息进行存储，先不进行采摘，而是到其余所有的感知点进行感知，在所有的感知点都感知完毕后，将所得到的所有位置信息进行综合处理，规划出一条最短路径，其采摘顺序可以简单表述为：感知+感知+感知→采摘。全局规划方法规划的路径较短，但是其实时性较差，所有的果实位置信息都是和第一次感知一起进行感知得到的，当果实枝干受到采摘机器人碰撞或者在采摘时发生的拽动等都会造成果实的易位，最后采摘的果实与感知发生的时间过久，位置信息很有可能发生变动，因此使用全局规划方法的采摘路径采摘成功率较低。

图3为批量感知方法规划的采摘顺序，采摘机器人在第一感知点感知之后，立马对感知到的果实进行路径规划和采摘，采摘机器人在采摘完于第一感知点感知到的所有果实后，才会到第二感知点进行感知，接下来依次进行感知→采摘→感知→采摘的循环，直到遍历所有感知点，采完所有果实。批量感知方法规划的采摘顺序实时性较好，但是其每次感知完全分离，规划的路径较长，采摘机器人经常在感知点和新感知到的果实之间运动，采摘效率较低。

图4为本发明所设计的动态收获感知方法规划的采摘顺序，采摘机器人在第一感知点进行感知之后，将采集到的果实信息和下一感知点的位置放在一起进行路径规划，在得到局部最短路径之后，采摘机器人进行采摘，当采摘机器人到达下一感知点时，深度相机再次进行感知，将在第二感知点感知到的果实和第一感知点采摘剩余的果实和下一感知点位置放在一起进行路径规划，接下来依次进行感知+采摘+感知+规划→采摘+感知+规划，直到遍历所有感知点，实现在采摘中进行感知的动态收获感知规划。在gazebo中对三种方法进行了多次仿真试验，仿真结果显示，采摘机器人使用动态收获感知方法进行采摘顺序规划完成采摘的时间比使用批量感知方法短12％，比使用全局感知方法短3％。虽然动态收获感知方法与全局采摘效率相差不大，但是在实际果实采集中，机械臂可能触碰植物或者在抓取收获过程中推动、拽动植物，导致其他果实易位。动态收获感知方法相较于全局采摘，采摘时距离获取果实位置信息时的时间间距更短，果实位置变动几率更小，位置信息更准确，抓取成功率更高。动态采摘规划方法不仅有着比全局感知方法更高效性的采摘路径并且有着批量感知方法的实时性。

结合图5为重叠果实路径规划方法进行详细说明。如图1所示，采摘机器人在对果实进行感知之后，需要对果实的重叠变化情况进行判断。由深度相机获取的果实位置信息是三维坐标信息，为了判断两个果实的重叠信息，需要判断两个果实在x-y平面的坐标位置之间是否有重叠情况，因为机械臂需要垂直于x-y平面进行抓取。为了避免视角变化带来的影响，所述进行对比的坐标都是由深度相机经过tf转换所获得的机械臂坐标系下的坐标。判断依据如式(3) 所示，当果实i和果实j的中心点距离l

如图5所示，如果采摘机器人没有用任何采摘规划策略进行随机采摘将可能先采摘j果实，那么i果实肯定会遭到机械臂的破坏或触碰导致掉落。因此当判定果实i和果实j发生重叠后，利用深度相机给出的深度信息比较两果实的深度位置情况，判定果实深度信息更深的果实j为被遮挡果实，i果实为遮挡果实，则需要改变被遮挡果实j的路径规划权重，使其采摘顺序排在遮挡果实i之后。令果实j与其他果实之间的权重为无穷大，与遮挡此果实的果实i之间的权重为0，则果实j的采摘顺序会排在遮挡果实i之后，如式(5)所示。动态规划方法在每次成功采摘后更新剩余果实位置并对果实重叠信息进行检测，如果果实i被采走，j果实不再被遮挡，则使j果实的权重恢复正常，如式(4)所示。则果实j将再次以正常未被遮挡果实被加入采摘规划序列进行采摘。