基于法向约束的背靠视觉端面对接定位方法

于长志，张连新，孙鹏飞，陈东生，宋颖慧，姚云飞，李 炼，李代杨

（中国工程物理研究院机械制造工艺研究所，四川绵阳 621999）

0 概述

对接定位，尤其是端面圆特征对接定位，在产品自动化装配［1-2］、航天交会对接［3-4］、飞机大部件对接［5-6］、电子产品贴合等工程领域应用广泛。其通过数字化测量技术获取零部件的空间状态信息，并反馈给相关设备进行位姿调整，可大幅提高对接过程的柔性化。端面圆特征定位通常可由空间中的圆特征在不同坐标系下的相对旋转量和相对位移量表示［7-8］，其检测精度将影响对接成功率、对接效率等。实际端面圆的材质、加工精度、镜面反射、环境光照等因素严重制约了端面圆的高精度检测。

目前，对接定位方法主要分为激光跟踪仪测量方 法［9］、激光雷达测量方法［10］、机器视觉测量方法［11-13］等。激光跟踪仪方法属于点位测量，可用于跟踪目标点的位置，但需要在产品表面安装特定标识。激光雷达测量方法属于海量点云测量，测量效率和数据处理效率较低，检测精度往往不高。机器视觉测量方法［12-14］属于非接触测量且不需要粘贴特定标识，通过视觉测量获取目标的空间位置和姿态，具有高精度、高效率的特点，成为当前的研究热点。文献［15-17］采用多目视觉对圆位姿进行求解，然而由于多目视觉测量方法需采用多部设备，因此限制了视觉测量的视场范围，导致该方法在测量范围和测量距离上有待提高。此外，由于多目视觉测量方法需要对目标的立体匹配及测量系统进行现场校准，因此其研究算法和系统步骤均较复杂［5-7］。相比多目视觉测量方法，单目视觉测量方法［18-20］仅釆用一部相机对空间物体特征进行测量，由于结构简单、计算速度快、测量准确度高等优点，其具有更高的研究价值。空间圆可以提供圆心坐标和法向量（即位姿解），然而在采用单目视觉测量方法时，圆特征的位姿解存在二义性问题［7-8］。ZHOU 等［2］为降低大型齿轮的装配误差，通过引入权重，提出改进的圆特征姿态计算方法。LI 等［15］分析三维物体圆形特征的投影与相应的符号距离函数之间的关系，提出一种利用水平集方法结合椭圆形状先验信息进行物体姿态估计的方法。WANG 等［17］针对圆位姿解的非唯一性及处理精度不高的问题，引入4 个共面特征点，并提出一种基于点特征的单目视觉位姿测量算法。ZHU［20］等提出基于连续图像的位姿测量算法，根据连续图像的相关性位姿得到位姿正确解。ZHANG等［21］通过增加空间特征点几何约束，证明特征点与光心和圆平面的相对位置，并得到唯一位姿解。ZHANG 等［22］借助激光测距仪，改进圆特征的单目视觉测量算法，并进行仿真验证。HAO 等［23］提出一种多因素融合的误差分析方法，通过抑制相机畸变和提高相机分辨率，提升位姿测量精度。QU 等［24］在分析圆形合作目标特征点的提取精度基础上，提出专门设计圆形和方形合作目标进行位姿测量的方法。

针对不同的应用背景，研究人员针对单目视觉圆特征位姿测量改进开展了大量研究，然而在获取高精度、高鲁棒圆特征位姿解方面仍存在一些不足，尤其是在机器人对接定位领域，由于需要通过背靠视觉对端面圆位姿进行高效、高精度解算，因此限制了其在工程领域的应用。

本文从提高测量精度和鲁棒性目的出发，以排列点激光位移传感器和背靠视觉为测量手段，提出一种基于法向约束的背靠视觉端面圆对接定位方法。针对端面圆检测中由图像噪声、边缘识别精度等因素引起的椭圆拟合中心不准确问题，引入随机采样一致性（Random Sample Consensus，RANSAC）算法进行椭圆拟合，降低噪声点和无序点对拟合中心的影响。针对圆位姿解二义性问题，采用排列点激光构建法向约束，通过设定法向阈值判定圆位姿解，从而得到准确的圆位姿解。

1 相机模型坐标系

