0 引言

对传统的手眼机器人系统, 获取摄像机坐标系与机器人末端执行器之间的相对位置关系是非常必要的。这种相对位置关系不可测量, 需要使用相应的算法求解, 求解的过程称为手眼标定^[1]。目前, 手眼标定问题已得到了广泛的研究。文献[2]提出了一种高效的线性算法求解手眼标定问题, 并且指出最少需要2个具有非平行旋转坐标轴的旋转运动才可以完成求解。文献[3]提出了一种采用四元数来表示旋转的算法, 并且给出了一个基于SVD的线性解决方案。文献[4]基于四元数的最小二乘法, 对手眼标定提出了一种八维空间的求解方法。文献[5]利用螺旋运动理论分析手眼标定, 提出了手眼几何的约束条件。基于文献[5]的工作, 文献[6]提出了一种基于对偶四元数的手眼标定算法, 该算法结合SVD算法可以同时求解旋转和平移参数。文献[7]提出了一种基于螺旋运动的算法, 并推导出了与文献[6]相似的求解方程。考虑到随机噪声的影响, 许多研究人员都会在手眼标定算法中引入最优化方法。文献[8]对旋转和平移参数的求解都采用非线性优化, 使用与弗罗贝尼乌斯范数相似的代价函数进行优化。文献[9]也提出了一种相似的优化策略。文献[10]在手眼标定中引入了正则坐标系, 简化了优化参数的特征描述。文献[11]对采用四元数的旋转参数进行了非线性优化, 同时也介绍了在非线性优化中同时求解机器人-世界和手-眼的位置关系的方法^[12]。文献[13]针对手眼标定提出了一种基于新的SE(3)度量的非线性优化算法, 该新度量有自动最优化权重的能力。虽然以上研究工作取得了很好的结果, 但是涉及的优化过程都是局部优化, 可能由于迭代初值选取不当而陷入局部最小化陷阱。为此文献[14]提出了一种基于凸优化的手眼标定算法, 该算法是一种全局优化算法, 不会陷入局部最小值陷阱, 但优化过程需要较多的迭代步数, 运算复杂度较大。本文提出了一种无需迭代计算和初始值估计的非最小优化的方法。与其它使用局部、全局优化的方法相比, 这种方法计算复杂度低, 运算时间短, 并表现出了较高的精确性和可靠性。

1 手眼标定问题描述

手眼标定问题通常使用齐次矩阵来描述, 如图 1所示, A_i(i=1, 2)表示从摄像机坐标系到世界坐标系的第i个相对位置关系矩阵, B_i(i=1, 2)表示从机器人基坐标到末端执行器坐标系的第i个姿势矩阵, X表示机器人末端执行器到摄像机坐标系的相对位置关系矩阵, 可以得到

${\boldsymbol{A}_1}\boldsymbol{X}{\boldsymbol{B}_1} = {\boldsymbol{A}_2}\boldsymbol{X}{\boldsymbol{B}_2}。$

(1)

在式(1)的两边分别乘以A₁^-1和B₂^-1, 可以得到

$\boldsymbol{A}_1^{ - 1}{\boldsymbol{A}_2}\boldsymbol{X} = \boldsymbol{X}{\boldsymbol{B}_1}\boldsymbol{B}_2^{ - 1}。$

(2)

式(2)即为文献[1]首先提出的著名的手眼标定方程

$\boldsymbol{AX} = \boldsymbol{XB},$

(3)

式中:X为手眼标定参数矩阵, 在立体视觉的机器人系统中, X表示机器人末端执行器到基准摄像机坐标系的相对位置关系; A=A₂A₁^-1, B=B₂^-1B₁。方程里每个矩阵都是齐次的, 其形式为:

$D = \left[{\begin{array}{*{20}{c}} {{\boldsymbol{R}_D}}&{{\boldsymbol{t}_D}} \\ {{0^{\text{T}}}}&1 \end{array}} \right],$

式中:矩阵R_D和t_D分别表示齐次矩阵D的旋转矩阵和平移矢量。因此, 公式(3)也可以写作

