一种基于非最小化最优化算法的机器人手眼标定方法

摘要

本发明公开了一种基于非最小化最优化算法的机器人手眼标定方法，该方法基于非最小化最优化算法，能够用特征值计算解决标定问题，并且不需要设置初值。本方法包括两个部分：利用运动选择算法选择合适的手眼运动数据；非最小化最优化算法，即利用张量积构造手眼标定系统的线性方程，进而得到最优化代价方程，利用特征值计算求得特征矩阵(y)的最优解。本发明提供的方法采用特征值计算代替传统的二范数计算，减少了计算量，而且避免了初始值选取不恰当而造成的问题。在噪声环境下，该方法的旋转误差和平移误差均能够满足一般应用要求。

说明书

技术领域

本发明涉及一种基于非最小化最优化算法的机器人手眼标定方法，涉及工业机器人视觉 标定技术领域，可以用于机器人手术等技术领域。

背景技术

机器人手眼标定是机器视觉中一个基本问题。机器人进行的具体操作主要是通过位于机 器人手臂最后一节的末端执行器完成的，末端执行器也就是机器人的“手”，为了对机器人的 操作进行定位，末端执行器上通常固定有相机，作为机器人的“眼”。一般来说，相机只能拍 摄镜头对面的物体，而不能拍摄自身所在的末端执行器，因此不能用相机直接拍摄的方法来 确定机器人手的位置。机器人手眼标定就是测量相机和末端执行器之间的相对位置和方向关 系，以便确定机器人末端执行器的实际位置。手眼标定的精确性部分决定了机器视觉应用于 机器人时的精度，因此在实际应用中十分重要。

手眼关系的计算一般要求机器人至少独立运动2次，而且2次运动的旋转轴不得平行。 进行在线手眼标定时，机器人运动是由具体应用确定的，而并非专门为手眼标定而设计，所 以在机器人运动时采集到的运动数据就很可能有不适于手眼标定的情形。另外，采样运动中 旋转角度过小或者平移过大，或者2次运动的旋转轴之间的夹角过小时，都会使最后标定结 果产生较大的误差。为了避免上述情况，实际应用中需要进行运动选择，以便挑选合适的运 动数据进行手眼标定运算。

发明内容

本发明的目的就是为解决上述问题，提供一种基于非最小化最优化算法的机器人手眼标 定方法，它具有利用特征值计算求解手眼标定方程。该方法能够简化计算过程，并且使手眼 标定结果更加准确。

为了实现上述目的，本发明采用如下技术方案：

一种基于非最小化最优化算法的机器人手眼标定方法，包括如下步骤：

步骤(1)：利用运动选择算法，筛选机器人末端执行器的运动数据；计算每组运动数据 对应的从相机到世界坐标系的变换矩阵A和机器人基部到末端执行器的变换矩阵B；

步骤(2)：利用步骤(1)的从相机到世界坐标系的变换矩阵A和机器人基部到末端执 行器的变换矩阵B，同时假设待测的从相机到末端执行器的手眼标定矩阵为X，利用变换矩 阵A、变换矩阵B和手眼标定矩阵X构造手眼标定方程；

步骤(3)：利用非最小化最优化算法对步骤(2)的手眼标定方程进行求解，得到非最小 化最优化解，然后利用该非最小化最优化解得到手眼标定矩阵X。

所述步骤(1)的利用运动选择算法，筛选机器人末端执行器的运动数据的步骤为：

从机器人末端执行器上的摄像机记录的运动中获取运动数据集，根据手眼标定需求，设 定最优化误差阈值；

对于运动数据集中的第一个运动，构建第一个运动数据矩阵；

由第一个运动数据矩阵计算出第一个最优化矩阵；

求解第一个最优化矩阵的特征值，找到最小的特征值及最小的特征值对应的特征向量；

判断最小特征值是否大于最优化误差阈值，如果大于，则从运动数据集和第一个运动数 据矩阵中移除此次运动，否则保留；

随后选取下一个运动，重复计算步骤直到获得适当数量的运动为止。

所述步骤(1)的利用运动选择算法，筛选机器人末端执行器的运动数据的步骤为：

步骤(1-1)：从摄像机记录的运动中获取N个运动，N＞2，N为整数；构建运动数据集 C和d，C＝{C_i}，d＝{d_i}，i＝1，2，…，N。根据手眼标定精度需求，设定最优化误差阈值e；C_i的 参数含义是运动数据集C的第i组运动数据；d_i的参数含义是运动数据集d的第i组运动数据；

步骤(1-2)：对于运动数据集中第1个运动，i＝1，构建第1个运动数据矩阵 U⁽¹⁾＝[C₁-d₁]；C₁的参数含义是运动数据集C的第1组运动数据，d₁的参数含义是运动数 据集d的第1组运动数据；

