席晓亮，杨 洋，程 勇，陆 坤

(1.中国科学院等离子体物理研究所，合肥 230031；2.中国科学技术大学，合肥 230026)

0 引言

随着机器人在工业范围的普及，一系列具有机器视觉感知功能的智能机器人把工业自动化带到了一个新的高度，视觉技术已广泛应用于机器人领域研究，如实时定位[1]，视觉伺服[2]，控制[3]和校准[4]。在这类机器视觉应用到机器人过程中，视觉传感器相对于机械臂的手眼关系的标定十分重要，即手眼关系的标定是实现机械臂视觉引导和物体抓取的重要基础。

Tsai和Lenz提出了两步法的手眼关系标定法，将旋转矩阵和平移向量进行分步求解[5]；Ma提出了共同标定法，但系统的精度无法得到保证[6]；Ruda提出了基于四元数工具用非线性优化的工具求解手眼关系[7]；也有文献中提出了基于最大似然估计的非线性手眼标定方法[8]。非线性手眼标定法相对复杂，实用性较弱。

在传统的手眼标定中，一般人工给机器人摆位姿，然后人工判断当前位姿下摄像机采集的图像是否有效，如果有效抓取当前位姿下的图像，然后再去摆下一个位姿。一旦发生手眼关系意外变动的情况，如连接件松动、镜头损坏、焦距调整等，就需要重新人工标定手眼关系，效率较低。采用人工拖拽法，过程枯燥繁复效率低[9]；手眼关系自动化标定的实现原型，通常结果误差较大[10]；而发展标定姿态自动生成的算法，会带来的各种噪声很大程度影响了标定结果[11]。因此，研究一种可靠、精准的手眼关系自动化标定算法很有必要。

在本文中，对Eye-in-Hand系统提出一种手眼标定算法。在人工拖拽获得初始手眼关系的基础上，建立空间姿态约束模型，生成等分空间的位姿点，引入误差评估机制，对异常样本进行剔除。实验表明，引入异常值剔除后，能够有效减少收敛所需的样本数量和提升标定精度：平移误差稳定在0.4mm，旋转角度误差最终稳定在0.0018rad。

1 手眼关系标定过程

1.1 标定算法实现过程

手眼关系的自动化标定涉及到图像位姿检测、机械臂运动等诸多功能模块，具体的标定框架过程如图1所示。UR5驱动、相机驱动以及场景描述文件事先准备完成，相机内参标定采用张正友单平面棋盘格的相机标定法。人工拖拽机械臂四次，确保在机械臂对应位姿情况下，能够从相机获取的图像中计算出marker相对于相机的位姿信息，通过以上四个标定样本计算得到手眼的初始关系，从而得到marker相对于机械臂基座的初始位姿关系。在初始手眼关系的基础上，结合空间约束条件，生成相机将要跟随的等分空间位姿点，然后再转化为机械臂末端在机器人基座坐标系下要跟随的位姿点，通过RobWork进行路径规划，当机械臂到达相应位姿点后，判断当前相机视野是否可以计算出marker的位姿，如果可以，就将这组marker位姿和对应的机器人关节位姿存入标定样本集中去计算，否则继续下一个位姿点的跟随。最终通过循环计算，得出标定结果。

图1 算法实现过程

1.2 等分空间约束的位姿生成

手眼关系的自动化标定，离不开可行位姿点的生成。文献[5]中指出提高手眼标定精度的四个重要准则：

1）尽可能增大相对运动旋转轴之间的夹角；

2）尽可能增大相对运动的旋转角度；

3）尽可能减小标定板到相机光轴中心的距离；

4）尽可能减小末端在不同姿态之间的距离。

基于这些准则，以marker的中心为原点建立球坐标系，合理约束相机坐标系原点到球坐标系原点的距离r，以及相机光轴与z轴夹角θ。设定相机坐标系原点到marker中心最小距离rmin=0.2m最大距离rmax=0.38m，夹角θmin=0，θmmax=π/6，在xoy平面与x轴的夹角αmin=0，αmax=2π。

