CN116021519A

CN116021519A - 一种基于tof相机采摘机器人手眼标定方法以及装置

Info

Publication number: CN116021519A
Application number: CN202211738143.3A
Authority: CN
Inventors: 张相胜; 姚猛; 张雨生
Original assignee: Jiangnan University
Current assignee: Jiangnan University
Priority date: 2022-12-31
Filing date: 2022-12-31
Publication date: 2023-04-28

Abstract

本发明公开了一种基于TOF相机采摘机器人手眼标定方法、装置以及设备，包括本发明采集多组标定板深度图和对应点云数据，利用TOF深度相机对标定板进行成像并建立点云模型，并通过中心点接触式测量出标志点在机器人基坐标系下的坐标值，即可解算出手眼关系的初始旋转矩阵和平移矢量，并估计各个点位残差，重新分配标志点的权重系数，对手眼参数进行迭代优化。该方法操作简单，标定板易于制作且成本低。采用JAKA六轴机器人与深度相机搭建实验平台，实验结果表明，该方法具有良好的稳定性，适合现场标定，标定精度达到4mm，且满足水果采摘的作业精度要求。

Description

一种基于TOF相机采摘机器人手眼标定方法以及装置

技术领域

本发明涉及3D机器视觉技术领域，特别是涉及一种基于TOF相机采摘机器人手眼标定方法、装置以及设备。

背景技术

目前果蔬采摘主要以人力采摘为主，但是随着城镇化进程加剧了劳动力的短缺，造成果蔬采摘成本大幅提高，所以研发能够提高生产效率、降低采摘成本的采摘机器人，是果蔬生产的必然趋势。

采摘机器人的工作环境复杂，不确定性因素多，采摘难度大，急需一种高效快捷稳定的果蔬采摘机器人系统。研究表明，视觉与机器人的关联及精确定位，即手眼标定是实现采摘机器人主动视觉和自动采摘的关键技术与前提。手眼标定的主要目的是获取相机到机器人末端的转换关系，便于控制机器人的手臂去完成相应的任务，因此手眼标定的研究对于采摘机器人有着重大的意义。

近些年来，基于传统2D相机的手眼标定方法不断得到研究，但大多是从彩色图像上提取轮廓、角点等信息进行手眼标定，但复杂的户外环境(光照等)，让利用彩色图像的标定方法难以保证鲁棒性，且难以反映采摘对象的实际大小和位置关系。随着深度相机的发展，深度相机越来越多地被应用到视觉采摘领域中，提高了果蔬采摘的识别能力和效率，除此之外深度相机也被应用在手眼标定中。TOF深度相机是一款具有主动视觉测量功能的相机，它的工作原理是通过给目标连续发送光脉冲，然后用传感器接收从物体返回的光，通过探测光脉冲的飞行往返时间来得到目标物距离，相较于其他3D相机，它具有价格便宜、体积小、功耗低、抗光干扰性强以及计算深度信息快速的优点，非常适合野外水果采摘的应用场景。

综上所述可以看出，如何利用TOF深度相机提高采摘对象的大小和位置是目前有待解决的问题。

发明内容

本发明的目的是提供一种基于TOF相机采摘机器人手眼标定方法，解决了现有技术中2D识别方法的精确度低的弊端。

为解决上述技术问题，本发明提供一种基于TOF相机采摘机器人手眼标定方法，包括：

S1：利用TOF相机对标定板进行拍摄，得到多组标定板深度图像、多组点云信息以及多组位姿参数；

S2：通过圆心定位排序算法对所述多组标定板深度图像进行处理，得到多组标定板图像圆心标志点的像素坐标；

S3：根据所述多组点云信息，计算点云坐标二维系到三维系的映射关系，并依据所述映射关系将每组标定板图像中圆心标志点的像素坐标映射为点云坐标；

S4：将每组位姿参数转化为机器人基坐标系到末端坐标系的旋转平移矩阵，得到多组旋转平移矩阵；

S5：根据多组旋转平移矩阵将机器人基坐标系下的初始圆心标志点坐标转化为机器人末端坐标系下的圆心标志点坐标；

S6：根据对偶四元数法、每组圆心标志点的点云坐标和机器人末端坐标系下的圆心标志点坐标，计算初始手眼标定矩阵；

