1 引言（Introduction）

随着工业现代化的快速发展，大量的工业机器人被应用到分拣、装配和循迹等生产制造任务中，其中最突出的问题之一就是Bin-Picking问题。对于人类来说，从料箱中拾取随机堆叠的物体是一件很简单的事，但是实现其自动化却非常复杂，因为除了要识别出对象的位置与姿态外，还必须确保能够找到无碰撞的路径来安全地抓取对象^[1]。

一个机器人抓取系统包括以下几个子系统：抓取检测系统、抓取规划系统和控制系统，其中抓取检测系统是最重要的部分^[2]。

早期的机器人抓取研究多侧重于如何稳定可靠地把单个规则物体抓起来^[3-5]，远未达到工业应用的要求。2D机器视觉系统成熟后，通过2D相机识别零件的位置与方向，可以实现平面摆放零件的抓取。但遇到散乱堆叠的零件时，2D视觉往往无能为力。随着3维传感器技术的发展和成本的降低，机器人抓取方面的研究开始面向随机堆叠的复杂场景，在这种场景下，机器人抓取方法可以分为3类：基于可抓取部位检测的方法、基于机器学习的方法和基于位姿估计的方法。

基于可抓取部位检测的物体抓取方法只需掌握抓取器件的先验知识而无需掌握抓取对象的先验知识，通过检测点云或深度数据中的可抓取部位的特征能筛选出抓取姿态。Domae等^[6]利用抓取器件的掩模图和横截图进行卷积计算得到可抓取区域和碰撞区域，并通过相与运算得到安全的抓取区域。Buchholz等^[7]把深度图中的可抓取区域看作该深度图的特征区域，根据抓取器件的特征设计滤波器并对图像进行滤波，以此检测可抓取的区域。苏杰等^[8]利用简化的抓取器件的几何形状约束，结合力封闭约束和力平衡约束筛选出抓取姿态。上述方法可以对未知物体进行抓取，但对噪声较为敏感，因此在实际工业场景中容易出现抓取错误。

随着近年来机器学习（特别是深度学习）的快速发展，基于机器学习的机器人抓取方法成为研究热点。Lenz等^[9]先通过一个轻量级深度网络检测候选抓取姿态，再通过一个大型深度网络筛选出最佳抓取姿态；Redmon等^[10]则利用CNN（卷积神经网络）进行抓取点检查。但以上方法都只能完成单个物体的抓取。Harada等^[11]利用SVM（支持向量机）加随机森林的方法实现了随机堆叠零件的抓取。Mahler等^[12-13]利用MV-CNN（多视角卷积神经网络）生成带抓取点的数据集，然后输入到设计好的GQ-CNN（抓取质量卷积神经网络）中求取最佳的抓取姿态。Asif等^[14]设计了一个抓取部位检测网络GraspNet，其中包括一个编码网络和一个解码网络，能降低计算资源的消耗。Zeng^[15]等先采用一个FCRN（全卷积残差网络）预测抓取的可靠性，再利用Two-Stream-CNN（双流卷积神经网络）进行跨域图像匹配从而识别未知对象。文[16-17]提出了基于自监督深度强化学习的VPG（visual pushing and grasping）算法，实现了大量未知堆叠物体的抓取，但其训练需要大量的计算资源。NVIDIA（英伟达）公司研究员^[18]提出了6-DOF-GraspNet，该网络通过“评估—优化”模型使机器人可以抓取任意物体。上述基于机器学习的方法虽然在未知物体的抓取上有不俗的表现，但其研究的抓取对象多为日常生活用品而非工业零件。与生活上的物品相比，工业零件主要存在2个特点：1) 工业零件往往具有相同的颜色；2) 工业零件由各种规则形状构成。这些特点大大提高了模型的训练难度、降低了模型的泛化性能，同时，以上方法的数据标记具有工作量大、计算资源消耗高等缺点，因此难以应用到实际工业生产中。

基于位姿估计的物体抓取方法需要预先在物体上设置抓取点，然后计算待抓取物体的6自由度位姿，并根据抓取点与物体的变换关系间接计算出候选抓取姿态，最后通过碰撞检测、合理性检测等过程筛选出最佳抓取姿态，因此对抓取对象和抓取器件均要求有先验知识。近几年国内外的3D视觉公司已经纷纷推出商业化的Bin-Picking系统，例如荷兰ESPS公司的Pick-It 3D系统、斯洛伐克Photoneo公司的Photoneo-3D系统、丹麦SCAPE公司的Scape-Tech系统等，以及国内的梅卡曼德、埃尔森智能科技等致力于3D视觉的独角兽企业推出的成熟的解决方案，表 1为以上各种方案的对比。

在工业零件的抓取问题上，大量实际应用检验的产品均主要采用了基于位姿估计的抓取方法，可见在目前基于位姿估计的方法依旧是最稳定可靠的零件抓取策略。该方法的第一个关键问题是物体的位姿估计，在该问题上国内外的研究学者展开了大量的研究。目前常用的物体位姿估计方法有基于局部或全局特征匹配的方法和基于位姿投票的方法等，详见文[19]，其中基于点对特征（point pair feature，PPF）^[20-21]的方法因其抗噪能力强、鲁棒性高等特点而被广泛应用。而宋薇等^[22]则把Canny边缘处理和按倾角分层的Chamfer距离映射相结合，并采用加入了爬山法局部优化的遗传算法搜索最优匹配位姿，实现了基于单目视觉的工业零件抓取，但只适用于零件简单摆放的情况，难以应用到复杂的工业场景。

