散乱堆叠药盒三维视觉抓取系统设计*

甄瑞涛 韩建海，2 郭冰菁，2 李向攀，2

（1.河南科技大学机电工程学院 洛阳 471003）

（2.河南省机器人与智能系统重点实验室 洛阳 471003）

1 引言

我国是世界最大的原料药生产及出口国家，医药行业是我国国民经济的重要组成部分［1］。如此大的生产需求单靠人工劳动力，劳动力成本过高并且难以满足需求，所以将视觉引导机器人抓取系统应用于医药生产线上，可以灵活地完成生产、分拣、包装等工作，提高医药生产的智能化和自动化，将大大降低劳动力成本。

场景分割与位姿估计是视觉引导机器人抓取的关键，场景分割实现抓取对象与载物台的分离以及抓取对象之间的分割，位姿估计获取目标在相机坐标系下的位置和姿态。场景分割可分为处理图像数据的二维场景分割以及处理三维点云数据的三维场景分割。二维场景分割在弱纹理、颜色或亮度相近、堆叠或有遮挡等情况下，通常难以得到理想的分割结果，三维点云包含场景的深度信息，因此三维场景分割依据点云表面法线、曲率、凹凸性等特征便可以判断物体之间位置的相对差异，完成分割［2～5］。本文选择基于三维视觉进行系统设计。

目前，对于散乱堆叠放置物体的抓取，现有的视觉引导机器人抓取系统多是通过基于卷积神经网络或深度神经网络的方法进行场景分割，需要预先标注大量的训练样本进行特征训练，且运算量巨大，对硬件有较高要求［6～9］；通过模板匹配的方法估计目标位姿，需要预先建立模板库，并且模板匹配运算量大耗时较长，当目标物大小和形状等特征变化时，需要重新创建模板库［10～13］。为此，针对药盒这一规则体的堆叠抓取，根据药盒大小多变且表面光滑的特征，本文设计了散乱堆叠药盒三维视觉抓取系统，基于点云预处理与点云分割算法设计了堆叠药盒场景分割算法，并且通过计算药盒上表面点云的质心以及质心点表面法线获取药盒位姿，无需标注训练样本进行特征训练，也无需建立模板库，能够很好地适应不同大小的药盒。系统整体实现了场景分割、位姿估计及抓取功能。

2 系统整体方案设计

首先，通过深度相机获取堆叠药盒场景点云数据；其次，对获取的原始点云数据进行预处理，提高数据的精确性和后期处理速度。预处理包括：直通滤波去除载物台背景、体素化网格方法降采样、统计滤波删除离群点；然后，通过场景分割完成不同药盒点云之间的分割以及药盒上表面点云与侧面点云的分割；最后，根据单个完整药盒上表面点云的点个数设定的阈值判断，先处理表层未被遮挡的药盒，依据药盒完整上表面点云进行位姿估计，获取单个药盒上表面点云质心以及质心点表面法线，得到药盒位姿，由机器人及安装在末端的吸盘完成抓取。根据是否存在倾斜放置药盒，分为场景一“散乱无倾斜堆叠”与场景二“散乱有倾斜堆叠”两种场景对系统进行测试试验。系统工作流程如图1所示。

图1 系统工作流程图

药盒为规则体，其表面光滑，多为特征均匀的矩形或方形平面，相比于通过手爪抓取药盒，通过吸盘吸取药盒更加快速准确，根据药盒的尺寸大小和质量设计了真空吸盘，配备真空泵完成药盒的抓取，吸盘实物如图2 所示。系统硬件组成部分主要包括UR5 机器人、COMATRIX 深度相机、计算机、真空吸盘、真空泵等，如图3 所示。相机相对机器人安装位置可分为eye-in-hand 和eye-to-hand 两种方式，本系统设计采用eye-in-hand 方式。选用COMATRIX 深度相机，深度图分辨率（Depth Map）200 万个点，每个点都有独立的（X，Y，Z）坐标，Z 轴准确度为200μm。选用UR5 机器人，有效负载5kg，工作范围850mm，重复定位精度±0.1mm。

图2 真空吸盘

图3 系统硬件结构

3 系统视觉算法设计

3.1 场景点云数据获取及预处理

相机固定于机器人末端，设置机器人初始位置，即相机每次拍照位置，使相机位于垂直于载物台正上方760mm 处，相机垂直于载物台拍摄获取场景数据。获取的数据包含载物台背景，并且由于相机误差和环境因素的影响，获取的原始点云数据存在噪声，且密度过大，所以需要对原始点云数据进行直通滤波、降采样、离群点删除等预处理操作。

3.1.1 去除载物台背景

