CN103950033A - 水果采摘机器人的机械臂和末端执行器及水果采摘方法 - Google Patents

Abstract

本发明公开了一种水果采摘机器人的机械臂和末端执行器，包括果实采摘机械装置和控制系统，果实采摘机械装置包括母机械臂、不少于一个的子机械臂和末端执行器；母机械臂包括母臂Z轴、母臂r轴、母臂θ轴、底座、端架和支架；子机械臂包括子臂Z轴、子臂r轴、子臂θ轴和子臂法兰；末端执行器包括接果器、基部法兰、拢果器、刀片、收果筒、旋转底盖、传动轮、传动带、主动齿轮、从动齿轮、架台、横杆、拢果器轴、刀片轴和旋转底盖轴；控制系统包括工控机、双目摄像机、转接卡、母机械臂控制系统、转换器和子机械臂与末端执行器控制系统；本发明还公开一种水果采摘机器人的机械臂和末端执行器及水果采摘方法。

Description

水果采摘机器人的机械臂和末端执行器及水果采摘方法

技术领域

本发明涉及农业采摘机器人领域，特别涉及一种水果采摘机器人的机械臂和末端执行器及水果采摘方法。

背景技术

随着社会人口的老龄化和农业劳动力的逐步减少，一些繁重、单调的农业生产越来越需要机械化、自动化、智能化的机器来替代。计算机技术、传感器技术、电子通信技术、自动控制技术的发展给农业机器人技术的发展提供了前所未有的可能，因此，农业机器人技术在很多国家取得了较大的发展。采摘作业是水果生产中最耗时、最费力的一个环节，其费用约占成本的50%～70%。水果采摘机器人的机械臂和末端执行器是农业机器人的重要类型，它能够降低工人劳动强度和生产费用，具有很大发展潜力。

水果采摘机器人的机械臂和末端执行器一般由视觉系统、行走机构、控制系统、采摘机械臂和末端执行器等部分组成。末端执行器是安装在采摘机器人机械臂前端直接作用于作业对象的部分，是采摘机器人核心部件之一。水果采摘机器人的机械臂和末端执行器作业时，由视觉系统获得果树图像交与计算机处理，获得目标果实的空间三维坐标后，由采摘机械臂将末端执行器送达目标果实位置，最后由末端执行器完成果实采摘。从上个世纪80年代中期以来，国内外研究人员开发设计了多种水果采摘机器人的机械臂和末端执行器。这些水果采摘机器人的机械臂和末端执行器基本设计有单个末端执行器，采摘作业时采摘机械臂为一个末端执行器服务，每采摘一个果实均需要一个完整的采摘流程，每个果实的平均采摘周期较长，采摘效率较低。研究人员也发明了多种末端执行器。这些末端执行器大致可分为吸入式、夹持式和吸持式三种结构，结构及其控制比较复杂，果实的获取与回收工作环节较多，采摘速度不高。目前大多数水果采摘机器人的机械臂和末端执行器及其末端执行器没有商品化，有的只是在实验室或温室中得到了初步应用。当前影响水果采摘机器人的机械臂和末端执行器及其末端执行器实用化的因素主要有（1）机器人及其末端执行器结构、作业方法、控制系统及驱动方式复杂，采摘速度较慢。（2）采摘准确率有待提高。影响采摘准确率的原因主要有：机器人目标果实视觉定位及机械臂位置定位误差较大；末端执行器不能成功获取果实或获取后果实失落；采摘过程中自然风等因素引起果实晃动而使果实定位不准确等。（3）末端执行器采摘水果种类单一，通用性不好，制造和运行成本高。

发明内容

本发明所要解决的技术问题，针对现有技术中的水果采摘机器人的机械臂和末端执行器基本设计有单个末端执行器，采摘作业时采摘机械臂为一个末端执行器服务，每采摘一个果实均需要一个完整的采摘流程，每个果实的平均采摘周期较长，采摘效率较低的技术问题。

本发明的目的是提供一种水果采摘机器人的机械臂和末端执行器及水果采摘方法，可以实现苹果、柑橘、梨等球形水果的高效、高准确率采摘。

本发明为解决上述技术问题所采用的技术方案如下：

一种水果采摘机器人的机械臂和末端执行器，包括果实采摘机械装置和控制系统，果实采摘机械装置包括母机械臂、固定在母机械臂上的不少于一个的子机械臂和固定在子机械臂上的末端执行器；

本发明所述的母机械臂的工作内容为将安装在支架上双目摄像机送到指定图像采集点和将子机械臂及固定在子机械臂上的末端执行器送达指定果实采摘点，末端执行器完成目标果实采摘后，母机械臂将子机械臂和末端执行器送达果实回收点。

本发明中所述的母机械臂包括母臂Z轴、母臂r轴、母臂θ轴、底座、端架和支架；母臂θ轴转动设置在底座上，母臂Z轴垂直固定设置在母臂θ轴上，母臂r轴垂直设置在母臂Z轴上，所述母臂r轴沿着母臂Z轴的轴向方向滑动，所述子臂θ轴绕着自身的中心轴线转动；端架固定在母臂r轴的端头，支架固定在母臂r轴上。

本发明所述子机械臂的工作内容为在母机械臂的配合下完成将末端执行器送达指定采摘位置，末端执行器完成目标果实采摘任务后，子机械臂在母机械臂的配合下完成将末端执行器送达果实回收点。

本发明中所述子机械臂的安装数量，具体可根据实际情况进行设置。设置的子机械臂数量多，其可采摘的范围大；反之，可采摘的范围小。

本发明中所述的子机械臂包括子臂Z轴、子臂r轴、子臂θ轴和子臂法兰；子臂Z轴与前述端架相连，子臂θ轴设置在子臂Z轴上，子臂θ轴沿着子臂Z轴的轴向方向滑动；子臂r轴与子臂θ轴相连，所述子臂r轴沿着子臂r轴的轴向方向伸缩，子臂θ轴可在水平面内左右方向旋转；子臂法兰设置在子臂r轴的端头。

本发明中所述末端执行器的工作内容为完成目标果实的采摘与回收。