基于位姿估计的物体抓取方法的另一个关键问题是对候选抓取点的设置和筛选，但与物体位姿估计研究相比，该问题的研究进展相对缓慢。Spenrath等^[23]对3维传感器建立视点射线模型，把整个扫描空间分为安全区域、碰撞区域和未知区域，进而对抓取点进行分类。Buchholz等^[24]提出了KGF（key grasp frame）概念，使抓取点可以在1~2个自由度上滑动。Spenrath等^[25]针对候选抓取点数量太多、逐个计算效率太低的问题，以抓取点计算顺序为节点构建了一棵搜索树，并对每一种节点定义对应的评估函数进行启发式搜索，提高了最佳抓取点的计算效率；同时从零件高度与位置的相对关系以及抓取方向与箱体中心的相对关系来优化评估函数^[26]，并采用FANN（快速人工神经网络）训练关键节点的评估函数以指导节点展开^[27]，使得搜索效果得到了提高。虽然上述方法能够筛选出无碰撞风险的抓取姿态，但无论是表 1中所列的商业化方案还是上述方法，所设置的抓取点均为离散的固定点，这种抓取点设置方式不仅忽略了零件上大量的可抓取点，导致抓取成功率降低，还需要对候选抓取姿态逐个评估，导致其效率低下。为解决上述问题，本文提出了一种基于抓取簇和碰撞体素的抓取姿态检测算法，面向的抓取对象为工业生产过程中常见的散乱堆叠的零件，由于本文研究重点在抓取点的设置和筛选问题上，零件的位姿估计将采用文[28]的方法完成，本文不再详细讨论。图 1为本文算法的原理图。

总结来说，本文的主要研究内容和贡献如下：

1) 提出抓取簇的概念，采用包含连续抓取点的抓取区域替代传统方法中的离散固定抓取点，提高了抓取姿态的合理性和成功率，根据抓取簇的几何关系可直接计算出最佳抓取姿态，无需对候选抓取姿态进行逐个评估，提高了计算效率；

2) 提出一种体素化的碰撞模型，利用体素化思想对抓取场景进行合理的碰撞区域划分，通过简化抓取路径完成快速的碰撞检测，提高了抓取的安全性和计算效率；

3) 利用Unity3D软件对多种常见的工业零件生成仿真数据，进行抓取姿态检测的仿真实验，并与传统方法进行比较，本文算法表现出了较好的性能；

4) 搭建完整的Bin-Picking系统，对上述工业零件进行实际抓取实验，并与传统方法和基于深度强化学习的VPG^[17]算法进行比较，对比可知，本文算法的抓取平均成功率达92.2%，料箱平均清空率达87.2%，均优于上述2种方法，且抓取过程未发生碰撞，证明该算法可以满足工业实际生产要求。

2 抓取姿态描述（The description of grasping pose）

如图 2所示，抓取姿态定义为机器人在执行抓取动作时，抓取器件坐标系{$ {G} $} 相对于基座坐标系{$ W $} 的变换$ {{{\mathit{\boldsymbol{T}}}}}_{G}^{W} $。本文采用文[28]的方法计算出目标零件的位姿，假设$ {{{\mathit{\boldsymbol{T}}}}}_{O}^{W} $ 为目标零件在基座坐标系{$ {W} $} 的姿态，则$ {{{\mathit{\boldsymbol{T}}}}}_{G}^{W} $ 可表示为

$ \begin{equation} {{{\mathit{\boldsymbol{T}}}}}_{G}^{W} ={{{\mathit{\boldsymbol{T}}}}}_{O}^{W}{{{\mathit{\boldsymbol{T}}}}}_{G}^{O} \end{equation} $

(1)

其中$ {{{\mathit{\boldsymbol{T}}}}}_{G}^{O} $ 为抓取器件坐标系$ \{G\} $ 相对于零件坐标系$ \{O\} $ 的变换，则抓取姿态的检测问题可转化为寻找最佳变换$ {{{\mathit{\boldsymbol{T}}}}}_{G}^{O} $。

为方便讨论，现以参考坐标系{$ R $} 替代零件坐标系{$ {O} $}，设抓取器件坐标系{$ {G} $} 的原点为抓取点$ p_{{\rm g}} $，$ z_{G} $ 轴正方向为抓取方向$ {{{\mathit{\boldsymbol{v}}}}}_{{\rm g}} $，则一组抓取点和抓取方向可表示为$ G(p_{{\rm g}}, {{{\mathit{\boldsymbol{v}}}}}_{{\rm g}}) $，考虑抓取器件具有中心轴（即$ z_{G} $ 轴）旋转对称的特点，$ {{{\mathit{\boldsymbol{T}}}}}_{G}^{R} $ 由2次变换组成：

变换①：抓取器件坐标系$ \{G\} $ 相对于参考坐标系{$ R $} 的变换$ {{{\mathit{\boldsymbol{T}}}}}_{G}^{{R} ^{\prime}} $；

变换②：绕抓取器件自身坐标系$ \{G\} $ $ z_{G} $ 轴的旋转变换$ {{{\mathit{\boldsymbol{r}}}}}_{z} (\gamma) $（其中$ {{{\mathit{\boldsymbol{r}}}}}_{z} (\gamma) $ 表示绕$ z $ 轴旋转$ \gamma $ 角度的罗德里格斯变换得到的3$ \times $3旋转矩阵）。

变换①确定了在参考坐标系{$ R $} 下的抓取点和抓取方向$ G \left({p_{{\rm g}}^{R}, {{{\mathit{\boldsymbol{v}}}}}_{{\rm g}}^{R}} \right) $。设$ {\mathit{\boldsymbol{T}}}^{R^\prime}_{G} $ 为

