一种基于复用结构的双阶段机械臂抓取规划方法和系统

技术领域

本发明属于机器人应用技术领域，更具体地，涉及一种基于复用结构的双阶段机械臂抓取规划方法和系统。

背景技术

近年来，工业机器人已应用于焊接、喷涂、码垛、装配和机械加工等生产制造过程。但目前机器人工业应用大多依赖于结构化的生产环境，通过人工示教的方法采集一些固定轨迹点，由机器人依据预先设定的轨迹点完成一些特定动作。当工厂生产环境发生变化时，需花费大量时间重新采集轨迹点，将大大增加工业生产成本。另外，近年来仓储物流、家庭环境等非结构化环境的需求日益增多，机器人技术的智能化成为主流发展方向。其中机器人抓取任务尤为常见与重要，是当前机器人领域的研究热点，因此深入研究机器人抓取技术对推进机器人智能化进程和提高生产效率具有重要意义。

现有研究表明，基于深度视觉的对接与抓取方法具有较好的泛用性、灵活性及性能，然而机器人抓取任务的鲁棒性受到任务环境、算法性能、数据处理方法及数据质量等因素影响。因而深入研究基于深度视觉的机器人抓取规划方法，设计鲁棒的算法模型，对于推动机器人抓取作业智能化进程具有重要意义。

由此可见，现有技术存在抓取策略不合理、算法鲁棒性差、抓取成功率低的技术问题。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种基于复用结构的双阶段机械臂抓取规划方法和系统，由此解决现有技术存在抓取策略不合理、算法鲁棒性差、抓取成功率低的技术问题。

为实现上述目的，按照本发明的一个方面，提供了一种基于复用结构的双阶段机械臂抓取规划方法，包括：

将待抓取场景单视角点云输入抓取姿态预测模型，得到点云中每个点对应的抓取姿态，组成抓取姿态集合，将抓取姿态集合中的抓取姿态输入抓取姿态评估模型，对得到的抓取姿态集合中各抓取姿态按照质量评分进行排序并选取排名靠前的K个抓取姿态，用于指导机械臂进行抓取作业；

所述抓取姿态预测模型通过如下方式训练得到：

通过改变抓取场景采集多视角数据，对多视角数据进行三维重建获取场景完整点云，利用场景完整点云生成抓取姿态，组成抓取姿态预测数据集，利用抓取姿态预测数据集训练抓取姿态预测网络至收敛，得到抓取姿态预测模型；

所述抓取姿态评估模型通过如下方式训练得到：

将抓取姿态预测数据集中不与机器人末端夹持器发生碰撞且位于机器人末端夹持器夹持范围内的点云作为抓取姿态评估数据集，利用抓取姿态评估数据集训练抓取姿态评估网络至收敛，得到抓取姿态评估模型；

所述抓取姿态预测网络和抓取姿态评估网络均采用复用结构，所述复用结构表示除第一层网络外，后续每层网络的输入都与第一层网络的输入数据或上一层网络的输出连接。

进一步地，所述抓取姿态预测网络中除第一层网络外，后续每层网络的输入都与第一层网络的输入数据连接。

进一步地，所述抓取姿态评估网络中除第一层网络外，后续每层网络的输入都与上一层网络的输出连接。

进一步地，所述多视角数据包括相机位姿与深度图像，通过如下方式采集：

针对某一抓取姿态，执行抓取任务，通过减少场景中物体数量改变抓取场景；

针对某一抓取姿态，从外围向中心方向水平推动一定距离，改变物体位置，从而改变抓取场景；

针对某一抓取姿态，执行抓取动作后放置到场景中心上方，使其自由下落，从而改变抓取场景；

随机选择上述改变抓取场景的方式之一，若某一抓取姿态执行成功则继续进行多视角数据采集，若不能规划成功，则再从剩余改变抓取场景的方式中随机选择，若最终没有抓取姿态被执行时，则结束本轮数据采集。

进一步地，所述利用场景完整点云生成抓取姿态的具体方式包括：

对于场景完整点云C中的任意单视角点云C′，随机采样n个点，得到点集P；针对点集P中的每个点，在场景完整点云C中通过半径查询获得某一半径内所有点的法线集合，由此建立局部坐标系；