相机每次拍照相对载物台的位置是固定的，即相机坐标系下载物台表面的深度值是不变的。药盒有一定厚度，所以药盒上表面的深度值小于载物台表面的深度值。对三个坐标维度分别进行直通滤波，如式（1）所示，（xmin，xmax），（ymin，ymax），（zmin，zmax）分别是三个维度坐标设定的阈值，在保证药盒点云数据完整的情况下，将载物台背景完全去除。

如图4 所示，直通滤波去除载物台前后对比，图4（a）和（b）为场景原图，图4（c）和（d）为场景一去除载物台前后对比，图4（e）和（f）为场景二去除载物台前后对比。

图4 去除载物台

3.1.2 体素化降采样

通过体素化网格方法对去除载物台背景的稠密药盒点云数据降采样，精简冗余点。对输入的点云创建一个三维的体素栅格，每个体素内的所有点以该体素所含点的质心替代。依据点云数据的密度，在保持点云形状特征的前提下，设置体素叶子尺寸为1mm。如图5 及表1 所示，对比了降采样前后点云数据的变化。

表1 体素降采样前后点云数据对比

图5 体素降采样

由采样结果可知，在保持点云形状特征的前提下，降采样对药盒点云数据进行了大比率精简，后续能够有效地提升目标点云分割速度。

3.1.3 统计滤波离群点删除

由于相机测量误差以及降采样，会造成稀疏的离群点，使点云局部特征的估计变得复杂，导致错误的结果，从而致使点云分割等后期处理的失败。采用统计滤波进行离群点删除，计算每个点与其邻域点的平均距离并进行统计分析，得到具有均值和标准差的高斯分布，根据全局距离均值和标准差设置合适的阈值，修剪掉平均距离不在标准范围内的离群点。设置对每个点分析的邻域点个数为50，并将标准差倍数设置为2。离群噪声去除前后对比如图6 所示，统计滤波可以有效的去除离群点，为后期的目标点云分割提供精确的点云数据。

图6 统计滤波去除离群点

3.2 场景目标点云分割

点云分割算法主要有区域生长、欧式聚类、随机采样一致性等方法［14～15］，本文主要任务是将不同药盒的点云分割开，同时实现药盒上表面点云与侧面点云的分割，结合药盒点云数据均为特征均匀的矩形或方形平面的特征，通过区域生长算法实现场景目标点云的分割。

区域生长算法依据点法线之间的夹角大小进行分割，比较种子点法线与其邻域点法线之间的夹角，将夹角小于阈值的点存入同一聚类集群。为此，需要计算目标点云表面法线。点云为多个离散的空间点组成，不是连续的光滑表面，因此需要在其每个点处根据邻域点近似的计算表面法线。求取表面某点处的法线等同于估计该点处表面的相切面法线，就将求取表面法线的问题转换成了通过最小二乘法拟合估计切平面的问题。因此，求取表面法线的方法可简化为通过查询点的邻域元素创建一个协方差矩阵，求取该协方差矩阵的特征值和特征向量。对于点云中任意一点Pi，其相应的协方差矩阵C 以及特征值和特征向量计算如式（2）和式（3）所示。求得的特征值有多个，对应多个特征向量，选取最小特征值对应的特征向量，即为所拟合切平面的法向量。如图7 为药盒点云表面法线估计结果。

图7 表面法线估计

式中：K 是任意点Pi邻域点的个数，为Pi邻域点的三维质心，λj是协方差矩阵C的第j个特征值，j是特征值λj对应的特征向量。

算法流程：1）将曲率最小的点设置为初始种子点并将其加入种子点序列，设置一个空的聚类集群，由初始种子点开始生长；2）搜索当前种子点邻域内的所有点，比较当前种子点与其邻域点之间的法线夹角，小于平滑阈值的邻域点加入到当前区域的聚类集群中，同时检测当前种子点邻域内所有点的曲率值，小于曲率阈值的点被作为新的种子点加入到种子点序列中；3）当前种子点邻域判断结束后，将该种子点删除，用加入的新的种子点继续生长，直到种子点序列清空，便完成了一个区域的生长；4）从剩余的部分寻找曲率最小的点作为新的初始种子点，开始一个新区域的生长，重复1）～3）的生长步骤，完成多个区域的生长，最终完成点云的分割。

算法关键参数的设置：最小点簇的点数量为100；最大点簇的点数量为15000；邻域点的搜寻方式设置为KdTree，因为获取的目标点云为无序的，需要通过KdTree 对无序的点云建立拓扑关系，依据所建立的拓扑关系对邻域内的点进行快速访问；搜寻的邻域点数目设置为50；平滑度阈值设置为0.0527rad；曲率阈值设置为1.5。分割结果如图8所示，实现了场景1 与场景2 中不同药盒点云的分割。