$ \begin{equation} {{{\mathit{\boldsymbol{T}}}}}_{G}^{{R}^{\prime}} = \begin{bmatrix} {{{{\mathit{\boldsymbol{r}}}}}_{G}^{R}} & {{{{\mathit{\boldsymbol{T}}}}}_{G}^{R}} \\ {{{{\mathit{\boldsymbol{0}}}}}_{1\times 3}} & 1 \\ \end{bmatrix} \end{equation} $

(2)

则$ G \left({p_{{\rm g}}^{R}, {{{\mathit{\boldsymbol{v}}}}}_{{\rm g}}^{R}} \right) $ 与变换$ {\mathit{\boldsymbol{T}}}^{R^\prime}_{G} $ 存在对应关系：

$ \begin{equation} \begin{cases} {{{\mathit{\boldsymbol{p}}}}_{{\rm g}}^{R} = \left({{{{\mathit{\boldsymbol{T}}}}}_{G}^{R}} \right)^{{\rm T}}}\\ {{{{\mathit{\boldsymbol{v}}}}}_{{\rm g}}^{R} ={{{\mathit{\boldsymbol{r}}}}}_{G}^{R}{{{\mathit{\boldsymbol{U}}}}}_{z}} \\ \end{cases} \end{equation} $

(3)

其中$ {{{\mathit{\boldsymbol{U}}}}}_{z} $ 为$ z $ 轴的单位向量。

变换②由抓取器件绕中心轴旋转的角度$ \gamma $ 决定，以实际作业中最常用的吸盘和二指夹具为例，见图 2(a)，吸盘可绕中心轴旋转任意角度。对于二指夹具，当以图 2(b)所示的方式抓取时，其对称旋转角为$ {\rm{\pi }} $，因此确定了初始旋转角$ A_{{\rm i}} $ 后，可绕中心轴旋转$ {\rm{\pi }}\times n $（$ n\in{\rm N} $）。但以图 2(c)所示的方式抓取时，则其可绕中心轴旋转任意角度。可见，不同类型的抓取器件或抓取方式会引入不同的对称旋转角$ A_{{\rm r}} $（特别地，记可自由旋转时$ A_{{\rm r}} =0 $），因此有变换集合：

$ \begin{align} \left\{\left({{{\mathit{\boldsymbol{T}}}}}_{G}^{{R}^{\prime \prime}} \right)_{i} \right\}\!=\!\left\{\begin{bmatrix} {{{{\mathit{\boldsymbol{r}}}}}_{z} (\gamma)} \!\!&\!\! {{{{\mathit{\boldsymbol{0}}}}}_{3\times 1}} \\ {{{{\mathit{\boldsymbol{0}}}}}_{1\times 3}} \!\!&\!\! 1 \end{bmatrix}\right\}, \; \gamma\!=\!\!\begin{cases} A_{{\rm i}} \!\!+\!\!nA_{{\rm r}}, \!\!&\!\! A_{{\rm r}} \!\!\ne\!\! 0\\ {\rm R}, \!\!&\!\!A_{{\rm r}} \!\!=\!\!0 \end{cases} \end{align} $

(4)

使得$ \forall\; {{{\mathit{\boldsymbol{T}}}}}_{G}^{{R}^{\prime \prime}} \in \{({{{\mathit{\boldsymbol{T}}}}}_{G}^{{R}^{\prime \prime}}) _{i} \} $ 均满足变换②，$ {\rm R} $ 代表实数集。为保证机器人在执行抓取时关节量的变化尽量小，$ \gamma $ 通常取离抓取姿态前一个姿态最靠近的值。最终抓取姿态可表示为

$ \begin{equation} {{{\mathit{\boldsymbol{T}}}}}_{G}^{R} ={{{\mathit{\boldsymbol{T}}}}}_{G}^{{R}^{\prime}}{{{\mathit{\boldsymbol{T}}}}}_{G}^{{R}^{\prime \prime}} \end{equation} $

(5)

为简化计算，把抓取路径定义为抓取方向的反方向$ -{{{\mathit{\boldsymbol{v}}}}}_{{\rm g}} $ 上的某一点$ p_{{\rm h}} $ 到抓取点$ p_{{\rm g}} $ 的线段$ S_{{\rm hg}} $，见图 3，其中点$ p_{{\rm h}} $ 为起始点，在实际抓取中，机器人末端（即抓取器件）应先到达起始点$ p_{{\rm h}} $，然后通过线性运动到达抓取点$ p_{{\rm g}} $ 并执行抓取动作，最后再通过线性运动回到$ p_{{\rm h}} $，至此完成一次抓取。把抓取器件在起始点$ p_{{\rm h}} $ 处的姿态称为起始姿态，其相对于抓取姿态的变换为

$ \begin{equation} {{{\mathit{\boldsymbol{T}}}}}_{G_{{\rm h}}} ^{G} =\begin{bmatrix} {{{{\mathit{\boldsymbol{I}}}}}_{3\times 3}} & \left(0, \; 0, \; \left| {\overrightarrow {p_{{\rm h}} p_{{\rm g}}}} \right|\right)^{{\rm T}} \\ {{{{\mathit{\boldsymbol{O}}}}}_{1\times 3}} & 1 \\ \end{bmatrix} \end{equation} $

(6)

基于上述分析，抓取姿态的检测问题最终转化为对$ {{{\mathit{\boldsymbol{T}}}}}_{G}^{{R}^{\prime}} $ 和$ {{{\mathit{\boldsymbol{T}}}}}_{G_{{\rm h}}} ^{G} $（即$ \left| {\overrightarrow {p_{{\rm h}} p_{{\rm g}}}} \right| $）的求取。

3 基于抓取簇和碰撞体素的抓取姿态检测算法（Grasping pose detection algorithm based on grasping cluster and collision voxel） 3.1 抓取簇

