基于3D 视觉的机械臂抓取控制方法①

潘 聪 方灶军 张群莉 孙晨阳 戴伊宁

(*浙江工业大学机械工程学院 杭州 310014)

(**中国科学院宁波材料技术与工程研究所 宁波 315201)

(***浙江省机器人与智能制造装备技术重点实验室 宁波 315201)

0 引言

近年来,机器人抓取技术在工业生产中扮演着越来越重要的角色。但传统的二维(2D)平面物体抓取满足不了日趋复杂化的生产环境[1]。随着三维(3D)视觉技术的发展,包含深度信息的立体抓取技术成为国内外学者的研究重点。3D 视觉技术通过位姿估计实现定位,但物体的位姿识别容易受到环境干扰,因此,可靠、精确地识别物体位姿仍是该领域的一个难点。

国内外各个研究者在这一领域提出了各种不同的位姿估计算法。其中,基于模板匹配的方法被广泛用于物体的识别定位中。文献[2]提出了点对特征算法(point pair features,PPF)来进行三维目标识别,它通过点对间的距离和法线夹角,利用投票方案得出最优的位姿。文献[3]使用计算机辅助设计(computer aided design,CAD)模型,并结合Hough 投票的方法来识别不同的零件。投票算法需要建立物体的特征库,并进行在线搜索,算法整体复杂度和处理时间大大增加。配准算法不但复杂度低于投票算法,而且能保证有效的精度,算法主要分为粗配准和精配准两步,精配准通常选用迭代最近点算法(iterative closest point,ICP)[4]或者它的一些变种[5-6],但ICP 算法容易陷入局部最优解[7]。因此,为了避免这种情况生成,需要粗配准算法提供初始位置。其中,文献[8]结合采样一致性初始配准算法(sample consensus initial aligment,SAC-IA)和ICP 算法实现了点云精确配准定位,但SAC-IA 算法由于是一种贪婪算法,配准效率低。文献[9]采用了共面四点集算法(4-points congruent sets,4PCS),4PCS 算法会在点云中强制选取共面的4 个点进行配准,但由于点云对称会导致配准错误。因此,改进现有算法和提出新算法对位姿估计问题很有必要。

可靠的位姿估计算法是视觉定位的关键,但点云的前期处理也能影响定位精度,包括点云滤波[10-11]、点云简化[12-13]和点云分割[14-16]等。其中,点云滤波和点云简化能够减少无用点对位姿估计的影响,点云分割可以将整个点云按特征分为多个聚类,对其中特定聚类进行处理能够提高运算效率。因此,如何选择合适的前期处理算法来提高视觉定位的精度和效率也是本文研究的重点。

针对上述问题,本文提出了一种基于3D 视觉的机械臂抓取控制方法,主要对点云数据进行处理,获取目标物体的3D 位姿。本文主要贡献如下:在点云预处理阶段,通过差分法和加权下采样算法简化点云;在点云分割阶段,使用欧式距离结合法线夹角变化率的二次分割方法分离点云;在位姿估计阶段,使用主成分分析算法(principal component analysis,PCA)和ICP 算法进行两步配准定位。通过在机械臂上的实验,证明了该方法的有效性和可行性。

1 目标物体位姿估计

为了识别目标物体的三维位姿,本文使用结构光相机采集目标物体的三维点云,点云处理主要由点云预处理、点云分割和点云匹配3 部分组成。

通过三维建模软件获得目标物体完整的计算机辅助设计(computer aided design,CAD)模型,经离散化获得模板点云,并在模板中定义物体的世界坐标系,通过匹配算法可以获取世界坐标系到相机坐标系的转换矩阵,即位姿。此方法具有高精度、操作方便的特点。

1.1 点云预处理

点云预处理主要对原始点云数据进行环境点云去除、去噪和下采样等处理,能够大幅度减少点云的数据量,提高计算机的处理速度。

1.1.1 环境点云去除

在机械臂抓取的工作场景中,环境点云主要为载物面,属于平面点云的一种,本文提出用差分法去除载物面点云,如下所示:

其中,q1和q2是源点云和环境点云的对应点对,D(x,y,z)是差分点,D(x,y,z)=1 代表前景点,D(x,y,z)=0 代表背景点,‖q1-q2‖代表q1与q2之间的距离,H为距离阈值。

利用式(1)提取出源点云中D=1 的点集便是差分结果。差分法不需要迭代,在相同作业环境中也不要更改参数,效率高于拟合算法。

结构光相机采集的点云数据会引入大量的噪点,会对后续的操作产生极大的影响。本文采用半径滤波算法来清除孤立点[17],通过双边滤波算法平滑点云,在去噪的同时能保持边缘信息不丢失[18]。半径滤波算法的原理是通过设定滤波半径,计算每个点在其半径范围内的其他点的个数。半径范围内其他点个数少于某一设定的阈值的点将被滤除。双边滤波算法的原理是为每个点建立k邻域并估算法向量,对该点及其邻域点拟合的平面作为视平面,并建立一个双边滤波器来进行噪点的处理。

