机械臂论文笔记（一）【基于卷积神经网络的二指机械手 抓取姿态生成研究 】

我现在研二，发表这篇博客的时候，我刚开始做毕业论文。
毕业论文的方向是机械臂，具体做机械臂的什么领域还不知道。
为了对机械臂领域了解更多，明确自己要做什么，这个系列，我将阅读一些机械臂的论文（时间有限，没办法大量看，可能就10多篇吧），我会将比较好的论文做成笔记分享出来，供后来人借鉴。

论文下载

基于卷积神经网络的二指机械手抓取姿态生成研究_李耿磊

摘要

本课题立足机械手自主抓取，研究利用卷积神经网络实现物体表面抓取点生成的解决方案。本课题以场景的深度图像作为输入信息，采取
“先采样，后预测”的两步走抓取生成方案。首先利用 Laplace 方法在深度图像中提取物体边缘像素点，并利用对跖法生成抓取空间；然后基于重要性采样方法从抓取空间中采样获得候选抓取集，最后利用训练好的抓取预测卷积神经网络模型预测候选集每个抓取的成功置信度，取其中极大者作为结果指导机器人完成抓取。

为将卷积神经网络应用于抓取预测，本课题建立了基于卷积神经网络的抓取预测模型。将抓取预测问题抽象为深度学习中的分类问题，网络输出为某抓取点属于成功抓取的置信度。本课题定义一组抓取为抓取中心坐标和二指机械手开合方向的组合，以抓取中心像素坐标为中心取 32×32 像素大小的深度图像作为网络的输入信息，将网络的输出与标签数据的交叉熵距离和作为控制网络训练的成本函数，使网络能够提取深度图像中的高级特征来学习深度图像、抓取类别之间的关系，实现抓取预测。

为生成候选抓取预测集合，本文研究了基于Laplace算子的物体边界提取方法，构建了对跖抓取点空间，确定了基于重要性采样的候选抓取采样策略，并利用交叉熵方法优化得到了高质量抓取点分布的概率密度函数，使得候选抓取集能够最大程度的包含对跖抓取点空间中抓取成功置信度较高的抓取点。

关键词：深度图像；二指机械手；卷积神经网络；抓取生成；候选抓取

第1章 绪论

1.1 抓取生成国内外研究现状

1.1.1已知物体抓取生成

这类抓取规划问题假设待抓取物体的外形是已知的。研究人员通常会离线制作一个抓取数据库，其中包含待抓取物体的 3D 模型及对应的若干组稳定抓取配置。因此，此类抓取规划问题简化为识别物体形状并估计物体位姿，然后从抓取数据库中检索出合适的抓取。整个方案可以由图 1-1 抽象表示。

美国哥伦比亚大学Andrew T. Miller等人将物体简化为一系列形状单元组合，这些单元包括球、圆柱体、圆锥或者长方体等规则形状，如图 1-2 a)所示。通过预先定义形状单元的抓取配置实现对复杂物体的抓取生成，然后将生成的抓取依次输入到仿真环境 GraspIt！[8]进行抓取排序，从而建立物体-抓取数据库。之后，Andrew T. Miller 等人又对上述方法提出改进，通过超二次曲面分解细化物体的分解，如图 1-2 b)所示。瑞典皇家理工学院 Kai Huebner 等人[10]更是直接将物体简化为长方体，如图 1-2 c)所示。通过考虑抓取任务、方向以及物体形状等条件采用启发式方法在长方体表面法向向量中选择完成抓取生成，然后利用离线训练的神经网络模型获得最优抓取。

基于仿真环境进行抓取规划的方法，其衡量指标大多是目前广泛采用的$ϵ$-metric ，但该指标对于误差的敏感性较高。比如当物体实际位姿与理论位姿出现偏差时，容易造成抓取失败，如图 1-3 所示。Jonathan Weisz 和 Peter K. Allen 提出了具有鲁棒性的 p(f) 指标，将物体的位姿视为满足一定概率分布的随机变量，通过随机采样的方式获得若干物体位姿，分别计算抓取在不同位姿上的抓取质量
$ϵ$-metric ，以其均值作为鲁棒抓取指标 p( f) 。通过实物实验发现， p(f) 的效果远超前者，尤其是在抓取较大的物体方面，如图 1-4 所示。

抓取生成和排序不仅可以来源于仿真环境分析和启发式方法，还可以从人类直接示教或者间接学习获得。俄勒冈州立大学 Ravi 等人提出一种物理人类互动指导的方法完成抓取生成，由人类缓慢拖动机械手到达抓取位姿（如图 1-5 所示），记录手爪姿态；Ekvall 等人提出一种基于形状单元和人类示教生成抓取的方法，通过综合感知信息和经验选择合适的抓取靠近方向。整个系统会首先识别人类的抓取类型和抓取物体的类别，记录数据手套的传感器数据（如图 1-5 所示），然后映射到机械手空间中，完成抓取生成和抓取规划。

1.1.2相似物体抓取生成

相似物体的抓取规划遵循一个朴素的经验想法：同一类物体的抓取可行区域大多数情况下是相同的，或者具有相似形状的局部区域的抓取质量是相似的。因此可以通过提取物体的低级或者高级特征表征物体之间的相似程度，进而基于已知物体的抓取去规划对相似物体的抓取。基于数据驱动的相似物体抓取规划整体方案可以由抽象表示。离线部分，人工定义低级或者高级特征，学习这些特征与抓取生成及抓取质量之间的关系，作为相似物体抓取预测的模型；在线部分几乎与已知物体的抓取规划方案相同。

加州大学伯克利分校 Jeffrey Mahler 等人同样建立了一个拥有 10,000 个 3D 模型、总共 25 万抓取点的大型抓取数据库，并提出一种具有相关性奖励的多臂老虎机模型学习特征和抓取之间的关系。作者提出了一种 MV-CNN 多视角卷积神经网络学习其形状相似指标，如图 1-7 a)所示。同时考虑了物体的局部表面高度图（heightmaps，如图 1-7 b）所示），提高了对相似物体的抓取成功率。