传统的抓取姿态设置方法是以零件坐标系$ \{O\} $ 作为参考坐标系直接定义一系列离散的抓取姿态，见图 4，该方法不仅丢失了大量的可抓取点，且需要对候选姿态进行逐个筛选，效率较低。为了得到连续的、高自由度的抓取姿态，本文考虑了工业零件表面的规则特征，提出了抓取簇的概念。

3.1.1 抓取簇的定义

如图 5所示，在零件坐标系$ \{O\} $ 下定义局部坐标系{$ C $}，另定义变换规则$ \varPsi $，可使抓取器件坐标系$ \{G\} $ 相对于局部坐标系{$ C $} 进行连续变换，从而得到变换集合$ \{({{\mathit{\boldsymbol{T}}}}_{G}^{C})_{i} \} $。记该局部坐标系{$ C $} 到零件坐标系$ \{O\} $ 的变换为$ {{{\mathit{\boldsymbol{T}}}}}_{C}^{O} $，则有抓取器件坐标系$ \{G\} $ 到零件坐标系$ \{O\} $ 的变换集合：

$ \begin{equation} \left\{{ \left({{{\mathit{\boldsymbol{T}}}}_{G}^{O}} \right)_{i}} \right\}=\left\{ {{{{\mathit{\boldsymbol{T}}}}}_{C}^{O} {{\mathit{\boldsymbol{T}}}}_{G}^{C} \left| {{{\mathit{\boldsymbol{T}}}}_{G}^{C} \in} \right.\left\{{ \left({{{\mathit{\boldsymbol{T}}}}_{G}^{C}} \right)_{i}} \right\}} \right\} \end{equation} $

(7)

使得$ \forall\; {{{\mathit{\boldsymbol{T}}}}}_{G}^{O} \in \{({{\mathit{\boldsymbol{T}}}}_{G}^{O}) _{i} \} $ 均为可能的抓取姿态。

把上述基于变换$ {{{\mathit{\boldsymbol{T}}}}}_{C}^{O} $ 和变换规则$ \varPsi $ 生成的抓取姿态集合$ \{({{\mathit{\boldsymbol{T}}}}_{G}^{O}) _{i} \} $ 称为抓取簇$ \varTheta ({{{\mathit{\boldsymbol{T}}}}}_{C}^{O}, \varPsi) $，该局部坐标系{$ C $} 称为簇坐标系。

3.1.2 抓取簇中的映射关系

由抓取簇定义可知，变换规则$ \varPsi $ 本质上描述的是抓取器件坐标系$ \{G\} $ 到簇坐标系{$ C $} 的变换过程，若把簇坐标系{$ C $} 作为参考坐标系，则该变换过程即为第2节所述的变换①。设$ \{G_{i} (p_{{\rm g}}^{C}, {{{\mathit{\boldsymbol{v}}}}}_{{\rm g}}^{C}) \} $ 为抓取簇$ \varTheta $ 在簇坐标系{$ C $} 下抓取点和对应抓取方向的集合，$ \{({{\mathit{\boldsymbol{T}}}}_{G}^{C}) _{i} \} $ 为其生成的变换集合，则由式(3) 的对应关系得到$ \{({{\mathit{\boldsymbol{T}}}}_{G}^{C}) _{i} \} $ 到$ \{G_{i} (p_{{\rm g}}^{C}, {{{\mathit{\boldsymbol{v}}}}}_{{\rm g}}^{C}) \} $ 的一一映射：

$ \begin{equation} f_{{\rm t}}: \left\{{ \left({{{\mathit{\boldsymbol{T}}}}_{G}^{C}} \right)_{i}} \right\}\to \left\{{G_{i} \left({p_{{\rm g}}^{C}, {{{\mathit{\boldsymbol{v}}}}}_{{\rm g}}^{C}} \right)} \right\} \end{equation} $

(8)

其中集合$ \{G_{i} (p_{{\rm g}}^{C}, {{{\mathit{\boldsymbol{v}}}}}_{{\rm g}}^{C})\} $ 包含的所有抓取点组成了连续的线段或曲面，称为抓取区域$ \varOmega $。本文考虑工业零件表面的规则特点，设计了多种变换规则$ \varPsi $，如图 6所示。不同的变换规则$ \varPsi $ 将产生不同类型的抓取区域，按此分类，把抓取簇分为点型、直线型、圆周型、平面型、柱面型、锥面型和球面型。根据零件表面的几何特征合理设置相应的抓取簇，其构成的所有抓取区域将覆盖零件的大部分可抓取点。表 2列出了各种抓取簇的变换规则$ \varPsi $ 及其对应生成的变换集合$ \{({{\mathit{\boldsymbol{T}}}}_{G}^{C})_{i} \} $。

设$ \{(p_{{\rm c}}^{C}) _{i} \} $ 为在簇坐标系{$ C $} 下满足条件$ {{ Q}} $ 的点集，则有对应关系$ {{{\mathit{\boldsymbol{F}}}}} $，使得$ \forall p_{{\rm c}}^{C} \in \{(p_{{\rm c}}^{C}) _{i} \} $ 均有唯一的$ {{\mathit{\boldsymbol{T}}}}_{G}^{C} \in \{({{\mathit{\boldsymbol{T}}}}_{G}^{C}) _{i} \} $ 与之对应，定义该对应为映射：

$ \begin{equation} f_{{\rm p}}: \left\{{ \left({p_{{\rm c}}^{C}} \right)_{i}} \right\}\to \left\{{ \left({{{\mathit{\boldsymbol{T}}}}_{G}^{C}} \right)_{i}} \right\} \end{equation} $

(9)

其中条件$ Q $ 和对应关系$ {\mathit{\boldsymbol{F}}} $ 由抓取区域$ \varOmega $ 的空间几何参数确定，且因抓取簇类型而异，详见表 3。