针对点集P中的每个点，在场景完整点云C中通过半径查询获得某一半径内所有的点的集合P′，将局部坐标系围绕y轴旋转获得抓取坐标系，将集合P′中每个点从世界坐标系变换到抓取坐标系后，选取集合P′中满足机器人末端夹持器闭合区域高度范围的点，组成局部区域点集；

将抓取坐标系沿着x轴远离被抓取物体方向后退一定距离，得到抓取姿态初始位置，沿着y轴方向设置多组夹持器二指位置，当局部区域点集中的点云与夹持器二指模型碰撞，且夹持器两个平行手指的闭合区域内含有该点云时，将该组夹持器二指位置作为该点云的抓取姿态中的y轴方向的抓取位置；

从该点云的抓取姿态中的y轴方向的所有抓取位置中选取一个居中位置，然后在抓取坐标系中以固定步长向x轴方向推进，直到该点云与夹持器二指模型碰撞，将此时的x轴方向的推进位置作为该点云的抓取姿态中的x轴方向的抓取位置。

进一步地，所述抓取姿态预测模型的训练还包括：

针对局部区域点集中每个点云的抓取姿态进行质量评分，所述质量评分通过如下方式计算：

找到局部区域点集中点云对于抓取姿态在y轴上的最大值max与最小值min，分别统计满足y＞max-thr与y＜min-thr条件的点作为两组接触点，计算每组接触点位置均值作为代理接触点，thr表示距离阈值；

依据两个代理接触点组成的向量v，针对每组接触点，统计满足v与其中每个点的法线之间的角度θ

按照以下公式计算最终抓取姿态质量评估分数score：

其中，score

计算最终抓取姿态质量评估分数即后续训练中的真实分数。

进一步地，所述抓取姿态预测模型的训练还包括：

利用抓取姿态预测数据集训练抓取姿态预测网络，将输出的抓取姿态位置预测偏移量与真实偏移量之间的误差作为偏移量损失函数，将预测抓取姿态x方向的单位向量与x方向的真实向量之间的误差作为x方向的损失函数，将预测抓取姿态y方向的单位向量与y方向的真实向量之间的误差作为y方向的损失函数，将预测抓取姿态宽度与真实抓取姿态宽度之间的误差作为均方差损失函数，将预测的抓取姿态质量评估分数与真实分数之间的误差作为质量评估损失函数，以偏移量损失函数、x方向的损失函数、y方向的损失函数、均方差损失函数和质量评估损失函数最小为目标，训练抓取姿态预测网络至收敛。

其中，抓取姿态位置预测偏移量、预测抓取姿态x方向的单位向量、预测抓取姿态y方向的单位向量、预测抓取姿态宽度、预测的抓取姿态质量评估分数均为抓取姿态预测模型预测得到的，真实偏移量、x方向的真实向量、y方向的真实向量、真实抓取姿态宽度、真实分数则通过相关计算得到。

进一步地，所述质量评估损失函数为：

其中，i，j，k分别表示预测的抓取姿态质量评估分数大于设定的大分数阈值，预测的抓取姿态质量评估分数在设定的小分数阈值至设定的大分数阈值之间，预测的抓取姿态质量评估分数小于设定的小分数阈值的点集合n

进一步地，所述位于机器人末端夹持器夹持范围内的点云坐标满足以下条件：

其中，outer_diameter为夹持器外宽度，hand_depth为夹持器闭合区域的宽度，hand_height为夹持器闭合区域的高度。(x，y，z)是抓取坐标系下的点云坐标。

按照本发明的另一方面，提供了一种基于复用结构的双阶段机械臂抓取规划系统，包括：

抓取姿态预测模型训练模块，用于改变抓取场景采集多视角数据，对多视角数据进行三维重建获取场景完整点云，利用场景完整点云生成抓取姿态，组成抓取姿态预测数据集，利用抓取姿态预测数据集训练抓取姿态预测网络至收敛，得到抓取姿态预测模型；