1.1.3 未知物体抓取生成

对于未知物体的抓取规划，建立在数据库中没有该物体甚至完全没有数据库的前提下，利用启发式方法建立感知数据和抓取点之间的直接映射关系。根据处理方式的不同，可以将这类问题的解决方法分为三类：1）利用感知数据重构物体模型，通过分析抓取的力封闭性规划抓取点；2）通过感知数据（例如点云）提取低级特征，使用一系列启发式方法作为判断；3）高度依赖被感知物体的整体形状信息进行判断规划。这类问题的解决方案可以抽象总结为图 1-8。

德国慕尼黑工业大学 Michael Beetz 等人提出一种基于单一角度方向感知数据建立物体模型的方法。他们首先根据跳边检测和统计分析去除错误的测量值，然后估算每个表面体素的法线。接下来，通过使用 RANSAC 算法的有效变种检测水平感兴趣区域（例如桌子、平台等）来限制搜索范围，因为通过这些平面可以找到其支撑的物体，然后对感兴趣区域内的点进行聚类分割物体点云。在对场景中的点云进行分割后，再次对每一个物体的点云进行重采样，并估计每个点的表面法线和最小曲线半径。最后将拟合出的物体模型和剩余点云的三角化表面进行合并，重建三维模型，利用分析式抓取规划方法生成抓取点，选择最满足力封闭性的抓取。

1.2 深度学习在抓取领域的研究现状

美国康奈尔大学 Lenz 等人提出了一种基于 RGBD 图像的级联神经网络模型对目标物体进行抓取规划。整个系统分为候选抓取集采样和抓取评估选择两部分，每一部分分别由一个级联神经网络完成。候选抓取集采样是从图片中所有可能的抓取区域进行评分筛选分数相对较高的少部分，然后再进入第二个级联神经网络进行更加精细的评分和筛选，如图 1-10 所示。

谷歌、剑桥大学以及加州大学伯克利分校成立研究团队提出一种基于 Q 函数的强化学习算法训练真实环境中的机器人进行抓取，共同探讨如何将深度强化学习应用在机器人操作中。美国华盛顿大学 Joseph Redmon 等人提出一种基于卷积神经网络的实时、准确抓取检测算法，可以在不使用标准滑动窗口或者区域建议技术的情况下对可抓取的边界框执行单阶段回归。同时，该网络可以同时执行分类，以便在一个步骤中识别对象并找到一个良好的抓取矩形，对该模型的修改通过使用局部约束的预测机制来预测每个对象的多个抓取，如图 1-12 所示。局部约束模型的表现明显更好，特别是对于可以通过各种方式掌握的物体。

1.3 主要研究内容

本文主要针对二指机械手抓取姿态生成问题进行研究，采取“先采样，后预测”的两步走抓取方法。第一步，以场景的深度图像作为先验信息，基于 Laplace 方法提取物体边界像素点以构建对跖抓取点空间，利用重要性采样方法生成候选抓取集；第二步，利用训练的抓取预测卷积神经网络模型对各抓取进行抓取成功置信度预测，从中选择极大者作为结果指导机器人自主抓取物体。具体研究内容如下：

（1）第二章主要研究建立卷积神经网络解决抓取预测问题的数学模型，确定卷积神经网络模型的输入输出形式，推导控制网络训练的目标函数。同时，研究并建立抓取系统的概率扰动模型，引入并确立力封闭概率 E(g)作为抓取质量的衡量指标计算数据标签，以生成网络训练需要的数据集。
（2）第三章主要研究并搭建搭建抓取预测卷积神经网络架构，研究卷积神经网络训练中涉及的优化算法选择、学习率指数衰减以及模型过拟合等超参数调整问题，保证训练得到的模型具有较高的预测准确率和较好的预测泛化能力。
（3）第四章主要研究候选抓取集的采样策略，保证最大程度的获得高成功率抓取点。区别于以往研究者常采用的随机采样法，本文采用重要性采样方法进行抓取采样，利用交叉熵方法优化确定指导采样的概率密度函数。
（4）第五章主要工作是进行综合抓取验证实验，包括仿真实验和实物实验。仿真主要是借助 ROS、MoveIt!和 gazebo 软件，在 gazebo 中搭建一个简单的生活场景，将得到的抓取点通过 ROS 话题发布给 MoveIt!进行机械臂的轨迹规划控制完成抓取尝试；实物实验部分首先研究实验准备阶段的相机标定和手眼标定等工作，然后搭建实验场景进行多组多次不同实验评估抓取姿态生成算法的实用性。

第2章 抓取预测模型建立与数据集制作

2.1 引言

基于深度学习或者卷积神经网络的研究方案成为近几年机器人自主抓取领域的研究热点。但来自真实环境的抓取数据集获取是一项费时费力的工作，且很难达到网络训练所需要的数据集规模，造成训练得到的抓取模型非常容易过拟合，泛化能力较差。本章旨在建立二指机械手抓取模型和基于卷积神经网络的抓取预测模型，研究通过力封闭分析方法从仿真环境中获得大规模抓取数据集及抓取质量标签的可行性，为第三章抓取预测网络模型的训练提供数据基础。

2.2 基于卷积神经网络的抓取预测建模

本课题将抓取预测视为机器学习中的分类问题，使训练好的网络模型能够给出当前抓取姿态下成功和失败抓取物体的概率 p 和1-p ，从候选抓取集合中选择成功概率最大的一组抓取点执行抓取动作。为获得足够规模的抓取数据集，避免训练过拟合，提高模型的泛化预测能力，本课题从仿真环境而非真实场景中获取抓取数据集，并通过力封闭方法分析得到数据标签。

