基于双臂机器人和三维视觉的物体抓取和轴孔装配

徐友法，王卫军，王兆广，陈宝

(中国电子科技集团公司第二十一研究所，上海 200233)

0 引 言

随着三维物体抓取需求的增加，基于三维视觉的抓取得到了广泛研究。目前，研究领域主要集中在三维抓取标定、姿态估算、抓取路径规划等领域。

姿态估算中，由于采集的图象已经转换成点云图，因此姿态估算主要研究为点云的匹配技术。点云匹配一般分为粗匹配和精匹配，粗匹配技术主要有随机抽样一致性[1-3](RANSAC，Random Sample Consensus)、主成分分析[4](PCA，Principal Component Analysis)等，精匹配有近点迭代法[5-6](ICP，Iterative Closest Points)等。

点云匹配时，需要有一个模板作为标准，即模板建立。点云模型建立主要有二种方式获取： 基于三维CAD建图软件建立物体的三维模型[7-8]，基于视觉处理软件获得三维模型[9-10]。视觉处理方式采用深度相机或双目相机对物体沿不同方向拍摄，通过多图象合成物体的三维模型。摄象机拍摄主要是抓取物体表面特征，或者通过几次拍摄获取多个表面特征，虽然也能改善之前的一些特殊情况的局限性，但仍然存在产品自身拍摄的遗漏。

在理论研究匹配算法得到广泛研究时，基于匹配算法的实际抓取研究则主要集中在二维视觉领域，其主要原因为理论匹配矩阵和实际抓取时机器人的姿态和抓取点无法方便地建立联系。对于平面物体的抓取，我们可以通过旋转角度和中心点的坐标直接进行抓取[11-12]。对于三维物体定位，物体的抓取点确认和抓取姿态确认则无法通过简单的模板匹配矩阵直接获取。现在常用的方法是通过深度学习方法直接获取抓取位置和姿态[13-14]，但是此方法要求较高，需要深度学习算法的训练，并且深度学习算法会引入一定的误差，对于固定物体抓取不适用。

本文基于目前研究比较成熟的模板匹配矩阵，通过机器人示教的方式获取物体的抓取位置和抓取位姿。整个抓取过程中，只有匹配的精度误差，基于匹配矩阵获取抓取位置和抓取位姿的方式不会引入新的误差。假设在匹配精度零误差的情况下，示教获取的抓取位置和抓取姿态，在新的抓取过程中是完全复制的过程。

1 点云匹配矩阵获取

基于点云模板的加工工件自动识别算法，其算法的核心是先利用基于点云模板的匹配算法RANSAC进行点云粗匹配，将工件点云数据和数字模型库中的数模转换到较近的位置，再使用经典的点云最近点迭代法(ICP)进行精匹配，完成目标文件的数字模型自动识别。

在点云匹配之前，需要对3D传感器获取的点云进行分割获取目标物体的点云。本文采用区域分割的方法对采集的点云进行分割，基本原理：先对需要分割的区域找出一个曲率最小的种子作为生长的起点，然后将种子领域中具有相同性质的点归并到种子点中，新的种子点继续作为种子寻找下一个种子点，直到没有新种子点出现，所有的种子点就形成一个分割的区域。分割完成后，根据点云规模获取目标物体的点云，即可进行模板匹配获取匹配矩阵。

1.1 粗匹配获取旋转矩阵

本文采用RANSAC预匹配方法，其主要过程：估计点云的参考系，计算点云参考系与正则坐标系对齐的变换，然后将点云通过坐标转换移动到正则坐标系下，使采集点云和模板点云都移动到正则坐标系下的位置，作为精匹配的初始位姿。此方法实施简单，可靠性较高，可以作为分割效果较好的点云预匹配方法。

点云的参考系采用主成分分析法获取：利用PCA(主成分分析)建立参考系，最小的特征值对应的特征向量作为坐标系的Z轴，为了保证Z轴指向观察点，如果Z轴和观察方向不同(根据夹角范围判断)，则舍弃。最大的特征值对应的特征向量作为坐标系的X轴，Y轴通过X轴和Z轴叉乘得到。

(1)

然后，可以得到C的特征值λj和对应的特征向量vj，j∈{1,2,3}。特征值按升序排列，采用与最小特征值相关的特征向量v1作为参照系的z轴。参考坐标系的x轴是与最大特征值相关的特征向量v3。y轴由v2=v3×v1给出。

从参考系中，可以得到一个转换矩阵[R|t]，使它与正则坐标系对齐。然后来用[R|t]矩阵转换点云，使所有点云转换到正则坐标系下，定义如下：

(2)

通过预匹配，效果如图1所示，其中深色为采集点云，浅色为模板点云。

图1 粗匹配结果

1.2 精匹配获取高精度旋转矩阵

采集点云和模板点云都移动到正则坐标系下，基于ICP算法进行匹配，通过刚性矩阵变化，实现采集点云和模板点云的精匹配。刚性匹配矩阵如下:

(3)

式中：R3×3是旋转矩阵；T3×1是平移矩阵，表达式如下：

(4)

(5)

式中：α，β，γ分别为绕X，Y，Z轴的旋转角度；tx，ty，tz分别表示点沿着X，Y，Z轴的平移量。

ICP算法基本原理：分别在待匹配的目标点云PX和源点云Q中，按照一定的约束条件，找到最临近点(pi,qi)，然后计算出最优匹配参数R和t，使得误差函数最小。误差函数定义：

(6)

通过ICP算法匹配效果如图2所示，其中深色为采集点云，浅色为模板点云。

图2 精匹配结果

2 机器人抓取位姿获取

2.1 机器人抓取姿态获取方法设计

