1 引言

近些年来,随着工业机器人和机器视觉得到越来越广泛的应用,工业产线的自动化程度不断提高。工业机器人可以准确高效地完成重复性的工作,但缺乏柔性,故需要添加机器视觉系统。常见的工业自动装配通常需要用到机械臂的抓取技术^[1]。而面向散乱堆放物体的无序抓取问题,由于该场景下背景杂乱、物体之间存在遮挡,物体无序抓取有着较高的难度,近年来也逐渐成为研究热点。而无序抓取的核心问题之一是对物体的位姿估计,即从场景中识别并定位出物体。

目前,位姿估计算法按不同的处理对象可分为基于二维(2D)图片的算法和基于三维(3D)点云的算法。早期工业相机相对点云获取设备较为廉价,且取像迅速,抓取系统多基于2D图片实现^[2]。Choi和Christensen^[3]采用2D关键点特征匹配的方法识别3D物体并进行位姿估算,以此实现机械臂抓取。Ulrich等^[4]提出了一种基于2D边缘匹配和图像金字塔级的方法,通过搜索相似的2D模型估计3D物体的位姿。但是,基于2D图片的位姿估计方法在处理无序抓取问题时,只有在物体位姿变化很小时才起作用,且难以很好地处理遮挡情况。

近些年来,3D点云获取设备越来越便宜,研究人员开始更多地研究基于3D点云的位姿估计方法^[5]。面向散乱堆放物体的位姿估计算法主要有两种^[6]:1)分别计算模板点云与场景点云的特征描述子(如快速点特征直方图描述子),取特征空间上距离最近的点作为对应点进行匹配,然后在“假设-验证”框架(如随机采样一致性算法框架)下不断寻找可能的位姿,直到满足条件后,退出循环^[7];2)使用点云表面上的点和法线作为基元定义点对特征,模板点云被表示为一系列的点对特征存储在哈希表中,以便检索,场景点云中的每个点使用类似霍夫投票的方式获取位姿,最终选取票数最高的位姿作为最终的位姿^[8]。第一种方法快速、高效,但依赖于特征描述子的选取,如果选择全局特征描述子,该方法容易忽略局部细节,导致识别力不强,往往需要先从场景中分割出目标物体,且对物体遮挡、背景杂乱的情况不够鲁棒;如果选择局部特征描述子,其只考虑局部,往往对噪声非常敏感。更重要的是,描述子的旋转不变性、平移不变性导致算法处理工业中较常见的旋转对称物体(如圆柱体)时,会根据物体计算出大量重复的特征,影响对应点的匹配,导致位姿估计的效果不理想^[9]。第二种方法使用点对特征,计算简单,使用类似霍夫投票的方式获得位姿,对传感器噪声、杂乱背景、物体遮挡均具有很好的鲁棒性。

赵银帅等^[10]提出了一种基于边缘点对特征的三维目标识别与定位方法,并将其成功应用在了钢板打磨项目中,但由于钢板较大,能充满视野,这并不属于无序抓取问题;鲁荣荣等^[11]提出了一种基于增强型点对特征的三维目标识别算法,在开放数据集和实际采集的数据集上均取得了比基于原始点对特征的算法更好的效果,但其只考虑了点云匹配,并没有考虑机械臂抓取相关的问题。本文基于点对特征,面向散乱堆放的物体,进行了一系列改进,以取得更理想的位姿估计结果。

2 基于点对特征的位姿估计算法

基于点对特征的位姿估计算法^[8]分为离线建模和在线识别两个阶段,如图1所示。在离线建模阶段,使用待抓取物件的CAD模型,将其离散化为点云,或使用点云成像设备扫描待抓取物件获得点云,估算点云的法线,然后从点云中提取点对特征,并将其保存在哈希表中,完成离线建模;对于在线识别阶段,使用点云成像设备扫描散乱堆放的待抓取物件的场景,获得场景点云,使用与离线建模阶段同样的方法获得点对特征,然后进行点对特征对齐和位姿估计,在位姿参数空间使用投票策略进行模板匹配,对得到的多个位姿进行聚类,选择得分高的位姿作为最终的输出位姿。

图 1. 基于点对特征的位姿估计算法流程图

Fig. 1. Flow chart of pose estimation algorithm based on point pair features

下载图片 查看所有图片

2.1 离线建模阶段

