CN113524216B - 一种基于多帧融合的果蔬采摘机器人及其控制方法 - Google Patents

Abstract

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

Description

一种基于多帧融合的果蔬采摘机器人及其控制方法

技术领域

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

背景技术

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

在众多农业工作中果蔬的采摘是其中劳动力最为密集的工作。鲜食的果蔬对外皮的完整性有着较强的要求，目前只能使用人工逐个采摘。随着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帧时刻深度摄像头在局部地图中的空间位姿；

上述两公式联立消除

得到如下公式：

上式中，inv(·)表示对齐次矩阵求逆的运算过程；

为第i+1帧时刻机械臂末端在机器人基坐标系的空间位姿；

为第i+1帧时刻深度摄像头在局部地图中的空间位姿；

采用最小二乘法求解得到

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

在机械臂移动过程中，重复执行步骤S1至步骤S2，通过多帧的测量得到多个等式，采用最小二乘法求解得到

以此实现外参的动态标定。

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

S41.在第i帧时刻获取机械臂末端坐标F_i，基于深度摄像头的外参变换得到深度摄像头的坐标T_i；

S42.通过深度学习提取目标上的特征点作为关键点p，基于RGBD数据得到关键点p对应的空间中的3D点P，根据针孔成像模型得到如下投影公式：

上式中，K∈R^3×3表示深度摄像头的内参矩阵；T_i∈R^3×4表示第i帧时刻深度摄像头的外参矩阵；P∈R³为点P对应的3D空间中的点采用；p∈R³为关键点p在图像中的齐次表示形式。

根据一种优选实施方式，步骤S4还包括：

S43.采用最小化多帧投影误差的平方获取目标的空间位姿，投影公式表示为：

上式中，m为机械臂移动过程中提取的帧数据数量，n为每帧对应的目标中的关键点数量；

S44.在机械臂移动过程中，重复执行步骤S42至步骤S43，将优化出的结果P作为抓取的目标点，控制机械臂移动至该点进行抓取。

根据一种优选实施方式，所述深度摄像头安装于机械臂末端。

本发明还提供一种基于多帧融合的果蔬采摘机器人，包括深度摄像头、机械臂、移动平台、末端工具手和处理单元，所述处理单元用于执行上述所述的基于多帧融合的果蔬采摘机器人控制方法；所述深度摄像头安装于机械臂末端。

根据一种优选实施方式，还包括：图像处理模块、机械臂驱动控制模块、移动平台控制模块和末端工具手驱动控制模块，所述图像处理模块、机械臂驱动控制模块、移动平台控制模块和末端工具手驱动控制模块均与处理单元具有信号连接；

所述图像处理模块与深度摄像头连接，所述机械臂驱动控制模块与机械臂连接，所述移动平台控制模块与移动平台连接，所述末端工具手驱动控制模块与末端工具手连接。

本发明实施例的技术方案至少具有如下优点和有益效果：本发明省略了手动标定外参的过程，通过动态外参标定，不仅简化了部署的过程，还解决了过程中外参变动不能及时更改的问题；通过多帧融合，优化观测的最大后验概率，使得估计目标物***置满足多帧的观测，极大的提高了测量的精度，减少了空间感知的误差。

附图说明

图1为本发明实施例1提供的一种基于多帧融合的果蔬采摘机器人控制方法的逻辑流程图；

图2为本发明实施例1提供的一种基于多帧融合的果蔬采摘机器人的结构框图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。

实施例1

参阅图1所示，图1示出本实施例提供的一种基于多帧融合的果蔬采摘机器人控制方法的逻辑流程图。

经申请人研究发现，目前业界常通过立体视觉传感器(双目、结构光、TOF)等获取目标的单帧数据，以识别并定位果蔬在摄像头坐标系中的3D位置，并通过机械臂带动机械手进行采摘。但在此过程中由于光照条件多变、采摘对象遮挡、采摘对象形态多样、采摘对象空间位置不定、传感器精度差、外参标定精度、深度相机噪声、目标识别精度以及单帧观测视野小，观察目标在单帧中不完整等因素时常导致被采摘果蔬的位置获得不准确，最终导致采摘失败。所以，本申请提供了一种基于无监督对话预训练的对话理解与答案配置方法，通过动态标定外参以及采集多帧3D视觉数据结合采集数据时候机械臂末端的姿态，采用视觉捆集调整算法BA构建图优化模型，其中目标点的3D位置为图优化模型中的一个顶点，通过优化顶点综合计算出目标物体的空间点云。使得目标物体的信息更为丰富，不单解决了单帧观测视野狭窄，更提高了目标空间信息提取的准确度。具体内容如下所述。包括如下步骤：

