CN115299245B - 一种智能水果采摘机器人的控制方法及控制*** - Google Patents

Abstract

本发明公开了一种智能水果采摘机器人的控制方法及控制***，其中智能水果采摘机器人的控制方法包括以下步骤：构建待采摘农场的实时三维地图及环境地图信息；规划搭载有用于采摘的采摘机械臂的移动机器人的运动路径；根据不同的待采摘水果的深度图像特征信息，构建YOLOv4深度网络模型；利用YOLOv4深度网络模型对该图像信息进行对待采摘的水果的图像信息目标识别，获取待采摘水果的中心点坐标信息；根据待采摘水果的中心点坐标信息以及所规划的移动机器人的运动路径，控制移动机器人运动到目标位置，并控制移动机器人上的采摘机械臂进行抓取采摘。本发明实现了高精度获取目标图像的中心坐标信息，实现对待采摘水果进行高精度定位抓取。

Description

一种智能水果采摘机器人的控制方法及控制***

技术领域

本发明涉及水果采摘机器人技术领域，具体为一种智能水果采摘机器人的控制方法及控制***。

背景技术

随着智能机器人的发展，其在人们日常生产作业中越来越展现出方便快捷高效等优点。其中采摘机械臂作为一种新兴的辅助人力进行作业的执行机构，在工程机械焊接、汽车零部件装配、农业果园采摘生产、零部件机加工、大型设备整机涂装等领域应用广泛。

现阶段，我国人口众多，而且老龄化问题日趋严重，城镇化进程驱使传统手工业者向劳动密集型产业靠拢。并且大多数人都向着服务业等方向的转变，农业劳动力短缺问题日益严重。虽然我国水果产业快速发展，但也面临着一系列急需解决的问题，其中，我国农业机械化、智能化水平较低。绝大部分水果的采摘要靠人工来完成。生产技术的落后导致果农劳动强度大、作业环境恶劣、投入成本高以及生产效率低下。水果采摘的工作非常繁重，包括对果实成熟度的判断。因此，在农业发展方面，引入智能化的机器人可以极大节约劳动力，提高了劳动生产率。要保证移动机器人在果园中能够精准采摘果实，需要解决两方面问题：

(1)由于果实生长的随意性和果实复杂多样的生长环境，采摘机器人如何准确地对自然环境中的果实进行定位检测和分类识别。

(2)激光雷达传感器扫描观测距离有限、易受到环境中复杂几何结构影响。相机对机器人周围环境的照明条件有一定的要求。编码器电机经过长时间工作会产生累计误差。

发明内容

本发明提出了一种智能水果采摘机器人的控制方法及控制***，通过在原始的YOLOV4深度学习网络模型的检测网络中加入多模态注意力融合网络，将水果的全局特征与局部特征进行融合，提高对于水果检测识别的召回率，并解析出目标图像的中心点，获取目标图像的中心坐标信息，实现定位抓取功能；通过获取待采摘的水果的位置，利用搭载有用于进行采摘的采摘机械臂的移动机器人进行自主运动路径导航进行进行采摘规划，有助于提高移动机器人的避障及采摘时移动的。

本发明提供了一种智能水果采摘机器人的控制方法，包括以下步骤：

构建待采摘农场的实时三维地图及环境地图信息；

根据待采摘农场的实时三维地图及环境地图信息，规划搭载有采摘机械臂的移动机器人在待采摘农场移动时的运动路径；

根据不同的待采摘水果的深度图像特征信息，构建YOLOv4深度网络模型；

实地拍摄获取目标位置处的待采摘的水果的图像信息，并利用YOLOv4深度网络模型对该图像信息进行目标识别，获取待采摘水果的中心点坐标信息；

根据待采摘水果的中心点坐标信息以及所规划的移动机器人的运动路径，控制移动机器人运动到目标位置，并控制移动机器人上的采摘机械臂对该水果进行抓取采摘。

进一步地，所述构建待采摘农场的实时三维地图，包括：

根据待采摘农场的场景以及待采摘农场中存在的障碍物，利用三维物理仿真平台Gazebo构建所述待采摘农场的实时三维地图。