本发明中所述的末端执行器包括接果器、基部法兰、拢果器、刀片、收果筒、旋转底盖、传动轮、传动带、主动齿轮、从动齿轮、架台、横杆、拢果器轴、刀片轴和旋转底盖轴；所述接果器为一端封闭，另一端上设有斜开口的圆筒；基部法兰与前述子臂法兰相连，基部法兰通过横杆与接果器的封闭端相连，刀片通过刀片轴转动设置接果器的斜开口端上，拢果器通过拢果器轴转动设置在接果器的斜开口端上，主动齿轮与刀片轴同轴安装，从动齿轮与拢果器轴同轴安装，且主动齿轮与从动齿轮相啮合；传动带连接刀片轴；收果筒设置在接果器的斜开口端上，且所述接果器的斜开口端上设有与收果筒相连通的通孔；架台设置在接果器的中部；旋转底盖通过旋转底盖轴转动设置在收果筒的底端。

本发明中所述的横杆为现有技术中的金属圆柱体。

本发明中所述的接果器为一端封闭，另一端上设有斜开口的圆筒；所述接果器的内表面层和外表面层为现有技术中的橡胶材料制成，中间层为现有技术中的轻型合金材料制成。本发明中接果器的内表面层和外表面层为现有技术中的橡胶材料制成，其主要作用是降低果实采摘过程中的冲击力，达到保护果实的作用。

本发明中所述的收果筒的内壁为现有技术中的软橡胶材料制成，其主要作用是降低果实掉落下的冲击力，达到保护果实的作用。

本发明所述的控制系统实现果树图像拍摄、图像处理、目标果实的识别与定位以及母机械臂、小机械臂运动控制和末端执行器采摘控制。

本发明中所述的控制系统包括工控机、双目摄像机、转接卡、用于控制母机械臂动作的母机械臂控制系统、转换器和驱动子机械臂和控制子机械臂与末端执行器动作的子机械臂与末端执行器控制系统；所述双目摄像机固定在前述的支架上，且双目摄像机通过转接卡与工控机相连，所述子机械臂与末端执行器控制系统通过转换器与工控机相连。

本发明所述工控机为现有技术中的工控机，其具体结构和工作过程本发明不作详细的说明。本发明所述工控机安装有图像处理软件系统，能够处理果树图像，提取果树图像中成熟果实的空间坐标和尺寸信息。所述工控机能够计算母臂Z轴伺服电机、母臂r轴伺服电机和母臂θ轴伺服电机运动参数，并能将参数传送给第一运动控制卡。所述第一运动控制卡能够根据工控机传送来的母臂Z轴伺服电机、母臂r轴伺服电机和母臂θ轴伺服电机运动参数，通过母臂Z轴伺服电机驱动器、母臂r轴伺服电机驱动器和母臂θ轴伺服电机驱动器驱动母臂Z轴伺服电机、母臂r轴伺服电机和母臂θ轴伺服电机运转，最后驱动母臂Z轴、母臂r轴和母臂θ轴工作。

本发明所述双目摄像机为现有技术中的双目摄像机，其具体结构本发明不作详细的说明。本发明所述双目摄像机的作用是拍摄果树图像，并能通过转接卡将果树图像传送给工控机。

本发明中所述的转接卡为现有技术中的与双目摄像机配合使用的转接卡，其具体结构和工作过程本发明不作详细的说明。

本发明中所述的转换器为现有技术中的转换器，本发明优先采用现有技术中型号为R232/485转换器。

进一步，本发明所述母机械臂控制系统包括第一运动控制卡、母臂Z轴伺服电机驱动器、母臂Z轴伺服电机、母臂Z轴伺服电机编码器、母臂r轴伺服电机驱动器、母臂r轴伺服电机、母臂r轴伺服电机编码器、母臂θ轴伺服电机驱动器、母臂θ轴伺服电机和母臂θ轴伺服电机编码器；

所述母臂Z轴伺服电机驱动器的输入端与第一运动控制卡的输出端相连，母臂Z轴伺服电机驱动器的输出端与母臂Z轴伺服电机的输入端相连；所述母臂Z轴伺服电机编码器能够检测母臂Z轴伺服电机旋转角度位移量，母臂Z轴伺服电机编码器的输出端与第一运动控制卡的输入端相连；所述母臂Z轴伺服电机能够驱动母臂Z轴运动。所述母臂Z轴伺服电机编码器能够将检测到的母臂Z轴伺服电机旋转角度位移量传送给第一运动控制卡，第一运动控制卡根据此信息实现母臂Z轴运动的精确控制。

所述母臂r轴伺服电机驱动器的输入端与第一运动控制卡的输出端相连，母臂r轴伺服电机驱动器的输出端与母臂r轴伺服电机的输入端相连；所述母臂r轴伺服电机编码器能够检测母臂r轴伺服电机旋转角度位移量，母臂r轴伺服电机编码器的输出端与第一运动控制卡的输入端相连；所述母臂r轴伺服电机能够驱动母臂r轴运动；所述母臂r轴伺服电机编码器能够将检测到的母臂r轴伺服电机旋转角度位移量传送给第一运动控制卡，第一运动控制卡根据此信息实现母臂r轴运动的精确控制。

所述母臂θ轴伺服电机驱动器的输入端与第一运动控制卡的输出端相连，母臂θ轴伺服电机驱动器的输出端与母臂θ轴伺服电机的输入端相连；所述母臂θ轴伺服电机编码器能够检测母臂θ轴伺服电机旋转角度位移量，母臂θ轴伺服电机编码器的输出端与第一运动控制卡的输入端相连；所述母臂θ轴伺服电机能够驱动母臂θ轴运动。所述母臂θ轴伺服电机编码器能够将检测到的母臂θ轴伺服电机旋转角度位移量传送给第一运动控制卡，第一运动控制卡根据此信息实现母臂θ轴运动的精确控制。所述第一运动控制卡与工控机相连。

本发明所述的第一运动控制卡为现有技术与工控机配合使用的运动控制卡，其具体结构和工作过程本发明不作详细的说明。

进一步，本发明所述子机械臂与末端执行器控制系统包括下位机、单目摄像机、位置传感器、第二运动控制卡、子臂Z轴伺服电机驱动器、子臂Z轴伺服电机、子臂Z轴伺服电机编码器、子臂r轴伺服电机驱动器、子臂r轴伺服电机、子臂r轴伺服电机编码器、子臂θ轴伺服电机驱动器、子臂θ轴伺服电机、子臂θ轴伺服电机编码器、旋转底盖电机驱动器、旋转底盖电机、拢果器电机驱动器和拢果器电机；

