基于双目结构光的工件位姿检测方法

宛江洪，曹雏清，高 靖

(1.安徽工程大学电气工程学院，安徽芜湖 241000；2.安徽工程大学计算机与信息学院，安徽芜湖 241000；3.芜湖哈特机器人产业技术研究院有限公司，安徽芜湖 241000)

0 引言

工业机器人在生产中执行抓取、搬运任务时需要获取工件位姿，常用的工件位姿检测方法是基于双目视觉位姿检测。王才东等[1]利用双目视觉检测机械零件的位姿,Z轴方向重复误差达到了3.42°；江士雄等[2]基于双目视觉对4 cm×2 cm×2 cm大小的矩形块检测位姿，X轴方向平均平移误差和平均角度误差为3.1 mm和6°；牛晨等[3]基于双目视觉检测15 cm×10 cm×4 cm大小的箱体位姿，重心坐标误差为1.57 mm，Z轴方向误差达到了3.33°。由于双目视觉测量会随着基线距离的增加而误差增大[4-5]，较少用在工件放置范围大或大尺寸工件位姿检测的应用场景。因此，提出了一种面向较大范围的物体空间位姿检测系统，实现了在标准托盘空间范围内对工件的准确位姿检测。

1 位姿检测系统方案

双目结构光的位姿检测系统由双目结构光视觉采集系统、数据处理、点云配准3个主要部分组成，其工作流程图如图1所示。

图1 位姿检测系统工作流程

图1中，双目结构光获取点云信息后，进行点云数据处理，通过直通滤波、分层遍历寻找、点云数据矫正后，将点云信息转换为2D图像，再进行图像处理，通过图像二值化、腐蚀、轮廓检测完成多个工件分割并得到位置信息。在点云配准中，利用图像处理中获得的位置信息进行点云粗配准，再进行ICP(iterative closesf point)精配准[6]，获得模板与工件的转换矩阵，从而得到工件位姿信息。

2 双目结构光三维视觉系统

双目结构光三维视觉系统如图2(a)所示，主要由固定机架、2个工业相机和1个投影仪组成，实验平台如图2(b)所示，双目结构光三维视觉系统安装高度为3 m，实验在1 200 mm×1 000 mm标准托盘上进行，实验工件选取600 mm×400 mm×300 mm料框。

(a)双目结构光三维视觉系统

(b)位姿检测实验平台图2 双目结构光和位姿检测实验平台

3 数据处理

3.1 点云处理

在采集工件点云数据时，利用直通滤波将获取的点云数据限定在需要的范围内。寻找工件最上层，是通过对相机Z轴分层遍历完成。具体步骤如下：

(1)找到相机Z轴点云数据最小值Zmin并确定分层高度H和阈值S；(2)循环遍历(Zmin，Zmin+H)、(Zmin+H，Zmin+2H)…范围内的点云数目；(3)当范围内的点云数目超过阈值S时，循环停止，此时认为找到了工件的最上层。

安装双目结构光三维视觉系统时，不能保证相机垂直于地面，因此得到的点云数据会有一定程度的倾斜。为矫正点云数据，利用工件最上层平面法向作为Z轴，重新建立坐标系来矫正原始点云数据。具体步骤如下：

(1)对最上层点云数据RANSAC平面拟合，得到平面方程见式(1)，由平面方程知法向量为(A，B，C)，并求得质心坐标Cm(xm,ym,zm)。

Ax+By+Cz+D=0

(1)

(2)以工件最上层平面法向量为Z轴，保持Y轴主方向不变，利用Y轴和Z轴叉乘得到X轴，通过X轴和Z轴叉乘得到Y轴，并以质心Cm为坐标原点建立坐标系。设原坐标系为(e1,e2,e3)，其中e1(1,0,0)T，e2(0,1,0)T,e3(0,0,1)T，新坐标系为(e′1,e′2,e′3)，已知e′3=(A,B,C)T，则

e′2=e′3×ei=(0,C,-B)T

(2)

e′1=e′2×e′3=(B2+C2,-AB,-AC)T

(3)

则原坐标系到新建坐标系的旋转矩阵m为

(4)

(3)已知质心Cm在原坐标系坐标为(xm,ym,zm)且新建坐标系原点在质心处，从原坐标系到新建坐标系的转换矩阵为T，利用转换矩阵T完成点云数据的矫正。

(5)

3.2 图像处理

3.2.1 点云转图像

矫正点云数据后，将上层点云映射到平面，利用KD-tree近邻检索[7]，将映射到平面的每个点云数据对应成像素点转成2D图像如图3所示。具体步骤如下：

(1)工件上层点云数据映射到平面；(2)计算映射到平面的极值Xmax、Xmin、Ymax、Ymin，并根据极值创建相应大小的Mat；(3)建立kdtree索引，由Xmin开始逐行近邻检索，有点云数据对应Mat存入像素点，直到全部搜索完成，得到图像数据。

