基于线结构光视觉的方形状物体位姿估计*

叶文达，侯宇瀚，陈洪佳，李艺帆

(广东工业大学 机电工程学院，广东 广州 510006)

0 引言

大部分攀爬机器人是在本体两端安装夹持器[1-3]，一端吸附或者抓取环境中的杆状物，另一端寻找下一个抓夹点[4],两个夹持器交替轮流抓夹来实现机器人攀爬运动。机器人基座夹持器需要紧紧地抓住目标杆件以防止本体的掉落，这就要求机器人末端能正确估计目标杆件的位姿。基于方形状物体的截面为矩形的思想,我们提出一种方形状物体位姿估计方法，该方法的原理是将两束线结构光投射到方形状物体表面上，产生两个矩形条纹用以拟合矩形，再将矩形的信息映射到三维空间中用以估计目标方形状物体的轴线向量、参考点及边长，从而实现非接触自动化操作。

1 硬件系统搭建

我们提出了一种新颖、简单、低成本的传感装置来估计方形状物体的位姿，其结构如图1所示，主要组成部分有工业相机、线结构光发生器、滤光片以及安装夹具。当系统运行时，线结构光发生器投射一字型线结构光到方形状物体表面并反射，在相机的CMOS芯片中形成了反映方形状物体特征的条纹。此外滤光片的存在，可以滤除部分环境干扰的自然光以及工件表面反射出来的光，从而只有特定波长的光能够在相机中成像。由于在检测过程中需要同时利用到两个线结构光发生器，所以需要根据实际情况来确定相机与线结构光发生器的安装位置。两个线结构光发生器均与相机呈一定夹角放置,如此设置可以在方形状物体上形成明显的光条纹，且可以较好地吸收来自被测对象的反射光，适用于表面光滑、反光能力强的工件。

图1 硬件系统搭建

2 系统标定

系统标定是根据系统的模型标定出相关的系统参数，这是将相机中的像素信息与三维坐标系下的方形状物体位姿信息进行转化的重要步骤之一，标定结果的准确度直接影响着估算结果的准确性。

2.1 相机标定

采用张正友相机标定法[5]获取相机的内参和外参。相机的成像模型是由相机坐标系、图像物理坐标系、像素坐标系、世界坐标系之间相互转换建立的，相机的成像模型[6]如图2所示。

图2 相机成像模型

假设空间点P为在世界坐标系OW-XWYWZW中的一点,点P对应在相机坐标系OC-XCYCZC中的坐标为(xc,yc,zc),投影在图像物理坐标系O1-XY中的点P1的坐标为(x,y),转换在像素坐标系o-uv中的坐标为(u,v)。相机的成像模型如下：

(1)

其中：fx、fy分别为x轴和y轴上的归一化焦距；u0、v0为相机图像的主点坐标；R、T分别为相机外参中的旋转矩阵和平移矩阵；MC为相机内参；MW为相机外参。

2.2 线结构光标定

线结构光发生器投射出来的线结构光可以理解为在三维坐标系下的一个平面[7]，因此线结构光标定可以等效为拟合在相机坐标下的一个平面，当测量的参考坐标系为相机坐标系时，线结构光平面的方程为:

ZC=AlXC+BlYC+Cl.

(2)

其中：Al、Bl、Cl均为光平面参数。假设Mi为第i张标定图像的外参，如下式所示:

(3)

其中：n=[nx,ny,nz],o=[ox,oy,oz],a=[ax,ay,az]分别为世界坐标系OW-XWYWZW的X轴、Y轴、Z轴在图像物理坐标系O1-XY中的方向向量；p=[px,py,pz]为世界坐标系OW-XWYWZW的原点在图像物理坐标系O1-XY中的坐标。由此可知标定板平面在相机坐标系下的平面方程为：

ax(x-px)+ay(y-py)+az(z-pz)=0.

(4)

通过联立式(1)和式(2),可以得到线结构光平面上的特征点在相机坐标系下的三维坐标，通过对这些点进行平面拟合，即可得到相机坐标系下的光平面方程，从而求得Al、Bl、Cl。线结构光平面拟合结果如图3所示。

图3 线结构光平面拟合结果

2.3 手眼标定

我们采用Eye-in-Hand的安装方式，手眼标定目的是求得相机坐标系与机械手坐标系的转换关系。标定过程中的变量有两个，一个是机器人末端相对于机器人基坐标系的位姿关系，要求能够记录不同拍摄角度下机器人当前的位姿，这个变量会显示在机器人的示教面板上，是已知参数；另一个是标定板坐标系相对于相机坐标系的位姿矩阵，这个变量可以通过相机的内外参数计算得到。对于标定过程中任意两个拍摄角度，可以列出如下关系式：

(5)

式(5)可以改写成下面的形式：

(6)

上式中，X即为需要求解的手眼关系矩阵。

3 位姿估计算法实现

3.1 图像预处理

原始图像如图4所示，传感系统所采集的图像并不能直接应用于方形状物体位姿的信息提取, 必须先通过特定的图像预处理方法提取方形状物体表面的特征点，再进行计算以求解位姿。

图4 原始图像

为了尽可能地排除环境光的干扰，需要对原始图像进行处理。首先将相机设置为完全曝光，再经过中值滤波去掉图像中的部分噪声，最后通过阈值分割后得到只保留激光线条纹的二值化图像，处理结果如图5所示。

图5 图像预处理

3.2 中心线提取及聚类