基于点对特征的位姿估计算法^[8]使用点对特征描述两个点的相对位置和法线方向。对于任意两个点m₁、m₂(图2),其法线分别为n₁、n₂,则定义一个4维向量作为这两个点之间的特征描述F(m₁,m₂):

F(m1,m2)=(f1,f2,f3,f4)=[‖d‖2,∠(n1,m1m2),∠(n2,m1m2),∠(n1,n2)],(1)

式中:f₁为点m₁与点m₂之间的欧氏距离;f₂为法线n₁与连线m₁m₂之间的夹角;f₃为法线n₂与连线m₂m₁之间的夹角;f₄为法线n₁与法线n₂之间的夹角。

基于点对特征的位姿估计算法使用全局模型描述模板点云。全局模型描述可计算模板点云上的所有点对特征,并将相似的点对特征存储在哈希表的同一个键值下,如图3所示。图中A为哈希表中某一键对应的值的集合,其元素为若干个点对特征,F

图 2. 点对特征示意图

Fig. 2. Diagram of point pair feature

下载图片 查看所有图片

图 3. 建立哈希表示意图

Fig. 3. Diagram of building a Hash table

下载图片 查看所有图片

为A对应的键。

2.2 在线识别阶段

对于场景点云上的任意一个参考点s_r,如果其在待检测的目标物体上,则一定存在一个来自模型上的点m_r与之对应(图4)。设模板点云上有点对(m_r,m_i),场景点云上有点对(s_r,s_i),且它们的点对特征近似相同,通过变换T_s→g将参考点s_r及其法线 nrs对齐到原点和x轴,并通过T_m→g将点m_r及其法线 nrm对齐到原点和x轴,从而使得点s_r、m_r重合于原点,法线 nrs、 nrm重合于全局坐标系的x轴。再围绕s_r的法线 nrs旋转角度α即施加变换R_x(α)可让模板点云和场景点云对齐,从而生成目标物体的位姿。模型点云到场景点云的刚性变换由模型上的一个参考点m_r和一个旋转角α来表示,(m_r,α)即为与参考点s_r相关的模板点云的局部坐标,模板点云上的任一点m_i与场景点云上的任一点s_i满足

si=Ts→g-1Rx(α)Tm→gmi,(2)

图 4. 局部坐标示意图

Fig. 4. Diagram of local coordinates

下载图片 查看所有图片

图 5. 投票过程示意图

Fig. 5. Diagram of voting process

下载图片 查看所有图片

式中: Ts→g-1为T_s→g的逆过程。基于点对特征的位姿估计算法^[8]使用广义霍夫投票取得提议位姿。如图5所示,具体投票过程如下:

1) 由场景点云参考点s_r和场景点云中的任意其他点s_i组成点对,并计算点对特征 F(sr,si);

2) 将 F(sr,si)作为键值,在全局模型描述哈希表中进行检索,返回具有相似点对特征的模板点云上的点对 (mr,mi);

3) 对于 (sr,si)和 (mr,mi),利用(2)式计算旋转角 α;

4) 构建二维累加器,行为下采样后的模板点云的点索引,列为对 α进行离散化后的值索引。通过计算得到旋转角 α后,将得到的二维累加器的对应元素加1。

完成上述投票过程后,取投票数最高的局部坐标对应的位姿作为目标物体在场景中的输出位姿。

3 面向散乱堆放物体的优化

针对工业环境中的散乱堆放物体(如电容、转子、火花塞等),本文在基于点对特征的位姿估计算法基础上,进行了一系列改进,以取得更理想的位姿估计结果。

3.1 场景点云法线方向一致性调整

点对特征依赖于点的位置和方向,位置即点的坐标,方向即点的法线,得到准确的法线方向尤为重要。常用的点云法向量估计方法是局部表面拟合的方法,其基本思路是假设点云表面为光滑的曲面,则点云中的任一点的邻域都可以由平面近似拟合,即对于点云中的每一个点p,若能找到k个距离最近的邻近点,则可根据最小二乘的原理,解得点p与k个邻近点的最小二乘意义上的局部拟合平面S,而平面S的法向量即为点云中点p的法向量^[12]。

但是,采用该点云法线估计方法所求得的法线方向具有二义性,即法线的方向可能朝向点云内部,也可能朝向点云外部。点云法线方向的不一致将直接影响到点对特征计算的准确性,造成点云匹配结果不佳。因此需要对法线方向进行一致性调整^[13]。