联立$ f_{{\rm t}} $ 和$ f_{{\rm p}} $，可得到$ \{(p_{{\rm c}}^{C}) _{i} \} $ 到$ \{G_{i} (p_{{\rm g}}^{C}, {{{\mathit{\boldsymbol{v}}}}}_{{\rm g}}^{C}) \} $ 的映射：

$ \begin{equation} f_{{\rm g}}: \left\{{ \left({p_{{\rm c}}^{C}} \right)_{i}} \right\}\to \left\{{G_{i} \left({p_{{\rm g}}^{C}, {{{\mathit{\boldsymbol{v}}}}}_{{\rm g}}^{C}} \right)} \right\} \end{equation} $

(10)

3.2 体素化的碰撞模型

有效的抓取姿态必须是无碰撞风险的，即抓取器件在其抓取路径上不能与其他零件及料箱发生碰撞，为实现对抓取路径的快速碰撞检测，本文利用3维体素^[29]的规则化特点，提出体素化的碰撞模型。

设料箱底部中心点为料箱坐标系{$ B $} 的原点，$ x_{B} $ 轴和$ y_{B} $ 轴分别平行于料箱的长和宽所在的直线，$ z_{B} $ 轴正方向朝上。如图 7所示，在料箱坐标系{$ B $} 下，对整个料箱及堆叠于其中的零件空间进行体素化，体素边长$ L_{{\rm v}} ={D_{{\rm g}}}/2 $，$ D_{{\rm g}} $ 为抓取器件最大外接直径。把该体素空间记为$ \varPi_{{\rm c}} (L_{{\rm bl}}, L_{{\rm bw}}, L_{{\rm h}}, L_{{\rm v}}) $，其中$ L_{{\rm bl}} $ 和$ L_{{\rm bw}} $ 分别为料箱的长和宽，$ L_{{\rm h}} $ 为零件堆叠的最大高度（若零件高度小于料箱高度，则$ L_{{\rm h}} $ 为料箱的高度）。对体素空间$ \varPi_{{\rm c}} $ 中的每一个体素进行以下标记：

s1) 若体素内包含零件点云（目标零件除外）或料箱边界，则把该体素标记为碰撞体素，否则标记为安全体素；

s2) 对于每个安全体素，检测其所有邻近体素是否存在碰撞体素，如存在则把该安全体素更改为风险体素。

如图 8所示，在标记后的碰撞体素空间中，计算抓取路径$ {\overrightarrow {p_{{\rm h}} p_{{\rm g}}}} $ 所经过的体素集合$ {\rm \{(}V_{{\rm p}}) _{i} \} $，由于体素边长为抓取器件最大外接直径的一半，因此当$ {\rm \{(}V_{{\rm p}})_{i} \} $ 中不存在碰撞体素或风险体素时，即可确保抓取器件在该抓取路径上不会与箱体或零件碰撞。

3.3 计算抓取簇中的最佳抓取姿态

对于抓取簇$ \varTheta $，记$ {\rm \{(}V_{{\rm t}}) _{i} \} $ 为碰撞体素空间$ \varPi_{{\rm c}} $ 顶层的体素集合，$ {\rm \{(}p_{{\rm t}}^{C}) _{i} \} $ 为这些体素的中心点在其簇坐标系{$ C $} 下的点集，若点$ p_{{\rm t}}^{C} \in \{(p_{{\rm t}}^{C}) _{i} \} $ 满足条件$ {{ Q}}_{\varPhi} $，则可以根据映射$ f_{{\rm p}} $ 和$ f_{{\rm g}} $ 分别计算对应的变换$ {{\mathit{\boldsymbol{T}}}}_{G}^{C} $、抓取点及抓取方向$ (p_{{\rm g}}^{C}, {{{\mathit{\boldsymbol{v}}}}}_{{\rm g}}^{C}) $。

记$ (p_{{\rm g}}^{C}, {{{\mathit{\boldsymbol{v}}}}}_{{\rm g}}^{C}) $ 变换到料箱坐标系{$ B $} 下为$ (p_{{\rm g}}^{B}, $ $ {{{\mathit{\boldsymbol{v}}}}}_{{\rm g}}^{B} (u_{{\rm g}}^{B}, v_{{\rm g}}^{B}, w_{{\rm g}}^{B})) $，$ p_{{\rm g}}^{B} $ 点坐标为$ (x_{{\rm g}}^{B}, y_{{\rm g}}^{B}, z_{{\rm g}}^{B}) $，另设$ {{{\mathit{\boldsymbol{v}}}}}_{{\rm g}}^{B} $ 与$ z_{B} $ 轴的负方向夹角为$ \theta $，因为机器人只能从上往下进行抓取，所以当$ \theta < {{\rm \rm{\pi } }}/2 $ 时，$ (p_{{\rm g}}^{B}, {{{\mathit{\boldsymbol{v}}}}}_{{\rm g}}^{B}) $ 才可能是有效的抓取点和抓取方向，且$ \theta $ 越小，其碰撞风险越小。设$ L_{{\rm hg}} $ 为由$ (p_{{\rm g}}^{B}, {{{\mathit{\boldsymbol{v}}}}}_{{\rm g}}^{B}) $ 确定的直线，$ \varGamma _{{\rm c}} $ 为碰撞体素空间$ \varPi_{{\rm c}} $ 的上表面，则可以把$ L_{{\rm hg}} $ 与$ \varGamma_{{\rm c}} $ 的交点（若存在）作为起始点$ p_{{\rm h}}^{B} $，且有：