在相机测量模型中［15，25］，主要使用世界坐标系、图像坐标系、像平面坐标系和相机坐标系表征物体与图像像素点之间的转换关系，如图1 所示。世界坐标系描述物体的空间位姿，图像坐标系表示像素所在的列数和行数，像平面坐标系描述像素在图像中的物理位置。相机坐标系与世界坐标系之间的关系可以用物体与相机的平移与旋转来表示。

图1 相机模型的坐标系描述Fig.1 Coordinate description of camera model

根据相机的理想小孔成像模型［14，24］，世界坐标系下点(xW,yW,zW)对应的相机图像像素坐标如式（1）所示：

其中：zc表示物体在相机坐标系中的z坐标；dx和dy分别是像素的尺寸因子；s为相机像平面坐标轴不正交导致的倾斜因子；(u,v)表示像素坐标，(u0,v0)是图像主点坐标；f是相机焦距；K称为相机内参矩阵；C=[R H]表示相机外部参数，其中R表示旋转矩阵，H表示平移向量。

2 本文方法

2.1 方法流程

在机器人对接装配中，需要以下工件的端面为基准，对上工件进行姿态调节和位移调节，从而确保上下工件完成对接装配。本文方法的流程如图2所示。

图2 本文方法流程Fig.2 Procedure of method in this paper

本文方法的具体流程如下：

1）进行背靠相机标定和机器人手眼标定，得到相机的内参以及相机到机器人基坐标系的转换关系；

2）下相机采集下工件端面图像，经由特征提取、RANSAC 算法进行特征拟合得到下工件端面的中心位置，并将其作为上下工件对接的基准位置；

3）上相机采集上工件端面图像，经由特征提取、RANSAC 算法进行特征拟合得到图像坐标系下的椭圆方程；

4）根据单目相机圆位姿模型，计算在相机坐标系下以相机光心为顶点、目标圆为底面的椭圆锥方程，得到二义性的特征圆位姿解；

5）下相机采集排列点激光的激光点，通过光点中心以及激光测距信息进行平面拟合，以平面法向量作为法向约束；

6）将法向约束与得到的圆位姿解进行计算，得到正确的圆位姿解；

7）判断圆姿态偏差，若不满足阈值（一般取阈值ε=0.01 °）则由机器人进行调整，重复步骤2～步骤7，直至满足阈值要求；

8）计算上下工件端面中心位移偏差量，判断位移偏差量是否在阈值（一般取η=0.01 mm）范围内，否则由机器人进行位移调整，重复步骤2～步骤8，直至满足阈值范围，从而完成上下工件端面的对接定位调节。

本文方法的特点在于背靠视觉获取上下工件端面特征图像，采用RANSAC 算法进行圆特征拟合，并引入排列点激光作为法向约束，实现端面圆位姿参数的准确获取。此外，通过机器人进行姿态迭代调整和位移迭代调整，完成上下工件姿态和位移的调节，从而实现准确的端面对接。

2.2 基于RANSAC 的端面圆特征拟合

相机采集的工件端面圆图像通常表现为椭圆，需先进行特征亚像素边缘提取［25］，然后根据边缘提取结果进行椭圆拟合，从而得到椭圆的空间表达式。设成像椭圆在像面XY坐标系中的方程［13］如式（2）所示：

则根据最小二乘拟合原理，可通过取如式（3）所示的目标函数最小值来决定参数(a,b,c,d,e)。

其中：κnm表示边缘点的数量。

令每个参数的偏微分算子均为0，即：

可得如式（5）所示的线性方程：

通过求解式（5），可以得到上述椭圆参数。最小二乘椭圆拟合算法属于无偏估计，即所有点均参与拟合，因此拟合误差大，且鲁棒性较差［25］。而RANSAC 算法能够迭代地从含有大量局外点的观测数据集中估计设定数学模型的参数，剔除了噪声极值、异常值等边缘点带来的影响，从而可以提高拟合结果的鲁棒性和准确性［25］。RANSAC 算法的步骤如下：

1）从工件端面椭圆边缘点中随机抽取样本点，根据式（2）～式（5）进行最小二乘椭圆拟合，得到椭圆参数；

2）计算所有样本点与得到的椭圆参数的拟合残差，并选择小于容许误差的样本数据和样本数目；

