一种异形机械臂标定方法及系统与流程

本发明涉及机械臂标定，特别涉及一种异形机械臂标定方法及系统。

背景技术：

1、在工业机械臂领域，一般通过相机识别目标物体在工件中的位置，然后引导机械臂运动至目标位置完成作业，然后相机获取的位置是基于相机坐标系的，而机械臂的运动指令是基于机械臂基底坐标系的，两者之间缺乏直接的对应关系，因此需要通过“手眼标定”来确定相机坐标系与机械臂基底坐标系之间的变换关系。

2、常见的手眼标定方法通常采用两步法：首先获取多组机械臂末端位姿与标定板在rgb相机下位姿的对应数据，然后基于手眼标定方程进行求解。

3、通过rgb相机采集数据时，需要通过标定板已知尺寸和特征点之间的几何约束来求解位姿，且仅能采集二维信息，深度信息需要通过标定板的已知尺寸间接推算，无法直接获取精确的距离信息；对于异形机械臂，标定板容易被遮挡而出现不能拍全的情况，导致标定失败。

4、而且现有技术中在进行标定矩阵的优化时，一般将旋转误差和平移误差混合在一起进行度量，由于旋转误差与平移误差具有不同的物理单位和量纲——旋转误差的度量单位为弧度，平移误差的度量单位为毫米，两者物理意义不同、数值量级差异较大——简单混合难以合理平衡两类误差对标定结果的影响。当旋转误差与平移误差的量级相差较大时，量级较大的误差项将在目标函数中占据主导地位，导致优化方向向该误差项偏倚，而量级较小的误差项则难以得到有效优化，最终使得标定结果的综合精度下降。

技术实现思路

1、基于此，本发明的目的是提供一种异形机械臂标定方法及系统，以解决上述背景技术中提出的问题。

2、为实现上述目的，本发明首先提出一种异形机械臂标定方法，包括以下步骤：

3、s1，预设多个拍照位，获取机械臂末端位于拍照位时相对于基底坐标系的位姿数据，将靶点置于拍照位，获取靶点在rgb像素图像中的像素坐标、靶点在tof相机坐标系下的三维坐标；

4、s2，将靶点在rgb像素图像中的像素坐标、靶点在tof相机坐标系下的三维坐标转换到rgb相机坐标系下，获取靶点在rgb相机坐标系下三维坐标；

5、s3，基于所述位姿数据构建机械臂末端相对于基底坐标系的位姿矩阵，基于靶点在rgb相机坐标系下三维坐标构建靶点相对于rgb相机坐标系的位姿矩阵b；

6、s4，给定初始的标定矩阵，以机械臂末端相对于基底坐标系的位姿矩阵为真实值，基于标定矩阵将靶点相对于rgb相机坐标系的位姿矩阵b转换到基底坐标系下得到靶点相对于基底坐标系的位姿矩阵，以靶点相对于基底坐标系的位姿矩阵作为估计值，构建真实值与估计值之间的残差矩阵，提取残差矩阵的旋转部分与平移部分并计算真实值与估计值之间的差值；

7、s5，构建以真实值与估计值之间的差值最小化为目标的目标函数，对目标函数进行迭代优化，确定标定矩阵的最优解。

8、作为本发明进一步的方案，所述步骤s1中，所述拍照位的数量为13～20。

9、作为本发明进一步的方案，所述步骤s2包括：

10、s21，基于tof相机坐标系和rgb相机坐标系间的相对位置关系，将靶点在tof相机坐标系下的三维坐标转化到rgb相机坐标系下，获取第一坐标；

11、s22，基于rgb相机的畸变系数对靶点在rgb像素图像中的像素坐标进行校正；基于第一坐标中的z轴坐标值、rgb相机的内参将校正后的像素坐标转换到rgb相机坐标系下，获取第二坐标；

12、s23，基于第二坐标的x、y轴坐标值，第一坐标中的z轴坐标值构建靶点在rgb相机坐标系下的三维坐标。

13、作为本发明进一步的方案，所述步骤s21包括：

14、获取靶点在第个拍照位的tof点云图像，基于靶点在第个拍照位的tof点云图像获取靶点在tof相机坐标系下的三维坐标，其中，为拍照位的总数；

15、获取tof相机坐标系和rgb相机坐标系间的相对位置关系，tof相机坐标系和rgb相机坐标系间的相对位置关系由旋转矩阵和平移矩阵表征；

16、获取第一坐标，公式为：