所述下位机通过前述的转换器与工控机相连；所述单目摄像机设置在前述的架台上，且单目摄像机与下位机相连接；位置传感器设置在接果器的斜开口端的内壁上；位置传感器与下位机相连接。所述拢果器电机设置在接果器的封闭端上，拢果器电机的电机轴上安装有传动轮，传动轮通过连接在刀片轴上的传动带将拢果器电机的动力传递给主动齿轮；

单目摄像机为现有技术中的单目摄像机，其具体结构和工作原理本发明不作详细的说明。

本发明中所述的位置传感器为现有技术中的光电对射式传感器，包括信号发射部件和信号接收部件，信号发射部件镶嵌安装于接果器的斜开口端的内壁上，信号接收部件同种方法安装于接果器的斜开口端的与信号发射部件相对的一侧内壁上。

本发明所述的下位机为现有技术中与工控机配合使用的下位机，其具体结构和工作原理本发明不作详细的说明。

子臂Z轴伺服电机驱动器的输入端与第二运动控制卡的输出端相连，子臂Z轴伺服电机驱动器的输出端与子臂Z轴伺服电机的输入端相连；所述子臂Z轴伺服电机编码器能够检测子臂Z轴伺服电机旋转角度位移量，子臂Z轴伺服电机编码器的输出端与第二运动控制卡的输入端相连；所述子臂Z轴伺服电机能够驱动子臂Z轴运动；所述子臂Z轴伺服电机编码器能够将检测到的子臂Z轴伺服电机旋转角度位移量传送给第二运动控制卡，第二运动控制卡根据此信息实现子臂Z轴运动的精确控制。

子臂r轴伺服电机驱动器的输入端与第二运动控制卡的输出端相连，子臂r轴伺服电机驱动器的输出端与子臂r轴伺服电机的输入端相连；所述子臂Z轴伺服电机编码器能够检测子臂r轴伺服电机旋转角度位移量，子臂r轴伺服电机编码器的输出端与第二运动控制卡的输入端相连。所述子臂r轴伺服电机能够驱动子臂r轴运动。所述子臂r轴伺服电机编码器能够将检测到的子臂r轴伺服电机旋转角度位移量传送给第二运动控制卡，第二运动控制卡根据此信息实现子臂r轴运动的精确控制。

子臂θ轴伺服电机驱动器的输入端与第二运动控制卡的输出端相连，子臂θ轴伺服电机驱动器的输出端与子臂θ轴伺服电机的输入端相连；所述子臂θ轴伺服电机编码器能够检测子臂θ轴伺服电机旋转角度位移量，子臂θ轴伺服电机编码器的输出端与第二运动控制卡的输入端相连；所述子臂θ轴伺服电机能够驱动子臂θ轴运动；所述子臂θ轴伺服电机编码器能够将检测到的子臂θ轴伺服电机旋转角度位移量传送给第二运动控制卡，第二运动控制卡根据此信息实现子臂θ轴运动的精确控制。所述第二运动控制卡与前述的下位机相连接。

旋转底盖电机驱动器的输入端与第二运动控制卡的输出端相连，旋转底盖电机驱动器的输出端与所述旋转底盖电机相连。作为优选，旋转底盖电机选用步进电机。

拢果器电机驱动器的输入端与第二运动控制卡的输出端相连，拢果器电机驱动器的输出端与所述拢果器电机相连。作为优选，旋转底盖电机选用步进电机。

本发明所述的第二运动控制卡为现有技术与工控机配合使用的运动控制卡，其具体结构和工作过程本发明不作详细的说明。

本发明所述的母臂Z轴伺服电机驱动器、母臂r轴伺服电机驱动器、母臂θ轴伺服电机驱动器、子臂Z轴伺服电机驱动器、子臂r轴伺服电机驱动器和子臂θ轴伺服电机驱动器均为现有技术中的伺服电机驱动，其具体结构和工作原理本发明不作详细的说明。

本发明所述的母臂Z轴伺服电机、母臂r轴伺服电机、母臂θ轴伺服电机、子臂Z轴伺服电机、子臂r轴伺服电机和子臂θ轴伺服电机均为现有技术中的伺服电机，其具体结构和工作原理本发明不作详细的说明。

本发明所述的母臂Z轴伺服电机编码器、母臂r轴伺服电机编码器、母臂θ轴伺服电机编码器、子臂Z轴伺服电机编码器、子臂r轴伺服电机编码器和子臂θ轴伺服电机编码器均为现有技术中伺服电机编码器，其具体结构和工作原理本发明不作详细的说明。

本发明所述的旋转底盖电机驱动器和拢果器电机驱动器均为现有技术中的电机驱动器，其具体结构和工作原理本发明不作详细的说明。

本发明所述的旋转底盖电机和拢果器电机均为现有技术中电机，其具体结构和工作原理本发明不作详细的说明。

进一步，本发明所述母臂Z轴和母臂r轴均为双平行臂结构。本发明中母臂Z轴和母臂r轴均为双平行臂结构，其主要作用是增强母臂Z轴和母臂r轴的强度，提高母臂Z轴和母臂r轴承重量，延长使用寿命。

进一步，本发明所述母臂θ轴和底座均为圆柱体结构，母臂θ轴的半径小于底座的半径。

进一步，本发明所述母机械臂还包括斜柱；所述斜柱的一端固定在母臂Z轴上，另一端固定在母臂θ轴上。本发明设置斜柱，其主要作用是增强母臂Z轴稳定性，延长使用寿命。

本发明还公开了一种水果采摘的方法，包括以下步骤：

1)双目摄像机检测到目标果树时，将目标果树图像传送给工控机，工控机图像处理获得目标果树尺寸信息和树冠中心点空间坐标信息后，确定图像采集点数及各采集点空间位置坐标，然后确定母机械臂果实采摘点数及各采摘点空间坐标；

2）工控机通过串口向第一运动控制卡发送控制指令，使母机械臂运动至第一图像采集点；

3)双目摄像机采集果树图像，并将图像传送给工控机；

4）工控机通过串口向第一运动控制卡发送控制指令，使母机械臂运动至第一果实采摘点；

5)工控机进行图像处理，完成图像内目标果实识别与定位，并获得各个果实的空间位置和尺寸信息，计算每个目标果实与各子机械臂的距离，根据距离最近原则分配末端执行器采摘果实任务，并将各末端执行器采摘任务果实的空间位置和尺寸信息传送给对应的下位机；

