[发明专利]一种基于多帧融合的果蔬采摘机器人及其控制方法

说明书

本发明提供了一种基于多帧融合的果蔬采摘机器人及其控制方法，包括如下步骤：基于获取RGBD数据，采用vSLAM构建具有实际尺度的局部地图，并获取当前帧时刻深度摄像头在局部地图中的空间位姿；基于已知的位置信息计算深度摄像头与机械臂末端的相对位置，对其外参进行标定；在机械臂移动过程中，重复上述步骤，对深度摄像头的外参进行动态标定；基于机械臂移动过程中采集到的多帧3D视觉数据，用BA构建图优化模型，通过优化综合计算出目标的空间点云。本发明通过动态外参标定，简化了部署过程，解决了过程中外参变动不能及时更改的问题；通过多帧融合提高了测量的精度，减少了空间感知的误差。

技术领域

本发明涉及智能农业机器人技术领域，具体而言，涉及一种基于多帧融合的果蔬采摘机器人及其控制方法。

背景技术

目前包括中国在内的许多国家均面临着人口老龄化，劳动力稀缺难题。从事农业的劳动力缺乏问题尤为突出。农业机器人能够逐步代替人力而且不断帮助农业生产降低劳动强度，提高劳动效率，因此越来越受到农业人口较少的发达国家的重视，也成为国际农业装备产业技术竞争的焦点之一。

在众多农业工作中果蔬的采摘是其中劳动力最为密集的工作。鲜食的果蔬对外皮的完整性有着较强的要求，目前只能使用人工逐个采摘。随着AI时代的到来，21世纪深度学习算法的提出使得视觉感知算法取得了长足的进步，这也加速了机器人的发展，国内外分别出现了带有视觉感知算法的果蔬采摘的机器人。其基本的思路为通过立体视觉传感器(双目、结构光、TOF)等获取目标的单帧数据，以识别并定位果蔬在摄像头坐标系中的3D位置，并通过机械臂带动机械手进行采摘。但在此过程中由于光照条件多变、采摘对象遮挡、采摘对象形态多样、采摘对象空间位置不定、传感器精度差、外参标定精度、深度相机噪声、目标识别精度以及单帧观测视野小，观察目标在单帧中不完整等因素时常导致被采摘果蔬的位置获得不准确，最终导致采摘失败。

发明内容

本发明的目的在于提供一种基于多帧融合的果蔬采摘机器人控制方法，通过动态标定外参以及采集多帧3D视觉数据结合采集数据时候机械臂末端的姿态，采用视觉捆集调整算法BA构建图优化模型，其中目标点的3D位置为图优化模型中的一个顶点，通过优化顶点综合计算出目标物体的空间点云。使得目标物体的信息更为丰富，不单解决了单帧观测视野狭窄，更提高了目标空间信息提取的准确度。

本发明的实施例通过以下技术方案实现：一种基于多帧融合的果蔬采摘机器人控制方法，包括如下步骤：

S1.通过深度摄像头获取RGBD数据，基于所述RGBD数据，采用vSLAM构建一个具有实际尺度的局部地图，并获取当前帧时刻深度摄像头在该局部地图中的空间位姿；

S2.基于已知的位置信息计算深度摄像头与机械臂末端的相对位置，从而对深度摄像头的外参进行标定，所述已知的位置信息包括：机械臂末端在机器人基坐标系的空间位姿、局部地图在机器人基坐标系中表示的空间位姿以及当前帧时刻深度摄像头在局部地图中的空间位姿；

S3.在机械臂移动过程中，重复执行步骤S1至步骤S2，对深度摄像头的外参进行动态标定，直至采摘目标成功；

S4.基于机械臂移动过程中采集到的多帧3D视觉数据，用BA构建图优化模型，其中目标点的3D位置为图优化模型中的一个顶点，通过优化顶点综合计算出目标的空间点云。

根据一种优选实施方式，步骤S2具体包括：

获取深度摄像头第i帧时刻在局部地图中的位置由于机器人基坐标相对于局部地图的坐标固定不变，根据空间变换的链式法则得到如下公式：

上式中，T为欧式空间的位姿，其具体形式采用4*4的齐次矩阵表达；为第i帧时刻机械臂末端在机器人基坐标系的空间位姿；为深度摄像头在机械臂末端坐标系中表示的空间位姿；为局部地图在机器人基坐标系中表示的空间位姿；为第i帧时刻深度摄像头在局部地图中的空间位姿；