S7：根据所述初始手眼标定矩阵将所述每组圆心标志点的点云坐标转化为机器人末端坐标系下的目标圆心标志点坐标，并结合所述机器人末端坐标系下的圆心标志点坐标，计算每组圆心标志点的点位残差；

S8：根据所述每组圆心标志点的点位残差重新分配权重系数，更新所述初始手眼标定矩阵的权重系数，返回步骤S7进行迭代优化，直至达到预设迭代次数或前后两次迭代残差的误差值小于等于误差阈值时，停止迭代，得到最终手眼标定矩阵。

优选地，所述步骤S1前包括：

控制所述机器人末端按照预设顺序，依次接触所述标定板的圆心标志点，得到所述机器人基坐标系下的初始圆心标志点坐标。

优选地，所述步骤S2前包括：

利用高斯滤波增强所述多组深度图片中边缘轮廓信息，得到多组高斯滤波处理后的深度图片。

优选地，所述步骤S2包括：

利用Canny算子提取所述多组高斯滤波处理后的深度图片的边缘轮廓，得到多组边缘轮廓信息；

根据预设面积和预设圆度对所述多组边缘轮廓信息进行筛选，得到具有亚像素的所有圆形靶标轮廓；

将所述所有圆形靶标轮廓进行椭圆最小二乘拟合，计算圆形靶标中心位置坐标；

利用基于向量夹角和质心坐标的圆心标志点排序方法，得到所述机器人每次拍摄的图片的圆心标志点亚像素坐标。

优选地，所述利用向量夹角和质心坐标的圆心排序方法，得到所述机器人每次拍摄的图片的圆心亚像素坐标包括：

选取所有圆形靶标轮廓中亚像素点最多的轮廓，并进行椭圆最小二乘拟合，得到初始点；

分别计算剩余圆形标靶轮廓中圆心标志点到所述初始点的欧式距离；

选取所有欧式距离中两个最小欧式距离，确定所对应的圆心标志点；

计算所述所有圆心标志点的质心坐标，根据所述所有质心坐标得到以初始点的方向向量。

优选地，所述根据所述多组点云信息，计算点云坐标二维系到三维系的映射关系，并依据所述映射关系将每组标定板图像中圆心标志点的像素坐标映射为点云坐标包括：

根据每一个三维点云坐标对应二维图像中的一个像素的原理，提取二维图像中一圆心标志点的像素坐标(u,v)，得到圆心标志点在三维坐标中的点云集合N，得到点云坐标二维系到三维系的映射关系N＝u*Col+Row。

针对提取到的每一个圆心标志点亚像素坐标为P＝(u,v)，则其周围四个整数像素坐标为P₀＝(u₀,v₀)、P₁＝(u₀+1,v₀)、P₂＝(u₀,v₀+1)、P₃＝(u₀+1,v₀+1)，以P为中心，将四个像素点围成的正方形划分为四个小矩形区域，计算这四个矩形区域的面积，得到基于面积倒数的插值权函数；

将所述四个整数像素坐标通过所述点云坐标二维系到三维系的映射关系初步定位其三维点云坐标，并根据所述插值权函数对四个三维点云坐标进行插值计算，得到每一个圆心标志点的点云坐标。

优选地，所述根据对偶四元数法、每组圆心标志点的点云坐标和机器人末端坐标系下的圆心标志点坐标，计算初始手眼标定矩阵包括：

通过所述对偶四元数

的实部

和虚部

以及所述旋转平移矩阵得到：

其中，R为旋转矩阵，T为平移向量；

基于圆心标志点的点云坐标集合为P_CamAll，机器人末端坐标系下的圆心标志点坐标集合为P_EndAll，得到所述初始手眼标定矩阵：

其中，w_i为权重系数，初值设为1，n为标志点个数。

优选地，所述根据所述每组圆心标志点的点位残差重新分配权重系数，更新所述初始手眼标定矩阵的权重系数包括：

将所述每组圆心标志点的点位残差均输入选择权重函数中，根据所述选取权重函数的结果，分配新权重系数；

将所述新权重系数输入所述初始手眼标定矩阵，更换之前的系数，得到新的手眼标定矩阵；