6)各下位机确定对应末端执行器目标果实采摘路径，并计算对应子臂r轴、子臂θ轴、子臂Z轴的运动参数，并将参数传送给第二运动控制卡；

7）第二运动控制卡控制子机械臂运动并带动末端执行器运动，子机械臂与末端执行器进行果实采摘；

8）第一果实采摘点内的果实采摘完毕后，子机械臂返回初始位置，母机械臂运动并带动子机械臂及末端执行器到达果实回收位置，末端执行器旋转底盖打开，果实从收果筒落入果篮，从而完成一次采摘；

9）母机械臂运动至下一图像采集点，开始下一次果实采摘。

进一步，本发明所述水果采摘的方法的步骤1）中的母机械臂果实采摘点数及各采摘点空间坐标的确定方法如下所述：

1）确定世界坐标系，取母机械臂的底座的圆柱坐标原点为世界坐标系原点，机器人行进方向为x轴正方向，垂直x轴机器人行进方向左侧为y轴正方向，垂直水平面向上方向为Z正方向；

2）确定图像采集点数及每个采集点的空间坐标；

设树冠中心点空间坐标为P₀（x₀，y₀，z₀），设子臂Z轴长度为d，子臂r轴最大伸缩长度为e，目标果树树冠高度为b，宽度为a，则图像采集点数为k=2m，其中m为的整数部分；根据图像采集点数将果树树冠均分为2列m行k个区域，每个区域对应一个图像采集点。设双目摄像机图像最佳拍摄点距离果树距离为c，若某图像采集点对应树冠区域空间坐标为P_q（x_q，y_q，z_q），则该图像采集点坐标为P_t（x_t，y_t，z_t）,其中x_t=x_q，y_t=（y_q-c），z_t=z_q；

3）母机械臂果实采摘点数与图像采集点数相同；设某采摘点空间坐标P_c(x_c，y_c，z_c)，则x_c=x_t，y_c=（y_t+c-e+β），z_c=z_t，其中x_t、y_t、z_t分别为该采摘点对应图像采集点x、y、z轴坐标，β>0；

进一步，本发明所述水果采摘的方法的所述步骤7）中所述的第二运动控制卡控制子机械臂运动并带动末端执行器运动，子机械臂与末端执行器进行果实采摘的具体方法如下：

1）子机械臂运动带动末端执行器伸向目标果实，果实进入接果器；末端执行器伸向目标果实的过程中，装在末端执行器前端的单目摄像机拍摄目标果实图像，并传送给下位机处理，获得果实型心坐标，根据果实型心坐标与图像中心坐标，计算子机械臂运动调整参数，并将参数传送给第二运动控制卡，调整分机械臂各轴运动，使末端执行器对准目标果实型心，实现果实动态定位与跟踪；

2）位置传感器检测到果实完全进入接果器后，将信号传送下位机；

3）下位机给第二运动控制卡发送指令使末端执行器拢果器电机旋转，拢果器与刀片旋转并咬合切断果梗，果实落入收果筒；

4）位置传感器检测到果实落入收果筒，向下位机传送果实采摘成功信息；下位机通过第二运动控制卡控制子机械臂与末端执行器运动进行下一果实采摘。

本发明与现有技术相比的优点：

（1）本发明采摘机器人可以设置多个末端执行器，一次可以完成一幅果树图像中所有目标果实的连续采摘。果实的平均采摘周期短，采摘效率高。

（2）机械臂采用母机械臂和子机械臂二级模式，机械臂总自由度不增加的前提下实现多个末端执行器的同时作业的要求，并且机械臂采用圆柱坐标结构，机械臂控制与运动路径简单，效率较高。

（3）末端执行器可实现苹果、柑橘、梨等多种果实采摘，通用性好。

（4）末端执行器为欠驱动式结构设计，整个采摘与控制过程简单，采摘时间短。末端执行器设有收果筒，可储存多个果实，实现采摘过程中水果连续采摘，节约回放果实时间，提高采摘效率。

（5）采用双目视觉与单目视觉相结合的方式，实现果实的动态定位于跟踪，能有效克服果实采摘过程中因果实晃动带来的定位不准、采摘失败问题。

附图说明

图1为本发明的机械臂结构图。

图2为本发明的末端执行器的第一结构示意图。

图3为本发明的末端执行器的第二结构示意图。

图4为本发明的机械臂与多末端执行器控制系统示意图。

图5为本发明的子机械臂与多末端执行器控制系统示意图。

图6为本发明的机械臂和末端执行器水果采摘流程图。

其中，1、母机械臂，2、子机械臂，3、母臂Z轴，4、母臂r轴，5、母臂θ轴，6、底座，7、斜柱，8、端架，9、支架，10、双目摄像机，11、子臂Z轴，12、子臂r轴，13、子臂θ轴，14、子臂法兰，15、母臂Z轴伺服电机，16、母臂r轴伺服电机，17、母臂θ轴伺服电机，18、子臂Z轴伺服电机，19、子臂r轴伺服电机，20、子臂θ轴伺服电机，21、接果器，22、基部法兰，23、拢果器，24、刀片，25、收果筒，26、旋转底盖，27、位置传感器，28、拢果器电机，29、旋转底盖电机，30、传动轮，31、传动带，32、主动齿轮，33、从动齿轮，34、架台，35、单目摄像机，36、横杆，37、拢果器轴，38、刀片轴，39、旋转底盖轴，40、通孔。

具体实施方式

为使本发明的内容更加明显易懂，以下结合附图1-图5和具体实施方式做进一步的描述。

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

本实施例以四个子机械臂以及四个末端执行器为例，来描述本发明的具体结构和工作过程。

本发明的水果采摘机器人的机械臂和末端执行器机械臂和末端执行器，包括果实采摘机械装置和控制系统。

如图1所示，果实采摘机械装置包括母机械臂1、四个子机械臂2和四个末端执行器。

母机械臂1包括母臂Z轴3、母臂r轴4、母臂θ轴5、底座6、斜柱7、端架8和支架9。

子机械臂2包括子臂Z轴11、子臂r轴12、子臂θ轴13和子臂法兰14。

末端执行器包括接果器21、基部法兰22、拢果器23、刀片24、收果筒25、旋转底盖26、传动轮30、传动带31、主动齿轮32、从动齿轮33、架台34、横杆36、拢果器轴37、刀片轴38和旋转底盖轴39。