上述过程涉及两个重要的数学模型，它们构成了机器人自主抓取系统的理论基础。

（1） 基于卷积神经网络方法的抓取预测模型：建立基于卷积神经网络的抓取预测模型，确定卷积神经网络的输入输出对象和训练集格式，定义抓取预测问题的成本函数，指导网络权重的更新方向。

（2） 机械手抓取模型：分析机械手接触模型，使用力封闭方法计算机械手抓取质量，为从而得到训练集数据标签。

总结人类对于各种物品的抓取不难发现，当物体以某一特定的稳定位姿摆放于平面上时，人眼只要获取有关物体的 2.5D 图像即可对某抓取的可行性做出正确率极高的判断。假设人眼获得的 2.5D 图像（深度图像）定义为 y ，某一抓取点定义为u ，抓取质量定义为 S，则人类可以依靠自身获得的经验在已知（u,y） 的情况下给出 S $\in$ {0,1} 。因此，u, y,S 之间必然存在某种必然的联系。本课题拟采用卷积神经网络模型学习三者之间的联系，生成抓取质量预测函数。

首先，本文假设待抓取的物体具有足够的刚度，保证在抓取的过程中物体不会因为受力而发生形变；其次，假设物体是以稳定位姿放置在平面上，排除比较极端的摆放姿态；最后，要求相机的安装位置始终位于物体的上方，简化后续渲染数据集的过程。

2.4 建立抓取数据库和训练集

卷积神经网络的训练需要相当规模的数据集，本课题采取利用仿真环境生成抓取训练集的方法首先建立一个包含 1500 个家庭常见生活物品的 3D 模型库，然后在每个模型表面生成至多 250 个抓取点，并计算、存储对应的鲁棒抓取质量 E（u） ；与此同时，记录每个物体至多 10 组稳定位姿，并针对每组稳定位姿生成并存储若干深度图像，构成抓取数据库。最后按照$(S_i,u_i,x_i,y_i)()$ 采样大量抓取训
练集。

2.4.1 建立抓取数据库

本课题构建的 3D 模型库由 3DNet 和 KIT 组成。3DNet[32]是奥地利 Vienna University of Technology 制作的用于物体识别和姿态估计的模型库，共包含 20 大类 1371 个三维模型；KIT 是专门面向家用服务机器人物体识别和姿态估计任务而建立的模型库，包含 129 个家庭常见物品的三维模型，图 2-6 中展示了家庭中经常能够见到几种物品，同时也是抓取任务中非常有代表性的对象。

2.5 本章小结

本章针对二指机械手的抓取质量分析进行了数学建模，并确定抓取力螺旋空间作为抓取质量的衡量指标。考虑到实际抓取中视觉感知、机械臂标定和摩擦系数的不确定性引入的误差，本章确定引入力封闭概率 E(u)保证误差存在的情况下抓取质量的鲁棒性。同时，本章还提出了基于卷积神经网络训练的方法寻找抓取点、深度图像和抓取质量之间的关系，完成对抓取质量的快速预测，同时，利用卷积神经网络的泛化能力，期望能解决对未知物体的抓取质量预测，这一部分能力将在第四章抓取实验中评估。最后，利用已有的 3D 模型库建立了抓取数据库，并通过采样的方式获得了规模可观的训练集供卷积神经网络学习使用。

第3章 抓取预测网络模型框架搭建与训练

3.1 引言

卷积神经网络在计算机视觉领域诸多方面拥有非常优异的表现，相比于传统的深度学习网络将一幅 24x24像素的图片调整为 576 维向量作为输入，卷积神经网络能够保留图像中各像素之间的位置结构信息，更好的学习图像的深层特征。本章将介绍本课题抓取预测卷积神经网络结构以及训练方法，并在测试集上测试模型对深度图像、抓取点和抓取质量预测之间关系的学习效果。最后，针对随机采样候选抓取集出现的问题提出一种新的采样方法，并验证新方法的实际效果。

3.2 抓取预测网络架构搭建

3.2.1 卷积层

卷积层是卷积网络的核心构建模块，其功能类似于普通神经网络的隐含层。卷积层的核心思想有两点，其一是允许每个神经元只与前一层局部的若干神经元连接，其二是同一隐含层的神经元共享权重参数，如图 3-1 所示，同一隐含层共享的权重参数称为“卷积核”。依然以 200x200x3尺寸的图像为例，使用卷积的第一隐含层的权重参数总共只有3x3 =9个，而同样神经元数目下全连接所需要的权重参数是5x5x9 =225个，相差达 25 倍，当图像尺寸更大时效果更明显。

3.2.2 池化层

通常，在卷积网络架构中连续卷积层之间会定期插入池化层。其功能是逐步减小表示的空间大小，以减小网络的计算量，控制过度拟合。池化层作用于输入的每个深度切片，如图 3-3 所示，因此池化层并不改变输入的深度，只是减小每个深度切片的尺寸。同卷积层一样，池化层也存在超参数：过滤器的尺寸 f 和步长 S 。目前有两种常用的池化方法：

（1）平均池化法：取过滤器范围内的平均值，能够减小领域大小受限造成的估计值方差增大，但是目前已经不常使用。

（2）最大池化法：取过滤器中最大的输入值，能够减小卷积层参数误差造成估计均值的偏移，更多的保留纹理信息。图 3-3 采用的就是最大池化方法。

3.2.3 整体网络架构建立

正如第一章综述中提到的，本课题按照抓取生成问题两步走的思路，首先生成候选抓取，然后进行抓取预测评估抓取可行性最高的抓取点。同时，本课题将抓取预测视为二分类问题：抓取成功 S = 1和抓取失败 S = 0 。按照 3.2.3 节中的内容，卷积网络最终的输出层是一个 softmax 层，输出在给定深度图像 y 下抓取点u 成功的概率 p（S =1| y,u） 和失败的概率 p（S =0| y,u）。