其中，选取权重函数为：

式中e_i为每一个标志点的点位残差，

为所有标志点的点位残差的绝对平均值，l为常数。

本发明还提供了一种基于TOF相机采摘机器人手眼标定的装置，包括：

采集模块，用于利用TOF相机对标定板进行拍摄，得到多组标定板深度图像、多组点云信息以及多组位姿参数；

处理模块，用于通过圆心定位排序算法对所述多组标定板深度图像进行处理，得到多组标定板图像圆心标志点的像素坐标；

计算模块，用于根据所述多组点云信息，计算点云坐标二维系到三维系的映射关系，并依据所述映射关系将每组标定板图像中圆心标志点的像素坐标映射为点云坐标；

计算矩阵模块，用于将每组位姿参数转化为机器人基坐标系到末端坐标系的旋转平移矩阵，得到多组旋转平移矩阵；

转化模块，用于根据多组旋转平移矩阵将机器人基坐标系下的初始圆心标志点坐标转化为机器人末端坐标系下的圆心标志点坐标；

构建手眼标定矩阵模块，用于根据对偶四元数法、每组圆心标志点的点云坐标和机器人末端坐标系下的圆心标志点坐标，计算初始手眼标定矩阵；

计算残差模块，用于根据所述初始手眼标定矩阵将所述每组圆心标志点的点云坐标转化为机器人末端坐标系下的目标圆心标志点坐标，并结合所述机器人末端坐标系下的圆心标志点坐标，计算每组圆心标志点的点位残差；

迭代优化模块，用于根据所述每组圆心标志点的点位残差重新分配权重系数，更新所述初始手眼标定矩阵的权重系数，返回步骤S7进行迭代优化，直至达到预设迭代次数或前后两次迭代残差的误差值小于等于误差阈值时，停止迭代，得到最终手眼标定矩阵。

本发明还提供了一种基于TOF相机采摘机器人手眼标定的设备，包括：

存储器，用于存储计算机程序；处理器，用于执行所述计算机程序时实现上述一种基于TOF相机采摘机器人手眼标定方法的步骤。

本发明所提供的一种基于TOF相机采摘机器人手眼标定方法，本发明进行多次旋转平移变换，利用TOF深度相机对标定板进行成像并建立点云模型，采集多组标定板深度图和对应点云数据，并测量出圆心标志点标志点在机器人基坐标系下的坐标值，即可解算出手眼关系的旋转矩阵和平移矢量。提出一种基于向量夹角和圆形标定板质心坐标的圆心标志点排序方法，可以解决圆心标志点提取乱序的问题，并指定排序方向，并估计点位残差，重新分配标志点的权重系数，对手眼参数进行迭代优化，提高了采摘机器人手眼标定的精度和稳定性。

附图说明

为了更清楚的说明本发明实施例或现有技术的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单的介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明所提供的基于TOF相机采摘机器人手眼标定方法的第一种具体实施例的流程图；

图2为本发明所提供的基于TOF相机采摘机器人手眼标定方法的第二种具体实施例的流程图；

图3为本发明中图像采集所用的圆形标定板；

图4为本发明中基于向量夹角和质心坐标的圆心排序方法示意图；

图5为本发明中圆心定位排序后的结果；

图6为本发明标志点在各个方向上点位残差图；

图7为本发明所提手眼标定方法与其他方法的抗差性对比图；

图8为本发明所提标定方法与传统对偶四元数法定位粗差能力对比图；

图9为本发明实施例提供的一种基于TOF相机采摘机器人手眼标定的装置结构框图。

具体实施方式

本发明的核心是提供一种基于TOF相机采摘机器人手眼标定方法，其通过多次迭代计算优化能够使得在户外采摘环境中，该方法和装置具有更大的稳定性，非常适合户外现场标定，标定精度可达到4mm。

为了使本技术领域的人员更好地理解本发明方案，下面结合附图和具体实施方式对本发明作进一步的详细说明。显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参考图1，图1为本发明所提供的基于TOF相机采摘机器人手眼标定方法的第一种具体实施例的流程图；具体操作步骤如下：