抓取姿态评估模型训练模块，用于将抓取姿态预测数据集中不与机器人末端夹持器发生碰撞且位于机器人末端夹持器夹持范围内的点云作为抓取姿态评估数据集，利用抓取姿态评估数据集训练抓取姿态评估网络至收敛，得到抓取姿态评估模型；

抓取规划模块，用于将待抓取场景单视角点云输入抓取姿态预测模型，得到点云中每个点对应的抓取姿态，组成抓取姿态集合，将抓取姿态集合中的抓取姿态输入抓取姿态评估模型，对得到的抓取姿态集合中各抓取姿态按照质量评分进行排序并选取排名靠前的K个抓取姿态，用于指导机械臂进行抓取作业；

所述抓取姿态预测网络和抓取姿态评估网络均采用复用结构，所述复用结构表示除第一层网络外，后续每层网络的输入都与第一层网络的输入数据或上一层网络的输出连接。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：

(1)本发明自监督的采集真实数据，并构建真实抓取姿态预测与评估数据集，可摆脱机械臂抓取规划方法对现有物体数据集的依赖，使得抓取姿态预测与评估模型提供的抓取策略更加合理。本发明构建基于复用结构的抓取姿态预测与评估网络，通过将当前层网络的输入与浅层网络的输出相连接，提高对浅层网络数据的利用率。通过抓取姿态预测模型预测高质量抓取姿态，然后通过抓取姿态评估模型进行二次质量评估以提高分数预测精度。本发明可以指导机械臂进行非结构化环境中未知物体的鲁棒抓取，提高机械臂的抓取成功率。

(2)本发明抓取姿态预测网络中除第一层网络外，后续每层网络的输入都与第一层网络的输入数据连接，这样设置的网络相比PointNet++基础网络性能在各方面都有所提升，approach角度与宽度误差分别降低了6.5％与6％，且引入该复用结构网络相比PointNet++基础网络前向传播时间增量非常小。

(3)本发明抓取姿态评估网络中除第一层网络外，后续每层网络的输入都与上一层网络的输出连接，，引入该复用结构的网络性能提升更大，召回率及精度分别提升了2.4％和1.8％，可以在保证实时性的前提下提升网络性能。

(4)本发明通过多种方式改变抓取场景，由此获取多种符合实际的数据，可以更好的应用到真实抓取环境，提高抓取成功率。抓取姿态生成方法的核心思想是点云中某点的抓取姿态x方向与点云表面法向量平行，符合人类直觉，可以生成高质量抓取姿态。

(5)本发明将复杂的力闭合条件简化为左右接触点与法向量的角度和左右接触点连线与y轴的角度，可以高效的对抓取姿态质量进行评估。对预测的抓取姿态进行碰撞检测，滤除低质量抓取姿态，再截取抓取姿态闭合区域点云。

(6)本发明通过多种损失函数约束，由此提高训练精度，使得预测结果更加准确。场景点云中高质量与低质量抓取姿态样本不平衡，高质量占比较小，损失函数按比例提高高质量抓取姿态损失的比例，进而提高对高质量抓取姿态的预测精度。

附图说明

图1是本发明实施例提供的一种基于复用结构的双阶段机械臂抓取规划方法流程图；

图2是本发明实施例提供的夹持器闭合区域点云获取方法；

图3中(a)是本发明实施例提供的第一种复用结构示意图；

图3中(b)是本发明实施例提供的第二种复用结构示意图；

图4是本发明实施例提供的一种抓取姿态预测网络结构图；

图5是本发明实施例提供的一种抓取姿态评估网络结构图；

图6是本发明实施例提供的夹持器二指模型示意图；

图7中(a1)是本发明实施例提供的单物体场景彩色图；

图7中(b1)是本发明实施例提供的单物体场景点云图；

图7中(c1)是本发明实施例提供的单物体场景抓取姿态结果图；

图7中(a2)是本发明实施例提供的多物体场景彩色图；

图7中(b2)是本发明实施例提供的多物体场景点云图；

图7中(c2)是本发明实施例提供的多物体场景抓取姿态结果图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

如图1所示，一种基于复用结构的双阶段机械臂抓取规划方法，包括：

