一种3D相机相对于机械臂的位姿标定方法与流程

一种3d相机相对于机械臂的位姿标定方法
技术领域
1.本发明涉及3d视觉技术领域，具体为一种3d相机相对于机械臂的位姿标定方法。


背景技术：

2.相比于2d视觉，3d视觉能获取更多的信息，特别是具备识别物体深度信息的能力，因此在机械臂应用中得到了越来越多的应用。3d相机获得的物体的位置信息是相对于相机坐标系的，为了将识别的位置信息用于机械臂的运动规划中，则需要将相机坐标系中的坐标转换到机器人的基坐标系中。为此，需要标定出3d相机相对于机械臂的位姿。
3.3d视觉获取的是三维位置信息，因此对于姿态的要求较高。相关文献 (201810442834.6，202110689530.1)提出了一种基于标准球的机器人手眼标定方法，但是他们将描述姿态矩阵的9个变量全部作为独立的未知量，然后通过求解超定方程的方法来进行标定。这种方法一是显著提高了标定变量的数量，实际上，姿态矩阵只需要3个独立变量就可以进行表达；二是求解超定方程需要计算大型矩阵的逆，计算时间长；第三则是破坏了姿态矩阵是单位正交阵的性质，在标定环节引入了额外的误差。
4.为了解决上述问题，本发明提供了一种新的姿态矩阵标定方法。该方法采用旋量坐标来表达姿态矩阵，在标定过程中始终保证姿态矩阵是单位正交的；通过迭代优化方法进行标定，降低了计算量，提高了收敛速度。


技术实现要素：