该网络采用双通道结构，如图 3-4 所示。上面通道获取深度图像，经四个卷积层、一个池化层以及一个全连接层输出 1024 维向量的抽象特征；下面通道单独处理抓取点的深度值 z ，经过包含 16 个神经元的全连接层分解成 16 维向量；然后将这两个通道的向量融合组成 1040 维向量，再次进入一个包含 1024 个神经元节点全连接层处理，最后由 softmax 层输出得到 $[p(S=1|y,u)p(S=0|y,u){]}^T$。该网络架构各层参数见表 3-1。

3.5 本章小结

本章详细介绍了抓取预测卷积神经网络的框架和训练过程，针对本网络中涉及的优化算法、成本函数正则化和输入特征归一化等训练技巧展开详细的叙述，对网络超参数的调整给出了训练技巧。最后在验证集上进行验证保证训练模型没有出现过拟合现象，数据表明本网络具有不错的预测泛化能力。

第4章 采样候选抓取预测集合

4.1 引言

采样候选抓取预测集合是抓取生成的最后一个步骤，利用第三章的抓取预测网络对候选抓取预测集合中的每一组抓取进行抓取预测和抓取成功置信度排序，从中挑选置信度最高的一组抓取完成抓取生成。本章的工作有两点：1）如何构建物体的抓取全集空间；2）如何保证候选抓取子集空间拥有尽可能多的高置信度抓取，最大可能的得到近似全局最优解。

4.6 本章小结

本章工作聚焦在如何更好的采样得到候选抓取预测集合。首先利用拉普拉斯算子提取了物体边缘，在其边缘像素中寻找所有满足对跖性的像素点对，构成抓取空间全集。然后利用交叉熵方法更新计算重要性函数 $f(X;w)$，本章最后进行了实物验证，证明了重要性采样方法获得的候选抓取预测集合优于随机采样方法，并基于$argmax{Q}_{\theta }(u,y),u\in U$ 确定了最终的抓取位姿。

第5章 综合实验平台搭建与算法验证

5.1 引言

本章的工作内容是搭建仿真和实物实验平台，整合第三、四章算法，基于 ROS（机器人操作系统）分布式框架优势搭建抓取姿态生成软件系统，设计实验验证本论文所提出的抓取算法的可行性。本章实验设计紧密贴合研究目标，一要测试系统在视觉感知和机械臂运动有误差情况下的性能，二要测试系统在有噪声真实环境下的抓取泛化预测能力。

5.2 抓取系统软件框架

图 5-1 是抓取系统的软件框架。硬件层包括传感器和直流（或者虚拟）电机，系统传感器件使用能够获取深度图像的 TSP-V3 特种相机，直流电机用于控制机械臂各关节运动，仿真平台使用到 Gazebo 软件，可以视为本系统中的虚拟硬件；算法层是本系统的核心层，依靠 ROS 系统的分布式框架优势，本课题将候选采样、 抓取预测和策略择优模块分散设计在不同的节点，模块之间的运行互不依赖，提高
程序的可读和可写性。为验证生成的一组抓取的真实抓取可行性，首先将抓取位置从相机坐标系转换到机器人坐标系，然后借助 MoveIt!开源运动规划库完成机械臂的轨迹规划。为保证使用者能够可视化抓取规划整个过程，本系统还提供用户交互界面。一方面，依靠 ROS 提供的 Rviz 软件和 Gazebo 用户界面使使用者可视化整个抓取过程；另一方面，在策略择优模块适时地显示最优抓取的深度图像，使使用者能够直观的判断生成的抓取姿态的好坏。

5.3 系统性能评价指标的建立

本课题旨在解决系统误差对机器人抓取的影响，并希望卷积神经网络能够提取图像高级特征泛化抓取预测的能力。基于此目标，本文在第二章数据集标签制作中引入力封闭概率 E（g）作为抓取质量的衡量指标，其发明者指出该指标相较于传统的力封闭指标能够明显提高生成的抓取的鲁棒性。本章实验目的有三点：

（1）验证仿真环境下抓取系统在有误差情况下对已知物体的抓取效果，抓取实验对象使用抓取数据库中的物体模型；
（2）验证仿真环境下抓取系统在有误差情况下对未知物体的抓取效果，抓取实验对象使用 YCB 数据集中的物体模型；
（3）验证真实环境中抓取系统的抓取效果，抓取实验对象使用生活中常见的几种物品。考查系统在获取的深度图像有噪声、系统误差不确定情况下对未知物体的抓取泛化能力。

在抓取规划系统程序的逻辑设计上，如果最优抓取预测的抓取成功置信度低于 50%，则输出“无法抓取”且不执行任何动作；反之，才输出到 MoveIt!轨迹规划模块控制机械臂运动。本课题将机械臂执行一次抓取动作视为一次“抓取”实验，将机械臂抓取物体并且在机械臂移动过程中不掉落视为一次“抓取成功”实验。统计实验总次数$N_1$ ，实际抓取成功的实验次数$N_2$ ，将$N2/N_1\times 100\%$ 作为本课题实验系统评价指标。

5.4 仿真环境下抓取生成算法测试

5.4.1 仿真实验平台搭建

ROS（Robot Operating System）是目前非常先进的跨平台机器人操作系统，拥有灵活、通用的机器人软件编写框架，并且提供强大的 Rviz 可视化界面。自 2007年斯坦福大学人工智能实验室开发 ROS 至今，已经有越来越多的研究团队通过开放源代码的方式加入到 ROS 开发中，使得越来越多的优秀机器人算法得以封装到ROS 中；另一方面，ROS 社区更是以极快的增长速度支持更多的机器人硬件，避免了后来人重复的进行机器人底层开发工作，缩短了机器人设计开发的周期，图5-2 给出 ROS 支持的部分机器人硬件平台。基于上述优点，本课题选择 ROS 作为机器人抓取生成算法开发平台，使用 MoveIt!作为机械臂轨迹规划的工具，同时使用与 ROS 兼容性较好的 gazebo 作为物理仿真平台。