将待抓取场景单视角点云输入抓取姿态预测模型，得到点云中每个点对应的抓取姿态，组成抓取姿态集合，将抓取姿态集合中的抓取姿态输入抓取姿态评估模型，对得到的抓取姿态集合中各抓取姿态按照质量评分进行排序并选取排名靠前的K个抓取姿态，用于指导机械臂进行抓取作业；

所述抓取姿态预测模型通过如下方式训练得到：

通过改变抓取场景采集多视角数据，对多视角数据进行三维重建获取场景完整点云，利用场景完整点云生成抓取姿态，组成抓取姿态预测数据集，利用抓取姿态预测数据集训练抓取姿态预测网络至收敛，得到抓取姿态预测模型；

所述抓取姿态评估模型通过如下方式训练得到：

将抓取姿态预测数据集中不与机器人末端夹持器发生碰撞且位于机器人末端夹持器夹持范围内的点云作为抓取姿态评估数据集，利用抓取姿态评估数据集训练抓取姿态评估网络至收敛，得到抓取姿态评估模型；

所述抓取姿态预测网络和抓取姿态评估网络均采用复用结构，所述复用结构表示除第一层网络外，后续每层网络的输入都与第一层网络的输入数据或上一层网络的输出连接。

具体地，推动策略包括：

针对某一抓取姿态，执行抓取任务，减少场景中物体数量，从而改变抓取场景；针对某一抓取姿态，从外围向中心方向水平推动一定距离，改变物体位置，从而改变抓取场景；针对某一抓取姿态，执行抓取动作后放置到场景中心上方，使其自由下落，从而改变抓取场景；

所述推动策略最终执行时通过随机选择，若某一抓取姿态执行成功则继续进行多视角数据采集，若不能规划成功，则再从剩余策略中随机选择，若最终没有抓取姿态可以执行时，则结束本轮数据采集；

所述多视角数据包括相机位姿与深度图像。

所述三维重建方法步骤如下：

(1)建立长方体包围盒：在需要三维重建的区域，建立长宽高分别为L、W、H的包围盒。

(2)体素化：将长方体包围盒划分为边长为L

(3)获取一帧深度图像depth

(4)从包围盒取一个体素g，先将体素g转换到世界坐标系下一点p，然后计算p在相机坐标系下的位置

(5)若点x处的深度值为val(x)，点υ到相机坐标系原点的距离为diυ(υ)，可以根据以下公式计算体素g的tsdf值：

其中sdf(g)为符号距离函数值，u为截断距离。

(6)根据公式w(g)＝cos(θ)/diυ(υ)计算体素g的权重，其中θ为投影光线与p点表面法向量的夹角。重复(4)-(6)步骤直至遍历完所有体素。

(7)将当前帧tsdf、权值w与全局TSDF、权值W按以下公式进行融合：

(8)重复(3)-(7)步骤直至遍历完所有深度图像，根据最终的TSDF模型，使用光线投射方法，输出场景完整点云。

利用机械臂定位精度高的特性，获得的相机位姿精度更高，解决三维重建中相机位姿估计精度较差的问题，然后对多视角深度图与相机位姿使用TSDF方法进行三维重建。

所述根据场景完整点云生成抓取姿态的方法如下：

(1)输入点云预处理：针对完整点云C，具体流程如下：1)随机采样；2)去除点云中无效的点；3)去除不在设定空间内的点；4)体素化点云；5)计算点云表面法线；6)重定义法线。针对单视角点云C′，在完整点云的流程上，随机采样n个点，得到点集P。

(2)局部坐标系计算：针对点集P中的每个点，在完整点云C中通过半径查询方法获得某一半径内所有点的法线集合

其中eigenυalue为特征值计算函数，eigenυector为特征向量计算函数，max与min分别为最大值与最小值函数，index为求索引函数，abs函数的功能是保证所求向量与法线方向相同，M是方阵，va是特征值，xe是特征向量。

(3)抓取姿态区域点云截取：针对点集P中的每个点，在完整点云C中通过半径查询方法获得某一半径内所有的点的集合