1.1.2 点云下采样

为了降低点云密度的同时保留局部特征,本文提出一种基于k邻域和距离加权的点云下采样方法。点云中的边缘和图像中的边缘一样,是蕴含特征最丰富的地方[19]。因此,边缘点不能和内点同时下采样,这会破坏边缘的一部分特征。

首先,本文将边缘点从源点云中剥离,然后着重处理内点。将源点云中的全部内点构造轴对齐包围盒(axis-aligned bounding box,AABB),通过AABB 包围盒的三坐标极值,将包围盒划分为边长L的小立方体栅格,栅格的数量为

其中,mx、my和mz分别为x、y、z轴上包围盒被分割的数量,max 和min 分别为x轴、y轴和z轴的最大值及最小值,ceil函数为四舍五入到大于或等于该元素的最接近整数。根据式(2)、(3)和式(4),小立方体栅格的总数m为

记录点云中每个点所在的位置,再对每个立方体栅格中的点求取它们的重心p。KD 树(K-dimensional tree,k-d tree)是一种高维索引树形数据结构,用于高维空间中的快速最近邻和近似最近邻查找[20]。本文利用k-d tree 查找重心点p的k邻域,k邻域中的点通过与点p的距离远近来进行加权,离重心越远的点权重越小,通过k邻域所有点的加权求取加权重心。因此,本文引入一个惩罚因子作为权重系数ωi,i代表第i次的k邻域采样:

其中d(p,pi) 为重心k邻域中的点pi到重心p的距离。根据式(6)可得加权重心p′:

在所有的体素栅格中,通过式(7)计算出加权重心来代替栅格里的所有点,以此来达到简化目的。

最后再将之前剥离的边缘点放入简化后的点云中,以此就可以在保留边缘特征的基础上对点云进行简化。

1.2 点云分割

点云分割是从点云数据中分离出目标物体。点云分割的结果对后面点云匹配起到了决定性作用,只有完整的分割出待匹配的物体点云,才能获得精确的位姿。

欧式聚类分割算法是一种基于欧式距离的聚类算法,原理是将欧式距离小于阈值的点聚合成单个聚类,从而达到分割目的[21]。从中可以看出,欧式聚类分割的优点在于只需比较点云间的欧式距离,所以处理速度较快;但缺点在于处理重叠在一起的物体点云时,往往会把这些点云当成一个聚类,无法进行分割。因此,本文提出一种在欧式聚类分割的基础上二次分割的方法。利用欧式聚类分割的优势,可以将一些易于分割的点云直接分离,对于那些重叠的点云,本文采用基于法线夹角变化率的方法在欧式聚类分割的基础上进行二次分割。

在欧式聚类分割后,需分辨出分割成功的点云和分割失败的点云,这需要一个指标来度量。本文提出用单个目标物体的实际体积作为指标,体积通过求取点云聚类的有向包围盒(oriented bounding box,OBB)得到。设V0为物体实际体积,V为包围盒体积。若V＜V0,则分割成功;若V＞V0,则分割失败,说明点云中含有2 个及以上物体点云,将进行二次分割。

二次分割采用基于法线夹角变化率的方法。自然界大部分事物都是由凸面构成,而不同物体的连接部分一般为凹面结构[22],所以可以用凹凸面的特性将物体分割开。因此,本文使用近邻点之间法线夹角变化率来代表凹凸面。首先,取一个物体的模型点云,估计表面法线,再计算法线间的平均夹角,记为θ。最后在处理的点云中,构造物体点云的k-d tree,对每个点取k邻域,计算点与其k领域中所有点的法线夹角,并计算夹角与θ的平均变化率σj:

其中,n为点的法向量,原理是根据邻域内的点拟合一个平面,则平面的法线方向即为点的法向量;ni为点的k邻域中第i个点的法向量。计算模型点云中所有点的法线夹角平均变化率σ0:

其中,e为模型点云的点数。

将σ0作为阈值,在分割失败的点云中用式(8)和(10)计算每个点与其k邻域中每个点的法线夹角平均变化率σ,若σ＞σ0,说明这个点为连接点。在点云中,将所有连接点去除,随后再次进行欧式聚类分割,将分割出来的聚类放入第一次欧式聚类分割产生的聚类集合中。

点云分割完毕后,需要选取适合机械臂抓取的点云聚类,但不同的标准会造成不同的选取结果。本文设计了一种抓取分数算法来选取合适的聚类,公式如下所示:

式(11)中G是得到的最终抓取分数,分数越高代表物体越好抓取,其中μ和ξ是比例因子,可以随具体场景改变。式(12)中Fz为z轴分数,zi为分割后的点云中属于一个聚类的所有点的z轴坐标。式(13)中Farea为面积分数,Sj为一个聚类的所有点在xoy平面上的投影面积,Smax是实际物体在xoy平面上的最大面积。由于相机垂直物体拍摄,所以抓取分数越高代表越上方的物体和越平坦的物体是越好抓取的。

1.3 点云配准

通过点云配准方法计算物体的位姿,机械臂便可以根据这个位姿实现物体的抓取。为此,本文提出一种基于PCA 算法和ICP 算法的点云配准算法,即使用PCA 算法实现点云的粗配准,使用ICP 算法实现点云的精配准。

1.3.1 点云粗配准

基于PCA 的粗配准算法如下所示。

(1)取由式(11)得到分数最高的点云聚类记为A,模板点云记为B,计算点云A和点云B的质心点。

(2)计算两组点云的协方差矩阵。

其中,CA为点云A协方差矩阵,CB为点云B协方差矩阵,T 为矩阵的转置。

(3)计算特征向量和特征值。根据式CX=λX,得到协方差矩阵CA和CB的特征值λ=(λ0,λ1,λ2),且λ0≥λ1≥λ2,对应特征向量X=(X0,X1,X2)。

(4)计算4 ×4 变换矩阵。其中,特征向量X=(X0,X1,X2)组成3 ×3 旋转矩阵R:

其中,T为3 ×1 平移矩阵,t为点云的质心坐标,t=。因此,H=(X0,X1,X2,T)为4 ×4 齐次变换矩阵。可得点云A和点云B之间的变换矩阵H:

其中,HA为点云A经PCA 变换的矩阵,HB为点云B经PCA 变换的矩阵,H代表从点云B变换到点云A。

由于PCA 算法存在主轴反向问题,因此传统的PCA 配准算法得到的初始矩阵H并不能直接用于精配准中,错误的初始矩阵会导致精配准往错误的方向迭代。为此,本文提出了一种误差矫正方法,解决主轴反向问题。

首先将点云B通过矩阵HB变换到降维后的坐标系得到点云Bnew:

其中,RB和TB分别为矩阵HB分解的旋转矩阵和平移矩阵。

再将点云Bnew通过HA的逆矩阵(HA)-1转换得到点云

其中,(RA)-1和(TA)-1分别为(HA)-1分解的旋转矩阵和平移矩阵。

最后使用式(14) 求取点云Bnew_A的质心点(xnew_A,ynew_A,znew_A),并且与点云A的质心点(xA,yA,zA) 相比较,若满足以下式子,则说明主轴反向。

其中,err为误差。若满足式(22)则说明x轴反向,满足式(23)则说明y轴反向,满足式(24)则说明z轴反向。

通过左乘旋转矩阵的方式来矫正主轴反向,创建旋转矩阵R180:

当R1=1、R2=-1、R3=-1 时,左乘R180代表绕x轴旋转180 °;当R1=-1、R2=1、R3=-1 时,左乘R180代表绕y轴旋转180 °;当R1=-1、R2=-1、R3=1 时,左乘R180代表绕z轴旋转180 °。

1.3.2 点云精配准

使用矩阵H为ICP 算法提供一个较好的初始位置。结合KD 树的近邻搜索[23],需要点云A中的点pi在点云B中通过迭代优化找到一个点qi,使得误差函数E最小:

其中,R为旋转矩阵,t为平移矩阵,n为近邻点对的数目。pi经过R和t的变换得到计算的距离d:

若式(27)中的d小于设定的平均阈值则ICP算法停止迭代。矩阵M=(R,t)可以转化为机械臂抓取需要的3D 位姿。

2 实验结果与分析

为了验证本文提出方法的有效性,设置了积木抓取的任务场景,如图1 所示。实验中使用的机械臂型号为TM 协作机器人;视觉检测设备型号为RVC X mini 3D 相机。使用笔记本电脑作为上位机接收相机拍摄的点云图,并控制机械臂运动,笔记本电脑操作系统为位,处理器为I7-6500U,双核,内存为8 GB。编程环境为Visual Studio2019 和PCL 库1.8.1,使用的编程语言为C/C++。

图1 任务场景及设备

2.1 点云差分和滤波实验

从图1 中的积木抓取任务场景中可知,环境点云主要是平面载物台。在本次实验中,积木抓取任务场景点云如图2(a)所示,环境点云如图2(b)所示。根据式(1),设置参数H为10-4m,即设定对应点之间的距离阈值为10-4m,若对应点的距离小于这个阈值,则被认为是背景点;大于这个距离,则被认为是前景点。最后差分的结果如图2(c)所示。