如图6所示, 我们利用Steger算法[8]对激光线条纹进行中心线提取，并过滤掉部分噪声。为了区分两个线结构光发生器对应的激光线条纹，在进行后续算法前，需要对所提取的条纹进行聚类，对应两个线结构光发生器。本文采用DBSCAN算法[9]对图像中的激光线条纹进行聚类，将图像中的激光线条纹的像素点分成两类。

3.3 特征点提取

因为两激光线条纹的特征点提取过程相同，故只需讨论其中一激光线条纹的特征点提取情况。为了获取条纹上的特征点从而进行矩形的拟合，需要提取激光线条纹端点的像素坐标以及两直线交叉点的像素坐标。

在图像像素坐标系下，其端点的像素坐标的u值分别对应于图像中像素坐标的最大值umax和最小值umin，因此对像素点进行遍历找到最大值和最小值，即可获取端点的坐标值。根据下式可得出端点对应的像素坐标ue1和ue2的值：

(7)

同时根据u值找到对应的v值,即得到两端点的像素坐标(ue1,ve1)和(ue2,ve2)。

对于交叉点的像素坐标，本文采用霍夫变换算法[10]寻找直线，先是检测到两直线并求得直线的方程l1和l2，再联立两方程求解交叉点坐标(ui,vi)。

根据以上步骤，可获取端点和直线交叉点的像素坐标，即可得到矩形拟合所需特征点的像素坐标，如图7所示。

3.4 矩形拟合及点映射

将已经计算出来的端点和交叉点的像素坐标通过OpenCV的矩形拟合算法对特征点计算最小外包旋转矩形,如图8所示。

图6 提取中心线 图7 特征点提取 图8 矩形拟合

设p1(x1,y1)、p2(x2,y2)、p3(x3,y3)、p4(x4,y4)为求得拟合矩形的四个顶点坐标，矩形中心点的像素坐标为[11]：

(8)

根据下式将像素点(uc,vc)映射到空间三维点中，得到空间矩形的三维中心点(xw,yw,zw)[12]：

(9)

3.5 位姿估计

由于可以同时获取两拟合矩形的像素中心点坐标，我们可以分别得到两组空间矩形的三维中心点坐标：

(10)

因此根据两中点的坐标，我们可以获得方形状物体的轴线，轴线的方程可以表示为：

(11)

(12)

同理，通过把交叉点和端点映射到空间三维点，我们可以求得方形状物体的边长，矩形相邻的两边长分别为：

(13)

(14)

若方形状物体截面为一般矩形，则d1≠d2,若为正方形，则d1≈d2。对于方形状物体的轴线方向的测量，如果捕获了不止一帧图像，则可以在花费更多计算时间的情况下获取更精准的结果，对多帧图像分别提取拟合的矩形中心点坐标和特征点坐标，并对这些坐标进行最小二乘法拟合[13]，得到更加准确的轴线向量和边长。

4 实验验证

为了验证所提出的方案和分析结果，本文进行实验来验证传感方法的有效性，实验平台如图9所示。

图9 实验平台

两端带有尖锥的方形杆相对于机器人底座固定，规格为5 cm×5 cm×50 cm。传感系统通过固定夹具安装在UR5机器人末端，设计一个机器人尖点探头将机器人的工具中心点转移到尖点处。实验中被检测方形杆件两端安装有尖点，通过尖点探头与杆件尖点触碰的方式获取杆件位姿的真实值，然后采用下式计算方形杆件轴线方向向量：

(15)

其中:Qt和Qb分别为从UR5示教器中读取的方形杆件两端尖锥的中心点相对于基坐标系的坐标，即Qt=(xt,yt,zt),Qb=(xb,yb,zb)。

为实现机器人对杆件的抓夹，通过手眼关系转换得到被测方形杆件相对于机器人基坐标系的位姿。在实验中，机械手携带传感系统进行移动，使得观测点每次都发生变化。为获得对单帧图片检测结果，我们对每个观测点拍摄一张图像，共进行10次；为检验算法的迭代优化能力，我们逐轮组织检测实验，每轮10个观察点，总共进行10轮实验。利用真值与被测轴线向量的角度误差以及真值与被测的边长误差来验证检测结果的准确性。单帧检测结果如图10所示，迭代检测结果如图11所示。

图10 单帧检测结果

图11 迭代检测结果

从图10中可以看到，在仅仅提供一帧图片时，本文提出的检测系统检测轴线的向量角度误差控制在2°以内，方形杆长的误差小于2 mm。如图11所示，对多帧图片的检测，轴线向量角度的最大误差小于1.63°，可以返回一个平均角度误差为1.58°的轴线向量检测值；边长的最大误差小于1.67 mm，可以返回一个平均误差为1.62 mm的边长检测值，因此我们所提出的传感系统不仅能够通过一次检测返回高精度的结果，而且能够对多次检测的结果进行优化。

5 总结

杆件的位姿估计对于攀爬机器人尤为重要，可以赋予机器人感知外界的能力，指导机器人进行抓夹或者攀爬。本文提出一种基于线结构光视觉的方形状物体位姿估计方法，首先通过对激光线条纹图像进行处理提取特征点，再通过矩形拟合的方法得出中心点坐标，继而映射到空间三维中，获取目标杆件的轴线向量、轴上的参考点以及边长，从而估计出被测方形状物体的位姿。本文提出的位姿估计方法简单、有效、易行，通过实验验证了方法的可行性，利用该方法能较好地实现爬杆机器人对方形状物体的位姿估计。