步骤(1-3)：由第一个运动数据矩阵U⁽¹⁾计算第一个最优化矩阵V⁽¹⁾，

$V^{\left(1\right)}=\frac{1}{12N}{\Sigma }_{i=1}^{12N}{u_1}^T{u}_1,$

其中，u₁代表U⁽¹⁾的第1行，上标T表示转置；

求解V⁽¹⁾的最小特征值λ⁽¹⁾，以及最小的特征值对应的特征向量

步骤(1-4)：如果λ⁽¹⁾＞e，则从C，d和U⁽¹⁾中移除第1个运动，否则保留；

步骤(1-5)：对于第i个运动，构建第i个运动数据矩阵$U^{\left(i\right)}=\left(\begin{array}{c}{U}^{(i-1)}\\ \left(\begin{array}{cc}{C}_i& -d_i\end{array}\right)\end{array}\right),$以及第i个最 优化矩阵$V^{\left(i\right)}=\frac{1}{12N}{\Sigma }_{i=1}^{12N}{u_i}^T{u}_i;$

求解V⁽ⁱ⁾的最小特征值λ⁽ⁱ⁾以及对应的特征向量，如果λ⁽ⁱ⁾＞e，则从C，d和U⁽ⁱ⁾中移除第 i个运动，否则保留；U^(i-1)的参数含义是第i-1个运动数据矩阵；u_i的参数含义是代表U⁽ⁱ⁾的 第i行；u_i^T是u_i的转置；

步骤(1-6)：反复进行步骤(1-5)计算，直到获得足够数量的运动记录为止。

所述步骤(3)的步骤为：

步骤(3-1)：通过引入张量积和列向量算子，将步骤(2)的手眼标定方程变形，得到一 个线性的最优化方程；

步骤(3-2)：利用线性的最优化方程，定义最优化矩阵V，并利用V构造最优化代价方 程J(y)；

步骤(3-3)：将最优化代价方程转换为估算方程，对估算方程求解特征值，最小特征值 对应的特征向量，即是最优化代价方程的非最小化最优化解；利用非最小化最优化解，即可 得到手眼标定矩阵X。

所述步骤(1)中，从相机到世界坐标系的变换矩阵A和机器人基部到末端执行器的变 换矩阵B，形式分别为：

$A_i=\left(\begin{array}{cc}{R}_{A_i}& {\overrightarrow{t}}_{A_i}\\ 0^T& 1\end{array}\right),B_i=\left(\begin{array}{cc}{R}_{B_i}& {\overrightarrow{t}}_{B_i}\\ 0^T& 1\end{array}\right);$

其中，R代表旋转矩阵，代表平移分量，下标i表示第i个运动。

A_i的参数含义是从相机到世界坐标系的第i个变换矩阵；

的参数含义是从相机到世界坐标系的第i个变换矩阵的旋转矩阵；

的参数含义是从相机到世界坐标系的第i个变换矩阵的平移分量；

0^T的含义是0矩阵的转置；

B_i的参数含义是机器人基部到末端执行器的第i个变换矩阵；

的参数含义是机器人基部到末端执行器的第i个变换矩阵的旋转矩阵；

的参数含义是机器人基部到末端执行器的第i个变换矩阵的平移分量。

所述步骤(2)中，待测的从相机到末端执行器的手眼标定矩阵X为：

$X=\left(\begin{array}{cc}{R}_X& {\overrightarrow{t}}_X\\ 0^T& 1\end{array}\right);$

其中，

R_X的参数含义是待测的从相机到末端执行器的旋转矩阵；

的参数含义是待测的从相机到末端执行器的平移分量；

所述步骤(2)中，手眼标定方程为：

$\left(\begin{array}{cc}{I}_9-R_{A_i}\otimes R_{B_i}& 0_{9\times 3}\\ I_3\otimes {\overrightarrow{t}}_{B_i}^T& I_3-R_{A_i}\end{array}\right)\left(\begin{array}{c}vec\left(R_X\right)\\ {\overrightarrow{t}}_X\end{array}\right)=\left(\begin{array}{c}{0}_{9\times 3}\\ {\overrightarrow{t}}_{A_i}\end{array}\right)$

其中，表示张量积，vec表示列向量算子，I₉表示9×9的单位矩阵，0_9×3表示9×3的零 矩阵；0_9×1的参数含义是9×1的零矩阵；I₃的参数含义是3×3的单位矩阵；

的参数含义是机器人基部到末端执行器的第i个变换矩阵的平移分量的转置；

vec(R_X)的参数含义是R_X的列向量算子；

R_X表示X的旋转矩阵；

如果M是m×n的矩阵，m，n∈R，元素由M_ij表示，1≤i≤m，1≤j≤n，P是任意矩阵， 那么，张量积表示为：