5.基于此，本发明的目的是提供一种3d相机相对于机械臂的位姿标定方法，以解决的技术问题。
6.为实现上述目的，本发明提供如下技术方案：一种3d相机相对于机械臂的位姿标定方法，包括以下步骤：
7.(1)将标定球放在机械臂的工作空间内，标定过程中保持静止不动，机械臂带动相机在不同位置对标定球进行拍照，获得标定球的多组点云数据以及机械臂对应的末端位姿
8.(2)根据所述点云数据，采用随机采样一致算法，对球面进行拟合，获得球心在相机坐标系中的位置
9.(3)将球心坐标变换到机械臂的基坐标系下，得到球心坐标在基坐标系中的表达以相对于第一次拍照的位置偏差建立目标函数；
10.(4)采用梯度下降方法，对所求的相机相对于机械臂末端的位姿矩阵进行迭代，实现相对位姿的标定。
11.所述方法的步骤3具体包括：
12.球心位置在基坐标系中的表达和在相机坐标系中的表达之间的关系为：
13.由于标定球相对与基坐标系保持静止不动，则每次拍照对应的应该是一样的，因此，可以建立目标函数：
14.当目标函数优化到最小时，则说明得到了准确的估计。
15.所述方法的步骤4具体包括：
16.采用指数坐标描述姿态，从而保证在标定过程中姿态矩阵为始终单位正交阵。指数坐标ζ＝[ζ
x ζ
x ζ
x
]
t
与姿态矩阵r的对应关系为：式中，将ζ转变为反对称矩阵
[0017]
则所求的相机相对于机械臂末端的位姿矩阵可以通过6个变量来表达，
[0018]
ξ＝[ζ
t p
t
]
t
＝[ζ
x ζ
y ζ
z p
x p
y pz]
t
[0019]
采用有限差分方法，计算目标函数f
obj
对ξ的梯度对ξ的梯度每个元素可以通过下式计算
[0020][0021]
式中，ei表示第i个元素为1的单位列向量。
[0022]
根据梯度下降法，ξ的更新方程为
[0023][0024]
当ξ相邻迭代次数的差异σ＝||ξ
(k+1)-ξ
(k)
||小于一定范围时，标定算法收敛，实现相机位姿的标定。
[0025]
综上所述，本发明主要具有以下有益效果：
[0026]
1、本发明实施简单，仅需要采用一个标准的球体，如台球，即可完成3d 相机的标定，不需要加工满足严格约束关系的标定物；
[0027]
2、本发明标定过程中始终保证姿态矩阵是单位正交阵，能有效消除标定方法带来的原理上的误差；
[0028]
3、本发明计算量小，收敛速度快。要标定的变量为6个，是最小且完备的；采用梯度下降优化方法，不需要求解大型矩阵的逆。
附图说明
[0029]
图1为本发明的机器人3d手眼标定方法的流程示意图；
[0030]
图2为本发明的机器人3d手眼标定方法的步骤图。
具体实施方式
[0031]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。
[0032]
下面根据本发明的整体结构，对其实施例进行说明。
[0033]
一种3d相机相对于机械臂的位姿标定方法，如图1所示，本发明提供的标定方法通过以下步骤实现：
[0034]
(1)将标定球放在机械臂的工作空间内，标定过程中保持静止不动，机械臂带动相机在不同位置对标定球进行拍照，获得n组标定球的点云数据 {cpi}
(n)
(n＝1，
…
，n)以及对应的机械臂末端位姿
[0035]
(2)根据所述点云数据，采用随机采样一致算法(ransac算法)，对球面进行拟合，获得球心在相机坐标系中的位置通过点云数据拟合的方式计算球心坐标能抑制相机噪声带来的误差；
[0036]
(3)将球心坐标变换到机械臂的基坐标系下，得到球心坐标在基坐标系中的表达以相对于第一次拍照的位置偏差建立目标函数；
[0037]
(4)采用梯度下降方法，对所求的相机相对于机械臂末端的位姿矩阵进行迭代，实现相机位姿的标定。
[0038]
在步骤(3)中，根据位置变换的传递关系，可以得到球心位置在基坐标系中的表达和在相机坐标系中的表达之间的关系：
[0039][0040]
由于标定球相对于基坐标系保持静止不动，每次拍照对应的应该是一样的。因此，如果给定一个初始值，则由可以计算出对应的根据与第一次采样数据的差异，可以建立目标函数：
[0041][0042]
此时，标定问题变成了一个优化问题。当目标函数优化到最小时，则说明能够保证由计算出的对应点是在一个固定点上，即说明3d相机的位姿得到了准确的估计。
[0043]
在步骤(4)中，采用指数坐标描述姿态，从而能采用最少数量的变量来表达并且保证在标定过程中姿态矩阵为始终单位正交阵。指数坐标ζ＝[ζ
x ζ
x ζ
x
]
t
与姿态矩阵r的对应关系为：
[0044][0045]
式中，将ζ转变为反对称矩阵
[0046]
[0047]
于是，所求的相机相对于机械臂末端的位姿矩阵可以通过6个变量来表达，
[0048]
ξ＝[ζ
t p
t
]
t
＝[ζ
x ζ
y ζ
z p
x p
y pz]
t
[0049]
目标函数f
obj
对于变量ξ无法显式地求出梯度可以采用有限差分方法进行近似计算。对ξ的每个分量，添加一个微小的步长h，则f
obj
的差值除以步长即可得到每个分量的偏微分
[0050][0051]
式中，ei表示第i个元素为1的单位列向量。
[0052]
计算出梯度后，可以根据梯度下降法，对ξ进行迭代更新
[0053][0054]
式中，α表示学习速率。
[0055]
当ξ相邻迭代次数的差异σ＝||ξ
(k+1)-ξ
(k)
||小于一定范围时，标定算法收敛，目标函数f
obj
到达最小值。然后由变量ξ即可计算出相对位姿矩阵
[0056]
下面通过一个标定实例对上述方法进行阐述。
[0057]
所提供的方法的步骤(1)和(2)需要操作实际设备，获取数据后运用 pcl库可以得到在此不做详细说明。假设得到的球心坐标和机械臂末端位姿如下表所示，待标定的相对位姿矩阵为[1 1 1 100 100 100]
t
。
[0058][0059][0060]
采用本发明提供的标定算法，得到的相对位姿为 [0.998 0.995 1.002 99.265 99.8 100.9]
t
[0061]
与实际相对位姿矩阵相比，姿态误差为0.3
°
，位置误差为1.2mm，能够满足实际使用的需求。另外，通过调整学习速率α，可以进一步提高标定精度。
[0062]
尽管已经示出和描述了本发明的实施例，但本具体实施例仅仅是对本发明的解释，其并不是对发明的限制，描述的具体特征、结构、材料或者特点可以在任何一个或多个实施例或示例中以合适的方式结合，本领域技术人员在阅读完本说明书后可在不脱离本发明的原理和宗旨的情况下，可以根据需要对实施例做出没有创造性贡献的修改、替换和变型等，但只要在本发明的权利要求范围内都受到专利法的保护。