进一步地，所述构建环境地图信息，包括：

利用移动机器人上搭载的激光雷达获取待采摘农场的环境地形的点云数据；

对点云数据进行树莓派处理，并利用gmapping算法得到待采摘农场的二维栅格形式的环境地图信息。

进一步地，所述根据待采摘农场的实时三维地图及环境地图信息，规划搭载有采摘机械臂的移动机器人在待采摘农场移动时的运动路径，包括：

根据环境地图信息，利用ROS三维可视化工Rviz对移动机器人的运动进行模拟仿真，并规划搭载有采摘机械臂的移动机器人的运动路径；

通过将所规划的运动路径与所述实时三维地图进行联合仿真，对所规划的运动路径进行改进、优化；

所述运动路径包括移动机器人的避障运动路径以及在进行采摘时的移动运动路径。

进一步地，所述根据不同的待采摘水果的深度图像特征信息，构建YOLOv4深度网络模型，包括：

获取不同的待采摘水果的单模态图像数据以及多模态图像数据；

利用一部分所述单模态图像数据以及多模态图像数据，构建多模态注意力融合网络，并采用多尺度特征融合增加多模态注意力融合网络的感受野；

对所述多模态注意力融合网络进行图像深度特征提取，并使用注意力机制自适应地对图像深度特征通道进行标定；

将所标定的所述图像深度特征与可见光融合，构建融合后的多模态注意力融合网络；

利用另一部分所述单模态图像数据以及多模态图像数据，训练融合后的多模态注意力融合网络得到YOLOv4深度网络模型。

进一步地，所述利用ROS三维可视化工具Rviz，模拟仿真搭载有采摘机械臂的移动机器人的避障运动路径以及在进行采摘时的移动运动路径，包括：

利用所述移动机器人上设有的惯性测量单元以及轮式里程计分别获取所述移动机器人的运动数据；

利用卡尔曼滤波算法，将所述移动机器人的运动数据以及所述移动机器人上搭载的激光雷达获取的待采摘农场的环境地形的点云数据进行融合；

根据融合后的数据，利用SLAM算法构建所述移动机器人的运动地图，并对运动中的所述移动机器人进行定位；

根据融合后的数据，对所述移动机器人进行路径规划。

进一步地，所述根据融合后的数据，利用SLAM算法构建所述移动机器人的运动地图，并对运动中的所述移动机器人进行定位，包括：

利用Cartographer算法构建移动机器人运动时的地图；

利用AMCL算法对移动机器人进行定位。

进一步地，所述根据融合后的数据，对移动机器人进行路径规划，包括：

利用启发式搜索的A*算法对移动机器人的全局运动路径进行规划；

采用TEB算法对移动机器人的局部运动路径进行规划。

进一步地，所述根据待采摘水果的中心点坐标信息以及所规划的移动机器人的运动路径，控制移动机器人运动到目标位置，并控制移动机器人上的采摘机械臂进行抓取采摘，包括：

根据待采摘水果的中心点坐标信息建立运动学方程；

将运动学方程进行运动学逆解；

利用ROS***同时控制所述移动机器人进行移动及所述所述采摘机械臂进行抓取运动，使所述移动机器人移动到待采摘水果处，并使所述采摘机械臂抓取待采摘水果。

本发明提供一种智能水果采摘机器人的控制***，包括：

地图信息构建模块，用于构建待采摘农场的实时三维地图及环境地图信息；

运动路径获取模块，用于根据待采摘农场的实时三维地图及环境地图信息，规划搭载有采摘机械臂的移动机器人在待采摘农场移动时的运动路径；

模型构建模块，用于根据不同的待采摘水果的深度图像特征信息，构建YOLOv4深度网络模型；

识别模块，用于实地拍摄获取目标位置处的待采摘的水果的图像信息，并利用YOLOv4深度网络模型对该图像信息进行目标识别，获取待采摘水果的中心点坐标信息；

采摘执行模块，用于根据待采摘水果的中心点坐标信息以及所规划的移动机器人的运动路径，控制移动机器人运动到目标位置，并控制移动机器人上的采摘机械臂对该水果进行抓取采摘。

与现有技术相比，本发明的有益效果：