(4)评估夹持器二指放置位置：具体流程如下：1)将坐标系沿着x轴方向后退一定距离，得到抓取姿态初始位置；2)沿着y轴方向设置多组夹持器二指位置；3)依据局部区域点集中点云是否与二指模型碰撞和闭合区域是否含有点云两个条件评估夹持器二指位置是否成立，若多组二指位置都不成立则表示该点位置没有可行抓取。

(5)抓取姿态位置计算：主要目的是获取一个合适的抓取姿态三维坐标，其具体流程如下：1)y方向调整，从所有满足步骤(4)条件的二指位置中选择一个居中的位置；2)x方向调整，以步长step向x轴方向推进，直到局部区域点集中点云与二指模型碰撞为止。

(6)抓取姿态宽度计算：利用夹持器闭合区域点云获取方法获得闭合区域点云，然后统计所有点云在y轴方向的最大值与最小值，而它们差的绝对值即为宽度。

(7)抓取姿态质量评估：针对每个抓取姿态，首先利用夹持器闭合区域点云获取方法获得闭合区域点云，然后利用基于力闭合的抓取姿态质量评估方法对抓取姿态进行质量评估。

基于力闭合的抓取姿态质量评估方法如下：

(1)统计左右接触点：首先找到局部点云集合中点云在y轴上的最大值max与最小值min，分别统计满足y＞max-thr与y＜min-thr条件的点，然后计算两组接触点位置均值作为代理接触点。thr表示距离阈值。

(2)计算接触点角度：依据两个代理接触点组成的向量v，针对每组接触点，统计满足v与其中每个点的法线之间的角度小于某固定值θ的数量，若数量大于设定值min_num，则认为此接触点在摩擦锥角度为θ时满足力闭合条件，最后求满足上述条件的最小θ

(3)计算分数：按照以下公式计算最终抓取姿态质量评估分数：

其中θ

如图2所示，所述截取抓取姿态预测数据集区域点云方法如下：

(1)坐标系变换：由于点云和抓取姿态都处于世界坐标系下，而抓取姿态多种多样，不能很方便的截取闭合区域点云，需要将点云先转换到抓取姿态坐标系下；

(2)碰撞检测：根据夹持器二指模型进行碰撞检测；

(3)点云截取：按照以下公式条件截取没有发生碰撞的点云，得到原始局部点云，组成原始局部点云集合；

(4)重采样：对原始局部点云集合随机采样至固定数目的点云，组成抓取姿态评估数据集。

如图3中(a)所示，所述第一种复用结构处理方式为：除了第一层网络外，后续每层网络的输入都与输入数据进行连接，保证每层网络都会对输入数据进行重复利用。

如图3中(b)所示，所述第二种复用结构处理方式为：除了第一层网络外，后续每层网络的输入都与上一层的输出进行连接，保证每层网络都会对之前的所有数据进行重复利用。

如图4所示，抓取姿态预测网络输出包含Offset Block、Approach Block、CloseBlock、Width Block及Score Block，含义及损失函数如下：

Offset Block目的是预测抓取姿态空间偏移量，具体公式如下：

其中

Approach Block目的是预测抓取姿态x方向的单位向量，使该单位向量尽量与真实向量接近，因此损失函数可以设计为计算预测向量与真实向量之间的角度差，且越小越好。具体公式如下：

其中

Close Block目的是预测抓取姿态y方向的单位向量，具体公式如下：

其中

Width Block的损失函数为均方差损失，具体公式如下：

其中

Score Block预测点云中每个点的抓取姿态质量评估分数，具体公式如下：

其中i，j，k分别表示预测分数s＞0.7、0.01＜s＜0.7及s＜0.01的点集合索引，n表示点云总数。

如图5所示，所述抓取姿态评估网络输出Score Block，含义为预测点云中每个点的抓取姿态质量评估分数，损失函数如下：

其中，s

表1引入复用结构的抓取姿态预测网络训练结果

图4和图5中的点云网络模型为复用结构。复用结构1为第一种复用结构，复用结构2为第二种复用结构，为了有效评估本发明所提复用结构的有效性，对基于PointNet++的抓取姿态预测网络分别引入复用结构1和复用结构2，实验结果如表1所示，从数据中可以分析出：1)本发明引入复用结构后的网络相比PointNet++基础网络性能在各方面都有所提升，其中引入复用结构1的整体性能最好，approach角度与宽度误差分别降低了6.5％与6％，其他方面提升较为一般；2)引入额外模型参数的复用结构网络相比PointNet++基础网络前向传播时间会有所增加，而引入参数多的复用结构2会比引入参数少的复用结构1实时性稍差，但差距仅在10ms左右。