5.4.1.1 编写 URDF

URDF（全称 Unified Robot Description Format）是一种统一机器人描述文件，使用 XML 格式来描述机器人模型，使用关键字 link、joint 分别定义机器人连杆和关节属性。其中 link 标签下会定义连杆的质量 mass、惯性 inertia、形状等性质，joint 标签会定义关节的连接形式 type、关节转动限制 limit 以及相邻连杆坐标系的位姿关系等属性。通过上述内容就可以建立最基本的机器人模型描述文件。图 5-3 a)是本课题使用的机器人在 SolidWorks 中三维模型展示图，图 5-3 b)是该机器人URDF 文件的部分内容，图 5-3 c)是通过 Rviz 进行可视化的结果。

5.4.1.2 配置 MoveIt!

MoveIt!是目前针对移动操作最先进的开源软件库，结合了运动规划、操作、三维感知、运动学、控制和导航的最新进展，广泛用于工业、商业领域。MoveIt!核心功能是由 move_group 节点提供的，可实现的功能如图 5-4 所示。该节点一方面会获取 ROS 参数服务器中的机器人模型信息 URDF 和关节约束、正运动学及运动规划等配置信息；另一方面还提供 action 和 service 通信机制，支持实现抓（pick）、放（place）和运动规划的 action 请求，提供机器人正逆运动学查询、路径合法性等查询服务；同时，还接收传感器传输的数据进行三维环境感知，从 joint_state 话题接收机器人各关节的状态，通过 JointTrajectoryAction 与机器人控制器建立联系，控制机器人运动。

在使用 move_group 节点进行运动规划之前，需要根据自己的机器人配置MoveIt!，包括设置机器人的免碰撞检测矩阵、机器人运动规划群组、确定末端执行器。图 5-5 a)给出 MoveIt!配置中最重要的机器人运动规划群组的设置，图 5-5 b)在 Rviz 中可视化 move_group 节点完成的一次机械臂运动规划。

5.4.1.3 搭建 ROS 和 Gazebo 联合仿真平台

Gazebo 是一款机器人动力学仿真软件，能够提供高精度的物理仿真。同时，它还提供一个丰富的机器人环境库和一系列传感器、控制器插件。相比 V-REP 和OpenRAVE，Gazebo 开发者通过维护 gazebo_ros 软件包更好使用 ROS 与 Gazebo进行节点间的通信，从而成为 ROS 系统中使用的默认机器人仿真软件。

ROS 作为机器人顶层算法开发平台，如何与底层硬件进行通信是一大难题。为解决这一问题，ROS 维护 ros_control 软件包，图 5-6 能够跟踪数据流的运动。每个机器人会有若干个控制器，每个控制器可以完成对关节的控制，请求下层的硬件资源，同时每个控制器还可以进行 PID 调节。ros_control 提供了多种控制器，包括力矩控制器、位置控制器、速度控制器以及关节轨迹控制器，可以满足对不同控制的需求，关节状态控制器可以实时读取硬件资源接口中各关节的旋转角度反给上层算法。同时，为了能够方便、有效的管理各种控制器，ros_control 提供了controller_manager 节点。硬件资源接口层（Hardware Resource Interface Layer）用来发送命令给硬件，并且从硬件中接收反馈数据；硬件抽象层（RobotHWSim）直接和 Gazebo 中的硬件打交道，通过 write 和 read 的方式来完成硬件的操作。

因此，要完成 ROS 和 Gazebo 的联合仿真，一方面要向机器人描述文件中添加更多关于 Gazebo 的信息，此时不再使用 URDF 文件描述机器人模型，而是使用另一种 xacro 文件，在保留原 URDF 内容的基础上，添加 gazebo 标签，增加针对深度图像获取的传感器插件，确定硬件接口类型；另一方面还要做好各控制器的配置工作，本课题需要规划机械臂运动轨迹完成抓取任务，同时实时获得各关节的旋转角度信息，因此选择启用 joint_state_controller 和 joint_trajectory_controller 两个控制器，为每个控制器编写相关配置文件。图 5-7 a)和 b)表示的是 MoveIt!中运动规划的起始和终止状态，c)图和 d)图展示的是通过 ros_control 控制仿真机器人的运动状态，可以发现两者能够达到同步，因此说明 ROS 和 Gazebo 的联合仿真平台已经搭建成功，可以通过在 ROS 中编写算法程序实现控制仿真环境中的机器人运动。

图 5-8 是 Gazebo 下搭建的仿真实验环境，由带有机械手的机器人、深度相机、待抓取物体和储物盒组成。图中机械臂的位置为抓取生成前的观察位置，每次实验前机械臂都将运动到该位置获取物体的深度图像，实验过程中将物体平稳的抓起放到储物盒中视为一次成功抓取。

5.4.2 仿真实验方案

由于抓取预测网络模型的训练集来自于仿真环境，基于控制变量的思想，首先在仿真环境中验证抓取系统对已知和未知物体进行抓取测试。为在仿真环境中模拟系统误差，将其定义为物体在放置平面上的移动和转动，用$(x,y,\theta )$ 表示，如图5-9 所示，其中 x, y 表示物体在平面上的移动距离，$\theta$ 表示绕垂直平面方向的旋转角度。物体的移动和旋转是以规划得到的抓取中心为原点。

根据 Peter K. Allen 等人的研究，误差模型参数的变动范围见表 5-1。

仿真实验中的抓取物体模型来自于已有的数据集，利用 Gazebo 中 add_model命令添加物体.stl 格式三维模型，建立 Gazebo 仿真软件标准的物体描述文件（sdf文件）。图 5-10 给出 Gazebo 中抓取仿真实验所要抓取的物体模型。

