结构光视觉引导的轨迹跟踪系统的标定技术

李 龙，白瑞林，吉 峰，郭新年

LILong1,BAIRuilin1,JIFeng2,GUO Xinnian1

1.江南大学 轻工过程先进控制教育部重点实验室，江苏 无锡 214122

2.无锡信捷电气有限公司，江苏 无锡 214072

1.Key Laboratory of Advanced Process Control for Light Industry（Ministry of Education）,Jiangnan University,Wuxi,Jiangsu 214122,China

2.Xinje Electronic Co.,Ltd,Wuxi,Jiangsu 214072,China

1 引言

视觉测量与控制在机器人领域占有重要的地位。在工业机器人领域，视觉主要用于目标和机器人末端位姿的测量以及对机器人末端位姿的控制，其典型应用包括焊接、喷漆、装配、搬运等作业。视觉测量与控制的关键部分就是标定技术。

刘苏宜等[1]提出了一种同时标定摄像机和机器人的方法。依据机器人手眼矩阵和机器人手爪对基坐标系位姿矩阵之间的特定关系，一次标定求解出摄像机参数和机器人手眼关系矩阵。谢凯等[2]将BP神经网络与线性标定方法结合，不仅标定了结构光三维检测CCD相机的内外参数，而且利用神经网络的非线性逼近能力，克服了镜头径向畸变、切向畸变等引起的系统非线性误差；Jan Heller[3]等提出了一种手眼标定新方法。在使用校准靶标不可行的情况下，采用结构的运动方式进行手眼标定；贺俊吉等人[4]探讨了被多种噪声干扰的光条图像的处理和精确提取光条中心的方法，给出了一种消除环境干扰的滤波模板并实验验证了其滤波效果，使光条中心的提取精度达到更高。

本文在充分分析结构光视觉引导系统的基础上，提出了一种新的标定方法，采用平面靶标，充分利用实验数据，简化标定流程，控制机械臂以3个以上任意位姿拍摄固定靶标，即可实现相机标定、线激光光平面方程标定和手眼矩阵的标定，为精确跟踪提供了充分条件。

2 标定原理

基于结构光视觉传感器引导的机器人跟踪系统的标定流程由相机标定、线激光光平面方程标定、手眼变换矩阵标定以及工件偏移标定四部分组成，控制机械臂以3个以上任意位姿拍摄固定靶标，利用得到的圆靶标圆心像素坐标以及其与世界坐标的匹配数据完成相机内外参计算，利用得到的外参和靶标图片提取的线激光线方程标定出激光光平面方程[5]，利用每两次机械臂移动中的外参变化和机械臂位姿变化构造AX=XB方程，并利用四元数法[6]求解手眼变换矩阵H既方程中的X。

2.1 靶标圆心点像素坐标与世界坐标匹配

自制靶标如图1所示，其同时适用于相机标定和手眼变换矩阵标定过程中，并且其匹配方法简单，匹配结果稳定，能有效简化整个系统的标定复杂度。

图1 自制靶标

具体匹配步骤如下：

（1）实时采集带圆点靶标图像，采用大津法对图像二值化；对图像进行闭运算，去除噪声干扰；利用聚类算法，对图像中目标进行分类。

（2）计算目标周长C和面积S，利用圆形度表达式e=C2/(2×π×S)，区分圆点与干扰。

（3）采用重心法求取每一个圆点的圆心坐标。

（4）靶标为自制靶标，相邻两行之间的行距为15 mm，同一行中相邻的2圆心点距离为10 mm，在第一行的第一个点行方向上5 mm处有一个额外的圆，首先对所有圆心点进行排序，排序过程如下：

①找出所有有效圆点的圆心距离最小的两个点，其中离所有圆心点均值中心远的圆的定义为0点，另一个定义为1点。

②设置0，1点为已匹配点，在未匹配点中找与1点最近的点定义2点。

③设置2点为已匹配点，在未匹配点中找与2点最近的点定义3点，…以此类推直至把所有点都找全，完成所有圆心点的排序。

（5）根据上述圆靶标图像中圆点排序，与给定圆靶标信息，完成圆靶标图像中圆心点像素坐标与世界坐标的匹配。

2.2 结构光视觉传感器标定

结构光视觉传感器标定主要有相机标定和激光光平面方程标定两部分组成，其中相机标定采用张正友标定法[7]，以不同位姿拍摄3幅以上靶标图片，进行靶标匹配后，用张正友方法求取相机内参并同时求得相机外参。

在拍摄每一幅靶标图片的位姿后，打开线激光，拍摄一幅带激光光条的靶标图片，提取光条，细化光条，通过Hough变换求出光条线方程。结合上一步得到的外参矩阵利用投影空间中两条激光交线及其相关联的平面的对偶关系就可以得到线激光光平面在相机坐标系下的平面方程。