列向量算子vec(M)＝[M₁₁，…，M_1n，…，M₂₁，…，M_mn]^T；

其中，M_mn的参数含义是矩阵M的第m行第n列的元素。

所述步骤(3-1)的步骤为：

令

$x=\left(\begin{array}{c}vec\left(R_X\right)\\ {\overrightarrow{t}}_X\end{array}\right)$

$C_i=\left(\begin{array}{cc}{I}_9-R_{A_i}\otimes R_{B_i}& 0_{9\times 3}\\ I_3\otimes {\overrightarrow{t}}_{B_i}^T& I_3-R_{A_i}\end{array}\right)$

$d_i=\left(\begin{array}{c}{0}_{9\times 1}\\ {\overrightarrow{t}}_{A_i}\end{array}\right)$

利用手眼标定方程，得到一个线性的最优化方程：

C_ix＝d_i。

其中，x表示一个矢量。

所述步骤(3-2)的步骤为：

将线性最优化方程变形为：

$\left(\begin{array}{cc}{C}_i& -d_i\end{array}\right)\left(\begin{array}{c}x\\ 1\end{array}\right)=0_{12\times 1}$

构造运动数据矩阵U：

$U=\left(\begin{array}{cc}{C}_1& -d_1\\ \begin{array}{c}.\\ .\\ .\end{array}& \begin{array}{c}.\\ .\\ .\end{array}\\ C_N& -d_N\end{array}\right)$

令

$y=\left(\begin{array}{c}x\\ 1\end{array}\right)$

构造最优化代价方程J(y)：

$J\left(y\right)=\frac{1}{12N}\underset{i=1}{\overset{12N}{\Sigma }}{||u_{i}y||}^2=\frac{1}{12N}\underset{i=1}{\overset{12N}{\Sigma }}{y}^T{u_i}^T{u}_{i}y=y^T\left(\frac{1}{12N}\underset{i=1}{\overset{12N}{\Sigma }}{u_i}^T{u}_i\right)y=y·Vy$

其中，u_i代表U的第i行，最优化矩阵V定义为：

$V=\frac{1}{12N}{\Sigma }_{i=1}^{12N}{u_i}^T{u}_i;$

其中，0_12×1表示12×1的零矩阵；

C_N的参数含义是运动数据集C的第N组运动数据；

d_N的参数含义是运动数据集d的第N组运动数据；

y的参数含义是最优化矩阵V的最小特征值λ对应特征向量；

y^T的参数含义是最优化矩阵V的最小特征值λ对应特征向量的转置；

所述步骤(3-3)的步骤为：

为了使J→0，把估算方程写为：

Vy＝λy

由于运动数据矩阵U中包含噪声干扰，最优化矩阵V的秩恒大于12，最优化矩阵V的最小 特征值λ对应特征向量y，y即是代价方程J的非最小化最优化解；得到矩阵V之后，得到y的估 计值

$\hat{y}~\left(\begin{array}{c}x\\ 1\end{array}\right)$

由此解出x，进而可得到手眼标定矩阵X。

本发明的有益效果：与现有技术相比，本发明的机器人手眼标定算法，可以测量固定在 机器人手臂末端的相机和机器人末端执行器之间的相对位置和方向关系，实现方法简单，计 算量小，而且不需要考虑初始值的设定，结果准确，计算耗时短，实用性强，可以广泛应用 于医学、工业等方面的机器人测量。

附图说明

图1是手眼标定图示；

图2是本发明的整体方法流程图；

图3是运动选择算法流程图。

具体实施方式

下面结合附图与实施例对本发明作进一步说明。

A非最小化最优化

系统如图1所示，机器人基部通过第一旋转轴与机器人手臂连接，所述机器人手臂包括 由第二旋转轴连接的第一节手臂和第二节手臂，第二节手臂末端附有末端执行器，末端执行 器上固定有摄像机。机器人通过手臂进行运动，末端执行器随之移动到相应位置，运动时基 部保持固定。

如图2所示，

1.确定从相机到世界坐标系的变换矩阵A_i和机器人基部到末端执行器的变换矩阵B_i。

1)利用运动选择算法，选取机器人末端执行器的N个运动；

2)对于第i个运动，i＝1，2，…，N，测量并计算从相机到世界坐标系的变换矩阵A_i，以及 从机器人基部到末端执行器的变换矩阵B_i，形式分别为：

$A_i=\left(\begin{array}{cc}{R}_{A_i}& {\overrightarrow{t}}_{A_i}\\ 0^T& 1\end{array}\right),B_i=\left(\begin{array}{cc}{R}_{B_i}& {\overrightarrow{t}}_{B_i}\\ 0^T& 1\end{array}\right)$

其中R代表旋转矩阵，代表平移分量。