本发明所述的母机械臂1的工作内容为将安装在支架9上双目摄像机送到指定图像采集点和将子机械臂及固定在子机械臂2上的末端执行器送达指定果实采摘点，末端执行器完成目标果实采摘后，母机械臂将子机械臂2和末端执行器送达果实回收点。

母机械臂1中的各零部件之间的具体位置关系为：母臂θ轴5转动设置在底座6上，母臂Z轴3垂直固定设置在母臂θ轴5上，母臂r轴4垂直设置在母臂Z轴3上，所述母臂r轴4沿着母臂Z轴3的轴向方向滑动，子臂θ轴13绕着自身的中心轴线转动；端架8固定在母臂r轴4的端头，支架9固定在母臂r轴4上。

本发明所述子机械臂的工作内容为在母机械臂的配合下完成将末端执行器送达指定采摘位置，末端执行器完成目标果实采摘任务后，子机械臂在母机械臂的配合下完成将末端执行器送达果实回收点。

子机械臂2中的各零部件之间的具体位置关系为：子臂Z轴11与前述端架8相连，子臂θ轴13设置在子臂Z轴11上，子臂θ轴13沿着子臂Z轴11的轴向方向滑动；子臂r轴12与子臂θ轴13相连，子臂r轴12沿着子臂r轴12的轴向方向伸缩，子臂θ轴13可在水平面内左右方向旋转；子臂法兰14设置在子臂r轴12的端头。

本发明中所述末端执行器的工作内容为完成目标果实的采摘与回收。

末端执行器中的各零部件之间的具体位置关系为：末端执行器包括接果器21、基部法兰22、拢果器23、刀片24、收果筒25、旋转底盖26、传动轮30、传动带31、主动齿轮32、从动齿轮33、架台34、横杆36、拢果器轴37、刀片轴38和旋转底盖轴39；接果器21为一端封闭，另一端上设有斜开口的圆筒；基部法兰22与前述子臂法兰14相连，基部法兰22通过横杆36与接果器21的封闭端相连，刀片24通过刀片轴38转动设置接果器21的斜开口端上，拢果器23通过拢果器轴37转动设置在接果器21的斜开口端上，所述主动齿轮32与刀片轴38同轴安装，从动齿轮33与拢果器轴37同轴安装，且主动齿轮32与从动齿轮33相啮合；传动带31连接刀片轴38；收果筒25设置在接果器21的斜开口端上，且接果器21的斜开口端上设有与收果筒25相连通的通孔40；架台34设置在接果器21的斜开口端上；旋转底盖26通过旋转底盖轴39转动设置在收果筒25的底端。

本发明所述的控制系统实现果树图像拍摄、图像处理、目标果实的识别与定位以及母机械臂、小机械臂运动控制和末端执行器采摘控制。

本发明所述的控制系统包括工控机、双目摄像机10、转接卡、用于控制母机械臂1动作的母机械臂控制系统、转换器和控制子机械臂2和末端执行器动作的子机械臂与末端执行器控制系统；双目摄像机10固定在前述的支架9上，且双目摄像机通过转接卡与工控机相连，所述子机械臂与末端执行器控制系统通过转换器与工控机相连。

本发明所述母机械臂控制系统包括第一运动控制卡、母臂Z轴伺服电机驱动器、母臂Z轴伺服电机、母臂Z轴伺服电机编码器、母臂r轴伺服电机驱动器、母臂r轴伺服电机、母臂r轴伺服电机编码器、母臂θ轴伺服电机驱动器、母臂θ轴伺服电机和母臂θ轴伺服电机编码器；

所述母臂Z轴伺服电机驱动器的输入端与第一运动控制卡的输出端相连，母臂Z轴伺服电机驱动器的输出端与母臂Z轴伺服电机的输入端相连；所述母臂Z轴伺服电机编码器能够检测母臂Z轴伺服电机旋转角度位移量，母臂Z轴伺服电机编码器的输出端与第一运动控制卡的输入端相连；所述母臂Z轴伺服电机能够驱动母臂Z轴运动。所述母臂Z轴伺服电机编码器能够将检测到的母臂Z轴伺服电机旋转角度位移量传送给第一运动控制卡，第一运动控制卡根据此信息实现母臂Z轴运动的精确控制。

所述母臂r轴伺服电机驱动器的输入端与第一运动控制卡的输出端相连，母臂r轴伺服电机驱动器的输出端与母臂r轴伺服电机的输入端相连；所述母臂r轴伺服电机编码器能够检测母臂r轴伺服电机旋转角度位移量，母臂r轴伺服电机编码器的输出端与第一运动控制卡的输入端相连；所述母臂r轴伺服电机能够驱动母臂r轴运动；所述母臂r轴伺服电机编码器能够将检测到的母臂r轴伺服电机旋转角度位移量传送给第一运动控制卡，第一运动控制卡根据此信息实现母臂r轴运动的精确控制。

所述母臂θ轴伺服电机驱动器的输入端与第一运动控制卡的输出端相连，母臂θ轴伺服电机驱动器的输出端与母臂θ轴伺服电机的输入端相连；所述母臂θ轴伺服电机编码器能够检测母臂θ轴伺服电机旋转角度位移量，母臂θ轴伺服电机编码器的输出端与第一运动控制卡的输入端相连；所述母臂θ轴伺服电机能够驱动母臂θ轴运动。所述母臂θ轴伺服电机编码器能够将检测到的母臂θ轴伺服电机旋转角度位移量传送给第一运动控制卡，第一运动控制卡根据此信息实现母臂θ轴运动的精确控制。所述第一运动控制卡与工控机相连。

本发明所述子机械臂与末端执行器控制系统包括下位机、单目摄像机35、位置传感器27、第二运动控制卡、子臂Z轴伺服电机驱动器、子臂Z轴伺服电机编码器、子臂Z轴伺服电机、子臂r轴伺服电机驱动器、子臂r轴伺服电机、子臂r轴伺服电机编码器、子臂θ轴伺服电机驱动器、子臂θ轴伺服电机、子臂θ轴伺服电机编码器、旋转底盖电机驱动器、旋转底盖电机、拢果器电机驱动器和拢果器电机28；

下位机通过前述的转换器与工控机相连；所述单目摄像机35设置在前述的架台34上，且单目摄像机35与下位机相连接；所述位置传感器27设置在接果器21的斜开口端的内壁上；位置传感器与下位机相连接。所述拢果器电机28设置在接果器21的封闭端上，拢果器电机28的电机轴上安装有传动轮30，传动轮30通过连接在刀片轴38上的传动带31将拢果器电机28的动力传递给主动齿轮32；