S1.通过深度摄像头获取RGBD数据，RGBD数据指深度图像，可以反映周围环境的RGB图像和每个像素的深度信息。在本实施例中，所述深度摄像头安装于机械臂末端，相较于安装于机械臂基座的安装方式，安装在机械臂末端可以在抓取过程中通过机械臂末端的运动获取更多的信息。

进一步的，基于获取的所述RGBD数据，采用vSLAM构建局部地图，由于深度信息已知，因此所构建的地图具有实际的尺度；其中，vSLAM技术主要分为五个模块：传感器数据读取、前端、后端、建图和回环检测，采用vSLAM技术构建地图为本领域技术人员常规技术手段，在此不在赘述。进一步的，通过获取的所述RGBD数据，可直接获取当前帧时刻深度摄像头在该局部地图中的空间位姿。

在执行完步骤S1之后，接下来可进一步执行步骤S2；在一种实施方式中，步骤S2包括：基于已知的位置信息计算深度摄像头与机械臂末端的相对位置，从而对深度摄像头的外参进行标定，所述已知的位置信息包括：机械臂末端在机器人基坐标系的空间位姿、局部地图在机器人基坐标系中表示的空间位姿以及当前帧时刻深度摄像头在局部地图中的空间位姿。

在一种实施方式中，步骤S2具体步骤如下：获取深度摄像头第i帧时刻在局部地图中的位置

由于机器人基坐标相对于局部地图的坐标固定不变，根据空间变换的链式法则得到如下公式：

上式中，T为欧式空间的位姿，其具体形式采用4*4的齐次矩阵表达；

为第i帧时刻机械臂末端在机器人基坐标系的空间位姿；

为深度摄像头在机械臂末端坐标系中表示的空间位姿；

为局部地图在机器人基坐标系中表示的空间位姿；

为第i帧时刻深度摄像头在局部地图中的空间位姿。

上述两公式联立消除

得到如下公式：

上式中，inv(·)表示对齐次矩阵求逆的运算过程；

为第i+1帧时刻机械臂末端在机器人基坐标系的空间位姿；

为第i+1帧时刻深度摄像头在局部地图中的空间位姿；具体的，上式中，除

未知外，其他均为已知，通过计算得到

由此得到深度摄像头的外参。

进一步的，执行完步骤S2之后，接下来可进一步执行步骤S3；在一种实施方式中，步骤S3包括：在机械臂移动过程中，重复执行步骤S1至步骤S2，通过多帧的测量得到如上述所示的多个等式，采用最小二乘法求解得到

由此实现对深度摄像头的外参进行动态标定，直至采摘目标成功。

本发明实施例通过外参动态标定解决了部署机械臂采摘之前的外参标定问题，简化了标定的过程，提高了部署的效率，另外在摄像头受到外力干扰改变位置时，可以通过动态标定，自动更改外参参数，使抓取不受影响。

而在采摘过程中除了外参标定误差影响最终的采摘结果，目标定位的精度也对采摘的成功率有很大的影响。在发现采摘目标后，机械臂的末端需要移动到目标，现有技术仅用第一次观测的数据提取目标的空间信息，之后再整个靠近目标的移动过程采集的多帧数据均没有被使用，本发明实施例提出的多帧融合技术，在获得摄像头外参基础上，可以对抓取环进行多帧合成，更好的提取目标物体空间信息。

在执行完步骤S3之后，接下来可进一步执行步骤S4；在一种实施方式中，步骤S4包括：基于机械臂移动过程中采集到的多帧3D视觉数据，用BA构建图优化模型，其中目标点的3D位置为图优化模型中的一个顶点，通过优化顶点综合计算出目标的空间点云。

在一种实施方式中，步骤S4具体包括：

S41.在第i帧时刻获取机械臂末端坐标F_i，基于深度摄像头的外参变换得到深度摄像头的坐标T_i。

S42.通过深度学习提取目标上的特征点作为关键点p，基于RGBD数据得到关键点p对应的空间中的3D点P，根据针孔成像模型得到如下投影公式：

上式中，K∈R^3×3表示深度摄像头的内参矩阵；T_i∈R^3×4表示第i帧时刻深度摄像头的外参矩阵；P∈R³为点P对应的3D空间中的点采用；p∈R³为关键点p在图像中的齐次表示形式。

S43.采用最小化多帧投影误差的平方获取目标的空间位姿，投影公式表示为：

上式中，m为机械臂移动过程中提取的帧数据数量，n为每帧对应的目标中的关键点数量。

S44.在机械臂移动过程中，重复执行步骤S42至步骤S43，将优化出的结果P作为抓取的目标点，控制机械臂移动至该点进行抓取。

此外，本发明还提供一种基于多帧融合的果蔬采摘机器人，参阅图2所示，图2为本发明实施例1提供的一种基于多帧融合的果蔬采摘机器人的结构框图。