为了保证采样点在空间中的均匀分布，在采样过程中在约束的范围内对α,θ,r进行等分采样。为了确定(α,θ,r)位置处相机的姿态，定义相机的光轴指向球坐标原点的径向方向，相机光轴作为在该位置点处的zc轴，然后定义相机的xc与yc，具体如如图2所示。xc垂直于面OcOF，从而确定在该位置点处相机的方向信息，最终作为相机将跟随的一个位姿点。依据图2的示意图，可以得到位姿点处的笛卡尔坐标系相对于以marker中心为坐标系的笛卡尔坐标系的旋转矩阵和平移矩阵分别为：

图2 空间约束模型原理图

[R1丨t1]构成了生成位姿点的位置和姿态，基于手眼初始关系和[R1丨t1]可以得到生成的采样点在机械臂基座坐标系下的位姿，从而下一步基于RobWork对机器人进行采样点跟随的运动，使得机器人末端的相机到达预期位姿。

2 手眼关系标定原理

2.1 Tsai-Lenz 标定计算

Tsai-Lenz是一种计算机械臂末端坐标系与相机坐标系间手眼关系的算法。需要的数据有机械臂末端坐标系之间相对位姿变换矩阵、摄像机坐标系间的相对位姿变换矩阵。相机安装在机械臂上，并在相机视野内放置marker，则有以下变换矩阵关系[12]：

根据上述的变换矩阵关系式，可以得到：

在AX=XB求解中，至少需要两组A、B才能计算出结果，也就是至少需要采集3个标定样本。实际标定过程，需要采集更多的标定样本，来获得更好的标定精度。

2.2 pnp图像位姿求解原理

在本实验中，marker是一个平面，且marker的真实方块物理尺寸是已知的，这些角点在相机中的像素坐标也是已知的。对于DLT解法，场景中角点P的齐次坐标P=(x,y,z,1)，齐次化表示其在像素平面上对应的投影特征p=(u1,v1,1)点，s表示尺度因子。记增广矩阵[R丨t]是3*4的矩阵，即：

用式(3)最后一行把s消去，得到两个约束[13]：

每个特征点提供了关于t(t1,t2,t3)的两个线性约束，对于3*4的[R丨t]矩阵，6对匹配点即可求得求解相机与marker间的[R丨t]矩阵，即位姿矩阵。摄相机在某采样点拍摄照片如图3中左图所示，采用棋盘图检测法对左图进行内角点检测并按角点编号顺序连接，得到图3中右图。基于检测到的内角点，下一步使用上文所述的pnp算法求解。

图3 棋盘图在摄像机坐标系中的位姿求解

2.3 误差评估机制

手眼标定结果的精度直接取决于所获取计算数据的准确性。OpenCV棋盘图检测时，会存在有些图像因为光线等问题给图像带来噪声干扰[14]，导致个别图像计算出来的位姿出现很大误差，如果把这种异常样本放在后续的计算循环中，则会导致后面每一次更新计算出来的结果都会相应的产生较大误差，因此，在每次更新计算前，需要对新的图像位姿数据进行误差评估，如果是非异常值则添加入计算数据中进行当次的更新计算。

对于非异常值的图像计算序列chessPosem和对应的机械臂位姿计算序列tcpPosem(m=1,2,...,i,...n)，对应于前n组计算序列的得到的结果Rx和tx，按照参考文献[15]定义，(k=1,2,...,j,...n-1)。从E1到Ej服从正态分布的特征，求得其正态分布的均值mean和方差stdev。对于i+1第组的计算样本，定义允许的误差区间[floor,ceiling]：

当机械臂运行到下一个有效位姿点时，记录在这个位姿处的chessPosei+1和tcpPosei+1，结合tcpPosei和chessPosei，计算得到Ej+1：