步骤S101：利用TOF相机对标定板进行拍摄，得到多组标定板深度图像、多组点云信息以及多组位姿参数；

步骤S102：通过圆心定位排序算法对所述多组标定板深度图像进行处理，得到多组标定板图像圆心标志点的像素坐标；

步骤S103：根据所述多组点云信息，计算点云坐标二维系到三维系的映射关系，并依据所述映射关系将每组标定板图像中圆心标志点的像素坐标映射为点云坐标；

步骤S104：将每组位姿参数转化为机器人基坐标系到末端坐标系的旋转平移矩阵，得到多组旋转平移矩阵；

步骤S105：根据多组旋转平移矩阵将机器人基坐标系下的初始圆心标志点坐标转化为机器人末端坐标系下的圆心标志点坐标；

步骤S106：根据对偶四元数法、每组圆心标志点的点云坐标和机器人末端坐标系下的圆心标志点坐标，计算初始手眼标定矩阵；

步骤S107：根据所述初始手眼标定矩阵将所述每组圆心标志点的点云坐标转化为机器人末端坐标系下的目标圆心标志点坐标，并结合所述机器人末端坐标系下的圆心标志点坐标，计算每组圆心标志点的点位残差；

步骤S108：根据所述每组圆心标志点的点位残差重新分配权重系数，更新所述初始手眼标定矩阵的权重系数，返回步骤S107进行迭代优化，直至达到预设迭代次数或前后两次迭代残差的误差值小于等于误差阈值时，停止迭代，得到最终手眼标定矩阵。

在本实施例中，本发明首先对标定板进行拍摄，得到多组标定板深度图像、多组点云信息以及多组位姿参数，然后对拍摄的图像进行处理，然后图像坐标转化为三维点云坐标，然后将位姿坐标转化为旋转平移矩阵，基于旋转平移举证将机器人的初始圆心坐标转化为机器人末端下的坐标，根据对偶四元数法构建手眼标定矩阵构建初始手眼标定矩阵，计算标志带你坐标的残差，根据残差重新分配手眼标定矩阵的系数，重复迭代，直至达到预设迭代次数或残差小于阈值，停止迭代，得到最终的手眼标定矩阵。本发明利用残差更新手眼标定矩阵的系数，提高了采摘机器人手眼标定的精度和稳定性，满足水果采摘的作业精度要求。

请参考图2，图2为本发明所提供的基于TOF相机采摘机器人手眼标定方法的第二种具体实施例的流程图；具体操作步骤如下：

步骤S201：控制所述机器人末端按照预设顺序，依次接触所述标定板的圆心标志点，得到所述机器人基坐标系下的初始圆心标志点坐标；

控制机器人的末端执行机构(针尖)，按照左上、右上、左下、右下的固定顺序，分别接触标定板四个角上的圆心点，获得机器人基坐标系下的一组圆心点齐次坐标，并记录点的索引序号，请参考图3，图3为图像采集所用的圆形标定板；

对机器人进行位姿示教，保证每组位姿下，TOF相机都能拍摄到完整清晰的标定板图片。

步骤S202：根据机器人示教位姿对标定板进行拍摄，得到多组深度图片、点云信息以及机器人拍照时的位姿坐标；

步骤S203：对标定板深度图片进行预处理，通过圆心定位排序算法，得到每一组标定板四个角上圆心的像素坐标；

首先将标定板图像转换为灰度图像；然后利用最大类间差法对灰度图像进行二值化处理；最后利用膨胀腐蚀剔除掉灰度图像中的无关的信息，只保留最本质的椭圆信息。

S3.1：定义高斯函数，将标定板图像中包含封闭圆的区域利用高斯滤波器增强边缘轮廓信息，其中高斯函数定义为

式中，σ为标准方差，u为均值；

S3.2：利用Canny算子提取高斯滤波处理后的图像的边缘轮廓，设置面积、圆度对边缘轮廓进行筛选，得到所有圆形靶标轮廓；

S3.3：圆形靶标的平面影像为平面椭圆，利用S4.2中得到的亚像素边缘轮廓信息，进行椭圆最小二乘拟合，由得到的椭圆平面方程系数可直接计算出圆形靶标中心位置，平面椭圆的一般方程为Ax²+Bxy+Cy²+Dx+Ey+F＝0(2)，椭圆中心像素坐标为