基于交线图像的激光平面标定的原理如下，圆靶标平面在世界坐标系下的平面方程可以表示为(π1，x)=0，式中：π1=[0，0，1，0]T，利用第一步得到的外参数矩阵RT，计算得到在相机坐标系下圆靶标平面方程为(RT-Tπ1，x′)=0 ；其中的 x 与 x′分别为靶标平面在世界坐标系和相机坐标系下的平面法向坐标向量。

在相机坐标系中，设 w、wi、wj、λi、λj分别代表激光平面法向坐标向量、第i个平面靶标的法向坐标向量、第 j个平面靶标的法向坐标向量、第i个平面靶标内的激光交线图像的坐标向量、第 j个平面靶标内的激光交线图像的坐标向量。注意第i个平面靶标图像上的激光交线图像的方程 λi（像素坐标系下）与 λi（相机坐标系下）可以直接计算出来：

其中A为相机内参矩阵。

投影空间中两条激光交线及其相关联的平面的相互关系的对偶性表示如图2所示。

图2 两条激光交线在投影空间的对偶表示

图2 中，要标定的激光平面坐标w在投影空间的对偶表示就是两条激光交线的交点。由于误差的影响，两条激光交线在空间并不相交，因此用距离两条激光线最近的点表示w。而距离两条激光线最近的点位于两条激光线的公垂线上。因此，计算激光平面坐标为：

由于公垂线与两条激光线分别垂直，因此内积：

因此，可求得：

式中：D是一个2×2矩阵，b是一个二维向量，定义如下：

将式（7）（8）的值带入式（2）和（3），就会求得激光平面方程坐标w。

为了提高标定的精度，通常情况下选择多幅位于不同位姿的平面靶标图像进行标定。其中任意两幅就标定了一个光平面方程坐标。因此n个平面靶标图像，则可以标定个光平面方程标定参数。对每项排序，去除最大和最小值并求剩下的平均值来减少标定的光平面参数的误差。

2.3 手眼变换矩阵标定

本跟踪系统采用Eye-in-Hand系统，即视觉传感器固定在机械臂末端，相机坐标系相对于机械臂末端坐标系的关系是固定的。因此，对于Eye-in-Hand系统，手眼标定时求取的是相机坐标系相对于机械臂末端坐标系的关系。

对于手眼关系矩阵，直接测量几乎是不可能的，如图3，通过改变机械臂位姿，根据坐标变换链的封闭性可得：

其中TA1，TA2表示机器人变换位姿前后机械臂末端坐标系相对于机械臂基坐标系的位姿变换矩阵；TB1，TB2表示机器人变换位姿前后标靶所在世界坐标系相对于相机坐标系的齐次变换矩阵。

图3 手眼关系图

2.3.1 四元数与旋转矩阵变换

采用四元数可以描述通用旋转变换[8]。四元数是实数和复数以及三维空间点矢量的扩展，由实数部分s∈R和矢量部分v=(x，y，z)∈R3组成。四元数表示形式为：

式中，i2=j2=k2=ijk=-1，ij=k，jk=i，ki=j，ji=-k，kj=-i，ik=-j。当||q||=1时，称q为单位四元数。四元数可以描述三维刚体的姿态和旋转变换如图4，其理论基础是三维空间中绕单位轴n旋转2θ角可以用四元数q=[s，v]=[cosθ，sinθn]来描述。

图4 四元数描述三维旋转示意图

与单位四元数q=[s，(a，b，c)]对应的姿态矩阵为：

式中：

2.3.2 利用四元数法求解AX=XB

齐次变换矩阵A和B为已知参数，这里采用两步法求解该方程，把上式齐次矩阵分解成旋转和平移两部分：

展开得：

令 qA=[a0，aT]T，qx=[x0，xT]T，qB=[b0，bT]T为 RA，Rx，RB对应的四元数，方程RARx=RxRB可以表示为：

利用四元数乘法展开上式得：

因为

得到a0=b0，由上式可得：

令 z=x/x0，则

实验过程中可以利用等式a0=b0检查测试数据的准确性，并进行错误数据的剔除。

式（21）为形如Gz=c的形式，唯一的未确定z至少需要机械臂做两次运动。且两次运动的旋转轴不能平行。对于多次运动，使用最小二乘法确定z之后利用约束条件||qx||=1，即可得到qx。将得到的旋转矩阵代入式（16）得到平移向量tx。

2.4 工件修正

实验采用的是DENSO 6轴机械臂，通过发送六轴位姿来控制机械臂，在焊缝跟踪实验中，机械臂末端需要夹持焊枪，需要对末端工件即焊枪偏移进行修正，机械臂位姿参数可以分两部分，旋转部分和平移部分，所提取的焊缝特征点三维数据最后稳定的保存格式是在机械臂基坐标系下的坐标，工件的偏移修正必须转换到相应旋转位姿下的机械臂基坐标系的偏移向量。