算法1：计算抓取簇中的最佳抓取姿态
输入：碰撞体素空间的最上层体素集合$ \{(V_{{\rm t}}) _{i} \} $，上表面$ \varGamma_{{\rm c}} $，抓取簇$ \varTheta $
输出：最佳抓取姿态$ {{\mathit{\boldsymbol{T}}}}_{G}^{C} $ 和$ {{{\mathit{\boldsymbol{T}}}}}_{G_{{\rm h}}} ^{G} $     $ \{{G_{i} (p_{{\rm g}}, {{{\mathit{\boldsymbol{v}}}}}_{{\rm g}}) } \}\leftarrow {\rm EMPTY} ({\{{G_{i} (p_{{\rm g}}, {{{\mathit{\boldsymbol{v}}}}}_{{\rm g}}) } \}}) $     $ \{{({{\mathit{\boldsymbol{T}}}}_{G}^{C}) _{i}} \}\leftarrow {\rm EMPTY} ({\{{({{\mathit{\boldsymbol{T}}}}_{G}^{C}) _{i}} \}}) $
For $ i=1 $ To $ {\rm NUM} \left({\{(V_{{\rm t}}) _{i} \}} \right) $ Do   $ p_{{\rm c}} \leftarrow {\rm CENTER} \left({(V_{{\rm t}}) _{i}} \right) $   If $ p_{{\rm c}} $ 满足条件$ {{ Q}}_{\varPhi} $ Then     /*根据映射$ f_{{\rm g}} $ 和$ f_{{\rm p}} $ 计算抓取点、抓取方向和变换矩阵，并按$ \theta $ 从小到大插入*/     $ G(p_{{\rm g}}, {{{\mathit{\boldsymbol{v}}}}}_{{\rm g}}) \leftarrow {\rm GRASPING} \left({p_{{\rm c}}} \right) $     $ {{\mathit{\boldsymbol{T}}}}_{G}^{C} \leftarrow {\rm TRANSFORMATION} \left({p_{{\rm c}}} \right) $     $ \theta \leftarrow {\rm ANGLE} \left({{{{\mathit{\boldsymbol{v}}}}}_{{\rm g}}, -{{{\mathit{\boldsymbol{U}}}}}_{z}} \right) $     $ {\rm INSERT} \left({\left\{{G_{i} (p_{{\rm g}}, {{{\mathit{\boldsymbol{v}}}}}_{{\rm g}}) } \right\}, G(p_{{\rm g}}, {{{\mathit{\boldsymbol{v}}}}}_{{\rm g}}), \theta} \right) $     $ {\rm INSERT} \left({\left\{{({{\mathit{\boldsymbol{T}}}}_{G}^{C}) _{i}} \right\}, {{\mathit{\boldsymbol{T}}}}_{G}^{C}, \theta} \right) $  End If
End For
For $ i=1 $ To $ \; {\rm NUM}\left({\left\{{G_{i} (p_{{\rm g}}, {{{\mathit{\boldsymbol{v}}}}}_{{\rm g}}) } \right\}} \right) $ Do   $ L_{{\rm g}} \leftarrow \; {\rm LINE}\left({G_{i} (p_{{\rm g}}, {{{\mathit{\boldsymbol{v}}}}}_{{\rm g}}) } \right) $   $ p_{{\rm h}} \leftarrow \; {\rm CROSSPOINT}\left({L_{{\rm g}}, \varGamma_{{\rm c}}} \right) $   $ p_{{\rm g}} \leftarrow p_{{\rm g}}\; {\rm of}\; G_{i} (p_{{\rm g}}, {{{\mathit{\boldsymbol{v}}}}}_{{\rm g}}) $   /*计算抓取路径经过的体素集合*/   $ \{{(V_{{\rm p}}) _{j}} \}\leftarrow \; {\rm VOXELSPASSED}({p_{{\rm g}}, p_{{\rm h}}}) $   $ f_{{\rm all}} \leftarrow \; {\rm BOOL}({{\rm true}}) $   For $ j=1 $ To $ \; {\rm NUM}({\{{(V_{{\rm p}}) _{j}} \}}) $ Do     /*检查是否包含碰撞体素或风险体素*/     $ f_{{\rm temp}} \leftarrow \; {\rm CHECKSAFE}({(V_{{\rm p}}) _{j}}) $     If $ f_{{\rm temp}} ={\rm false} $ Then       $ f_{{\rm all}} ={\rm BOOL} ({{\rm false}}) $       Break     End If   End For   /*无碰撞体素或风险体素，则输出$ {{\mathit{\boldsymbol{T}}}}_{G}^{C} $ 和$ {{{\mathit{\boldsymbol{T}}}}}_{G_{{\rm h}}} ^{G} $*/   If $ f_{{\rm all}} ={\rm true} $ Then     $ {{\mathit{\boldsymbol{T}}}}_{G}^{C} \leftarrow ({{\mathit{\boldsymbol{T}}}}_{G}^{C}) _{i} $     根据式(6) 计算$ {{{\mathit{\boldsymbol{T}}}}}_{G_{{\rm h}}} ^{G} $     Return $ {{\mathit{\boldsymbol{T}}}}_{G}^{C} $, $ {{{\mathit{\boldsymbol{T}}}}}_{G_{{\rm h}}} ^{G} $   End If
End For
Return无有效的抓取姿态

$ \begin{equation} \left| {\overrightarrow {p_{{\rm h}} p_{{\rm g}}}} \right|={\left( {z_{{\rm g}}^{B} -L_{{\rm h}}} \right)}/{w_{{\rm g}}^{B}} \end{equation} $

(11)

基于上述讨论，在算法1中描述了计算最佳抓取姿态的方法。

4 实验结果及分析（Experimental result and analysis）