和

式中，x₀，y₀分别为椭圆圆心在x与y轴上的像素坐标，A、B、C、D、E、F为椭圆平面方程的系数；

S3.4：采用基于向量夹角和质心坐标的圆心排序方法，使圆心坐标读取顺序具有一致性，请参考图4和图5，图4为本发明中基于向量夹角和质心坐标的圆心排序方法示意图，图5为本发明中圆心定位排序后的结果。

依据Canny边缘检测以及轮廓筛选得到的圆轮廓，搜索轮廓上亚像素点最多的轮廓，再进行最小二乘椭圆拟合，即初始点P_start；

分别计算其他圆心到初始点的欧氏距离，并按照欧式距离进行从小到大排序，选择最近的两个点，即P_x0和P_y0；

求取所有圆心的质心坐标P_centroid，得到三个以P_start为起始点的方向向量，

根据

来判断向量之间的位置关系：若

在

的逆时针方向；若

在

的顺时针方向；若

与

共线；由此可以判断出，

三个方向向量的位置关系。本发明令

在

的逆时针方向，

即代表X方向；

在

的顺时针方向，

即代表Y方向；

计算其他圆心到起始点的向量与X方向的夹角α，设置角度阈值，符合条件的就是Y方向上的基准点，并按照与初始点的欧氏距离从小到大排序，即可得到P_y0和P_y1；

依次以Y方向上的基准点为参考点，计算其他圆心到参考点的向量与Y方向的夹角β，设置角度阈值，筛选出与X方向共线的圆心，并按照与参考点的欧氏距离从小到大排序。

步骤S204：根据多组点云信息，构建点云坐标二维到三维的关系，并根据关系，将每组标定板四个角上圆心的像素坐标映射为点云坐标。

S4.1：根据每一个三维点云坐标都对应图像中的一个像素的原理，通过提取某一特征点图像中的像素坐标(u,v)，可以初步定为此像素对应的三维坐标在点云集合中的序号N，假设图像大小为Row×Col，则序号N的计算方法为N＝u*Col+Row。

S4.2：针对提取到的每一个标志点亚像素坐标为P＝(u,v)，则其周围四个整数像素坐标为P₀＝(u₀,v₀)、P₁＝(u₀+1,v₀)、P₂＝(u₀,v₀+1)、P₃＝(u₀+1,v₀+1)，以P为中心，将四个像素点围成的正方形划分为四个小矩形区域，计算这四个矩形区域的面积，可以得到基于面积倒数的插值权函数；

S4.3：将S4.2中的四个整数像素坐标通过S4.1的方法初步定位其三维点云坐标，并根据插值权函数对四个三维点云坐标进行插值计算，得到每一个标志点的点云坐标。

步骤S205：将每组位姿参数转化为机器人基坐标系到末端坐标系的旋转平移矩阵；

设拍照位姿为：P＝[x y z Rx Ry Rz]，则旋转矩阵为：R＝R_x*R_y*R_z其中，

平移矩阵为：T＝[xyz]^T。

步骤S206：根据每组旋转平移矩阵将机器人基坐标系下的初始圆心标志点坐标转化为机器人末端坐标系下的圆心标志点坐标；

步骤S207：根据对偶四元数法，结合多组圆心点云坐标和机器人末端坐标系下的圆心标志点坐标，求解初始手眼标定矩阵；

步骤S208：计算前后机器人末端坐标系下圆心标志点坐标的差值，然后基于差值更新手眼标定矩阵的系数，进行迭代优化，直至前后机器人末端坐标系下圆心标志点坐标的差值小于误差阈值或者达到预设迭代次数，停止迭代，得到最终手眼标定矩阵。

通过对偶四元数的表达式：

及其性质：满足ε²＝0且ε≠0，其中

和

分别为对偶四元数

的实部和虚部，两者皆为四元数，利用对偶四元数的实部

表示坐标系间的旋转，结合虚部

表示坐标系间的平移，如公式(7)所示：

式中，R为旋转矩阵；T为平移向量；

假定圆心点云坐标集合为P_CamAll，机器人末端坐标系下的圆心坐标集合为P_EndAll，