包括深度摄像头、机械臂、移动平台、末端工具手和处理单元，所述处理单元用于执行上述所述的基于多帧融合的果蔬采摘机器人控制方法；所述深度摄像头安装于机械臂末端。

进一步的，还包括：图像处理模块、机械臂驱动控制模块、移动平台控制模块和末端工具手驱动控制模块，所述图像处理模块、机械臂驱动控制模块、移动平台控制模块和末端工具手驱动控制模块均与处理单元具有信号连接；

所述图像处理模块与深度摄像头连接，所述机械臂驱动控制模块与机械臂连接，所述移动平台控制模块与移动平台连接，所述末端工具手驱动控制模块与末端工具手连接。

综上所述，本发明实施例至少可实现如下技术效果：相较于目前所有机械臂抓取的过程外参的标定均采用标定板离线标定，而本发明通过在线标定，省略了手动标定外参的过程，通过安装在机械臂末端的深度摄像头采集RGBD数据，采用vSLAM在机械臂运动的过程中构建一个具有实际尺度的局部地图，并获得每一个数据帧的空间位置，由此实现动态外参标定，不仅简化了部署的过程，还解决了过程中外参变动不能及时更改的问题；通过多帧融合，优化观测的最大后验概率，使得估计目标物***置满足多帧的观测，极大的提高了测量的精度，减少了空间感知的误差。

以上仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种基于多帧融合的果蔬采摘机器人控制方法，其特征在于，包括如下步骤：

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

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

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

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

步骤S2具体包括：

获取深度摄像头第i帧时刻在局部地图中的位置

，由于机器人基坐标相对于局部地图的坐标固定不变，根据空间变换的链式法则得到如下公式：

上式中，T为欧式空间的位姿，其具体形式采用4*4的齐次矩阵表达；

为第i帧时刻机械臂末端在机器人基坐标系的空间位姿；

为深度摄像头在机械臂末端坐标系中表示的空间位姿；

为局部地图在机器人基坐标系中表示的空间位姿；

为第i帧时刻深度摄像头在局部地图中的空间位姿；

上述两公式联立消除

得到如下公式：

上式中，

表示对齐次矩阵求逆的运算过程；

为第i+1帧时刻机械臂末端在机器人基坐标系的空间位姿；

为第i+1帧时刻深度摄像头在局部地图中的空间位姿；

采用最小二乘法求解得到

。

2.如权利要求1所述的基于多帧融合的果蔬采摘机器人控制方法，其特征在于，步骤S3具体包括：

在机械臂移动过程中，重复执行步骤S1至步骤S2，通过多帧的测量得到多个等式，采用最小二乘法求解得到

，以此实现外参的动态标定。

3.如权利要求2所述的基于多帧融合的果蔬采摘机器人控制方法，其特征在于，步骤S4具体包括：

S41.在第i帧时刻获取机械臂末端坐标F _i ，基于深度摄像头的外参变换得到深度摄像头的坐标T _i ；

S42.通过深度学习提取目标上的特征点作为关键点p，基于RGBD数据得到关键点p对应的空间中的3D点P，根据针孔成像模型得到如下投影公式：

上式中，

表示深度摄像头的内参矩阵；

表示第i帧时刻深度摄像头的外参矩阵；

为点P对应的3D空间中的点采用；

为关键点p在图像中的齐次表示形式。

4.如权利要求3所述的基于多帧融合的果蔬采摘机器人控制方法，其特征在于，步骤S4还包括：

S43.采用最小化多帧投影误差的平方获取目标的空间位姿，投影公式表示为：

上式中，m为机械臂移动过程中提取的帧数据数量，n为每帧对应的目标中的关键点数量；

S44.在机械臂移动过程中，重复执行步骤S42至步骤S43，将优化出的结果P作为抓取的目标点，控制机械臂移动至该点进行抓取。

5.如权利要求1所述的基于多帧融合的果蔬采摘机器人控制方法，其特征在于，

所述深度摄像头安装于机械臂末端。

6.一种基于多帧融合的果蔬采摘机器人，其特征在于，包括深度摄像头、机械臂、移动平台、末端工具手和处理单元，所述处理单元用于执行权利要求1至5任一项所述的基于多帧融合的果蔬采摘机器人控制方法；

所述深度摄像头安装于机械臂末端。

7.如权利要求6所述的基于多帧融合的果蔬采摘机器人，其特征在于，还包括：图像处理模块、机械臂驱动控制模块、移动平台控制模块和末端工具手驱动控制模块，所述图像处理模块、机械臂驱动控制模块、移动平台控制模块和末端工具手驱动控制模块均与处理单元具有信号连接；

所述图像处理模块与深度摄像头连接，所述机械臂驱动控制模块与机械臂连接，所述移动平台控制模块与移动平台连接，所述末端工具手驱动控制模块与末端工具手连接。

Images