3)设待测的从相机到末端执行器的变换矩阵为：

$X=\left(\begin{array}{cc}{R}_X& {\overrightarrow{t}}_X\\ 0^T& 1\end{array}\right)$

2.构造手眼标定方程

$\left(\begin{array}{cc}{I}_9-R_{A_i}\otimes R_{B_i}& 0_{9\times 3}\\ I_3\otimes {\overrightarrow{t}}_{B_i}^T& I_3-R_{A_i}\end{array}\right)\left(\begin{array}{c}vec\left(R_X\right)\\ {\overrightarrow{t}}_X\end{array}\right)=\left(\begin{array}{c}{0}_{9\times 1}\\ {\overrightarrow{t}}_{A_i}\end{array}\right)$

其中表示张量积，vec表示列向量算子。

如果M是m×n的矩阵，P是p×o矩阵，那么张量积可以表示为：

列向量算子vec(M)＝[M₁₁，…，M_1n，…，M₂₁，…，M_mn]^T

3.利用非最小化最优化算法，求解x的最优解。

1)令

$x=\left(\begin{array}{c}vec\left(R_X\right)\\ {\overrightarrow{t}}_X\end{array}\right)$

$C_i=\left(\begin{array}{cc}{I}_9-R_{A_i}\otimes R_{B_i}& 0_{9\times 3}\\ I_3\otimes {\overrightarrow{t}}_{B_i}^T& I_3-R_{A_i}\end{array}\right)$

$d_i=\left(\begin{array}{c}{0}_{9\times 1}\\ {\overrightarrow{t}}_{A_i}\end{array}\right)$

利用手眼标定方程，得到一个线性的最优化方程：

C_ix＝d_i

2)将线性最优化方程变形为：

$\left(\begin{array}{cc}{C}_i& -d_i\end{array}\right)\left(\begin{array}{c}x\\ 1\end{array}\right)=0_{12\times 1}$

令

$U=\left(\begin{array}{cc}{C}_1& -d_1\\ \begin{array}{c}.\\ .\\ .\end{array}& \begin{array}{c}.\\ .\\ .\end{array}\\ C_N& -d_N\end{array}\right),y=\left(\begin{array}{c}x\\ 1\end{array}\right)$

构造最优化代价方程：

$J\left(y\right)=\frac{1}{12N}\underset{i=1}{\overset{12N}{\Sigma }}{||u_{i}y||}^2=\frac{1}{12N}\underset{i=1}{\overset{12N}{\Sigma }}{y}^T{u_i}^T{u}_{i}y=y^T\left(\frac{1}{12N}\underset{i=1}{\overset{12N}{\Sigma }}{u_i}^T{u}_i\right)y=y·Vy$

其中u_i代表U的第i行。最优化矩阵V定义为：

$V=\frac{1}{12N}\underset{i=1}{\overset{12N}{\Sigma }}{u_i}^T{u}_i$

3)为了使J→0，把估算方程写为：

Vy＝λy

由于U中包含噪声干扰，矩阵V的秩恒大于12，V的最小特征值λ对应特征向量y，y即是代 价方程J的非最小化最优化解。得到矩阵V之后，可以得到y的估计值：

$\hat{y}~\left(\begin{array}{c}x\\ 1\end{array}\right)$

由此可以解出x。

如图3所示，B运动选择(Motion Selection)

在非最小化最优化过程中，运动数据选择比较简单。运动选择需要事先设定一个最优化 误差阈值e，然后对摄像机记录的一系列运动依次进行筛选，选出适合进行手眼标定的运动。 运动选择的算法如下：

算法：

1.从摄像机记录的运动中获取一组运动数据集C＝{C_i，i＝1，2，…}，d＝{d_i，i＝1，2，…}， 并设定阈值e。

2.对于第1个运动，i＝1，构建运动数据矩阵U⁽¹⁾＝[C₁-d₁]。

3.由U⁽¹⁾计算最优化矩阵V⁽¹⁾，求解V⁽¹⁾的最小特征值λ⁽¹⁾，以及 对应的特征向量

4.如果λ⁽¹⁾＞e，则从C，d和U⁽¹⁾中移除第1个运动，否则保留。

5.对于第i个运动，i＝2，3…，构建运动数据矩阵$U^{\left(i\right)}=\left(\begin{array}{c}{U}^{(i-1)}\\ \left(\begin{array}{cc}{C}_i& -d_i\end{array}\right)\end{array}\right),$以及 最优化矩阵求解V⁽ⁱ⁾的最小特征值λ⁽ⁱ⁾以及对应的特征向量，如果 λ⁽ⁱ⁾＞e，则从C，d和U⁽ⁱ⁾中移除第i个运动，否则保留。

6.反复进行第5步计算，直到获得足够数量的运动记录为止。

上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限 制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付 出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。