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

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

【技术实现步骤摘要】
一种基于多帧融合的果蔬采摘机器人及其控制方法


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

技术介绍

[0002]目前包括中国在内的许多国家均面临着人口老龄化，劳动力稀缺难题。从事农业的劳动力缺乏问题尤为突出。农业机器人能够逐步代替人力而且不断帮助农业生产降低劳动强度，提高劳动效率，因此越来越受到农业人口较少的发达国家的重视，也成为国际农业装备产业技术竞争的焦点之一。
[0003]在众多农业工作中果蔬的采摘是其中劳动力最为密集的工作。鲜食的果蔬对外皮的完整性有着较强的要求，目前只能使用人工逐个采摘。随着AI时代的到来，21世纪深度学习算法的提出使得视觉感知算法取得了长足的进步，这也加速了机器人的发展，国内外分别出现了带有视觉感知算法的果蔬采摘的机器人。其基本的思路为通过立体视觉传感器(双目、结构光、TOF)等获取目标的单帧数据，以识别并定位果蔬在摄像头坐标系中的3D位置，并通过机械臂带动机械手进行采摘。但在此过程中由于光照条件多变、采摘对象遮挡、采摘对象形态多样、采摘对象空间位置不定、传感器精度差、外参标定精度、深度相机噪声、目标识别精度以及单帧观测视野小，观察目标在单帧中不完整等因素时常导致被采摘果蔬的位置获得不准确，最终导致采摘失败。

技术实现思路