对于工业环境中的散乱堆放物体,使用点云成像设备来获取场景点云数据。由于点云成像设备在相机坐标系下的位置坐标已知,即视点v_p,故只需要调整场景点云中所有点的法线,使其朝向视点v_p即可。设场景点云中某一点为p_i,其法向量为n_i,调整点p_i的法线方向,使其满足

nivppi→>0,(3)

式中: vppi→表示从点p_i指向点v_p的向量。

场景原始点云、法线方向调整前、法线方向调整后的结果分别如图6(a)~(c)所示。由图可知,法线方向调整前,约一半场景点云中点的法线方向指向点云内部,另一半指向点云外部。调整法线方向后,场景点云中点的法线方向全部指向点云成像设备。

图 6. 点云法线方向一致性调整示意图。(a)原始点云;(b)调整前的点云法线方向;(c)调整后的点云法线方向

Fig. 6. Diagram of consistency adjustment of point cloud normal direction. (a) Original point clouds; (b) point cloud normal direction before adjustment; (c) point cloud normal direction after adjustment

下载图片 查看所有图片

3.2 抓取位姿筛选策略调整

在实际工业环境中,对于散乱堆放物体的抓取问题,有两个要素决定了能否成功抓取物体。一是物体的位姿估计是否准确(如模板点云与场景点云中的某个物体是否匹配得足够好)?二是待抓取的物体是否可抓取(如该位姿下机械臂是否有运动学逆解?是否在抓取的路径上没有遮挡)?

基于点对特征的位姿估计算法,本质上是从场景点云中生成很多个提议位姿。为筛选出合理的抓取位姿,本文综合考虑重合率、倾斜、高度这三个要素。

1) 重合率。使用提议位姿将模板点云变换到场景点云中时,要求模板点云中的内点比例越高越好,内点是模板点云中与场景点云的最近邻点的距离小于阈值的点。

2) 倾斜。一般的抓取位姿都是竖直向下的,使用提议位姿将模板点云变换到场景点云中,要求模板点云的位姿的 z轴与机械臂坐标系的 z轴夹角越小越好,否则容易出现机械臂超出行程范围等问题。

3) 高度。使用提议位姿将模板点云变换到场景点云中时,要求模板点云的位姿的z值越大越好,因为对于乱序堆放物体,理应先抓上方的物体,再抓下方的物体,以避免抓取路径遮挡的问题。

本文使用的抓取位姿筛选策略优先考虑重合率,再考虑倾斜,最后考虑高度,即对于所有提议位姿,先按重合率降序排序,再筛选剩下的与机械臂坐标系的z轴夹角小于30°的提议位姿,最后取提议位姿中z值最大的位姿,则该位姿为合理的抓取位姿。待抓取物体以及计分排名前5的提议位姿、最终输出的位姿分别如图7(a)~(c)所示。

图 7. 抓取位姿筛选策略调整示意图。(a)待抓取物体;(b)排名前5的提议位姿;(c)输出位姿

Fig. 7. Diagram of gripping pose strategy adjustment. (a) Objects to be gripped; (b) top 5 proposed poses; (c) output poses

下载图片 查看所有图片

3.3 旋转对称引起的角度偏差调整

在实际工业场景下,很多待抓取的物体是旋转对称的,如电容、转子等。而包括点对特征在内的绝大多数的描述子具有旋转不变性、平移不变性,导致从旋转对称的物体上计算出大量重复的特征,造成估算出的位姿可能绕旋转轴有一定的角度偏差,而非最合理的位姿。

一般的抓取位姿都是竖直向下的,所以比估算出的位姿更合理的情况是:将其绕物体的旋转轴旋转一定角度,使待抓取物体坐标系的z轴与机械臂基坐标系的z轴夹角尽可能小,以便于机械臂抓取。如图8所示,设z为机械臂坐标系的z轴向量,r表示物体的旋转轴向量,p表示匹配出的最佳位姿的z轴向量,表示更合理位姿的z轴向量,设q=z×r。显然所求的p'即为z在p、q所在平面上的投影。求解∠(p,p')的步骤如下。

图 8. 旋转对称引起的角度偏差的调整示意图

Fig. 8. Diagram of adjustment of angle deviation caused by rotational symmetry

下载图片 查看所有图片