其中对已知物体抓取效果测试中的抓取对象来自于第二章提到的KIT数据集，从中选择 6 件物品：沐浴露瓶、黄盐方盒、牙膏、喷雾瓶、水杯和玻璃碗，见图图5-10 a)所示。对未知物体抓取效果测试的抓取对象来自于 YCB 数据集，YCB 数据集是由耶鲁大学、卡耐基梅隆大学和加州伯克利分校共同创建的物体模型数据集，是目前世界上公认的机器人操作基准测试集之一，从中挑选 8 件生活中常用的不规则物体：杯子、香蕉、可乐罐、汽水瓶、罐头、电钻、榔头和活动扳手，如图 5-10 b)所示。

实验设置 6 组误差参数，每组误差参数包含平面平移信息 x y 和绕垂直平面方向旋转信息$\theta$ ： $(0mm,0mm,0^{\circ })$,$(2mm,2mm,4^{\circ })$，$(4mm,4mm,8^{\circ })$，$(6mm,6mm,12^{\circ })$，$(8mm,8mm,16^{\circ })$，$(10mm,10mm,20^{\circ })$，每组误差参数均要抓满所有14 件物品，共 840次实验。其中$(0mm,0mm,0^{\circ })$代表无系统误差。

对已知物体和未知物体的抓取实验按照统一的实验步骤有序进行：

（1）预定义机械臂观察位置并固定，如图 5-11 a)所示。每次抓取实验前机械臂都将运动到此状态观察物体，获取场景的深度图像，如图 5-11 b)所示；
（2）选择一件物品以任意的稳定位姿放置在相机可视范围之内；
（3）启动抓取系统生成最优抓取位姿，将表征规划抓取在机械臂基坐标系下位姿的四元数和平移向量发送到话题等待 move_group 节点订阅完成轨迹规划；
（4）在 Gazebo 仿真环境中按照 6 组误差参数以抓取点为原点、增量式更改物体位置和姿态，如图 5-11 所示。分别执行抓取轨迹，记录抓取效果；

（5）重复（2）（3）（4）步骤直到抓完所有 14 件物品，统计抓取次数$N_1$ 和抓取成功次数$N_2$ 。

5.4.3 仿真实验验证

按照上一小节介绍的实验步骤开展实验，仿真实验运行在笔者的笔记本电脑上，搭载 UBUNTU 16.04 操作系统，CPU 为 Intel Core i5-4200M @ 2.50GHz x 4，GPU 为 AMD R5（此 GPU 仅仅用于抓取实验，卷积神经网络模型的训练是使用实验室工作站 GPU NVIDIA GeForce GTX 1080Ti）。附表 1 和附表 2 给出对已知物体的抓取仿真实验数据，表中各项数据自左向右依次是：实验次数$N_0$ 、抓取次数$N_1$ 和真实抓取成功次数$N_2$ 。

实验发现，本课题提出的抓取规划系统的抓取次数与实验次数几乎相当，说明其总“认为”物体能够被抓取，即总能够输出一个抓取姿态。在所有的抓取实验次数中，系统对已知物体的抓取成功率能够达到 90%左右，特别是在没有系统误差的情况下成功率能够达到 93.33%。另一方面，随着系统误差的增加，抓取成功率受到影响，但即使在最大误差 $(10mm,10mm,20^{\circ })$ 的情况下抓取成功率仍能达到86.67%，与无误差情况相比波动在 7 个百分点左右。说明对系统误差具备一定的鲁棒性。

附表 3 和附表 4 给出对未知物体的抓取仿真实验数据。虽然 YCB 数据集中的物体模型并不在训练集中，但是抓取系统对其抓取的成功率还是能够达到 85%左右。这是因为像杯子、方形罐头、可乐罐这些物体的形状与 KIT 数据集中的杯子、方形盒以及圆柱罐头外形相似，说明抓取预测网络模型在训练中能够提取深度图像中的抽象特征，将其泛化在对未知或相似物体的抓取质量预测上。另一方面，由实验数据可以看出，在对未知物体的抓取成功率也维持在 7 个百分点左右，进一步说明引入力封闭概率作为抓取质量的评价指标是一个正确的选择。

实验中发现，当规划得到的抓取姿态在一个狭小的近封闭空间中，这时得到的抓取点虽然直观上观察能够成功抓取，但是其对系统误差会更加敏感，在超过$(6mm,6mm,12^{\circ })$ 的误差后很容易导致机械手在自上向下抓取的过程中怼到物体上，或者直接运动到近封闭空间之外，根本无法抓取抓取，如图 5-12 右侧案例所示。左侧给出仿真实验中一部分抓取成功的实验示例。

5.5 实物实验平台搭建

5.5.1 相机内参标定

视觉感知非常重要的一步是根据某点在像素坐标系中的位置计算三维环境空间中该点在相机坐标系中的坐标，进而确定其在世界坐标系中的位置。这一步的准确程度直接影响视觉感知的精度，进一步会影响生成的最优抓取结果。因此准确的标定相机的内外参和畸变是实验准备环节的重要一环。

本课题使用的 TSP-V3 特种相机是由深圳繁维科技有限公司生产的基于 TOF技术的 CCD 小型深度相机模组产品，可通过 micro USB 接口输出 640*480 像素的深度图像、灰度图像和点云数据。其技术参数见表 5-2。

目前使用比较广泛的内外参标定方法是张正友标定法，通过记录 n 组点的世界坐标系及其对应的像素坐标系，利用最小二乘法进行内参数$k_x,k_y,t_0,t_0$ 的计算，已知相机内参之后，再求畸变系数$k_1,k_2,k_3,t_1,t_2$ 。目前 matlab 和 ROS 都有计算相机内外参数和畸变的高级封装，因为本课题对机械臂的控制以及抓取算法都是在ROS 下实现的，故使用 ROS 的 camera_calibration 软件包进行内参数的计算。首先将 TSP-V3 相机的驱动用 ROS 节点封装，将相机获得的红外图像视频流发布到话题“/TSP-V3/image_ir”，然后启动“/camera_calibrator”节点捕获并记录棋盘格黑白角点在世界坐标系和像素坐标系的坐标，如图 5-13 b)所示，本课题总共采集 46组数据，最后按照张正友内参标定方法完成相机内参和畸变系数的计算。整个计算过程中图像和数据传输的路线如图 5-13 c)所示。

