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

权利要求书

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所述的基于多帧融合的果蔬采摘机器人控制方法，其特征在于，

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