本发明为了能使智能采摘机器人能够在全天候的不同光照强度、果叶交叠遮挡等果园复杂多样的环境下，对不同成熟度的水果进行快速、准确的检测识别，提出一种改进YOLOv4的水果检测识别方法，在原始的YOLOV4深度学习网络模型的检测网络中加入多尺度特征融合模块Spatial Pyramid Pooling，将水果的全局特征与局部特征进行融合，提高对于水果检测识别的召回率，并解析出目标图像的中心点，获取目标图像的中心坐标信息，实现定位抓取功能。此外，针对传统RBPF-SLAM算法中存在粒子建议分布误差大、粒子消耗、算法运行时间长等问题，本发明将激光雷达、惯性测量单元、轮式里程计之间的数据进行融合，提出了多传感器融合SLAM的新方法，建立了基于惯性测量单元、轮式里程计融合的机器人运动模型，利用激光雷达观测信息融合运动模型优化粒子建议分布，解决***预测分布误差大与粒子内存***问题以及改进粒子重采样策略，保持粒子多样性，减缓粒子耗散问题。

附图说明

附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。在附图中：

图1是本发明提出的一种智能水果采摘机器人的控制方法的流程示意图；

图2是本发明提出的一种智能水果采摘机器人的控制方法的控制***流程图；

图3是本发明提出的一种智能水果采摘机器人的控制方法的实施例中所建立的农场三维地图；

图4是本发明提出的一种智能水果采摘机器人的控制方法的实施例中的农场的二维栅格地图；

图5是本发明提出的一种智能水果采摘机器人的控制方法中的智能水果采摘机器人的硬件连接图。

具体实施方式

下面结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整的描述，但应当理解本发明的保护范围并不受具体实施方式的限制。

实施例1

如图1-4所示，本发明提供一种智能水果采摘机器人的控制方法，包括以下步骤：

步骤1：构建待采摘农场的实时三维地图及环境地图信息；

步骤2：根据待采摘农场的实时三维地图及环境地图信息，规划搭载有采摘机械臂的移动机器人在待采摘农场移动时的运动路径；

步骤3：根据不同的待采摘水果的深度图像特征信息，构建YOLOv4深度网络模型；

步骤4：利用深度相机实地拍摄获取目标位置处的待采摘的水果的图像信息，并利用YOLOv4深度网络模型对该图像信息进行目标识别，获取待采摘水果的中心点坐标信息；

步骤5：根据待采摘水果的中心点坐标信息以及所规划的移动机器人的运动路径，控制移动机器人运动到目标位置，并控制移动机器人上的采摘机械臂对该水果进行抓取采摘。

步骤1具体包括如下步骤：

步骤1.1：构建待采摘农场的实时三维地图，包括：

根据待采摘农场的场景以及待采摘农场中存在的障碍物，利用三维物理仿真平台Gazebo构建待采摘农场的实时三维地图。

通过利用实时三维地图去模拟采摘机器人在实际采摘作业中所遇到的或即将遇到的问题，实现待采摘农场的多物理联合场景仿真。

步骤1.2：构建环境地图信息，包括：

利用移动机器人上搭载的激光雷达获取待采摘农场的环境地形的点云数据；

对点云数据进行树莓派处理，并利用gmapping算法得到待采摘农场的二维栅格形式的环境地图信息。

步骤2具体包括如下步骤：

步骤2.1：根据环境地图信息，利用ROS三维可视化工Rviz对移动机器人的运动进行模拟仿真，并规划搭载有采摘机械臂的移动机器人的运动路径，包括：

利用移动机器人上设有的惯性测量单元以及轮式里程计分别获取移动机器人的运动数据。其中惯性测量单元获取的数据分别为移动机器人的加速度、角速度；轮式里程计所获取的数据为移动机器人上的编码器所测量的速度值。

利用卡尔曼滤波算法，将移动机器人的运动数据以及移动机器人上搭载的激光雷达获取的待采摘农场的环境地形的点云数据进行融合，融合具体过程如下：