机器人姿态获取方法需要提前获取模板点云和采集点云的模板匹配矩阵，通过矩阵转换，将提前设定好的机器人抓取点和机器人抓取位姿转换成新采集物体点云的抓取点和抓取位置，流程如图3所示。

图3 机器人位姿获取流程

过程解释如下：

(1)将工件按标准姿态摆放

将工件的正面朝上，使方框内(方框如图4所示)平面与相机保持水平，同时也要和机器人的Z坐标水平，即平面的Z坐标在相机下和机器人下都是保持不变的；工件的箭头方向和机器人的X轴向相同。

图4 设置机器人抓取位姿

(2)在点云上面获取整个工件的中心点P0(x0,y0,z0),作为后面的抓取点；取平面的三个标记点P1(x1,y1,z1)，P2(x2,y2,z2)，P3(x3,y3,z3)作为判定工件方向的基准点，其中P1,P2两个点和机器人的X轴相同，P2,P3与机器人的Y轴相同，点的设置如图4所示。

(3)采集点云，获取所有点的点云坐标

(4)通过模板匹配，得到模板点云到采集点云的旋转平移矩阵和逆矩阵。

(6)采集新的点云，获取模板到采集点云新的旋转平移矩阵。

(7)

(8)

(9)

式中：α1为机器人绕X轴旋转角度；β1为机器人绕Y轴旋转角度；γ1为机器人绕Z轴旋转角度。

(10)

则机器人的抓取姿态为(xreal,yreal,zreal,α,β,γ)。

2.2 机器人预抓取位置设置

在机器人进行抓取时，机器人到达抓取位置姿态要保持不变，否则抓手运动轨迹可能会和物体本身、桌面产生干涉。

具体的预抓取位置设定方法如下：

设A(x1,y1,z1),B(x2,y2,z2),C(x3,y3,z3)是已知平面上的三个点。A,B,C可以形成三个向量:向量AB,向量AC和向量BC,则AB(x2-x1,y2-y1,z2-z1),AC(x3-x1,y3-y1,z3-z1),BC(x3-x2,y3-y2,z3-z2),设平面的法向量为(x,y,z)则有：

(11)

3 实 验

3.1 机器人抓取点位姿获取实验

本文采用三维相机直接获取点云的形式采集物体的三维点云数据集。通过CAD方式画出物体的标准三维图，通过模型转化的方式，获取物体的三维点云模型。

(1)设定机器人抓取点：在采集的点云上面取4个点，P0为抓取点，P1，P2，P3为姿势估计点，点的坐标为P0(34.445 8；-33.446 4；724.207)，P1(56.385 7；-30.631 7；724.446)，P2(41.570 2 ；-23.418；724.288)，P3(36.856 3；-32.024 6；723.995)，效果如图5所示。

图5 采集点云抓取点位姿设计

(2)创建物体的点云模板，并通过模板匹配，获取采集点云到模板的转换矩阵：

(3)通过矩阵旋转平移，标志点在模板上的坐标为P′0(-0.455 947 4;-1.197 418 2;0.058 593 75)，P′1(11.896 341 ;-19.549 126 ;-0.015 991 211)，P′2(11.702 364 ; -3.070 591 ;-0.024 963 379 )，P′3(1.892 770 3 ;-2.712 799 1; -0.195 495 61)，则标记点在模板上的位置如图6所示。

图6 模板点云抓取点位姿设计

(4)测试新的采集点云进行抓取点进行实验：

采集新的点云1：获取模板到点云的旋转平移矩阵，将模板点云上的4个标记点也通过旋转矩阵平移，得到如图7所示，其中浅色为模板点云，深色为采集点云，4个白色点为采集点云的抓取点。由于抓取点超出了采集点云范围，模板点云在此处显示的是抓取点的位置。

图7 新采集点云抓取点位姿

采集新的点云2：获取模板到点云的旋转平移矩阵，将模板点云上的4个标记点也通过旋转矩阵平移，得到如图8所示。

图8 新采集点云抓取点位姿

通过两次测试，测试效果较好。

3.2 基于ABB双臂机器人的抓取实验

本实验完成了抓取工件的制作和加工，采用轴孔装配的方式设计了如图9所示的装配工件，模拟电机转子向定子中装配的过程。

图9 抓取工件

图10 装配工作站

实验平台框架如图10所示，组成部件主要包括三维相机、双臂机器人、机械抓手、待抓取工件。各部件共同合作实现工件的智能抓取。三维相机和机械臂的标定方法采用常用的6点标定，即使用机器人的6个点和三维相机采集到的6个对应点获取转换矩阵即可。

为了测试抓取效果，本文设计了两个抓取工件，左机械臂设定为定点抓取，右机械臂通过三维视觉定位并抓取，然后两个物体进行装配，两个物体内外径直径差为2 mm，装配示意如图11所示。

图11 装配示意图

装配过程效果如图12所示。

图12 工作站装配效果

实验结果显示，实际抓取时右侧机械臂能够较准确地完成抓取。100组实验的抓取中，模板匹配成功率为80%，在模板匹配成功的条件下，抓取成功率100%，并能顺利完成装配，取得了较好的实验效果。

4 结 语

本文基于RANSAC和ICP模板匹配算法获取矩阵，研究了一种机器人抓取点确定和机器人末端姿态获取的方法；同时为机器人设计了一个预抓取位置，机器人在抓取时，调整抓取姿态到达预抓取位置后，保持抓取姿态不变，到达抓取点对物体进行抓取。抓取精度取决于模板匹配精度和示教抓取点精度，没有引入其他误差，具有较高的重复性精度。通过基于ABB双臂机器人的抓取实验验证了方法的可行性，解决了匹配矩阵在实际生产和抓取应用中难落地的情况。