本课题所使用的深度相机的相机内参数为$\left[\begin{array}{ccc}397.03& 0& 314.38\\ 0& 402.36& 113.29\\ 0& 0& 1\end{array}\right]$，径向畸变系数$k_1=0.1179$ , $k_2=-0.1643$ ，切向畸变系数$t_1=0.011$, $t_2=-0.00274$, $t_3=-0.4378 $ 。

5.5.2 机械臂 DH 参数标定

仿真实验中，建立的 URDF 模型一方面会作为参数输入 MoveIt!中进行运动学建模和轨迹规划，另一方面经 Gazebo 插件处理转换为 sdf 文件加载到仿真环境中，这样可以使得轨迹规划的机械臂模型同虚拟实验平台完全同步。但是在真实实验中，必须完全依据真实机械臂的 DH 参数建立 URDF 模型，才能尽可能减小规划模型和实际机械臂不同步所带来的“灾难性”误差。

本课题所用的实验平台为实验室自行研制的六自由度机械臂，末端搭载二指欠驱动操作手。其依托于环保部专项课题“失控放射源探测与处置智能机器人技术研究”，是一款用于危险环境中寻找和处置危险源的灵活机械臂。二指机械手的张合通过特殊的机械结构由一个电机控制，保证二指张合的同步性。图 5-14 a)给出六自由度机械臂的结构简图和前置 DH 参数法坐标系的建立，b)给出机械臂和相机之间的位置关系。

机械臂 DH 参数的标定使用的是 API 公司生产的激光跟踪仪和 Spatial Analysis软件，如图 5-15 所示。以 2 轴和 3 轴之间的杆长2a 标定为例，首先将激光跟踪仪放置在一空旷位置，软件将以跟踪仪所在位置建立世界坐标系，保持其他轴不动只转动第 2 轴，记录 30 组靶球位置，在 Spatial Analysis 软件中拟合点为右图中绿色小圆，然后同样保持其他轴不动只转动第 3 轴，同样记录 30 组靶球位置，拟合为右图中紫色大圆。查询两圆圆心距即能够确定杆长2a 。

本课题采用前置法建立机械臂关节坐标系，整个 DH 参数如表 5-3 所示。

5.5.3 手眼标定

通过视觉算法获得的抓取点坐标是相对于相机坐标系而言的，而要控制机械臂完成真正的抓取，就必须建立相机坐标系和机械臂基座坐标系的位姿关系${}^{base}{T}_{cam}$而这正是手眼标定所要实现的目标。图 5-16 显示的是“眼在手上”安装方案的标定原理。保持基坐标系固定不动，通过不断改变末端工具坐标系{E}的位置，获取图中多组 A 、B 、C 的关系，最终通过求解$A_{1}B{C}_1^{-1}=A_{2}B{C}_2^{-1}$ 计算相机和末端工具坐标系之间的位姿关系矩阵。

本课题在深入研究手眼标定的原理之后，使用 ROS 中封装的软件包visp_hand2eye_calibration 完成具体的手眼标定。该软件包提供了两套手眼标定的服务：compute_effector_camera 和compute_effector_camera_quick，前者是通过 ROS话题机制接收基座-末端坐标系关系 A 和相机-标定板坐标系关系C ，而后者是利用ROS 服务机制在向 visp_hand2eye_calibration 节点申请服务时传入申请服务的数据。在计算收集相机-物体坐标关系iC 的过程中，本文使用一种目前比较常用的能够简单有效的估计物体位姿的方法——Aruco Marker（以下简称靶码），如图 5-17 左图所示。其位姿估计的原理是通过检测角点的像素距离和实际距离之间的相似关系，计算相机和靶码之间的距离信息 z ，最后利用相机内参计算两者的坐标变换矩阵，图 5-17 右图给出的是机械臂基坐标系、末端工具坐标系、相机坐标系以及靶码坐标系在 Rviz 中的显示效果。

整个标定过程如图 5-18 所示。一方面，编写 TSP-V3 相机驱动节点，以 30Hz的频率不断拾取视频帧发送到话题/TSP-V3/image_ir，启动 aruco_ros 节点订阅该话题，利用 OpenCV 中角点检测函数获取靶码的角点像素坐标，利用靶码的实际长度与像素长度计算相机和靶码之间的距离信息，从而获得相机和靶码的位姿矩阵${}^{base}{T}_{maker}$ 发布到 tf 中；另一方面，获取机械臂关节角度状态值发布到 joint_state 话题，/Robot_state_publisher 会根据机器人的 urdf 信息建立正运动学模型，计算机械臂基坐标系和末端坐标系之间的关系${}^{base}{T}_{eff}$ ，同样发布到 tf 中。easy_handeye 节点从 tf 中获得 ${}^{base}{T}_{eff}$和${}^{base}{T}_{maker}$，申请 Visp_eye2hand_calibration 节点的服务。

最终得到手眼矩阵：

5.6 真实环境下抓取算法测试

5.6.1 实物实验方案

真实环境中获得的深度图像通常会引入自然噪声，这对于使用源自仿真环境的数据集训练的卷积神经网络模型的预测效果是一个很大的考验；另一方面，仿真环节中引入的误差参数模型难以准确描述实际存在的系统误差。因此，本课题拟利用真实环境中的抓取效果检测抓取系统的实际能力。与仿真实验不同，实物实验开始之前需要标定相机内参和畸变系数，确定像素坐标系和相机坐标系之间的位姿矩阵；然后进行手眼标定确定相机和机器人基坐标系之间的关系。规划出的抓取点与机器人坐标系之间的坐标转换如图 5-19 所示。