$\boldsymbol{{R_A}{R_X} = {R_X}{R_B}},$

(4)

$\boldsymbol{{R_A}{t_X} + {t_X} = {R_X}{t_B} + {t_X}}。$

(5)

之前的手眼标定算法^[8-13]全都使用了局部最优化方法, 如Levenberg-Marquardt算法等。目标函数格式通常为：

$J = \sum\limits_{i = 1}^N {\left( {{{\left\| {{\boldsymbol{r}_i}} \right\|}^2} + \lambda {{\left\| {{\boldsymbol{t}_i}} \right\|}^2}} \right)} ,$

式中: r_i是旋转参数误差项矩阵; t_i是平移参数误差项矩阵; N为误差项的个数; λ是权重系数；‖·‖是欧氏距离的度量。在计算机视觉理论中, 将式(4)中的平方和函数最小化是一个比较繁琐的非凸优化过程^[15-18], 通常有多个极小值, 往往会陷入局部最小值陷阱。这也是以式(4)为目标函数的非线性优化过程需要事先给出一个合适的初始值的原因。平方和目标函数实际上是一种L₂-范数误差公式。对于L₂-范数目标函数的最小化, 大多数的手眼标定算法(无论局部还是全局优化)都是以非线性捆绑调整的方式来执行的, 这种方法非常耗费运算时间。文献[19]提出了基于非最小化的统计优化理论, 并证明其性能优于最小化优化。非最小化优化理论主要通过构造线性矩阵方程而诱导出相应的代价函数, 通过代价函数推导出特征估计矩阵, 根据特征计算结果得到优化计算的估计值。受到文献[19]的启发, 本文在手眼标定中引入了基于非最小化优化理论。

2 基于非最小化优化的手眼标定

首先, 引入张量积运算符⊗和列向量运算符vec^[20]。如果M是一个m×n矩阵, P是一个p×o矩阵, M⊗P的张量积定义为^[21]:

$\boldsymbol{M} \otimes \boldsymbol{P} = \left[{\begin{array}{*{20}{c}} {{\boldsymbol{M}_{11}}\boldsymbol{P}}& \cdots &{{\boldsymbol{M}_{1n}}\boldsymbol{P}} \\ \vdots & \ddots & \vdots \\ {{\boldsymbol{M}_{m1}}}& \cdots &{{\boldsymbol{M}_{mn}}\boldsymbol{P}} \end{array}} \right],$

(6)

列向量运算符定义为:

${\text{vec}}\left( M \right) = {\left[{{M_{11}},\cdots ,{M_{1n}},\cdots ,{M_{21}},\cdots ,{M_{mn}}} \right]^{\text{T}}}。$

(7)

根据式(6)和(7), 可以将公式(4)和(5)改写为

$\left[{\begin{array}{*{20}{c}} {{\boldsymbol{I}_9} - {\boldsymbol{R}_{{A_i}}} \otimes {\boldsymbol{R}_{{B_i}}}}&{{0_{9 \times 3}}} \\ {{\boldsymbol{I}_3} \otimes \boldsymbol{t}_{{B_i}}^{\text{T}}}&{{\boldsymbol{I}_3} - {\boldsymbol{R}_{{A_i}}}} \end{array}} \right]\left[{\begin{array}{*{20}{c}} {{\text{vec}}\left( {{\boldsymbol{R}_{\text{X}}}} \right)} \\ {{\boldsymbol{t}_{\text{X}}}} \end{array}} \right] = \left[{\begin{array}{*{20}{c}} {{0_{9 \times 1}}} \\ {{\boldsymbol{t}_{{A_i}}}} \end{array}} \right],$

假定$\boldsymbol{x} = \left[{\begin{array}{*{20}{c}} {{\text{vec}}\left( {{\boldsymbol{R}_{\text{X}}}} \right)} \\ {{\boldsymbol{t}_{\text{X}}}} \end{array}} \right]$, ${C_i} = \left[{\begin{array}{*{20}{c}} {{\boldsymbol{I}_9} - {\boldsymbol{R}_{{A_i}}} \otimes {\boldsymbol{R}_{{B_i}}}}&{{0_{9 \times 3}}} \\ {{\boldsymbol{I}_3} \otimes \boldsymbol{t}_{{B_i}}^{\text{T}}}&{{\boldsymbol{I}_3} - {\boldsymbol{R}_{{A_i}}}} \end{array}} \right]$, 可以得到