3）当样本数目大于局内点个数阈值时，停止局内点搜索，保存此样本数据；

4）设置迭代次数并重复步骤1～步骤3，如果局内点个数小于局内点个数阈值，则停止局内点搜索，保存最大个数的局内点集合和此样本数据；

5）根据式（2）～式（5）对步骤4 得到的样本数据进行最小二乘椭圆拟合，得到椭圆表达式。

通过RANSAC 算法得到的椭圆表达式，由于剔除了异常值并降低了噪声值的影响，因此可以得到更高鲁棒性、更高准确性的椭圆拟合结果。

2.3 基于排列点激光法向约束的圆位姿求解

在单目相机圆位姿模型［18-20］中，相机光心到工件端面形成一个椭圆锥面。求解端面圆位姿即寻找一截平面，使该平面与椭圆锥相交形成一个已知半径为r的圆，其圆心坐标和法向量即为端面圆的空间位置和姿态。在单目视觉下空间端面圆描述一般需要圆心、半径、俯仰角及倾斜角［17］参数，空间圆的姿态可用俯仰角和倾斜角表示。如图3 所示，其中：(x0,y0,z0)为圆心位置；向量n为空间圆所在平面的法向量；向量np为n在平面OXY上的投影。俯仰角β为n与平面OXY的夹角，取值范围为；倾斜角α为np与正向轴OX的夹角，取值范围为[0,2π]。

图3 圆位姿示意图Fig.3 Schematic diagram of circular pose

相机光心到端面形成一个椭圆锥面，在相机坐标系XcYcZc中的方程如式（7）所示：

式（7）可重写为如式（8）所示：

相机坐标系下的点pi=(Xi,Yi,Zi)与对应图像坐标系下点(ui,vi)的坐标转换关系可表示为式（9）所示：

式（10）为相机光心与端面圆形成的椭圆锥，图像平面上的椭圆投影为椭圆锥的一个截平面。设Cc=PC′c为从相机坐标系到主轴坐标系的正交变换，其中，P3×3为正交矩阵。则公式（10）可转化为主轴坐标系下的椭圆锥，如式（11）所示：

若取P1、P2、P3分别为矩阵Q的特征向量，对应的特征值分别为λ1、λ2、λ3。其中λ1＞λ2＞0,λ3＜0。设正交矩阵P-1QP=PTQP=diag(λ1,λ2,λ3)，则 式（11）的椭圆锥可写为式（12）所示：

根据式（12），端面圆在主轴坐标系下的圆心和法向量可分别描述为式（13）所示：

将式（13）转换到相机坐标系下，从而得到端面圆的圆心坐标及法向量，如式（14）所示：

由式（14）得到端面圆的圆心坐标和法向量，其中包括了一个虚假解和一个正确解，如图4 所示。端面圆在椭圆锥中可以找到2 个截圆平面，分别表示一个虚假解和正确解。从式（14）中可以看出，虚假解和正确解的法向量均可以得到，为此，引入排列点激光作为法向约束，消除圆位姿解的二义性问题。

图4 端面圆的二义性位姿解示意图Fig.4 Schematic diagram of ambiguous pose solution of end-face circle

背靠相机分别采集点激光在工件端面圆的图像，以激光点轮廓的质心作为激光点对应的图像坐标。根据相机的光心和各点激光的距离，并由点激光测距结果，可得到激光点在相机坐标系下的坐标Li=(XUi,YUi,ZUi),i=1,2,3,4。由于空间上至少3 点可拟合一个平面，则上工件端面中的4 个激光点可通过最小二乘拟合方法得到激光平面，即：

利用拉格朗日乘数法求函数极值，如式（17）所示：

根据式（18）可以得到实对称矩阵，如式（19）所示：

由式（20）可以求解E的特征值，则最小特征值ηmin对应的特征向量即为激光平面方程的法向量nU。

对式（14）得到的二义性圆法向量根据式（21）分别计算其与对应平面法向量的夹角，即：

取对应最小夹角的圆法向量为端面圆圆法向量，从而通过增加排列点激光法向约束的方式得到端面圆法向量的正确解。

2.4 基于机器人的对接位姿调整