对机器人的当前位姿进行初始化，以及卡尔曼初始化，对惯性测量单所获取的数据进行解算，并对轮式里程计所获取的数据进行解算，将解算后的移动机器人的运动数据信息以及所获取的待采摘农场的环境地形的点云数据利用卡尔曼滤波算法进行融合，得到融合后的数据。

根据融合后的数据，利用SLAM算法构建移动机器人的运动地图，并对运动中的移动机器人进行定位；

根据融合后的数据，对移动机器人进行路径规划。

其中，利用SLAM算法构建移动机器人的运动地图，并对运动中的移动机器人进行定位，包括：

利用Cartographer算法构建移动机器人运动时的地图；

利用AMCL算法对移动机器人进行定位。

根据融合后的数据，对移动机器人进行路径规划，包括：

因移动机器人的运动路径规划方面主要分为全局路径规划和局部路径规划，则利用启发式搜索的A*算法对移动机器人的全局运动路径进行规划；

采用TEB算法对移动机器人的局部运动路径进行规划。局部路径算法是对全局路径生成的初始轨迹进行优化和修正。

步骤2.2：通过将所规划的运动路径与实时三维地图进行联合仿真，对所规划的运动路径进行改进、优化。

运动路径包括移动机器人的避障运动路径以及在进行采摘时的移动运动路径。通过对移动机器人的运动路径进行改进、优化，有利于得到移动机器人在采摘时的最优运动路径。

针对步骤3，因实际在进行水果采摘时，因待采摘的水果分为许多品种，不仅存在颜色上的差异，也存在形状上的差异，显然仅仅靠形状和颜色来区分茂密枝叶和水果是不现实的。而如果完全靠图像处理来实现标注水果，又非常耗费时间，并且在分类中，光靠两个变量是远远不够达到采摘目的的，所以需要借助鲁棒性更强的分类理念来进行目标检测。因此引入深度卷积神经网络的概念。基于深度卷积神经网络进行目标检测，本发明中选取YOLO轻量级算法网络进行目标检测。因此，步骤3中构建YOLOv4深度网络模型具体包括：

分别获取不同的待采摘水果的单模态图像数据以及多模态图像数据；

利用一部分单模态图像数据以及多模态图像数据，构建多模态注意力融合网络，并采用多尺度特征融合增加多模态注意力融合网络的感受野；

对多模态注意力融合网络进行图像深度特征提取，并使用注意力机制自适应地对图像深度特征通道进行标定；

将所标定的图像深度特征与可见光融合，构建融合后的多模态注意力融合网络；

利用另一部分单模态图像数据以及多模态图像数据，训练融合后的多模态注意力融合网络得到YOLOv4深度网络模型。

根据不同的待采摘水果的单模态图像数据以及多模态图像数据进行待采摘水果的目标检测研究，提出的多模态注意力融合网络采用多尺度特征融合增加感受野，使用注意力机制自适应地对特征通道进行标定，使网络关注于更重要的信息。又因为基于可见光图像的目标检测算法在光线较强和阴影条件下，容易产生误检与漏检等问题。因此本申请文件中通过融合可见光和待采摘水果的深度图像特征，当某个模态信息发生较大变化或者缺失时，以模态间信息互补的方式，从整体上提升检测器的准确性与健壮性。

接着将所设计的视觉***移植到树莓派4b开发板中，进行基于嵌入式平台的实物实验并对实验结果进行分析。

步骤5具体包括：

根据待采摘水果的中心点坐标信息建立运动学方程；

将运动学方程进行运动学逆解；

利用ROS***同时控制移动机器人进行移动及采摘机械臂进行抓取运动，使移动机器人移动到待采摘水果处，并使采摘机械臂抓取待采摘水果。

实施例2

本发明提供一种智能水果采摘机器人的控制***，包括：

地图信息构建模块，用于构建待采摘农场的实时三维地图及环境地图信息；

运动路径获取模块，用于根据待采摘农场的实时三维地图及环境地图信息，规划搭载有采摘机械臂的移动机器人在待采摘农场移动时的运动路径；

模型构建模块，用于根据不同的待采摘水果的深度图像特征信息，构建YOLOv4深度网络模型；