${\boldsymbol{C}_i}\boldsymbol{x} = {\boldsymbol{d}_i},$

(8)

式(8)是一个线性函数, 可以转换成

$\left[{{\boldsymbol{C}_i}\;\; - {\boldsymbol{d}_i}} \right]\left[{\begin{array}{*{20}{c}} \boldsymbol{x} \\ 1 \end{array}} \right] = {0_{12 \times 1}},$

(9)

将所有式(9)排列起来, 可以得到

$\underbrace {\left[{\begin{array}{*{20}{c}} {{\boldsymbol{C}_1}}&{ - {\boldsymbol{d}_1}} \\ \vdots & \vdots \\ {{\boldsymbol{C}_N}}&{ - {\boldsymbol{d}_N}} \end{array}} \right]}_U\underbrace {\left[{\begin{array}{*{20}{c}} \boldsymbol{x} \\ 1 \end{array}} \right]}_y = {0_{12N \times 1}},$

(10)

根据式(10), 可以构造非最小化优化的代价函数

$\begin{gathered} J\left( \boldsymbol{y} \right) = \frac{1}{{12N}}\sum\limits_{i = 1}^{12N} {{{\left\| {{\boldsymbol{u}_i}\boldsymbol{y}} \right\|}^2} = \frac{1}{{12N}}\sum\limits_{i = 1}^{12N} {{\boldsymbol{y}^{\text{T}}}\boldsymbol{u}_i^{\text{T}}{\boldsymbol{u}_i}\boldsymbol{y} = } } \\ {\boldsymbol{y}^{\text{T}}}\underbrace {\left( {\frac{1}{{12N}}\sum\limits_{i = 1}^{12N} {\boldsymbol{u}_i^{\text{T}}{\boldsymbol{u}_i}} } \right)\boldsymbol{y}}_V = \boldsymbol{y} \cdot \boldsymbol{Vy},\\ \end{gathered} $

其中, u_i是矩阵U的第i行。为了使J→0, 可以写出其估计方程^[16]

$\boldsymbol{Vy = \lambda y}。$

(11)

由于U的噪声干扰, 矩阵V的秩一般大于12, 矩阵V的最小特征值λ对应的特征向量y可以看作是代价函数J的非最小化最优解。得到的向量y的估计值

$\boldsymbol{\hat y} \propto = \left[{\begin{array}{*{20}{c}} \boldsymbol{x} \\ 1 \end{array}} \right].$

(12)

${\boldsymbol{\hat y}}$与[x  1]^T相差一个比例因子, 所以根据式(12), 可以计算得到向量y。

3 扰动分析

尝试通过使用定性的扰动分析对估计方程(11)进行误差分析, 有

${\boldsymbol{V}_{\text{e}}}{\boldsymbol{y}_{\text{e}}} = {\lambda _{\text{e}}}{\boldsymbol{y}_{\text{e}}},$

其中：V_e=V+Δ_V, 由系数矩阵V和噪声扰动矩阵Δ_V组成；同理有y_e=y+Δ_y和λ_e=λ+Δ_λ。得到

$\left( {\boldsymbol{V} + {\Delta _\boldsymbol{v}}} \right)\left( {\boldsymbol{y} + {\Delta _\boldsymbol{y}}} \right) = \left( {\lambda + {\Delta _\lambda }} \right)\left( {\boldsymbol{y} + {\Delta _\boldsymbol{y}}} \right).$

(13)

在高斯白噪声而无其它噪声情况下(λ=0), 式(13)的一阶等式可以表示为

$\boldsymbol{V{\Delta _y} + {\Delta _V}y = {\Delta _\lambda }y},$

(14)

将等式(14)两边分别关于y做内积, 得到