综合来看，本发明选择引入复用结构1的PointNet++网络作为抓取姿态预测网络。

表2引入复用结构的抓取姿态评估网络训练结果

为了有效评估本发明所提复用结构的有效性，对基于PointNet的抓取姿态评估网络分别引入复用结构1和复用结构2，实验结果如表2所示，从数据中可以分析出：1)网络性能方面，引入复用结构的网络性能均有所提升，其中引入复用结构2的网络提升最大，召回率及精度分别提升了2.4％和1.8％；2)实时性方面，与抓取姿态预测网络类似，由高到低依次为PointNet、复用结构1及复用结构2，但最大差距也仅为0.2ms。

综合来看，选择引入复用结构2的PointNet网络作为抓取姿态评估网络最好。

如图6所示，非阴影区域为夹持器二指模型本体，阴影区域为夹持器两个平行手指的闭合区域。夹持器二指模型可以用(hand_depth，hand_height，hand_width，finger_width)四个参数进行描述，其中hand_depth为夹持器闭合区域的宽度，hand_height为夹持器闭合区域的高度，hand_width为夹持器闭合区域的长度，finger_width为夹持器二指的厚度。

outer_diameter为夹持器外宽度，等于hand_width+2*finger_width

后退一定距离表示hand_depth-init_bite，其中init_bite为夹持器初始深度。

推动一定距离表示抓取姿态到场景中心的任意距离。

在本发明实施例中，finger_width＝0.01，hand_outer_diameter＝0.11，hand_depth＝0.06，hand_height＝0.02，init_bite＝0.01。

实施例1

一种基于复用结构的双阶段机械臂抓取规划方法，包括：

控制机械臂采用推动策略采集多视角数据，进行三维重建获取场景完整点云；

根据场景完整点云生成抓取姿态，获取抓取姿态预测数据集，截取抓取姿态预测数据集区域点云，获取抓取姿态评估数据集；

根据抓取姿态预测与评估数据集，训练基于复用结构的抓取姿态预测和评估网络，得到抓取姿态预测和评估模型；

将待抓取场景单视角点云作为输入抓取姿态预测，将预测结果输入抓取姿态评估模型，获取一组高质量的抓取姿态，用于指导机械臂进行抓取作业。

所述三维重建方法步骤如下：

(1)建立长方体包围盒：在需要三维重建的区域，建立长宽高分别为40cm、40cm、20cm的包围盒。

(2)体素化：将长方体包围盒划分为边长为2mm的小方格，即体素单元，一共可以划分个4000000体素。

(3)获取一帧深度图像depth

(4)从包围盒取一个体素g，先将体素g转换到世界坐标系下一点p，然后计算p在相机坐标系下的位置

(5)若点x处的深度值为，点υ到相机坐标系原点的距离为diυ(υ)，可以根据以下公式计算体素g的tsdf值：

其中sdf(g)为符号距离函数值，u为0.01m。

(6)根据公式w(g)＝cos(θ)/diυ(υ)计算体素g的权重，其中θ为投影光线与p点表面法向量的夹角。重复(4)-(6)步骤直至遍历完所有体素。

(7)将当前帧tsdf、权值w与全局TSDF、权值W按以下公式进行融合：

(8)重复(3)-(7)步骤直至遍历完所有深度图像，根据最终的TSDF模型，使用光线投射方法，输出场景完整点云。

所述根据场景完整点云生成抓取姿态的方法如下：

(1)输入点云预处理：针对完整点云C，具体流程如下：1)随机采样；2)去除点云中无效的点；3)去除不在设定空间内的点；4)体素化点云；5)计算点云表面法线；6)重定义法线。针对单视角点云C′，在完整点云的流程上，随机采样2048个点，得到点集P。