如果Ej+1满足条件(7)则把chessPosei+1和tcpPosei+1放入非异常值计算序列中，然后更新计算序列的误差区间[floor,ceiling]。如果Ej+1不满足条件(8)则认为chessPosei+1和tcpPosei+1是异常值，不放入非异常值计算序列中。接下来对下一个位姿点处的图像位姿数据和机械臂位姿数据同样按照这样去判定。

3 实验部分

机器人手眼标定实验在Ubuntu18.04操作系统下基于ROS搭建，采用OpenCV进行图像处理。RobWork是一个用于仿真模拟控制机器人的平台，能够和ROS很好的集成。

3.1 仿真实验

基于RobWork搭建仿真实验平台，添加虚拟相机获取图像信息。仿真实验平台如图4所示。按照如图1所示的算法实现流程开展实验。

图4 仿真实验平台

基于α,θ,r的范围约束[16]，采用等空间采样法，生成采样位姿点。在循环更新计算部分，当计算序列在4组及以上时开始计算，然后每到一个有效位姿点，就把该位姿处对应的图像位姿和机械臂位姿数据放入计算序列中，和之前的计算序列一起计算更新当前位姿下的手眼关系。tn、Rn定义用n组数据更新计算得到的平移矩阵和旋转矩阵。ttrue、Rtrue定义手眼关系的平移矩阵真值和旋转矩阵真值，可以直接从仿真环境配置文件中读出。

位置误差的定义由更新计算得来的平移向量与真值平移向量做差取模得出[17]：

角度误差的定义由计算得来的旋转矩阵与真值旋转矩阵的转置相乘后的结果通过罗德里格斯公式处理为旋转向量，再取模得出：

对应的ep_n、er_n曲线分别如图5、图6所示。

图5 仿真实验位置误差曲线

图6 仿真实验旋转角度误差曲线

图5横轴代表每次计算时所采用的计算样本数，纵轴代表对应序列数下所得值与真值的误差，位置误差曲线随着计算序列组迭代更新而整体持续下降，最终在第22次更新后达到0.2mm左右的稳定收敛状态。图6旋转误差呈现下降趋势，最终在第22次的更新后达到0.0008rad左右的收敛状态。

3.2 实体实验

本论文的实体实验平台如图7所示。采用UR5协作机器人末端搭载Stingray F504 CCD相机。通过ROS平台建立机器人、摄像机和标定程序之间通讯和控制网络。

图7 实体实验平台

依据Tsai-Lenz两步法求解方程AX=XB，至少需要两组的A和B才能得出一个初始的X结果。此处和的初始A和B计算样本数从4开始，机械臂每到一个采集点进行相机拍照，如果图像可以计算出位姿，则把当前的图像位姿信息和机械臂位姿信息和前面4组对应信息一起作为下一次更新计算手眼关系的数据。如果采集点机械臂不可达或机械臂到达后，相机图像不能计算出图像的位姿，则该位姿点跳过，继续进入到下一个位姿点处的相关计算流程。每一个位姿点处的X更新计算完后，计算当次计算结果的位置误差与旋转误差。实体实验中ep_n、er_n和式(8)、式(9)定义一致，得到对应的ep_n、er_n曲线如图8、图9所示。对130个标定采集样本，每次随机采样80组用于计算出一个t、R，总共取50次，最后对这50个t、R分别取均值，得出ttrue、Rtrue。

图8 实体实验位置误差曲线

图9 实体实验旋转角度误差曲线

实体实验使用固定焦距的CCD相机，相较于仿真环境，采集到的图像不可避免会存在瑕疵，前一次迭代样本中存在的误差会一直在后续计算组中去计算。在第65组新后，位置误差在0.6mm左右，旋转误差在0.0224rad左右。采集样本中的异常点，会导致标定误差波动，但随着样本数的增加，误差的波动范围降低了。

3.3 引入异常值剔除环节

在实体实验中引入误差评估机制，按照式(6)、式(7)对异常值进行剔除。每次非异常值计算序列更新后，计算得到一个对应于该组计算序列的手眼标定值，并依据式(8)、式(9)得到ep_n、er_n。得出的位置误差曲线如图10、图11所示。