$\boldsymbol{y \cdot V{\Delta _y} + y \cdot {\Delta _V}y = y \cdot {\Delta _\lambda }y},$

(15)

由于u_i·y=0, y·VΔ_y以及y·Δ_Vy都为0。根据式(15), 得到

$\boldsymbol{{\Delta _y} = - {V^ + }{\Delta _V}y},$

其中,V⁺V=I-yy^T, 显然E{Δ_y}=0。由上面的扰动分析可知, 数据扰动对本文提出的算法影响很小, 并且随着机器人运动次数的增多而变得可以忽略不计。

4 运动选择

由等式(11)可知, 如果给优化误差预定义一个阈值, 那么标定运动数据的选择将非常容易实现。运动选择算法^[22]如下:

算法1:

给定:运动数据集合C=｛C_i, i=1, 2, …｝、d=｛d_i, i=1, 2, …｝以及阈值e。

对于第i个运动,

构造${\boldsymbol{U}^{\left( i \right)}} = \left[{\begin{array}{*{20}{c}} {{\boldsymbol{U}^{i - 1}}} \\ {\left[{\begin{array}{*{20}{c}} {{\boldsymbol{C}_i}}&{ - {\boldsymbol{d}_i}} \end{array}} \right]} \end{array}} \right];$

由U⁽ⁱ⁾计算V⁽ⁱ⁾;

解出V⁽ⁱ⁾的最小特征值λ⁽ⁱ⁾和对应特征向量${{\hat y}^{\left( i \right)}}$;

如果λ⁽ⁱ⁾>e, 则从C, d和U⁽ⁱ⁾中删除第i个运动;

直至λ⁽ⁱ⁾≤e;

本研究所述运动选择的算法具有直接的几何和代数意义, 所以很容易理解和实现。

5 试验

分别使用仿真数据和实际数据进行试验以测试本研究提出的手眼标定方法, 并与3种已有方法进行比较。第一种方法是由文献[11]提出的方法。该方法使用了迭代非线性最小化算法, 需要适当的初始值, 为此使用文献[6]的方法估计初始值。第二种方法是由文献[8]提出的单级迭代算法。第三种方法是由文献[14]提出的基于凸优化的方法。在下面的试验和图表中, 本研究的方法将标注为“Ours”, 其他3种方法分别标注为“HDD”, “ZS”, “ZCP”。

5.1 仿真试验

在已知手眼关系(R_x, t_x)的情况下, 人工生成一系列的机器人手眼运动数据, 并在这些运动数据中添加标准差为σ的高斯噪声。在0～0.1调整σ的取值, 每个取值下所有手眼标定方法都运行100次, 并输出手眼参数的估计矩阵$\boldsymbol{{{\hat R}_x}}$和$\boldsymbol{{{\hat t}_x}}$。为了便于评价, 给出了旋转和平移误差

$\begin{gathered} {e_{\text{R}}} = \sqrt {\frac{1}{N}\sum\limits_{i = 1}^N {{{\left\| {{\boldsymbol{R}_{\text{X}}} - {{\boldsymbol{\hat R}}_{\text{X}}}} \right\|}^2}} } ,\hfill \\ {e_{\text{t}}} = \frac{{\sqrt {\frac{1}{N}\sum\limits_{i = 1}^N {{{\left\| {{\boldsymbol{t}_{\text{X}}} - {{\boldsymbol{\hat t}}_{\text{X}}}} \right\|}^2}} } }}{{{{\left\| {{\boldsymbol{t}_{\text{X}}}} \right\|}^2}}}。 \hfill \\ \end{gathered} $

本研究的算法在不同噪声等级下与HDD、ZS、ZCP 3种方法进行了比较, 如图 2所示。

由图 2可知:尽管HDD方法在旋转和平移方面误差更小, 但是考虑到HDD方法对初始值的依赖性, 在初始值不合适情况下, HDD方法不会取得较好的优化结果。出于同样的原因, ZS算法效果最差。本文提出的方法性能较好, 优于ZCP方法和ZS算法, 这主要是由于估计方程(14)中的矩阵V减弱了噪声的影响。