1)由于抓取位姿竖直向下,故匹配出的最佳位姿的z轴垂直于旋转轴,则有p⊥r,p'⊥r。显然当z,p,r共面时,∠(z,p)最小,抓取姿态最接近垂直方向。

2) 求q=z×r,则有q⊥z,q⊥r。由于z,p,r共面,故q⊥p',即∠(p',q)= π2。由于q⊥r,即q垂直于旋转轴,故有p,p',q共面。

3) ∠(p,p')=∠(p',q)-∠(p,q)= π2-arccos pq|p||q|,进而求得p'。

4 仿真和实验分析

本文通过构建完整的面向散乱堆放物体的抓取系统来测试本文算法的有效性,并进行了仿真和实验研究。

4.1 仿真

本文仿真借助机器人操作系统(ROS)中的仿真环境Gazebo搭建^[14]。本文使用的计算机配置为:处理器为Intel Core i5-8400,内存为24 GB,显卡为GTX1080TI,操作系统为Ubuntu16.04,ROS版本为Kinetic,Gazebo版本为7.0。Gazebo中深度相机使用微软公司的Kinect,待抓取物体使用近似为半径为5 mm、高度为17 mm的圆柱体的电容,六轴机械臂使用鲁班嫡系机器人公司的KentV2工业机器人,夹具使用半径为4 mm的吸盘。吸盘吸取的条件是吸盘与物体有碰撞。抓取采用眼在手外的方式,即深度相机固定,不随机械臂的运动而动。

Gazebo中的深度相机获取的点云是没有噪声的,为了更接近真实的情况,本文先估算真实实验中使用的深度相机所采集的点云的噪声,再将相同量级的噪声加到仿真中。求得的均值为0的高斯噪声的标准差σ为0.24,估算方法如下。

1) 将模板点云与真实点云完美匹配,则真实点云到模板点云上的最近点距离的平均值d_t为

dt=1M1∑x1∈M1minx2∈M2||x1-x2||2,(4)

式中:M₁为真实点云;M₂为模板点云;x₁为真实点云M₁上的一点;x₂为模板点云M₂上的一点。

2) 在模板点云上添加均值为0、标准差为σ的高斯噪声,求添加噪声的模板点云到模板点云上最近点的距离的平均值d_e。改变σ,使得d_t≈d_e。

3) 在仿真数据上添加均值为0、标准差为σ的高斯噪声,使得仿真数据与真实数据接近。

使用Gazebo生成20个散乱堆放物体的场景,每个场景在40 mm×70 mm×10 mm的范围内生成12个电容,表示电容位姿的旋转部分的三个欧拉角为(-90°,90°)范围内的随机数,经过刚体碰撞后,确保两层及以上的堆叠。使用Gazebo中的深度相机获取场景的点云,并添加均值为0、标准差为0.24的高斯噪声。

本文通过以下步骤构建面向散乱堆放物体的抓取系统:

1) 控制机械臂完成抓取单个电容的过程,并确保机械臂末端吸盘能完美吸取电容,记录此时的位姿为示教位的抓取位姿P_t,并用深度相机采集示教位的电容点云去除背景保留的单个电容,记为模板点云。

2) 用深度相机采集散乱堆放电容的抓取位的电容点云,将其记为场景点云。

3) 通过手眼矩阵,将模板点云和场景点云从相机坐标系转换到机械臂坐标系。用前文所述的基于点对特征的位姿估计算法,得到使得模板点云与场景点云匹配的变换关系,即机械臂坐标系下示教位到抓取位的变换关系^pT_t。

4) 用示教位位姿P_t左乘示教位到抓取位的变换关系^pT_t,得到抓取位下机械臂末端的位姿P_p,用该位姿吸取电容。

仿真环境下的抓取系统如图9(a)所示,点云的位姿估计结果如图10(a)所示。仿真中,统计了Gazebo生成的散乱堆放物体的场景下的抓取效果,用重合率和内点均方根误差(inlier RMSE)来衡量位姿估计准确度(判断内点的阈值取1 mm),用抓取成功率来衡量整个位姿估计算法的有效性。共研究20个场景,每个场景有12个电容,统计结果如图11(a)所示。可以看出,每个场景下电容的平均重合率均在95%以上,平均内点均方根误差均在0.1 mm以下。本文20个场景共计240个待抓取电容,该系统获得了97.1%的抓取成功率,其中有7次抓取失败(表1),失败的原因均是电容位姿过于倾斜,导致虽然点云匹配成功,但待抓取电容不在机械臂行程范围内。换句话说,在仿真环境下,因为机械臂DH参数是理想的,运动无误差,所以只要点云匹配成功,且电容位姿不过于倾斜,满足机械臂行程,就可以100%抓取成功。