所述子臂Z轴伺服电机驱动器的输入端与第二运动控制卡的输出端相连，子臂Z轴伺服电机驱动器的输出端与子臂Z轴伺服电机的输入端相连；所述子臂Z轴伺服电机编码器能够检测子臂Z轴伺服电机旋转角度位移量，子臂Z轴伺服电机编码器的输出端与第二运动控制卡的输入端相连；所述子臂Z轴伺服电机能够驱动子臂Z轴运动；所述子臂Z轴伺服电机编码器能够将检测到的子臂Z轴伺服电机旋转角度位移量传送给第二运动控制卡，第二运动控制卡根据此信息实现子臂Z轴运动的精确控制。

所述子臂r轴伺服电机驱动器的输入端与第二运动控制卡的输出端相连，子臂r轴伺服电机驱动器的输出端与子臂r轴伺服电机的输入端相连；所述子臂Z轴伺服电机编码器能够检测子臂r轴伺服电机旋转角度位移量，子臂r轴伺服电机编码器的输出端与第二运动控制卡的输入端相连。所述子臂r轴伺服电机能够驱动子臂r轴运动。所述子臂r轴伺服电机编码器能够将检测到的子臂r轴伺服电机旋转角度位移量传送给第二运动控制卡，第二运动控制卡根据此信息实现子臂r轴运动的精确控制。

所述子臂θ轴伺服电机驱动器的输入端与第二运动控制卡的输出端相连，子臂θ轴伺服电机驱动器的输出端与子臂θ轴伺服电机的输入端相连；所述子臂θ轴伺服电机编码器能够检测子臂θ轴伺服电机旋转角度位移量，子臂θ轴伺服电机编码器的输出端与第二运动控制卡的输入端相连；所述子臂θ轴伺服电机能够驱动子臂θ轴运动；所述子臂θ轴伺服电机编码器能够将检测到的子臂θ轴伺服电机旋转角度位移量传送给第二运动控制卡，第二运动控制卡根据此信息实现子臂θ轴运动的精确控制。所述第二运动控制卡与前述的下位机相连接。

进一步，本发明所述母臂Z轴3和母臂r轴4均为双平行臂结构。本发明中母臂Z轴3和母臂r轴4均为双平行臂结构，其主要作用是增强母臂Z轴3和母臂r轴4的强度，提高母臂Z轴3和母臂r轴4承重量，延长使用寿命。

进一步，本发明所述母臂θ轴5和底座6均为圆柱体结构，所述母臂θ轴5的半径小于底座6的半径。

进一步，本发明所述母机械臂1还包括斜柱7；所述斜柱7的一端固定在母臂Z轴3上，另一端固定在母臂θ轴5上。本发明设置斜柱7，其主要作用是增强母臂Z轴3稳定性，延长使用寿命。

本发明还公开了一种水果采摘的方法，包括以下步骤：

1)双目摄像机检测到目标果树时，将目标果树图像传送给工控机，工控机图像处理获得目标果树尺寸信息和树冠中心点空间坐标信息后，确定图像采集点数及各采集点空间位置坐标，然后确定母机械臂果实采摘点数及各采摘点空间坐标；

母机械臂果实采摘点数及各采摘点空间坐标的确定方法如下所述：

1.1确定世界坐标系，取母机械臂的底座6的圆柱坐标原点为世界坐标系原点，机器人行进方向为x轴正方向，垂直x轴机器人行进方向左侧为y轴正方向，垂直水平面向上方向为Z正方向；

1.2确定图像采集点数及每个采集点的空间坐标；

设树冠中心点空间坐标为P₀（x₀，y₀，z₀），设子臂Z轴长度为d，子臂r轴最大伸缩长度为e，目标果树树冠高度为b，宽度为a，则图像采集点数为k=2m，其中m为的整数部分；根据图像采集点数将果树树冠均分为2列m行k个区域，每个区域对应一个图像采集点。设双目摄像机图像最佳拍摄点距离果树距离为c，若某图像采集点对应树冠区域空间坐标为P_q（x_q，y_q，z_q），则该图像采集点坐标为P_t（x_t，y_t，z_t）,其中x_t=x_q，y_t=（y_q-c），z_t=z_q；

1.3母机械臂果实采摘点数与图像采集点数相同。设某采摘点空间坐标P_c(x_c，y_c，z_c)，则x_c=x_t，y_c=（y_t+c-e+β），z_c=z_t，其中x_t、y_t、z_t分别为该采摘点对应图像采集点x、y、z轴坐标，β>0；

2）工控机通过串口向第一运动控制卡发送控制指令，使母机械臂运动至第一图像采集点；

3)双目摄像机采集果树图像，并将图像传送给工控机；

4）工控机通过串口向第二运动控制卡发送控制指令，使母机械臂运动至第一果实采摘点；

5)工控机进行图像处理，完成图像内目标果实识别与定位，并获得各个果实的空间位置和尺寸信息，计算每个目标果实与各子机械臂的距离，根据距离最近原则分配末端执行器采摘果实任务，并将各末端执行器采摘任务果实的空间位置和尺寸信息传送给对应的下位机；

6)各下位机确定对应末端执行器目标果实采摘路径，并计算对应子臂r轴、子臂θ轴、子臂Z轴的运动参数，并将参数传送给第二运动控制卡；

7）第二运动控制卡控制子机械臂运动并带动末端执行器运动，子机械臂与末端执行器进行果实采摘；

第二运动控制卡控制子机械臂运动并带动末端执行器运动，子机械臂与末端执行器进行果实采摘的具体方法如下：

1.1子机械臂运动带动末端执行器伸向目标果实，果实进入接果器；末端执行器伸向目标果实的过程中，装在末端执行器前端的单目摄像机拍摄目标果实图像，并传送给下位机处理，获得果实型心坐标，根据果实型心坐标与图像中心坐标，计算子机械臂运动调整参数，并将参数传送给第二运动控制卡，调整分机械臂各轴运动，使末端执行器对准目标果实型心，实现果实动态定位与跟踪；

1.2位置传感器检测到果实完全进入接果器后，将信号传送下位机；

1.3下位机给第二运动控制卡发送指令使末端执行器拢果器电机旋转，拢果器与刀片旋转并咬合切断果梗，果实落入收果筒；