仿真试验使用MATLAB V7.8编写所有的算法。计算机系统配置如下:Intel core-i5 CPU 2.90 GHz, 4 G RAM。表 1记录了所有算法运行1 000遍的时间。从表 1可以看出, 本研究的算法用时约为ZS/HDD方法时间的1/10, 不到ZCP方法用时的1/3。试验结果表明, 本研究的算法用时大大小于其他3种算法。在充分考虑误差和时间两方面性能的情况下, 本研究的算法可以作为手眼标定方法的最好选择。

5.2 真实试验

在实际试验中, 使用了一个末端装有2个CCD摄像机的MOTOMAN HP3机器人(如图 3所示)。通过机器人姿态的改变, 摄像机相对标定物移动了16个不同的位置, 在每一个位置通过摄像机记录下标定物图像, 根据每幅图像提取出标定物的特征点集合, 由每幅图像的特征点集合计算出摄像机坐标系到世界坐标系的一个齐次相对位置关系。从第二幅图像开始, 每一幅图像对应的齐次相对位置关系与其前一幅图像对应的齐次关系的逆矩阵相乘, 计算出15个摄像机的运动数据A_i(i=1, …, 15), 这样计算可以有效地避免数据的误差传递。与摄像机的15个运动齐次矩阵相对应, 从机器人控制器里读取机器人末端执行器的15个运动齐次矩阵B_i(i=1, …, 15)。根据摄像机和机器人1～8次的运动数据, 可以运用4种手眼标定算法, 得到机器人系统的手眼关系矩阵X。然后, 根据机器人运动数据B_i(i=9, …, 15)来估计摄像机的运动

${{\boldsymbol{\hat A}}_i} = \boldsymbol{X}{\boldsymbol{B}_i}{\boldsymbol{X}^{ - 1}},i = 9,\cdots ,15.$

比较${{\boldsymbol{\hat A}}_i}$和A_i(i=9, …, 15), 并计算所有运动的旋转和平移误差。真实试验误差结果如图 4所示。由图 4可知, 与其它3种方法相比, 本文的算法旋转误差最小, 平移误差仅仅稍逊于HDD方法。由于HDD方法中采用了文献[6]的方法估计初始值, 该处估值方法实际上也采用了二位特征估计的方法计算初值, 这个思路跟本文方法有一定的类似之处, 所以真实试验和仿真试验的平移误差有少许区别。另外, 真实试验中的噪声分布不可能与仿真试验中高斯噪声的假设完全符合, 摄像机姿势估计^[23]产生的不确定性以及局部优化算法^[24-25]产生的多次迭代效应, 都会导致真实试验结果与仿真结果有所不同。

5.3 医学机器人应用

鉴于非最小化优化手眼标定算法在试验中表现出的优越性能, 将其应用到ViKY手术机器人系统(见图 5)的手眼标定中。ViKY是一种腹腔镜手术机器人, 在其末端装有奥林巴斯内窥镜, 并且可以通过内窥镜跟踪手术工具的位置(视觉跟踪/伺服)。利用非最小化优化计算出的手眼标定结果, ViKY机器人进行3 min的视觉伺服操作。计算了每一帧图像的跟踪误差, 第1～1 000帧的误差结果如图 6所示。从图 6中可以看到, 视觉跟踪的误差曲线大致符合阻尼振荡过程, 这表示伺服过程有很好的鲁棒性, 因此该手眼标定方法适用于实际机器人系统的标定。

6 结语

本研究提出了一种基于非最小化优化的手眼标定方法。与其它算法不同的是, 该算法使用了特征计算而非L₂-范数优化, 并采用估计方程实现手眼方程的求解。这种算法不是迭代优化的, 不需要初始值, 所以与其他算法相比, 计算起来更简单。试验结果表明:这种算法具有较强的鲁棒性和有效性。综合考虑标定的误差和运行时间, 这种方法在实际应用中具有明显的优越性, 适合于各种机器人系统的标定, 特别是医学机器人的标定。