图 9. 抓取系统示意图。(a)仿真环境;(b)真实环境

Fig. 9. Diagrams of gripping system. (a) Simulation environment; (b) real environment

下载图片 查看所有图片

图 10. 位姿估计示意图。(a)仿真环境;(b)真实环境

Fig. 10. Diagrams of pose estimation. (a) Simulation environment; (b) real environment

下载图片 查看所有图片

本文利用加噪后的仿真数据对比了所提位姿估计算法与原始的基于点对特征的位姿估计算法^[8]的点云匹配效果。对于20个仿真场景,每个场景分别使用原始算法与本文所提算法进行点云匹配,各个参数的设置相同,设定重合率大于80%且内点均方根误差小于1 mm时匹配成功,原始算法有17个场景匹配成功,本文所提算法在20个场景下均匹配成功,且本文所提算法的重合率远大于原始算法、内点均方根误差略小于原始算法,故本文所提算法优于原始算法。匹配结果如表2所示,表中的重合率、内点均方根误差只统计了点云匹配成功情况下的数据。本质上,本文所提算法相当于修改了原始算法的计分策略。原始算法的计分策略是按聚类后的票数降序排列,取票数最高的。本文所提算法进行了抓取位姿筛选策略调整、旋转对称引起的角度偏差调整,使得计分策略更加适合乱序场景。

图 11. 位姿估计结果折线图。(a)仿真环境;(b)真实环境

Fig. 11. Line charts of pose estimation results. (a) Simulation environment; (b) real environment

下载图片 查看所有图片

4.2 真实实验

本文真实实验中构建的面向散乱堆放物体的抓取系统与仿真中类似。使用鲁班嫡系机器人公司生产的双目面结构光传感器作为点云获取设备,使用近似为半径为5 mm、高度为17 mm的圆柱体的电容作为待抓物体,使用鲁班嫡系机器人公司生产的KentV2工业机器人作为抓取机器人,使用半径为4 mm的吸盘作为夹具。同样采用眼在手外的方式。

真实环境下的抓取系统如图9(b)所示,点云的位姿估计结果如图10(b)所示。真实实验中,摆放20个散乱堆放物体,在盒内放置12个电容,位姿任意。与仿真实验类似,通过统计重合率(fitness)、内点均方根误差和抓取成功率来评估位姿估计算法在真实场景下的有效性,统计结果如图11(b)所示。可以看出,每个场景下电容的平均重合率均在90%以上,平均内点均方根误差均在0.3 mm以下。本文20个场景共计240个待抓取电容,该系统获得了75.8%的抓取成功率,其中有58次抓取失败。分析导致抓取失败的原因主要有两类:1)机械臂行程范围不够,主要表现为点云匹配成功,但结果位姿不在机械臂行程范围内,有12次抓取失败归因于此;2)吸盘吸力不够,主要表现为点云匹配成功,机械臂末端看起来也到了合适的位姿,但由于吸盘基于气缸实现吸取,挤压使得吸盘与电容之间近似真空,而某些过于倾斜的位姿下无法顺利完成挤压,所以吸取失败,有46次抓取失败归因于此。换句话说,在真实环境下,如果标定机械臂DH参数后,机械臂精度足够高,则只要点云匹配成功且机械臂行程满足、吸盘吸力足够,就可以100%抓取成功。

5 结论

针对散乱堆放物体的场景,尤其是工业场景下的旋转对称物体,使用了基于点对特征的位姿估计算法,并进行了一系列改进,如场景点云法线方向一致性调整、抓取位姿筛选策略调整、旋转对称引起的角度偏差调整,从而取得了较为理想的位姿估计结果。在仿真环境下取得了97.1%的抓取成功率,在真实场景下取得了75.8%的抓取成功率。如果标定机械臂DH参数后,机械臂精度足够高,且机械臂行程满足、吸盘吸力足够,则可以取得近100%的抓取成功率。实验结果表明,所提算法在散乱堆放物体场景中的位姿估计效果比较理想。