1.4位置传感器检测到果实落入收果筒，向下位机传送果实采摘成功信息；下位机通过第二运动控制卡控制子机械臂与末端执行器运动进行下一果实采摘。

8）第一果实采摘点内的果实采摘完毕后，子机械臂返回初始位置，母机械臂运动至带动子机械臂及末端执行器到达果实回收位置，末端执行器旋转底盖打开，果实从收果筒落入果篮，从而完成一次采摘；

9）母机械臂运动至下一图像采集点，开始下一次果实采摘。

除上述实施例外，本发明还可以有其他实施方式。凡采用等同替换或等效变换形成的技术方案，均落在本发明要求的保护范围。

本发明未涉及部分均与现有技术相同或采用现有技术加以实现。

Claims (9)

1.一种水果采摘机器人的机械臂和末端执行器，包括果实采摘机械装置和控制系统，其特征是， 

所述果实采摘机械装置包括母机械臂（1）、固定在母机械臂（1）上的不少于一个的子机械臂（2）和固定在子机械臂（2）上的末端执行器； 

其中，所述母机械臂（1）包括母臂Z轴（3）、母臂r轴（4）、母臂θ轴（5）、底座（6）、端架（8）和支架（9）；母臂θ轴（5）转动设置在底座（6）上，母臂Z轴（3）垂直固定设置在母臂θ轴（5）上，母臂r轴（4）垂直设置在母臂Z轴（3）上，所述母臂r轴（4）沿着母臂Z轴（3）的轴向方向滑动，所述子臂θ轴（13）绕着自身的中心轴线转动；端架（8）固定在母臂r轴（4）的端头，支架（9）固定在母臂r轴（4）上； 

所述子机械臂（2）包括子臂Z轴（11）、子臂r轴（12）、子臂θ轴（13）和子臂法兰（14）；所述子臂Z轴（11）与前述端架（8）相连，子臂θ轴（13）设置在子臂Z轴（11）上，所述子臂θ轴（13）沿着子臂Z轴（11）的轴向方向滑动；子臂r轴（12）与子臂θ轴（13）相连，所述子臂r轴（12）沿着子臂r轴（12）的轴向方向伸缩，所述子臂θ轴（13）绕着自身的中心轴线转动；子臂法兰（14）设置在子臂r轴（12）的端头； 

所述末端执行器包括接果器（21）、基部法兰（22）、拢果器（23）、刀片（24）、收果筒（25）、旋转底盖（26）、传动轮（30）、传动带（31）、主动齿轮（32）、从动齿轮（33）、架台（34）、横杆（36）、拢果器轴（37）、刀片轴（38）和旋转底盖轴（39）；所述接果器（21）为一端封闭，另一端上设有斜开口的圆筒；所述基部法兰（22）与前述子臂法兰（14）相连，基部法兰（22）通过横杆（36）与接果器（21）的封闭端相连，刀片（24）通过刀片轴（38）转动设置接果器（21）的斜开口端上，拢果器（23）通过拢果器轴（37）转动设置在接果器（21）的斜开口端上，所述主动齿轮（32）与刀片轴（38）同轴安装，从动齿轮（33）与拢果器轴（37）同轴安装，且主动齿轮（32）与从动齿轮（33）相啮合；传动带（31）连接刀片轴（38）；所述收果筒（25）设置在接果器（21）的斜开口端上，且所述接果器（21）的斜开口端上设有与收果筒（25）相连通的通孔（40）；架台（34）设置在接果器（21）的中部；旋转底盖（26）通过旋转底盖轴（39）转动设置在收果筒（25）的底端； 

所述控制系统包括工控机、双目摄像机（10）、转接卡、用于控制母机械臂（1）动作的母机械臂控制系统、转换器和控制子机械臂（2）和末端执行器动作的子机械臂与末端执行器控制系统；所述双目摄像机（10）固定在前述的支架（9）上，且双目摄像机通过转接卡与工控机相连，所述子机械臂与末端执行器控制系统通过转换器与工控机相连。 

2.如权利要求1所述的水果采摘机器人的机械臂和末端执行器，其特征是，所述母机械臂控制系统包括第一运动控制卡、母臂Z轴伺服电机驱动器、母臂Z轴伺服电机、母臂Z轴伺服电机编码器、母臂r轴伺服电机驱动器、母臂r轴伺服电机、母臂r轴伺服电机编码器、母臂θ轴伺服电机驱动器、母臂θ轴伺服电机和母臂θ轴伺服电机编码器；所述母臂Z轴伺服电机驱动器的输入端与第一运动控制卡的输出端相连，母臂Z轴伺服电机驱动器的输出端与母臂Z轴伺服电机的输入端相连，母臂Z轴伺服电机编码器的输出端与第一运动控制卡的输入端相连；所述母臂r轴伺服电机驱动器的输入端与第一运动控制卡的输出端相连，母臂r轴伺服电机驱动器的输出端与母臂r轴伺服电机的输入端相连，母臂r轴伺服电机编码器的输出端与第一运动控制卡的输入端相连；所述母臂θ轴伺服电机驱动器的输入端与第一运动控制卡的输出端相连，母臂θ轴伺服电机驱动器的输出端与母臂θ轴伺服电机的输入端相连，母臂θ轴伺服电机编码器的输出端与第一运动控制卡的输入端相连；所述第一运动控制卡与工控机相连。 