图8 区域生长分割

3.3 待抓取目标位姿获取

通过吸盘实现对药盒的抓取，为使吸盘可以准确牢固的吸住药盒，需保证吸取药盒时吸盘垂直于药盒表面，且吸住药盒的中心，因此需要获取机器人基坐标系下待抓取目标的位姿。针对药盒这一扁平规则体，本文提出通过计算药盒上表面点云质心以及质心点表面法线获取药盒位姿。

对于每个药盒上表面点云，其质心Pc的计算方法如下，式（3）为通常物体求取质心Pc的方法，mi为质点对应的质量。对于点云质心Pc的计算，令mi=1即可，药盒点云质心的计算如式（4）所示。

对于每个待抓取药盒，依据点云分割获取的单个药盒上表面点云求取其表面法线，并以质心点为索引获取质心点法线。如图9 为分割出的单个药盒质心及质心点法线的显示。

图9 点云表面法线估计

求取的质心为centroid（x，y，z），对应的质心点法向量为normal（normal_x，normal_y，normal_z），其中normal_x、normal_y 和normal_z 为法向量的方向余弦，计算反余弦即可得到法线与相机坐标系三个轴的夹角（α，β，γ），即获得了药盒在相机坐标系下的位姿。通过机器人基坐标系与机器人末端坐标系的变换矩阵以及手眼标定所建立的机器人末端与相机之间的坐标转换关系获得机器人基坐标系下药盒的位姿。机器人末端所装工具为吸盘已确定，所以工具坐标相对于机器人末端坐标的转换矩阵是确定的，因此药盒位姿可实现相机坐标至基坐标的转换、基坐标至工具坐标的转换，引导机器人完成抓取。

4 实验与结果分析

为了测试系统的性能，分别从场景分割准确性及抓取效果两个方面进行实验分析。以6 种常见的不同尺寸大小的药盒为实验对象。

4.1 场景分割测试实验

系统图像处理单元采用Intel Core i7-8700k CPU、Ubuntu 16.04 64 位 操 作 系 统、点 云 库PCL1.9.1，通过clion集成开发。为进一步验证场景分割算法的有效性，对两场景进行了多次数据采集，并通过算法处理，如图10 为随机获取的两场景复杂堆叠数据的处理结果，观察分析处理结果发现：对于两场景的表层药盒均能够准确完整地将属于不同药盒的点云单独分割出来，并且能够实现倾斜药盒上表面与侧面点云的分割；对于两场景药盒的复杂堆叠，会存在表层药盒对下层药盒横断遮挡的情况，造成下层单个药盒的点云不在同一连续平面，从而使属于同一药盒的点云被聚类为多个簇。

图10 处理结果

表层药盒的精准分割为接下来的位姿识别做准备，表层未被遮挡的药盒点云是完整的，而下层被遮挡的药盒点云不完整，因此点云的点个数会明显缺失，根据单个完整药盒上表面点云的点数设定阈值，以此判断识别表层未被遮挡的药盒，先抓取表层药盒，并根据表层单个药盒点云的点个数设置处理优先级，由表层至下层逐个完成抓取。

4.2 抓取测试实验

系统能够成功实现抓取，末端抓取力的控制是关键。系统通过吸盘抓取药盒，吸盘吸附口直径设计为4cm，配合真空泵可提供足够的吸附力，机器人静止时吸盘最大安全吸取质量为2kg，为使机器人运动中吸盘可以牢固的吸住药盒，需保证吸取药盒质量小于1.5kg，药盒尺寸长宽应大于4cm，满足大部分药盒。

分别对6 种不同大小的药盒进行了多次抓取，经测试，系统能够很好地完成药盒的位姿估计和抓取任务，如图11为抓取药盒时吸盘的姿态。

图11 吸盘姿态

5 结语

针对药物生产线上药品分拣、包装等繁重的工作任务，设计了一种应用于药盒抓取的散乱堆叠药盒三维视觉抓取系统。根据药盒点云均为特征均匀的矩形或方形平面的特征，基于点云预处理与区域生长算法设计了场景分割算法；并且根据药盒多为扁平规则体的特征提出通过计算药盒上表面点云质心以及质心点表面法线的方法估计药盒位姿。经实验证明，系统整体很好地实现了场景分割、位姿估计及抓取功能，能够很好地适应不同大小的药盒，可以有效地提高药物生产线的智能化和自动化以及降低人工成本，具有实际的应用价值。