图3 图像数据

3.2.2 轮廓检测

图像轮廓不仅能提供物体的边缘，还能提供物体边缘之间的层次关系以及拓扑关系。通过轮廓检测函数检测图像中的轮廓信息，计算出各个轮廓之间的结构信息。最小外接矩形函数可以根据输入的二维点集合计算最小外接矩形并得到矩形的中心位置、矩形的边长以及矩形的旋转角度。通过轮廓检测函数找到最外层轮廓并判断是否有下一个同级轮廓来确定料箱个数i，用最小外接矩形函数计算出最小外接矩形，得到工件的中心位置(Xc,Yc)以及旋转角度angle用于接下来的点云粗配准。具体步骤如下：

(1)对图像进行二值化、腐蚀处理，寻找最外层轮廓；(2)计算最外层轮廓的最小外接矩形，输出矩形的中心坐标(Xc,Yc)及旋转角度angle；(3)判断有无同级轮廓，有同级轮廓则回到第2步，料箱个数为i=同级轮廓数+1；无同级轮廓则轮廓检测结束，料箱个数i=1。

4 点云配准

通过配准获得源点云和目标点云的转换矩阵计算工件位姿。点云配准分为粗配准和精配准2部分，在精配准中常用的方法是ICP算法[8]。ICP配准需要一个旋转平移矩阵的初值[9]，若初值选取的不好，会造成局部最优配准，影响配准效果。

4.1 粗配准

配准需要2个点云数据，目标点云和源点云。目标点云是工件上层点云，源点云是事先做好的模板如图4所示，并且源点云质心在坐标原点位置。将源点云与目标点云短边中点位置进行配准。首先对目标点云进行处理，将目标点云的质心移到坐标原点位置。在图像处理中，得到了在图像信息里工件中心位置(Xc,Yc)，工件长边L以及旋转角度angle，则得到转换矩阵T1，通过转换矩阵T1将目标点云的质心移到坐标原点处。

图4 源点云

(6)

为使ICP配准更准确快速，结合料框长边L得到ICP配准旋转平移矩阵T2(R,t)初值。

(7)

式中：R为旋转矩阵；t为平移向量。

若先配X轴正方向，则a=1，反之a=-1。

4.2 精配准

ICP算法的基本原理是：在源点云P和目标点云Q中，按照约束条件找到转换矩阵T(R,t)，使得源点云P中的点经过转换矩阵T(R,t)变换，与目标点云Q中的点误差最小，误差函数为E(R，t)。

(8)

其中pi∈P，qi为pi在目标点云Q中找到的配准点，配准结果如图5(a)所示。

4.3 真实场景粗配准分析

比较使用本文粗配准方法与常用的粗配准方法配准，再进行ICP配准所获得的结果，如图5所示。FPFH(fast point feature histograms)[10]粗配准方法效果如图5(b)所示，3DSC(3D shape context)[11]粗配准方法效果如图5(c)所示，配准结果均不能配准到短边中点位置，本文的粗配准方法配准(如图5(a)所示)效果较好。

(a)本文方法

(b)FPFH粗配准方法

(c)3DSC粗配准方法图5 配准方法实际效果

5 实验误差分析

为验证位姿测量数据的准确性，实验先对1个料箱位姿重复检测5次，实验数据如表1所示。再同时对2个料箱位姿重复检测5次，实验数据如表2所示。

表1 1个料箱位姿数据表

表2 2个料箱位姿数据表

表1是1个料箱位姿检测结果，系统对料箱在绕Z轴、Y轴和X轴方向旋转角度测量波动分别仅有0～0.073°、0～0.049°和0～0.104°，质心位置最大波动为0.413 mm。表2是同时放置2个料箱位姿检测结果，料箱1：在绕Z轴、Y轴和X轴方向旋转角度测量波动分别仅有0～0.092°、0～0.025°和0～0.234°，质心位置最大波动为0.37 mm。料箱2：在绕Z轴、Y轴和X轴方向旋转角度测量波动分别仅有0～0.118°、0～0.027°和0～0.283°，质心位置最大波动为0.265 mm。位姿测量结果稳定，可满足工业机器人上下料抓取需求，抓取实验如图6所示。

图6 抓取实验

6 结论

面向机器人在柔性上下料中工件尺寸大、摆放位姿不确定的应用场景，提出了一种基于双目结构光的框型工件位姿检测系统。视觉采集部分采用了双目结构光来获取点云信息；在数据处理部分，采用点云与2D图像相结合的方式，完成点云的粗配准以及工件分割；最终利用ICP配准，得到工件位姿信息。实验结果表明，无论是对一个料框还是多个料框的位姿检测效果都较好，检测系统稳定，并已经应用到料箱抓取搬运系统。