则基于对偶四元数求解初始手眼标定矩阵的数学模型为：

式中w_i为权重系数，初值设为1；n为标志点个数。

计算每组圆心标志点的前后的点位残差，并与预设误差值进行比较，若小于等于，则不改变权重，若大于，则修改权重。图6为本发明标志点在各个方向上点位残差图。

选择权重函数：

其中e_i为每一个标志点的点位残差，

为所有标志点的点位残差的绝对平均值，l为常数取2.5，将每一次迭代后重新分配得到的权重系数放入公式(8)中进行迭代求解，若达到迭代次数或者达到误差阈值，则终止迭代，输出最终手眼标定矩阵。

为了验证本发明的性能，利用本发明中的方法(加权DQ)和传统对偶四元数法(DQ)、权重函数为Danish的加权对偶四元数法和LM法对采摘机器人的手眼标定作为对比，以点位残差的绝对平均值

为评判标准，点位残差是指利用得到的手眼标定矩阵，将机械臂末端坐标系下的标志点转换到相机坐标系下，与之前得到的三维点云坐标求取其坐标之间的偏差，一般偏差越小，手眼标定精度越高。得到的结果如下表1所示，本发明方法和其他方法对比，精度更高。

表1无粗差下各算法

考虑到户外采摘对现场手眼标定的鲁棒性要求，本发明在机器人基座标系下点集合中，按照接触定位标志点的顺序，分别在标志点处添加(1,0,1)、(-1,0,1)、(0,-1,0)、(1,-1,0)，单位为mm的粗差，模拟因接触式定位误操作导致的人为因素引入的粗差，各个算法在不同粗差点个数下的点位绝对误差如附图6所示，可以看出，LM算法、Danish-DQ算法以及本发明提出的加权DQ算法，相较于DQ法的抗差性都有更好的效果，但是相较于来说本发明提出的方法的性能更好。

为了测试本发明提出的加权DQ法定位粗差的能力，在机器人基座标系下点集合中序号为3的标志点引入(10,10,10),单位为mm的大粗差，取前4组标定板的标志点数据进行分析，由附图7可得，其他三个标志点的绝对误差在正常数据和干扰数据下，比DQ法更加接近，而且能更有效定位粗差的位置，说明使用本发明提出的加权DQ算法可以更有效抑制单个干扰数据带来的影响，且具有较强的粗差识别的能力。

本发明还与现有技术中其他方法进行对比，请参考图7和图8，图7为本发明所提手眼标定方法与其他方法的抗差性对比图；图8为本发明所提标定方法与传统对偶四元数法定位粗差能力对比图。

本发明提出的基于优化的结合TOF相机的采摘机器人简单手眼标定方法，该方法只需要操作机器人进行多次旋转平移变换，利用TOF深度相机对标定板进行成像并建立点云模型，采集多组标定板深度图和对应点云数据，并通过执行机构的工具中心点(TCP)接触式测量出标志点在机器人基坐标系下的坐标值，即可解算出手眼关系的旋转矩阵和平移矢量。提出一种基于向量夹角和圆形标定板质心坐标的圆心排序方法，可以解决圆心提取乱序的问题，并指定排序方向，并估计点位残差，重新分配标志点的权重系数，对手眼参数进行迭代优化，提高了采摘机器人手眼标定的精度和稳定性，标定精度可达到4mm。

请参考图9，图9为本发明实施例提供的一种基于TOF相机采摘机器人手眼标定的装置结构框图；具体装置可以包括：

采集模块100，用于利用TOF相机对标定板进行拍摄，得到多组标定板深度图像、多组点云信息以及多组位姿参数；

处理模块200，用于通过圆心定位排序算法对所述多组标定板深度图像进行处理，得到多组标定板图像圆心标志点的像素坐标；

计算模块300，用于根据所述多组点云信息，计算点云坐标二维系到三维系的映射关系，并依据所述映射关系将每组标定板图像中圆心标志点的像素坐标映射为点云坐标；

计算矩阵模块400，用于将每组位姿参数转化为机器人基坐标系到末端坐标系的旋转平移矩阵，得到多组旋转平移矩阵；