[0004]本专利技术的目的在于提供一种基于多帧融合的果蔬采摘机器人控制方法，通过动态标定外参以及采集多帧3D视觉数据结合采集数据时候机械臂末端的姿态，采用视觉捆集调整算法BA构建图优化模型，其中目标点的3D位置为图优化模型中的一个顶点，通过优化顶点综合计算出目标物体的空间点云。使得目标物体的信息更为丰富，不单解决了单帧观测视野狭窄，更提高了目标空间信息提取的准确度。
[0005]本专利技术的实施例通过以下技术方案实现：一种基于多帧融合的果蔬采摘机器人控制方法，包括如下步骤：
[0006]S1.通过深度摄像头获取RGBD数据，基于所述RGBD数据，采用vSLAM构建一个具有实际尺度的局部地图，并获取当前帧时刻深度摄像头在该局部地图中的空间位姿；
[0007]S2.基于已知的位置信息计算深度摄像头与机械臂末端的相对位置，从而对深度摄像头的外参进行标定，所述已知的位置信息包括：机械臂末端在机器人基坐标系的空间位姿、局部地图在机器人基坐标系中表示的空间位姿以及当前帧时刻深度摄像头在局部地图中的空间位姿；
[0008]S3.在机械臂移动过程中，重复执行步骤S1至步骤S2，对深度摄像头的外参进行动态标定，直至采摘目标成功；
[0009]S4.基于机械臂移动过程中采集到的多帧3D视觉数据，用BA构建图优化模型，其中
目标点的3D位置为图优化模型中的一个顶点，通过优化顶点综合计算出目标的空间点云。
[0010]根据一种优选实施方式，步骤S2具体包括：
[0011]获取深度摄像头第i帧时刻在局部地图中的位置由于机器人基坐标相对于局部地图的坐标固定不变，根据空间变换的链式法则得到如下公式：
[0012][0013][0014]上式中，T为欧式空间的位姿，其具体形式采用4*4的齐次矩阵表达；为第i帧时刻机械臂末端在机器人基坐标系的空间位姿；为深度摄像头在机械臂末端坐标系中表示的空间位姿；为局部地图在机器人基坐标系中表示的空间位姿；为第i帧时刻深度摄像头在局部地图中的空间位姿；
[0015]上述两公式联立消除得到如下公式：
[0016][0017]上式中，inv(
·
)表示对齐次矩阵求逆的运算过程；为第i+1帧时刻机械臂末端在机器人基坐标系的空间位姿；为第i+1帧时刻深度摄像头在局部地图中的空间位姿；
[0018]采用最小二乘法求解得到
[0019]根据一种优选实施方式，步骤S3具体包括：
[0020]在机械臂移动过程中，重复执行步骤S1至步骤S2，通过多帧的测量得到多个等式，采用最小二乘法求解得到以此实现外参的动态标定。
[0021]根据一种优选实施方式，步骤S4具体包括：
[0022]S41.在第i帧时刻获取机械臂末端坐标F
i
，基于深度摄像头的外参变换得到深度摄像头的坐标T
i
；
[0023]S42.通过深度学习提取目标上的特征点作为关键点p，基于RGBD数据得到关键点p对应的空间中的3D点P，根据针孔成像模型得到如下投影公式：
[0024][0025]上式中，K∈R3×3表示深度摄像头的内参矩阵；T
i
∈R3×4表示第i帧时刻深度摄像头的外参矩阵；P∈R3为点P对应的3D空间中的点采用；p∈R3为关键点p在图像中的齐次表示形式。
[0026]根据一种优选实施方式，步骤S4还包括：
[0027]S43.采用最小化多帧投影误差的平方获取目标的空间位姿，投影公式表示为：
[0028][0029]上式中，m为机械臂移动过程中提取的帧数据数量，n为每帧对应的目标中的关键点数量；
[0030]S44.在机械臂移动过程中，重复执行步骤S42至步骤S43，将优化出的结果P作为抓取的目标点，控制机械臂移动至该点进行抓取。
[0031]根据一种优选实施方式，所述深度摄像头安装于机械臂末端。
[0032]本专利技术还提供一种基于多帧融合的果蔬采摘机器人，包括深度摄像头、机械臂、移动平台、末端工具手和处理单元，所述处理单元用于执行上述所述的基于多帧融合的果蔬采摘机器人控制方法；所述深度摄像头安装于机械臂末端。
[0033]根据一种优选实施方式，还包括：图像处理模块、机械臂驱动控制模块、移动平台控制模块和末端工具手驱动控制模块，所述图像处理模块、机械臂驱动控制模块、移动平台控制模块和末端工具手驱动控制模块均与处理单元具有信号连接；
[0034]所述图像处理模块与深度摄像头连接，所述机械臂驱动控制模块与机械臂连接，所述移动平台控制模块与移动平台连接，所述末端工具手驱动控制模块与末端工具手连接。
[0035]本专利技术实施例的技术方案至少具有如下优点和有益效果：本专利技术省略了手动标定外参的过程，通过动态外参标定，不仅简化了部署的过程，还解决了过程中外参变动不能及时更改的问题；通过多帧融合，优化观测的最大后验概率，使得估计目标物体位置满足多帧的观测，极大的提高了测量的精度，减少了空间感知的误差。
附图说明
[0036]图1为本专利技术实施例1提供的一种基于多帧融合的果蔬采摘机器人控制方法的逻辑流程图；
[0037]图2为本专利技术实施例1提供的一种基于多帧融合的果蔬采摘机器人的结构框图。
具体实施方式
[0038]为使本专利技术实施例的目的、技术方案和优点更加清楚，下面将结合本专利技术实施例中的附图，对本专利技术实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本专利技术一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本专利技术实施例的组件可以以各种不同的配本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于多帧融合的果蔬采摘机器人控制方法，其特征在于，包括如下步骤：S1.通过深度摄像头获取RGBD数据，基于所述RGBD数据，采用vSLAM构建一个具有实际尺度的局部地图，并获取当前帧时刻深度摄像头在该局部地图中的空间位姿；S2.基于已知的位置信息计算深度摄像头与机械臂末端的相对位置，从而对深度摄像头的外参进行标定，所述已知的位置信息包括：机械臂末端在机器人基坐标系的空间位姿、局部地图在机器人基坐标系中表示的空间位姿以及当前帧时刻深度摄像头在局部地图中的空间位姿；S3.在机械臂移动过程中，重复执行步骤S1至步骤S2，对深度摄像头的外参进行动态标定，直至采摘目标成功；S4.基于机械臂移动过程中采集到的多帧3D视觉数据，用BA构建图优化模型，其中目标点的3D位置为图优化模型中的一个顶点，通过优化顶点综合计算出目标的空间点云。2.如权利要求1所述的基于多帧融合的果蔬采摘机器人控制方法，其特征在于，步骤S2具体包括：获取深度摄像头第i帧时刻在局部地图中的位置由于机器人基坐标相对于局部地图的坐标固定不变，根据空间变换的链式法则得到如下公式：图的坐标固定不变，根据空间变换的链式法则得到如下公式：上式中，T为欧式空间的位姿，其具体形式采用4*4的齐次矩阵表达；为第i帧时刻机械臂末端在机器人基坐标系的空间位姿；为深度摄像头在机械臂末端坐标系中表示的空间位姿；为局部地图在机器人基坐标系中表示的空间位姿；为第i帧时刻深度摄像头在局部地图中的空间位姿；上述两公式联立消除得到如下公式：上式中，inv(
·
)表示对齐次矩阵求逆的运算过程；为第i+1帧时刻机械臂末端在机器人基坐标系的空间位姿；为第i+1帧时刻深度摄像头在局部地图中的空间位姿；采用最小二乘法求解得到3.如权利要求2所述的基于多帧融合的果蔬采摘机器人控制方法，其特征在于，步骤S3具体包括：在机械臂移动过程中，重复执行步骤S1至步骤S2，通过多帧的测量得到多个等式，采用最小二乘法求解得到以此实现外参的动态标定。...

【专利技术属性】
技术研发人员：陈启明，郑鑫，谌文亲，陈鑫华，过浩星，
申请(专利权)人：成都朴为科技有限公司，
类型：发明
国别省市：