由2.3 节可以得到端面圆特征的空间姿态和位置。由于下工件端面固定不动，机器人抓取上工件端面需在进行相对位置和姿态调整之后才可完成对接。因此，需将由背靠相机和排列点激光得到的工件端面圆特征的空间姿态和位置，转换为在机器人基座标系下得到的姿态调整量和位置调整量。

在对接装置中，测量单元安装在固定装置上，与机器人的标定属于手眼标定［4-6］，如图5 所示。

图5 机器人手眼标定Fig.5 Robot hand-eye calibration

因此需事先对机器人和背靠相机进行手眼标定，由于相机相对机器人基坐标系固定不动，则相机在机器人基坐标系下的位姿可描述为式（22）所示：

通过对背靠相机进行的标定，可以分别得到背靠相机的内部参数和外部参数。为了将下相机的坐标系转换到上相机坐标系中，设上下相机相对机器人基座标系的外参分别为CU和CD，则下相机坐标系到上相机坐标系的旋转和平移矩阵可表示为

已知下工件端面为对接装配的基准，设下工件端面的法向量nD=[0 0 1]，根据上下工件端面的法向量，在上相机坐标系中可得上下工件端面的法向量夹角如式（23）所示：

则上工件端面相对下工件端面的位置和姿态调整量可描述为如式（24）所示：

在机器人基座标系下的位置和姿态调整量如式（25）所示：

由于在机器人位姿调整过程中，姿态的偏移会影响到位置信息，因此先通过机器人进行姿态迭代调整，当满足姿态调整的阈值后，再进行位移迭代调整，直至满足位移偏差阈值，完成机器人对上工件端面的位姿调整，实现上下工件端面的对接。

3 实验结果与分析

图6所示为机器人对接装配装置，主要由机器人、测量装置、装配台等构成。图7为背靠视觉和排列点激光形成的测量装置。机器人型号为KR600（KUKA，最大负载600 kg）。点激光的型号为ACR-LDS510-10（上海达拉斯光电科技有限公司，测量范围为10 mm，分辨率为0.5 μm，重复精度为2 μm）。相机的型号为MV-CH650-90TM-M58S-NF（海康威视，分辨率为9 344×7 000 像素），镜头的型号为LSF2528，焦距为25 mm。软件工具为Halcon 20.05，运行平台为win10 X64、内存32 GB、硬盘1 TB、处理器i9-9900K。在图6中，上下工件的端面尺寸直径为204 mm、端面止口双向间隙为0.08 mm。

图6 机器人对接装配装置Fig.6 Robot docking assembly device

图7 背靠视觉和排列点激光测量装置Fig.7 Measuring device with back against vision and arrangement point lasers

图8 和图9 分别为背靠下相机和上相机的部分标定图像，由此可得到背靠相机的标定参数，以及上相机和下相机相对机器人基坐标系的外参变换关系。

图8 不同位姿下的下相机标定图像Fig.8 Calibration images captured by the down camera under different positions and attitudes

图9 不同位姿下的上相机标定图像Fig.9 Calibration images captured by the up camera under different positions and attitudes

为更好地判断本文方法的精度，将标定板安装在上工件端面，以标定板的中心点作为特征点，从而得到机器人的位移调整精度，如图10 所示。

图10 标定板位移验证Fig.10 Displacement verification with calibration plate

分别移动机器人示教器的X轴和Y轴，并固定步长，进行标定板位移偏差的重复测验，结果如表1 所示。从表1 可以看出，机器人的位置调整精度在X方向和Y方向均小于0.01 mm，产生误差的原因主要在于大负载机器人存在一定抖动、机器人定位精度误差等因素。图11 所示为下相机采集的下工件端面图像，图中显示了特征边缘提取及拟合中心。图12 为上相机采集的不同位姿下的上工件端面图像，图中显示了边缘提取及拟合中心。

图11 下工件端面Fig.11 End-face of down workpiece

图12 上工件端面的迭代调整图像Fig.12 Iterative adjustment images of the end-face of up workpiece

为证明本文方法的有效性，将本文方法与方法1［4-5］、方法2［7，26］和方法3［27］进行比较分析。由于物体的空间姿态会影响空间位置，因此首先对物体进行姿态调节，然后再进行位移调整。表2 为端面姿态测量验证结果。可以看出，端面姿态的测量结果相对机器人的姿态有约0.03°的偏差，相比表1、表2 的位移偏差明显增大，这是由于标定板图像中心点的边缘识别和提取精度更高，受视场和光照等因素影响，端面特征更难提取，导致位移偏差增大。