(2)局部坐标系计算：针对点集P中的每个点，在完整点云C中通过半径查询方法获得某一半径内所有点的法线集合

其中eigenυalue为特征值计算函数，eigenυector为特征向量计算函数，max与min分别为最大值与最小值函数，index为求索引函数，abs函数的功能是保证所求向量与法线方向相同。

(3)抓取姿态区域点云截取：针对点集P中的每个点，在完整点云C中通过半径查询方法获得某一半径内所有的点的集合

(4)评估夹持器二指放置位置：具体流程如下：1)将坐标系沿着x轴方向后退0.05m，得到抓取姿态初始位置；2)沿着y轴方向设置5组夹持器二指位置；3)依据点云是否与二指模型碰撞和闭合区域是否含有点云两个条件评估夹持器二指位置是否成立，若5组二指位置都不成立则表示该点位置没有可行抓取。

(5)抓取姿态位置计算：主要目的是获取一个合适的抓取姿态三维坐标，其具体流程如下：1)y方向调整，从所有满足步骤(4)条件的二指位置中选择一个居中的位置；2)x方向调整，以步长0.005m向x轴方向推进，直到点云与二指模型碰撞为止。

(6)抓取姿态宽度计算：利用夹持器闭合区域点云获取方法获得闭合区域点云，然后统计所有点云在y轴方向的最大值与最小值，而它们差的绝对值即为宽度。

(7)抓取姿态质量评估：针对每个抓取姿态，首先利用夹持器闭合区域点云获取方法获得闭合区域点云，然后利用基于力闭合的抓取姿态质量评估方法对抓取姿态进行质量评估。

进一步地，所述基于力闭合的抓取姿态质量评估方法如下：

(1)统计左右接触点：首先找到y轴上的最大值与最小值，分别统计满足y＞max-0.003与y＜min-0.003条件的点，然后计算两组接触点位置均值作为代理接触点。

(2)计算接触点角度：依据两个代理接触点组成的向量v，针对每组接触点，统计满足v与其中每个点的法线之间的角度小于某固定值θ的数量，若数量大于2，则认为此接触点在摩擦锥角度为θ时满足力闭合条件，最后求满足上述条件的最小θ

(3)计算分数：按照以下公式计算最终抓取姿态质量评估分数：

其中θ

所述截取抓取姿态预测数据集区域点云方法如下：

(1)坐标系变换：由于点云和抓取姿态都处于世界坐标系下，而抓取姿态多种多样，不能很方便的截取闭合区域点云，需要将点云先转换到抓取姿态坐标系下；

(2)碰撞检测：根据夹持器简化模型进行碰撞检测；

(3)点云截取：按照以下公式条件截取点云，得到原始局部点云；

(4)重采样：随机采样256大小点云。

基于深度学习的双阶段抓取规划方法步骤如下：

(1)预处理：包括点云坐标变换、体素化降采样点云、工作空间过滤点云及按比例采样点云；

(2)抓取姿态预测：将预处理后的单视角点云输入到改进的抓取姿态预测模型中，得到点云中每个点对应的抓取姿态；

(3)后处理：包括工作空间过滤抓取姿态、方向过滤抓取姿态、抓取姿态聚类及抓取姿态排序；

(4)抓取姿态计算：对后处理后的高质量抓取姿态重新计算抓取姿态以代替预测的抓取姿态，其计算方法是权利要求3所述方法的简化版，相比原版减少了抓取姿态质量分数标注过程；

(5)抓取姿态评估：针对每个抓取姿态，首先截取抓取姿态局部点云输入到抓取姿态评估模型，得到重新评估后的高质量抓取姿态集合；

(6)质量排序与选取，按照质量分数进行排序并选取排名靠前的5个抓取姿态。

如图7中(a1)、图7中(b1)、图7中(c1)、图7中(a2)、图7中(b2)、图7中(c2)所示，本发明提出的抓取规划方法在单物品与多物品场景中均可有效工作，鲁棒性较高。此外，还可以看出本发明方法能够根据场景中的物体分布，有效滤除物体拥挤的区域，优先寻找较为独立的、利于抓取的物体，从而为机械臂末端执行器留出抓取作业空间，符合人类的抓取习惯。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。