转化模块500，用于根据多组旋转平移矩阵将机器人基坐标系下的初始圆心标志点坐标转化为机器人末端坐标系下的圆心标志点坐标；

构建手眼标定矩阵模块600，用于根据对偶四元数法、每组圆心标志点的点云坐标和机器人末端坐标系下的圆心标志点坐标，计算初始手眼标定矩阵；

计算残差模块700，用于根据所述初始手眼标定矩阵将所述每组圆心标志点的点云坐标转化为机器人末端坐标系下的目标圆心标志点坐标，并结合所述机器人末端坐标系下的圆心标志点坐标，计算每组圆心标志点的点位残差；

迭代优化模块800，用于根据所述每组圆心标志点的点位残差重新分配权重系数，更新所述初始手眼标定矩阵的权重系数，返回步骤S7进行迭代优化，直至达到预设迭代次数或前后两次迭代残差的误差值小于等于误差阈值时，停止迭代，得到最终手眼标定矩阵。

本实施例的一种基于TOF相机采摘机器人手眼标定的装置用于实现前述的一种基于TOF相机采摘机器人手眼标定方法，因此一种基于TOF相机采摘机器人手眼标定的装置中的具体实施方式可见前文中的一种基于TOF相机采摘机器人手眼标定方法的实施例部分，例如，采集模块100，处理模块200，计算模块300，计算矩阵模块400，转化模块500，构建手眼标定矩阵模块600，计算残差模块700和迭代优化模块800，分别用于实现上述一种基于TOF相机采摘机器人手眼标定方法中步骤S101，S102，S103，S104，S105，S106，S107，S108所以，其具体实施方式可以参照相应的各个部分实施例的描述，在此不再赘述。

本发明具体实施例还提供了一种基于TOF相机采摘机器人手眼标定的设备，包括：存储器，用于存储计算机程序；处理器，用于执行所述计算机程序时实现上述一种基于TOF相机采摘机器人手眼标定方法的步骤。

本发明具体实施例还提供了一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现上述一种基于TOF相机采摘机器人手眼标定方法的步骤。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其它实施例的不同之处，各个实施例之间相同或相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

专业人员还可以进一步意识到，结合本发明中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

结合本发明中所公开的实施例描述的方法或算法的步骤可以直接用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。

以上对本发明所提供的一种基于TOF相机采摘机器人手眼标定方法、装置以及设备进行了详细介绍。本发明中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，这些改进和修饰也落入本发明权利要求的保护范围内。

Claims

1.一种基于TOF相机采摘机器人手眼标定方法，其特征在于，包括：

2.如权利要求1所述的基于TOF相机采摘机器人手眼标定方法，其特征在于，所述步骤S1前包括：

3.如权利要求1所述的基于TOF相机采摘机器人手眼标定方法，其特征在于，所述步骤S2前包括：

4.如权利要求3所述的基于TOF相机采摘机器人手眼标定方法，其特征在于，所述步骤S2包括：

5.如权利要求4所述的基于TOF相机采摘机器人手眼标定方法，其特征在于，所述利用向量夹角和质心坐标的圆心排序方法，得到所述机器人每次拍摄的图片的圆心标志点亚像素坐标包括：

6.如权利要求1所述的基于TOF相机采摘机器人手眼标定方法，其特征在于，所述根据所述多组点云信息，计算点云坐标二维系到三维系的映射关系，并依据所述映射关系将每组标定板图像中圆心标志点的像素坐标映射为点云坐标包括：

针对提取到的每一个圆心标志点亚像素坐标为P＝(u，v)，则其周围四个整数像素坐标为P₀＝(u₀，v₀)、P₁＝(u₀+1，v₀)、P₂＝(u₀，v₀+1)、P₃＝(u₀+1，v₀+1)，以P为中心，将四个像素点围成的正方形划分为四个小矩形区域，计算这四个矩形区域的面积，得到基于面积倒数的插值权函数；

7.如权利要求1所述的基于TOF相机采摘机器人手眼标定方法，其特征在于，所述根据对偶四元数法、每组圆心标志点的点云坐标和机器人末端坐标系下的圆心标志点坐标，计算初始手眼标定矩阵包括：

通过所述对偶四元数