图10 异常值剔除后的实体实验位置误差曲线

图11 异常值剔除后的实体实验角度误差曲线

从图10所示位置误差曲线中可以看出，引入异常值剔除环节后，当计算组更新到45组时已经趋于稳定，位置误差最终稳定在0.4mm左右，而且位置误差从始至终在极小的范围内波动（约为0.1mm）。相对于无异常剔除机制的测试结果：第65迭代达到的0.6mm位置误差，异常值剔除环节引入后在收敛速度和误差精度方面都起到了明显的优化效果，而且误差稳定了，不会出现大幅度波动的情况。

图11中所示的旋转误差曲线，计算更新到第45组时已经趋于收敛，旋转角度误差最终稳定在0.0018rad左右，引入误差剔除前的测试结果：第65组时角度误差稳定在0.0224rad，在计算速度和精度上都有了显著提升，而且旋转误差全程的波动范围在0.0012rad以内。

位置误差和旋转误差在更新中都存在微小的震荡，这是因为异常值判定标准当中的误差判定区间是[mean+2*stdev,mean-2*stdev]，对应于2倍标准差的区间，其置信概率是95.5%，这会导致有些误差较大但在判定区间内的序列也会被计入计算序列中，从而导致下次的异常值误差判定区间的范围又变大，而在更新计算环节中，当次加进来的计算序列也会在接下来的更新轮次中反复用于计算。如果把误差判定区间设置的较小，会导致对应的置信概率会比较小。

3.4 与主流方法的比较

现有主流的方法有NAVY算法（Park[18]）、INRIA算法（Horaud[19]）、对偶四元数算法（Daniilidis[20]）。依据式(8)、式(9)所定义的ep_n、er_n，采用现有主流方法进行计算，得出标定的位置误差和角度误差。NAVY算法（Park）的位置误差和角度误差结果分别如图12、图13所示。

图12 NAVY算法位置误差曲线

图13 NAVY算法角度误差曲线

从图1 2 可以看出，在8 5 次更新后，位置误差在0.529mm左右，角度误差在0.002114rad左右。对接下来的两种主流方法分别开展实验，发现这两种方法和INRIA算法得出的误差曲线趋势基本一致。

把仿真实验结果、实体实验结果、引入异常值剔除环节后的实验结果、主流方法结果整理如表1所示。

表1 标定仿真和实体实验结果汇总

表1中三种主流方法得到的实验结果基本一致。发现主流方法在位置误差和角度误差上优于实体实验（无异常值剔除）的结果。加入异常值剔除算法后，位置误差和角度误差结果优于主流方法。

在引入异常值剔除环节后，在标定计算精度和迭代次数上相对于表一中的其它方法有了较大提升，说明了该算法的有效性与优越性。

4 结语

本文提出了一种机器人手眼自动化标定的算法。首先人工拖拽机器人4个位姿，得出初始的手眼关系。在初始手眼关系的基础上，建立基于maker的等分空间位姿约束模型。机械臂对这些位姿进行逐一跟随，采集标定样本，基于Tsai-Lenz方法完成循环更新计算。在每一次更新计算完成后，计算当次的位置误差和旋转角度误差。

在仿真实验中，干扰因素较少，位置误差在第22组的更新后达到0.2mm左右的稳定收敛状态，旋转角度误差在第22组的更新后达到0.008rad左右的收敛状态。实体实验中，位置误差在第65组的更新后达到0.6mm左右，转角度误差在第65组的更新后达到0.0224rad左右，后续的样本中有异常出现，导致标定误差波动。针对这种情况，提出了误差评估机制，对异常计算序列进行剔除。加入异常值剔除环节后，位置误差在第45组的更新后达到0.4mm左右的稳定收敛状态，旋转角度误差在第45组的更新后达到0.0024rad左右的收敛状态。最后用三种主流方法进行实验，发现主流方法标定的位置误差和角度误差结果介于两者之间，说明了异常值剔除环节的有效性与优越性。