3.如权利要求1所述的水果采摘机器人的机械臂和末端执行器，其特征是，所述子机械臂与末端执行器控制系统包括下位机、单目摄像机（35）、位置传感器（27）、第二运动控制卡、子臂Z轴伺服电机驱动器、子臂Z轴伺服电机编码器、子臂Z轴伺服电机、子臂r轴伺服电机驱动器、子臂r轴伺服电机、子臂r轴伺服电机编码器、子臂θ轴伺服电机驱动器、子臂θ轴伺服电机、子臂θ轴伺服电机编码器、旋转底盖电机驱动器、旋转底盖电机、拢果器电机驱动器和拢果器电机（28）；所述下位机通过前述的转换器与工控机相连；所述单目摄像机（35）设置在前述的架台（34）上，且单目摄像机（35）与下位机相连接；所述位置传感器（27）设置在接果器（21）的斜开口端的内壁上；位置传感器（27）与下位机相连接；所述拢果器电机（28）设置在接果器（21）的封闭端上，拢果器电机（28）的电机轴上安装有传动轮（30），传动轮（30）通过连接在刀片轴（38）上的传动带（31）将拢果器电机（28）的动力传递给主动齿轮（32）；所述子臂Z轴伺服电机驱动器的输入端与第二运动控制卡的输出端相连，子臂Z轴伺服电机驱动器的输出端与子臂Z轴伺服电机的输入端相连，子臂Z轴伺服电机编码器的输出端与第二运动控制卡的输入端相连；所述子臂r轴伺服电机驱动器的输入端与第二运动控制卡的输出端相连，子臂r轴伺服电机驱动器的输出端与子臂r轴伺服电机的输入端相连，子臂r轴伺服电机编码器的输出端与第二运动控制卡的输入端相连；所述子臂θ轴伺服电机驱动器的输入端与第二运动控制卡的输出端相连，子臂θ轴伺服电机驱动器的输出端与子臂θ轴伺服电机的输入端相连，子臂θ轴伺服电机编码器的输出端与第二运动控制卡的输入端相连；所述第二运动控制卡与前述的下位机相连接；所述旋转底盖电机 驱动器的输入端与第二运动控制卡的输出端相连，旋转底盖电机驱动器的输出端与所述旋转底盖电机相连；所述拢果器电机驱动器的输入端与第二运动控制卡的输出端相连，拢果器电机驱动器的输出端与所述拢果器电机相连。 

4.如权利要求1所述的水果采摘机器人的机械臂和末端执行器，其特征是，所述母臂Z轴（3）和母臂r轴（4）均为双平行臂结构。 

5.如权利要求1所述的水果采摘机器人的机械臂和末端执行器，其特征是，所述母臂θ轴（5）和底座（6）均为圆柱体结构，所述母臂θ轴（5）的半径小于底座（6）的半径。 

6.如权利要求1所述的水果采摘机器人的机械臂和末端执行器，其特征是，所述母机械臂（1）还包括斜柱（7）；所述斜柱（7）的一端固定在母臂Z轴（3）上，另一端固定在母臂θ轴（5）上。 

7.一种利用如权利要求1至权利要求6任一项所述的水果采摘机器人的机械臂和末端执行器进行水果采摘的方法，其特征在于，包括以下步骤： 

1)双目摄像机检测到目标果树时，将目标果树图像传送给工控机，工控机图像处理获得目标果树尺寸信息和树冠中心点空间坐标信息后，确定图像采集点数及各采集点空间位置坐标，然后确定母机械臂果实采摘点数及各采摘点空间坐标； 

2）工控机通过串口向第一运动控制卡发送控制指令，使母机械臂运动至第一图像采集点； 

3)单目摄像机采集果树图像，并将图像传送给工控机； 

4）工控机通过串口向第一运动控制卡发送控制指令，使母机械臂运动至第一果实采摘点； 

5)工控机进行图像处理，完成图像内目标果实识别与定位，并获得各个果实的空间位置和尺寸信息，计算每个目标果实与各子机械臂的距离，根据距离最近原则分配末端执行器采摘果实任务，并将各末端执行器采摘任务果实的空间位置和尺寸信息传送给对应的下位机； 

6)各下位机确定对应末端执行器目标果实采摘路径，并计算对应子臂r轴、子臂θ轴、子臂Z轴的运动参数，并将参数传送给第二运动控制卡； 

7）第二运动控制卡控制子机械臂运动并带动末端执行器运动，子机械臂与末端执行器进行果实采摘； 

8）第一果实采摘点内的果实采摘完毕后，子机械臂返回初始位置，母机械臂运动至带动子机械臂及末端执行器到达果实回收位置，末端执行器旋转底盖打开，果实从收果筒落入果 篮，从而完成一次采摘； 

9）母机械臂运动至下一图像采集点，开始下一次果实采摘。 

8.如权利要求7所述的水果采摘方法，其特征是，所述步骤1）中的母机械臂果实采摘点数及各采摘点空间坐标的确定方法如下所述： 

1）确定世界坐标系，取母机械臂的底座（6）的圆柱坐标原点为世界坐标系原点，机器人行进方向为x轴正方向，垂直x轴机器人行进方向左侧为y轴正方向，垂直水平面向上方向为Z正方向； 

2）确定图像采集点数及每个采集点的空间坐标； 

设树冠中心点空间坐标为P₀（x₀，y₀，z₀），设子臂Z轴长度为d，子臂r轴最大伸缩长度为e，目标果树树冠高度为b，宽度为a，则图像采集点数为k=2m，其中m为的整数部分；根据图像采集点数将果树树冠均分为2列m行k个区域，每个区域对应一个图像采集点。设双目摄像机图像最佳拍摄点距离果树距离为c，若某图像采集点对应树冠区域空间坐标为P_q（x_q，y_q，z_q），则该图像采集点坐标为P_t（x_t，y_t，z_t）,其中x_t=x_q，y_t=（y_q-c），z_t=z_q； 

3）母机械臂果实采摘点数与图像采集点数相同；设某采摘点空间坐标P_c(x_c，y_c，z_c)，则x_c=x_t，y_c=（y_t+c-e+β），z_c=z_t，其中x_t、y_t、z_t分别为该采摘点对应图像采集点x、y、z轴坐标，β>0。 

9.如权利要求7所述的水果采摘方法，其特征是：所述步骤7）中所述的第二运动控制卡控制子机械臂运动并带动末端执行器运动，子机械臂与末端执行器进行果实采摘的具体方法如下： 

1）子机械臂运动带动末端执行器伸向目标果实，果实进入接果器；末端执行器伸向目标果实的过程中，装在末端执行器前端的单目摄像机拍摄目标果实图像，并传送给下位机处理，获得果实型心坐标，根据果实型心坐标与图像中心坐标，计算子机械臂运动调整参数，并将参数传送给第二运动控制卡，调整分机械臂各轴运动，使末端执行器对准目标果实型心，实现果实动态定位与跟踪； 

2）位置传感器检测到果实完全进入接果器后，将信号传送下位机； 

3）下位机给第二运动控制卡发送指令使末端执行器拢果器电机旋转，拢果器与刀片旋转 并咬合切断果梗，果实落入收果筒； 

4）位置传感器检测到果实落入收果筒，向下位机传送果实采摘成功信息；下位机通过第二运动控制卡控制子机械臂与末端执行器运动进行下一果实采摘。