识别模块，用于实地拍摄获取目标位置处的待采摘的水果的图像信息，并利用YOLOv4深度网络模型对该图像信息进行目标识别，获取待采摘水果的中心点坐标信息；

采摘执行模块，用于根据待采摘水果的中心点坐标信息以及所规划的移动机器人的运动路径，控制移动机器人运动到目标位置，并控制移动机器人上的采摘机械臂对该水果进行抓取采摘。

下面结合具体的实施例对本发明作进一步详细说明。

1、利用三维物理仿真平台Gazebo所建立的待采摘农场的实时三维地图如图3所示。

2、利用gmapping算法所建立的待采摘农场的环境地图信息如图4所示。

3、通过在PC端的Ubuntu20.04LTS***中，将待采摘水果的单模态图像数据以及多模态图像数据在多模态注意力融合网络中进行训练并生成YOLOv4深度网络模型，并对YOLOv4深度网络模型在进行识别的性能进行测试。

利用OrbbecAstra深度相机实地拍摄获取待采摘的水果的图像信息，利用摄像头对果树图像进行采集，通过YOLOv4深度网络模型对水果相关信息进行目标识别，得到水果的中心点坐标信息。

本发明通过融合可见光以及待采摘水果的深度图像特征，并通过选用带采摘水果的检测数据集来检测YOLO算法的可行性，再利用已有的ROS深度相机实地拍摄获取水果图片实验识别效果，再将采摘机械臂抓取与YOLO目标检测相结合，综合检测水果摘取的准确度。

4、本发明中的采摘机器人的硬件连接图如图5所示。其中包括：树莓派4b开发板、MCU控制器、运动机构，以及搭载运动机构上的激光雷达、ROS深度相机、超声波传感器、采摘机械臂、红外射管、远程遥控设备。其中运动机构为麦克纳姆轮车。

5、对控制***采用模块化设计，使用STM32F4系列芯片为主控，控制采摘机械臂各关节驱动器，使用上位机(PC机)+下位机(单片机)的模式来控制末端执行器的抓取与释放动作完成控制***，且在此基础上，使用QT可视化程序编写软件设计了上位机控制界面。

6、利用C++语言在ROS***中编写与底层控制板的通信节点程序实现信息的交互，从而实现对麦克纳姆轮车和采摘机械臂的实时控制。

7、在进行采摘工作时，将机器人放置于采摘环境中，确定采摘机械手的初始位姿，确定各关节及控制***处于正常状态，等待控制页面初始化完成。

8、初始化完成后，设定采摘目标位置及最优路径，设置采摘机械手的运动参数，开始采摘作业，采摘机械臂根据设定将末端执行器送至采摘果实位置。

最后说明的是：以上公开的仅为本发明的一个具体实施例，但是，本发明实施例并非局限于此，任何本领域的技术人员能思之的变化都应落入本发明的保护范围。

Claims (6)

1.一种智能水果采摘机器人的控制方法，其特征在于，包括以下步骤：

构建待采摘农场的实时三维地图及环境地图信息；

根据待采摘农场的实时三维地图及环境地图信息，规划搭载有采摘机械臂的移动机器人在待采摘农场移动时的运动路径；

根据不同的待采摘水果的深度图像特征信息，构建YOLOv4 深度网络模型；

实地拍摄获取目标位置处的待采摘的水果的图像信息，并利用YOLOv4 深度网络模型对该图像信息进行目标识别，获取待采摘水果的中心点坐标信息；

根据待采摘水果的中心点坐标信息以及所规划的移动机器人的运动路径，控制移动机器人运动到目标位置，并控制移动机器人上的采摘机械臂对该水果进行抓取采摘；

其中，所述根据待采摘农场的实时三维地图及环境地图信息，规划搭载有采摘机械臂的移动机器人在待采摘农场移动时的运动路径，包括：

根据环境地图信息，利用ROS三维可视化工 Rviz对移动机器人的运动进行模拟仿真，并规划搭载有采摘机械臂的移动机器人的运动路径；

通过将所规划的运动路径与所述实时三维地图进行联合仿真，对所规划的运动路径进行改进、优化；

所述运动路径包括移动机器人的避障运动路径以及在进行采摘时的移动运动路径；