根据实验结果可知差分法能够有效去除环境点云。滤波算法能去除外部噪点,平滑内部点云。

2.2 点云下采样实验

下采样处理能够减少无用点的数量,加快处理速度。为验证本文提出的下采样算法的可靠性,对比传统下采样方式。首先给出一个点数为153 860的点云模型如图3(a)所示,图3(b)是采用传统下采样算法处理的结果,图3(c)是采用本文提出的下采样算法处理的结果,在本次实验中,将式(7)中的k定为10。通过对比可以看出,本文算法在边缘以及角点特征较多的部位能够比传统算法保留更多特征。

图3 模型下采样结果

2.3 点云分割实验

第1 次分割采用传统欧式聚类分割方式。如图4所示,点云被分割成4 个聚类,其中,有3 块积木被完整地分割出来,有2 块因为粘连而导致欠分割。

图4 传统欧式聚类分割结果

通过点云聚类的OBB 包围盒体积和积木实际体积的比较,可以选出欠分割的点云聚类。实际的积木体积V0为2.1924 ×10-5m3,图4 中1～4 号聚类的OBB 包围盒体积如表1 所示。

表1 点云聚类的OBB 包围盒体积与V0 比较

对表格中的数据进行分析,序号2～4 点云聚类的体积小于V0,代表分割成功。序号1 的点云聚类体积大于V0,处于欠分割状态,需要进行二次分割。

在二次分割之前,需要计算模型点云法线间的平均夹角θ和法线夹角平均变化率σ0。本次实验的模型点云法线间的平均夹角θ=69.24°,通过式(8)和(10),设置k=10,σ0=32.0184,若点云中点的k邻域的法线夹角平均变化率σ＞σ0,则删去这个点,一直循环到点云中点的σ值都小于σ0,最后再次进行欧式聚类分割。图5 表示点云经第1 次分割和第2 次分割后的最终结果。

图5 本文算法分割结果

通过与传统的欧式聚类分割算法对比中可以看出,利用本文提出的基于改进欧式聚类的二次分割算法可以有效地分割出场景中堆叠的物体点云。

2.4 点云配准实验

为了比较SAC-IA +ICP 配准算法、4PCS+ICP配准算法和本文提出的配准算法的定位性能,选择图1 任务场景中的其中一块积木与其CAD 模型点云进行配准实验。图6(a)是SAC-IA +ICP 算法配准的结果,图6(b)是4PCS +ICP 算法配准的结果,图6(c)是本文算法配准的结果。点密集度小的形状为源点云,点密集度大的形状为模板点云。其中,经过配准变换,3 种算法得到的源点云与模板点云的重合率分别为81%、90%、96%。从中可以看出,本文提出的配准算法使得源点云与模板点云的重合率更高,因此精度更高。

图6 配准结果比较

为了进一步分析3 种算法的可靠性,选择结果点云和源点云中对应点的平均最小欧式距离来表示点云配准的误差,以此作为衡量算法定位性能的依据。图7(a)为x轴误差,图7(b)为y轴误差,图7(c)为z轴误差。可以看出,本文算法在x、y、z轴上的误差都要小于其他2 种算法,定位性能更加稳定。

图7 配准误差对比

本次实验统计了3 种算法的配准时间,从表2中可以看出,3 种算法在实验场景中的平均配准时间分别为83.5 s、5.9 s、1.0 s。因此,本文算法在计算效率上优于其他2 种算法。

表2 配准时间对比/s

本文提出的定位算法最终实验结果如图8 所示,本文算法成功定位到了每块积木。

图8 精配准结果

精配准得到的矩阵便是物体在相机坐标系下的位姿矩阵,将位姿矩阵与眼标定矩阵结合,并转换为3D 位姿发送给机械臂便能实现抓取,位姿结果如表3所示,抓取的结果如图9 所示。

表3 3D 位姿结果

图9 抓取结果

3 结论

机械臂抓取主要的难点是目标物体的定位,本文提出了一种基于3D 视觉的机械臂抓取控制方法,通过差分法和加权下采样的方法简化了点云,大幅度提高了数据处理的时间,通过二次分割的方式成功分离出堆叠的点云聚类,使用PCA 算法和ICP算法成功计算目标点云位姿。在达明协作机器人上的积木抓取实验证明了此方法的有效性,并且在相同的任务场景中,只需更换CAD 模板便能抓取到不同的物体。在本次任务场景中,对比需要提取特征点和计算特征描述子的配准方法,速度更快。在点云分割上,本文的算法对比于传统的分割算法能较好地处理堆叠场景。机械臂与3D 视觉结合是机器人抓取领域的研究热门,不同的点云前期处理算法和位姿估计算法也层出不穷,本文后续将对各种算法进行研究,使其能够适应更加复杂的环境。