实物实验所抓取的物体选自生活中 5 种常见物品：矿泉水瓶、香蕉、洗衣液、胶带和订书机，如图 5-20 所示。按照物体外形特征不同可以将这 5 种物品分为三类，香蕉、矿泉水瓶和订书机代表拥有明显长轴的物品，胶带代表环形物品，蓝月亮洗衣液代表外形比较复杂的一类抓取对象。抓取步骤同仿真实验相似，只是减少了物体按照误差参数移动的实验环节，因为本身提出误差参数模型也是为了能够在仿真环境中模拟系统误差的存在。上述 5 种物体没有任何一种同 KIT 数据集物体模型完全一致，使得实验结果能够验证本课题提出的抓取方案在真实环境中对相似或未知物体的规划抓取效果。

5.6.2 实物实验验证

相机内参和手眼标定完成代表实验前的准备工作已经就绪，按照 5.3.2.2 小节提到的场景布置抓取实验，整个流程如图 5-21 所示。每次实验机械臂会首先从初始位置 a)到达观察位置 b)，获取物体的深度图像，然后使用重要性采样方法生成候选抓取预测集合 c)，将每组抓取输入到抓取预测卷积神经网络模型中预测每组抓取的成功置信度，从中选择最优抓取 d)。然后利用坐标转换模块计算抓取点在机械臂基坐标系中的位姿，依靠 MoveIt!进行轨迹规划完成一次抓取。

按照上述步骤分别对五种实验对象进行抓取测试，每种物体抓 10 次，每次摆放的位置和角度不同，但保证在机械臂的可达空间之内，实验结果统计见表 5-4（实验数据一栏自左向右表示：实验次数、抓取次数、实际抓取成功次数）。实验发现，该系统对实际物体的规划成功率很高，几乎总能发现物体可抓取的位置，即系统总是“认为”物体是可抓取的。但实际抓取成功率相较于仿真环境稍低，对矿泉水瓶的抓取远远低于仿真环境。这是由于矿泉水瓶的透光性，导致相机中的红外接收器难以准确识别物体的深度和边缘，一方面影响了边缘提取算法的效果，另一方面噪声的混入使得预测网络模型的预测能力变弱。最终的抓取成功率仍能达到 76%以上，表明训练得到的网络模型有着较好的泛化能力。

5.7 本章小结

本章基于 ROS 搭建了抓取规划系统软件框架，并利用 ROS-MoveIt!-Gazebo 联合搭建了仿真实验平台，为验证自主抓取系统对物体位置姿态误差的鲁棒性，本课题提出一种简化的误差模型$(x,y,\theta )$ ，实验证明在误差高达$(10mm,10mm,20^{\circ })$的情况下抓取规划的成功率仍能达到 85%以上；与此同时，本课题搭建了实物实验平台验证抓取预测模型在有噪声的真实环境中的模型泛化能力，实验统计的抓取成功率在 76%左右，取得了良好的泛化效果。

结论

智能机器人概念的兴起使得人们不再满足于计算机拥有智慧的大脑，更需要机器人能够灵活精确的同物理世界进行交互，自主抓取就是其中一项非常重要的能力。本课题旨在将卷积神经网络方法同抓取规划问题联系在一起，建立基于卷积神经网络的抓取预测模型，将抓取姿态生成问题分解为“先采样，后预测”两个动作来实现。本论文的具体研究成果总结如下：

（1）建立了基于卷积神经网络的抓取预测模型，用抓取中心像素位置和抓取角度定义一组抓取点。研究了二指机械手接触抓取模型，针对传统力封闭方法对系统误差的敏感性，引入力封闭概率作为抓取质量的衡量指标，指导数据集标签的结果；

（2）搭建了抓取预测卷积神经网络框架，针对权重参数训练中的优化算法、成本函数正则等引入的超参数进行了不断的调整，直到模型的成本函数值能够收敛稳定在一个较小的范围。为了验证训练好的模型的泛化能力，又测试了模型在测试集上的预测效果，结果表明模型没有出现过拟合现象；

（3）解决了如何采样抓取成功率高的抓取点到候选抓取预测集合的问题。首先通过 Laplace 算子提取物体边缘及其法向量，使用对跖法确定合适的抓取集合C ；然后使用交叉熵优化方法得到重要性采样的概率密度函数$f(X;w)$，以此进行抓取采样就可以对抓取集合C 进行采样生成候选抓取预测集合；

（4）基于 ROS 搭建了抓取姿态生成系统框架。使用 C++和 python 编程，利用 Gazebo 搭建了机器人仿真环境，对抓取规划方法的误差鲁棒性进行评估；另一方面，整合实物平台，进行了相机内参标定和手眼标定，验证系统对带有噪声的真实环境的抓取规划泛化能力。实验数据显示，本方法在仿真环境中的抓取成功率达到 85%以上，在真实环境中的抓取成功率也能达到 76%，具有较强的实用性。

本课题的抓取方案在仿真和真实实验中取得了较好的抓取效果，但在以下两个方面后续还可以进行更加深入的研究。

（1）目前的抓取姿态生成仅停留在单物体场景中，无法抓取多物体场景中的指定物体。后续可以在抓取规划之前添加目标识别步骤，利用 bounding box 将待抓取物体框选出来，裁剪得到只包含单个物体的深度图像；然后再利用本课题算法进行规划最终的抓取姿态，实现多物体场景中指定物体抓取任务；

（2）在仿真和实际实验中发现每次抓取规划算法的总时长达 2s，使得该方案难以满足工业生产等紧凑环节的实时性需求。分析发现，其中采样候选抓取预测集合所用时间为 1.7s 左右，占抓取总时长的 85%。改进候选抓取预测集合的生成策略或者采样方法可以作为后续的研究和改进方向。