具体修正方法如下：

（1）在靶标所在世界坐标系下定义一个预设点，通过之前计算得到的相机内参，外参，手眼变换矩阵以及当前机械臂位姿矩阵，计算得到预设点在机械臂基坐标下的坐标值x。

（2）控制机械臂点触预设点，读取当前机械臂位姿，通过当前机械臂位姿得到机械臂末端坐标系与机械臂基坐标系的转换矩阵T6。

（4）使用时根据焊缝特征点在图像中所处的位置以及焊缝特征点所经过曲线切线方向，调整机械臂位姿的旋转部分，可以得到当前旋转位姿下机械臂末端坐标系与机械臂基坐标旋转变换矩阵T6'。

（5）x1=R6'*x′，通过上式计算得到工件在机械臂当前旋转姿态下的基坐标偏移量x1，R6'为T6'中的旋转部分。

3 实验测试与分析

本实验标定过程理论测试操作在Win7系统的MATLAB 2009a平台上完成和实现，实时跟踪在VS2010平台上完成，computar工业镜头，12 mm焦距，感光元件CCD 640×480。

标定实验流程如图5所示。

图5 标定流程图

3.1 匹配测试

测试相机从各个角度拍摄靶标得到的匹配结果如图6所示，其中图（a）~（d）为从各个角度拍摄的靶标图片，（a1）~（d1）为对应的匹配结果图，其中蓝点表示各个圆心点，红线表示圆心点排序示意。实验结果表明在相机光轴与靶标平面法向向量夹角小于45°时匹配结果鲁棒性强。

3.2 平面数据测试

使用在计算手眼矩阵时得到的机械臂基坐标与由靶标定义的世界坐标系的转换矩阵。测试靶标上的点在机械臂基坐标系下的坐标计算值与实际值的误差。实验数据如表1。

其中机械臂末端旋转矩阵表示方式为ZYX，工件偏差T7=[-42.41，0.82，350.33]，当前旋转位姿参数为 Rx=177.36，Ry=0.26，Rz=75.18，测试最大误差±0.28 mm。

3.3 三维数据测试

图6 匹配结果图

结合标定好的相机内参，线激光光平面方程，手眼矩阵，工件偏移参数，对三维阶梯形数据进行测试，测试结果如表2，其中最大误差为±1.16 mm。

多次实验结果表明采用光轴绕靶标平面法向量转动，所得到的实验数据精度较高。

4 结论

为实现工业现场三位特征点的跟踪，提出一种基于结构光视觉，六轴机械臂的整体标定方法，主要特点是：

（1）采用自制圆靶标，简化圆心点提取和特征点匹配算法，提高匹配算法的鲁棒性，极大减少运算时间。

（2）改进利用对偶关系求取线激光光平面方程的算法，避免错误数据对标定结果的影响。

（3）利用四元数求解AX=XB方程，并创新采用四元数错误数据剔除算法，使得标定结果受误差干扰大幅度减小。

（4）针对焊接工件的不确定性，通过简单的点触方法，修正在基坐标下的工件偏移。

实际测试表明：该方法鲁棒性好，能得到稳定的匹配结果，标定得到的相机内参，手眼矩阵和线激光光平面方程稳定，平面跟踪可达到±0.3 mm精度，三维跟踪可达到±1.2 mm精度，满足工业现场应用要求。

表1 平面测试数据

表2 三维测试数据

[1]刘苏宜，王国荣，石永华.激光视觉机器人焊接中摄像机和手眼的同时标定[J].华南理工大学学报，2008，36（2）：75-77.

[2]谢凯，刘宛予.结构光三维视觉检测系统的标定方法研究[J].光电子·激光，2007，18（3）：369-371.

[3]Heller J，Havlena M.Structure-from-motion based hand-eye calibration using L∞minimization[C]//Computer Vision and Pattern Recognition（CVPR），2011：3497-3503.

[4]贺俊吉，张广军.结构光三维视觉检测中光条图像处理方法研究[J].北京航空航天大学学报，2003，29（7）：593-597.

[5]毕德学，刘方滔，薛强，等.基于激光交线的结构光视觉传感器现场标定新方法[J].仪器仪表学报，2009，30（8）：1697-1701.

[6]李爱国.结构光视觉测量机器人标定技术研究[D].大连：大连海事大学，2009.

[7]孟海岗.基于平面约束的CCD相机标定方法改进[D].长春：吉林大学，2009.

[8] 刘俊峰.三维转动的四元数表示[J].大学物理，2004，23（4）：39-43.