17、；

18、其中，、和分别为第一坐标的x、y、z轴坐标值。

19、作为本发明进一步的方案，所述步骤s22中，基于rgb相机的畸变系数对靶点在rgb像素图像中的像素坐标进行校正的方法如下：

20、获取靶点在第个拍照位的rgb像素图像，基于靶点在第个拍照位的rgb像素图像获取靶点在rgb像素图像中的像素坐标；

21、获取rgb相机的畸变系数，rgb相机的畸变系数包括一阶径向畸变系数、二阶径向畸变系数、三阶径向畸变系数、x轴方向的切向畸变系数和y轴方向的切向畸变系数；

22、对靶点在rgb像素图像中的像素坐标进行校正，得到校正后的像素坐标，校正公式如下：

23、

24、其中，。

25、作为本发明进一步的方案，所述步骤s22中，获取第二坐标的方法包括：

26、获取rgb相机的内参，所述rgb相机的内参包括rgb相机x轴方向焦距长、rgb相机y轴方向焦距长、rgb相机光心的横坐标和rgb相机光心的纵坐标；

27、将校正后的像素坐标转换到rgb相机坐标系下，得到第二坐标，公式为：

28、；

29、；

30、其中，和分别为第二坐标的x、y轴坐标值。

31、作为本发明进一步的方案，所述步骤s23中，第个拍照位的靶点在rgb相机坐标系下的三维坐标为，其中：

32、；

33、；

34、。

35、作为本发明进一步的方案，所述步骤s4中，真实值与估计值之间的差值的计算方法如下：

36、s41，构建第个拍照位的残差矩阵；

37、其中：

38、，为拍照位的总数；

39、为第个拍照位时，靶点在相机坐标系下的位姿；

40、为第个拍照位时，机械臂末端在基底坐标系下位姿的逆矩阵；

41、s42，提取残差矩阵中旋转矩阵的旋转向量的l2范数和平移向量的l2范数作为差值的两个量化表征；

42、其中：

43、表示从残差矩阵中提取旋转矩阵，表示从残差矩阵中提取平移向量；

44、表示将旋转矩阵映射为旋转向量；

45、表示提取向量的l2范数。

46、作为本发明进一步的方案，所述步骤s5包括：

47、s51，将标定矩阵通过李代数进行指数映射，；

48、其中，为hat算子，用于将6维向量转换为4×4矩阵，为指数映射，用于将转换的4×4矩阵映射为标定矩阵；

49、s52，以所有拍照位的两个量化表征平方和最小化为目标，构建如下的目标函数：

50、；

51、s53，使用梯度下降法对目标函数进行迭代优化，求解李代数的最优解；

52、s54，基于李代数的最优解确定标定矩阵的最优解，其中。

53、本发明还提出一种异形机械臂标定系统，其用于实现如上所述的方法，机械臂标定系统包括：

54、机械臂；

55、靶点；

56、rgb相机和tof相机，二者分别用于获取包含靶点的rgb图像和tof点云图像；

57、数据采集模块，用于获取机械臂末端的位姿数据和rgb图像和tof点云图像；

58、坐标解算模块，与数据采集模块连接，用于根据rgb图像和tof点云图像解算靶点在rgb相机坐标系下的三维坐标；

59、标定模块，与坐标解算模块和数据采集模块连接，用于根据机械臂末端的位姿数据和靶点在rgb相机坐标系下的三维坐标构建目标函数，对目标函数进行迭代优化，确定标定矩阵的最优解。

60、与现有技术相比，本发明具有以下有益效果：

61、1、rgb像素图像负责提供靶点在图像中的精确平面位置，tof点云图像负责提供靶点相对于相机光心的精确深度信息，将二者得到的坐标进行融合，无需依赖标定板的已知尺寸间接推算深度信息，避免了“二维到三维”的推算误差，尤其适用于异形机械臂上无法安装标定板或者标定板易受遮挡、不容易拍全的场景；且利用rgb像素图像的高分辨率优势弥补了tof点云图像在目标边缘处定位精度差的缺陷，实现了rgb相机和tof相机的优势互补，显著提高了靶点三维坐标的计算精度。

62、2、将手眼矩阵的求解转化为非线性优化问题，构建了以旋转误差和平移误差分别度量的目标函数，分别提取残差矩阵的旋转部分和平移部分进行独立量化，有效避免了因旋转误差与平移误差物理单位和量纲不同而导致的优化方向偏离问题，提高了标定精度。