为了评估算法的性能，本文分别在仿真环境和真实环境下对抓取姿态检测算法进行实验。计算机配置为Intel$ ^\circledR $ Core$ ^{\rm {TM}} $ i7-5820 K 3.30 GHz，8 GB RAM，操作系统为Windows 7 64位，GPU为NVIDIA GeForce GTX 1080。实验中的抓取对象选取工业生产中常见的零件，分别为三通管、球形把手、锥形脚垫和发动机连杆，其真实形状和CAD模型如图 9所示。

同时，为验证算法的通用性，将采用吸盘和气动夹具2种抓取器件分别进行抓取实验，其尺寸及CAD模型如图 10所示。考虑抓取对象的几何形状，采用吸盘方式抓取的零件为三通管、球形把手和锥形脚垫，采用气动夹具抓取的零件为三通管和发动机连杆。

4.1 仿真实验 4.1.1 实验内容与评价指标

仿真实验的数据集主要通过Unity3D软件的物理仿真功能来生成。Unity3D软件内置的物理引擎可模拟物体自由落体运动和碰撞，先在虚拟空间中导入料箱模型，然后让零件模型以刚体形式从料箱上方位置随机落下，并记录所有落在箱体内部的零件的位姿，最后在料箱上方设置虚拟深度相机，从而得到该堆叠零件的场景点云。

采用上述方法针对4种实验零件各生成200个场景点云。为了单独验证本文算法的有效性，需要排除零件位姿识别结果对实验的影响，因此设计2组实验：第1组实验将从Unity3D软件中直接获取零件的真实位姿，并使用本文提出的算法按零件的高低顺序检测抓取姿态；第2组实验先使用文[28]的方法识别出目标零件姿态，再使用本文算法检测抓取姿态。若成功计算出最佳抓取姿态且其抓取路径没有与箱体或其他零件发生碰撞，则可视为一次成功的检测。仿真实验的评价指标主要包括抓取姿态的检测成功率、抓取倾斜度和算法运行时间。检测成功率定义为

$ \begin{equation} R_{{\rm s}} =\frac{n_{{\rm s}}} {N_{{\rm s}}} \times 100\% \end{equation} $

(12)

其中$ N_{{\rm s}} $ 表示总的场景个数，$ n_{{\rm s}} $ 表示成功检测出抓取姿态的场景个数。同时，为了分析零件位姿识别结果对本文抓取姿态检测算法的影响，将统计第2组实验的零件位姿识别的情况。

另外，与基于离散固定抓取点的传统方法进行对比，以验证本文提出的基于抓取簇的方法是否能明显提升抓取姿态的计算性能。该传统方法的设计内容主要包括抓取点设置和最佳抓取点计算：

(1) 抓取点设置：按平移步长5 mm、旋转角度步长30$ ^{\circ} $ 的方式在零件的所有可抓取区域设置抓取点；

(2) 最佳抓取点计算：得到目标零件位姿后，对所有设置在目标零件上的抓取点所对应的抓取路径进行基于方向包围盒（oriented bounding box，OBB）的碰撞检测，对于通过碰撞检测的抓取点，计算其对应抓取方向与$ z_{B} $ 轴负方向的夹角，并取该夹角最小的抓取点为最佳抓取点。

4.1.2 实验结果及分析

第1组和第2组的实验结果分别如表 4和表 5所示，图 11展示了部分抓取姿态的检测结果。

第1组实验中，由于零件位姿均为真实位姿，因此采用本文方法进行抓取姿态检测的平均成功率能达到94%，且所有零件的检测成功率均超过85%，比基于离散固定抓取点的传统方法提高了10% 以上。另外，采用本文方法的抓取倾斜度均在20$ ^{\circ} $ 以内，明显优于传统方法，证明了该算法设计的合理性和有效性。其中使用吸盘方式时检测成功率均超过95%，普遍高于使用气动夹具的方式，一方面是因为吸盘与零件接触面积相对较小，在零件表面有更大的可抓取区域；另一方面是因为碰撞体素的边长由抓取器件的最大外径决定，体素边长越小，安全体素的占比就越高，因而更容易检测出安全的抓取路径。在算法运行时间方面，本文方法仅需约20 ms，其效率是传统方法的数十倍，且对于不同零件均比较稳定。而传统方法的运行时间则会随所设置的抓取点数目的增加而线性增长，证明了本文算法具有良好的实时性，更符合工业生产要求。

第2组实验中，由于存在识别错误的零件位姿，因此抓取姿态检测的成功率有一定程度的降低，说明本文提出的抓取姿态检测算法在一定程度上依赖零件位姿识别的结果，可见提高零件位姿识别的准确率是提高抓取姿态检测成功率的关键之一。

4.2 实际抓取实验

机器人从料箱中分拣散乱堆叠的零件问题称为Bin-Picking问题。以本文提出的抓取姿态检测算法为核心，搭建了一套完整的Bin-Picking系统，并在该系统上完成零件的实际抓取实验。

4.2.1 Bin-Picking系统平台

搭建的Bin-Picking系统平台如图 12所示。其中1为固定于环境中的3D相机，采用投影仪加单相机的面结构光成像方案，其中投影仪为TI公司的DLP4500，搭配Point Gray工业相机；2~5分别为计安川机器人、Motocom控制柜、吸盘/气动夹具和装有堆叠零件的料箱；6为基于Qt库和C$ ++ $ 语言开发的Bin-Picking系统程序，其集成了系统配置、手眼标定、零件配置、机器人通信和作业控制等功能，可以完成整个零件抓取系统的部署和作业。

4.2.2 实验内容与评价指标