表2 端面圆姿态测量结果Table 2 Attitude measurement results of the end-face of up workpiece （°）

表3 为采用本文方法时机器人的位置调整情况，下工件端面基准为手控器中X轴和Y轴的数据，在3 次不同基准下，通过上工件端面的重复位移调整验证，可以看出X方向最大偏差为0.01 mm，Y方向的最大偏差为0.03 mm。

表3 不同基准下端面位置调整验证Table 3 Position adjustment verification of up workpiece under different position benchmarks

表4 为不同方法下的姿态测量结果，从表2 和表4 中可以看出：方法1 的姿态偏差约为0.1°，这是姿态解算精度不高引起的；方法2 的姿态偏差约为0.03°，方法3 的姿态偏差约为0.04°，虽然方法2 和方法3 的姿态精度非常接近本文方法，但本文方法具有更优的一致性，且收敛速度更快。

表4 不同方法下端面姿态调整比较Table 4 Comparison of end-face attitude deviation with different methods （°）

表5 为在不同基准下不同方法的端面位置偏差情况，图13 为在同一基准下进行多次重复实验时，不同方法的端面位置偏差情况。由图13 可以看出，方法1 的位移偏差最大，方法2 与方法3 的位移偏差接近，但方法3 的收敛性更快，本文方法具有最小的位移偏差和最多的迭代次数。从表2 和图13 可以看出，本文方法的姿态和位移需3～5 次迭代调整。

图13 重复实验下不同方法位移偏差和迭代次数Fig.13 Displacement deviation and iteration times of different methods in repeated test

表5 不同方法下端面位置调整比较Table 5 Comparison of end-face position adjustment with different methods

由表4、表5 和图13 可以看出，方法1 的姿态计算偏差和位移偏差最大，原因在于采用最小二乘法进行椭圆中心提取属于无偏差估计方法，受图像噪声、边缘提取精度影响较大，尤其在大视场下，图像噪声更易受光照影响。方法2 和方法3 的偏差优于方法1 的偏差，这是因为方法2 和方法3 对单目相机测量姿态产生的二义性解进行了正确解的判断和提取，仍存在较大偏差的主要原因是该方法受端面图像特征边缘的提取精度影响较大。方法2 和方法3均具有较大的姿态偏差和位移偏差，其位移计算与姿态计算相互影响，姿态的调整偏差影响了位移的调整。相比方法1、方法2 和方法3，本文方法获得了更小的姿态计算偏差和位移调整偏差。这是因为本文方法采用了RANSAC 算法进行椭圆拟合，可以减少噪声的影响，具有较高的鲁棒性。此外，本文方法采用点激光位移传感器建立法向约束，对单目相机测得的姿态进行正确解提取。由表4、表5 和图13 可以看出，相比方法1、方法2 和方法3，本文方法具有姿态调整准确、位移调整收敛快的特点，姿态对接角度最大偏差为0.03°，中心位移X方向的最大偏差为0.01 mm、Y方向的最大偏差为0.03 mm，证明本文方法的有效性和准确性。

图14 所示为机器人完成的对接装配，通过对上下工件的端面尺寸直径为204 mm、端面止口双向间隙为0.08 mm 的验证实验可知，姿态和位移需3 次～5 次迭代调整可完成对接装配。这证明了本文方法的有效性、鲁棒性和实用性。

图14 机器人完成的对接装配Fig.14 Docking assembly completed by the robot

4 结束语

本文从机器人对接装配工程应用出发，并考虑圆位姿解的二义性问题，提出一种背靠视觉端面对接定位方法。采用RANSAC 算法进行椭圆拟合，以避免在采用单目视觉进行测量时，姿态和中心位置图受图像噪声、边缘提取等因素影响。通过排列点激光建立法向约束，得到更准确的圆姿态解。实验结果表明，该方法的姿态对接角度最大偏差为0.03°，中心位移X方向最大偏差为0.01 mm、Y方向的最大偏差为0.03 mm，具有姿态调整准确、位移调整收敛快的特点。下一步将引入深度学习等人工智能技术，以减少该定位方法的位姿调整次数，提高对接效率。