所述根据不同的待采摘水果的深度图像特征信息，构建YOLOv4 深度网络模型，包括：

获取不同的待采摘水果的单模态图像数据以及多模态图像数据；

利用一部分所述单模态图像数据以及多模态图像数据，构建多模态注意力融合网络，并采用多尺度特征融合增加多模态注意力融合网络的感受野；

对所述多模态注意力融合网络进行图像深度特征提取，并使用注意力机制自适应地对图像深度特征通道进行标定；

将所标定的所述图像深度特征与可见光融合，构建融合后的多模态注意力融合网络；

利用另一部分所述单模态图像数据以及多模态图像数据，训练融合后的多模态注意力融合网络得到YOLOv4 深度网络模型；

所述利用 ROS 三维可视化工具 Rviz，模拟仿真搭载有采摘机械臂的移动机器人的避障运动路径以及在进行采摘时的移动运动路径，包括：

利用所述移动机器人上设有的惯性测量单元以及轮式里程计分别获取所述移动机器人的运动数据；

利用卡尔曼滤波算法，将所述移动机器人的运动数据以及所述移动机器人上搭载的激光雷达获取的待采摘农场的环境地形的点云数据进行融合；

根据融合后的数据，利用SLAM算法构建所述移动机器人的运动地图，并对运动中的所述移动机器人进行定位；

根据融合后的数据，对所述移动机器人进行路径规划；

所述根据融合后的数据，利用SLAM算法构建所述移动机器人的运动地图，并对运动中的所述移动机器人进行定位，包括：

利用Cartographer 算法构建移动机器人运动时的地图；

利用AMCL 算法对移动机器人进行定位。

2.根据权利要求1所述的一种智能水果采摘机器人的控制方法，其特征在于：所述构建待采摘农场的实时三维地图，包括：

根据待采摘农场的场景以及待采摘农场中存在的障碍物，利用三维物理仿真平台Gazebo构建所述待采摘农场的实时三维地图。

3.根据权利要求2所述的一种智能水果采摘机器人的控制方法，其特征在于：所述构建环境地图信息，包括：

利用移动机器人上搭载的激光雷达获取待采摘农场的环境地形的点云数据；

对点云数据进行树莓派处理，并利用gmapping算法得到待采摘农场的二维栅格形式的环境地图信息。

4.根据权利要求1所述的一种智能水果采摘机器人的控制方法，其特征在于：所述根据融合后的数据，对移动机器人进行路径规划，包括：

利用启发式搜索的 A*算法对移动机器人的全局运动路径进行规划；

采用 TEB 算法对移动机器人的局部运动路径进行规划。

5.根据权利要求1所述的一种智能水果采摘机器人的控制方法，其特征在于：所述根据待采摘水果的中心点坐标信息以及所规划的移动机器人的运动路径，控制移动机器人运动到目标位置，并控制移动机器人上的采摘机械臂进行抓取采摘，包括：

根据待采摘水果的中心点坐标信息建立运动学方程；

将运动学方程进行运动学逆解；

利用ROS***同时控制所述移动机器人进行移动及所述采摘机械臂进行抓取运动，使所述移动机器人移动到待采摘水果处，并使所述采摘机械臂抓取待采摘水果。

6.一种基于权利要求1-5任一项所述的智能水果采摘机器人的控制方法的控制***，其特征在于：包括：

地图信息构建模块，用于构建待采摘农场的实时三维地图及环境地图信息；

运动路径获取模块，用于根据待采摘农场的实时三维地图及环境地图信息，规划搭载有采摘机械臂的移动机器人在待采摘农场移动时的运动路径；

模型构建模块，用于根据不同的待采摘水果的深度图像特征信息，构建YOLOv4 深度网络模型；

识别模块，用于实地拍摄获取目标位置处的待采摘的水果的图像信息，并利用YOLOv4深度网络模型对该图像信息进行目标识别，获取待采摘水果的中心点坐标信息；

采摘执行模块，用于根据待采摘水果的中心点坐标信息以及所规划的移动机器人的运动路径，控制移动机器人运动到目标位置，并控制移动机器人上的采摘机械臂对该水果进行抓取采摘。

Images