在搭建的Bin-Picking系统平台上进行零件的实际抓取实验，根据4种零件的5种抓取方式进行分组，每组每次取20个相同零件随机堆放在料箱中，机器人将尽可能地把料箱中的全部零件分拣到另一个料箱中，当全部零件分拣完成或已经无法检测出可抓取姿态时，记为1次分拣结束，重复分拣10次，统计抓取结果。实际抓取实验的评价指标主要包括抓取成功率、料箱平均清空率和抓取周期。定义抓取成功率为

$ \begin{equation} R_{{\rm g}} =\frac{n_{{\rm g}}} {N_{{\rm g}}} \times 100\% \end{equation} $

(13)

其中$ N_{{\rm g}} $ 表示总的抓取次数，$ n_{{\rm g}} $ 表示抓取成功的次数，一次成功的抓取是指在一次点云采集周期内把一个零件从料箱中成功抓取出来，并放置到另一个料箱中，且该过程没有发生碰撞。

由于位于料箱边角处的零件更容易被遮挡或在抓取过程更容易发生碰撞，在实际工业生产中，每一箱来料的零件通常都难以全部分拣完成，为分析本文算法对料箱边角零件的处理能力，定义料箱清空率：

$ \begin{equation} R_{{\rm c}} =\frac{\sum\limits_{i=1}^m {\frac{n_{i}} {N_{i}} \times 100\%} }{m} \end{equation} $

(14)

其中$ m $ 表示重复分拣的次数，$ N_{i} $ 表示第$ i $ 次分拣中料箱所含零件的总个数，$ n_{i} $ 表示第$ i $ 次分拣中成功抓取的零件个数，本文实验中$ m=10 $，$ N_{i} $ 均取20。

机器人的运动速度设置为16 mm/s。定义系统响应时间为$ T_{1} $，包括点云采集时间、零件姿态估计时间和抓取姿态的计算时间；定义机器人从抓取起始点到把零件抓离料箱的时间为抓取时间$ T_{{\rm 2}} $，并把机器人从离开料箱的位置到放置完零件再回到抓取起始点的时间定义为放置时间$ T_{{\rm 3}} $，由于系统在机器人离开料箱后就马上开始采集点云并计算抓取姿态，即$ T_{1} $ 和$ T_{3} $ 存在重叠关系，因此可把一个抓取周期的总时间定义为

$ \begin{equation} T=T_{2} +\max (T_{1}, T_{3}) \end{equation} $

(15)

4.2.3 实验结果及分析

在Bin-Picking系统平台上进行实际抓取实验，结果如表 6所示，图 13为部分抓取实验中识别到的抓取姿态及其对应的机器人实际抓取过程。

从表 6可以知道，实际抓取的平均成功率达92.2%，使用吸盘方式抓取的成功率均超过95%，使用气动夹具方式抓取的成功率均在85% 左右，与仿真实验结果相符，进一步证明了本文算法的有效性和通用性。料箱的平均清空率能达到87.2%，也进一步证明本文算法在一定程度上能解决边角零件的抓取问题。抓取的平均周期在11 s左右，满足实际生产的时间节拍要求。另外，在实验的整个抓取过程中都没有发生碰撞，证明了本文算法能够保证抓取的安全性。

经统计，在实验中造成抓取失败或检测不到抓取姿态的原因主要有：1) 受限于3D相机性能，部分零件因表面光滑或反光导致获取的场景点云质量较差，影响了零件位姿识别的效果；2) 部分零件可抓取部位较少，难以计算出安全的抓取姿态；3) 部分零件体积较小，因此这部分位于料箱边角处的零件难以计算出安全的抓取姿态；4) 部分零件因表面光滑导致在抓取过程中发生了滑动。

从实际抓取实验的结果来看，以本文算法为核心搭建的Bin-Picking系统能够快速、准确、安全地完成散乱堆叠零件的抓取分拣任务，可满足实际工业应用的要求。

4.2.4 与其他抓取方法对比

为比较本文算法和目前流行的基于深度学习的抓取方法在实际应用上的性能，本文还分别测试了传统方法和文[17]提出的基于自监督深度强化学习的VPG算法在面向工业零件时的实际抓取表现，由于VPG算法仅支持夹具抓取方式，本次实验将只选取三通管和发动机连杆作为抓取对象，分别在表 7和表 8中展示了这2种方法与本文方法的抓取对比结果。由表 7和表 8可知，VPG算法在面向工业零件（颜色单一、形状规则的抓取对象）时表现不及传统方法和本文方法，一定程度上说明了在目前研究阶段，基于位姿估计的抓取策略在面向工业零件的抓取问题上更能满足实际工业生产的要求。另外，本文算法在抓取成功率、料箱清空率和抓取效率上均优于VPG算法和传统方法，进一步验证了本文算法的实用性。

5 结论（Conclusion）

针对工业生产过程中随机堆叠零件的分拣问题，本文提出一种基于抓取簇和碰撞体素的抓取姿态检测算法。提出的抓取簇解决了传统方法中因采用离散固定抓取点而导致可抓取点丢失、筛选效率低等问题，提高了抓取成功率和计算效率。提出的体素化碰撞模型实现了抓取路径的快速碰撞检测，保证了抓取算法的安全性。实验表明，在面向工业零件的场景下，本文算法的性能优于目前流行的深度学习方法，以其为核心搭建的Bin-Picking系统对于常见的工业零件能够快速、准确地检测出安全的抓取姿态并完成分拣，零件抓取的平均成功率达92.2%，料箱的平均清空率达87.2%，且检测时间不到20 ms，可应用到实际工业生产中。

在未来的工作中，将对体素化的碰撞模型进行改进，如结合抓取路径方向进一步细分化体素等，使其更适应大尺寸的抓取器件，提高抓取算法的适用性和鲁棒性。同时，为了进一步提高抓取姿态检测的成功率，零件位姿识别算法有待进行进一步研究。