一种抓取机器人的物体识别与位姿估计方法

本发明属于目标检测位姿估计领域，特别涉及一种抓取机器人的物体识别与位姿估计方法。

背景技术：

1、近年来抓取机器人技术得到了快速的发展，在实际应用中，结构化场景中的实际抓取物体往往有多种位姿，如果多目标物体零散摆放，其中存在着遮挡、堆叠等情况会导致抓取机器人无法进行抓取任务。因此，研究人员开始将视觉技术与抓取机器人技术相结合。抓取机器人可以通过视觉传感器获得环境信息，再使用相应的计算机视觉算法，抓取机器人便可以对目标物体进行定位，从而实现抓取机器人进行自主分拣、搬运等操作。位姿估计数据的准确度会影响抓取机器人的抓物操作，机器视觉可以实现不与待抓取物体发生接触的情况下，完成对待抓取物体的图像采集与处理并判断出目标物体类别。在视觉检测与定位中，二维视觉检测技术已经发展的比较成熟了，二维视觉检测技术是通过相机拍摄物体图像信息，根据图像特征筛选出匹配程度最高的物体位置。

2、然而在实际机器人抓取系统中，二维视觉检测不能获得目标物体的深度信息，无法给抓取机器人抓取提供准确的位姿信息，在面对多物体零散摆放、遮挡摆放、堆叠摆放等情况时，由于对场景中存在多目标物体之间的空间关系理解不足，会导致抓取机器人不能发挥稳定的分拣效果，甚至抓取过程中损坏了目标物体。

技术实现思路

1、本发明的目的，在于提供一种抓取机器人的物体识别与位姿估计方法，解决二维视觉检测不能获得目标物体的深度信息，无法给抓取机器人抓取提供准确的位姿信息的技术问题。

2、为了达成上述目的，本发明的解决方案是：

3、一种抓取机器人的物体识别与位姿估计方法，包括如下步骤：

4、步骤1，采集待识别区域的2d彩色图和3d深度图，根据2d彩色图和3d深度图得到点云图；所述点云图由场景点云组成，所述场景点云包括背景点云和刚性物体点云，所述背景点云包括工作台平面和除刚性物体点云外的所有点云，所述刚性物体点云位于工作台平面上；

5、步骤2，构建yolov7-tiny-cbam深度学习神经网络模型并进行训练，通过训练完毕的yolov7-tiny-cbam深度学习神经网络模型对2d彩色图进行目标检测识别，得到目标物体定位框；

6、步骤3，对点云图进行处理并结合步骤2得到的目标物体定位框，得到待识别区域中各个目标物体；

7、步骤4，通过改进后的主成分分析pca算法对各个目标物体进行计算，得到各个目标物体的位姿信息；

8、步骤5，构建抓取优先级函数，根据各个目标物体的位姿信息进行抓取优先级计算，得到优先抓取结果。

9、上述步骤1中，2d彩色图提供点云图的(x',y')坐标，3d深度图提供点云图的z'坐标，各个物体的(x',y',z')坐标构成点云图。

10、上述步骤2中，yolov7-tiny-cbam深度学习神经网络模型是将cbam卷积注意力模块加入yolov7-tiny的主干网络中构建得到，具体是在yolov7-tiny的主干网络部分输出的三个有效特征层上增加了cbam卷积注意力模块，cbam卷积注意力模块将注意力实现到独立的通道维度和空间维度上，通过cam通道注意力获得加权结果之后通过sam空间注意力进行二次加权，再对注意力图乘积输入的特征图，从而实现了自适应特征细化处理。

11、上述步骤2中，通过图像数据增强的方法对yolov7-tiny-cbam深度学习神经网络模型进行训练，使用imgaug、albumentations等数据增强库进行数据增广操作，采用高斯噪声、旋转变换、高斯滤波、透视变换的数据增强方式扩充数据集，使用labelimg、rolabelimg等图像标注工具对获得的数据集进行手动标注；

12、目标检测对象包括四个类：球体网球、长方体快递盒、圆柱体瓶子和不规则的鼠标，需要进一步进行说明的是，四个类共2400张样本图片。

13、在目标检测完成后，需要对目标位姿进行估计以供机械臂抓取，目标物体的位姿估计是利用深度相机所拍摄出的点云图进行计算的。由于深度相机会受到自身测量精度及各种环境因素的影响，原始点云数据具有信息量巨大、包含很多噪声信息、复杂性高的特点，存在的冗余数据会降低点云处理的精度与算法的运行效率，导致后续的点云分割与位姿估计受到影响，因此需要先对原始点云进行滤波处理，在处理冗余点云数据时还要保证目标点云数据的完整性，需要将多种点云滤波算法结合使用才能达到理想的效果。

14、上述步骤3中，通过直通滤波算法缩小场景点云的范围，得到缩小范围后的场景点云；通过体素栅格降采样滤波算法对缩小范围后的场景点云进行降采样处理并抑制噪声点；通过随机采样一致算法提取工作台平面；通过平面分割的方法将刚性物体点云与工作台平面进行分割；通过聚类算法将每一个物体单独分割出来，结合目标物体定位框得到待识别区域中各个目标物体。

15、上述直通滤波算法的具体内容是：直通滤波算法通过坐标范围进行数据裁剪，对点云数据在单一维度上进行滤波，直通滤波算法是基于距离的，可以快速滤除背景点云。在x、y、z三个坐标轴上定义阈值范围，对点云图生成的阈值范围进行点云去除，限定区域范围的公式如下所示：

16、xmin≤x≤xmax

17、ymin≤y≤ymax

18、zmin≤z≤zmax

19、在三个坐标轴上找到阈值外与阈值内的点云数据，可以将x、y、z方向上的不必要点云数据进行删减，更加突出感兴趣区域，大大提高了点云处理速度。通过距离阈值选择去除地面点云，本发明仅去除x与y方向上的不必要点云数据得到工作台和工作台上的待抓取物体点云，将大量冗余点云裁剪。

20、深度相机采集的点云数据往往信息巨大，大量的信息数据会导致对点云处理的时间较慢，所以需要在不破坏点云数据本身几何结构的前提下进行降采样处理。体素栅格降采样滤波算法具有效率高、易实现的优点，上述体素栅格降采样滤波算法的具体内容是：在x、y、z方向上建立包围盒坐标系，获取包围盒的最大长宽高(xmax,ymax,zmax)与最小长宽高(xmin,ymin,zmin)，包围盒的x、y、z边长的公式如下所示：

21、

22、其中，lx、ly、lz分别表示包围盒x、y、z方向上的边长；

23、创建体素栅格子空间，将该包围盒分割成若干个小立方体包围盒，每个立方体就是一个体素，使用自适应系数指数设置体素栅格子空间大小，对点云图进行划分，以ddist＝πdiam(m)为栅格子的空间边长，获取体素栅格子空间集合，以整数为基础，根据场景点云划分栅格子空间数量为m×n×l，所述场景点云为未进行直通滤波算法处理前的原始场景点云，计算公式如下所示：

24、

25、其中integer(x)代表整数的取值，m、n、l分别表示栅格子长、宽、高上的数量，l代表小立方体栅格边长，使用最邻近搜索算法对每个栅格子空间的点集求解，计算每个栅格子空间重心，公示如下所示：

26、

27、其中，c0表示每个栅格子空间重心，k表示栅格子内点的个数，pj表示第j个点的重心，用同一个重心点来表示每一个栅格中的原始数据，实现点云数据降采样。

28、场景点云经直通滤波算法和基于体素栅格降采样滤波算法处理后，需要将场景点云中的不同物体通过点云分割提取出来单独处理。在降采样后场景点云中还包含背景点云，背景点云的存在会严重影响目标物体识别定位和姿态提取的准确性，降低了算法的处理速度。考虑到场景点云数据中最无关的点云是放置物体的工作台，因此本发明通过平面分割的方法将刚性物体的点云与工作台的点云进行分割。主要思想是随机性和假设性。在大量的噪声点云数据中使用随机采样一致算法ransac算法可以有效的提取出目标工作台平面，并且不会受到噪声点云的影响。ransac算法假设数据由两部分组成，即“内点”和“外点”。规定一个模型，然后找到满足模型的“内点”，与不满足模型的“外点”进行分离，具体内容是：首先规定一个模型，要求所有的局内点都能适应这个模型，随机选中三个局内点p1、p2、p3，设定一个重复次数n，随机选取的三个局内点求解出一个参考平面模型s，表示为a0x+b0y+c0z+d0＝0；用求得的平面模型检验其他的点云数据，如果检验的点到平面s的距离在预先设定的阈值内则该点为局内点，将局内点扩充；对前两个步骤重复n次，选择局内点数量最多的参考模型为最终的模型，并将局内点数量过少的参考模型舍弃。

29、在获得场景点云后，场景点云中待抓取目标可以有一个也可以有多个，需要对目标物体进行提取，需要通过聚类算法将每一个物体单独分割出来，再结合yolov7-tiny-cbam算法获得的目标物体定位框确定分割点云物体类别。其中常用的经典聚类分割算法有基于区域生长分割算法和欧式聚类分割算法，本发明使用基于欧式聚类分割算法对目标物体进行提取实验。

30、在聚类分割得到各个目标物体实例后，需要对各个物体进行位姿估计，使用pca(主成分分析)算法对点云位姿估计，位置最终由点云pca获取的质心决定。由于传统的pca算法得到的箱体姿态坐标系与机械臂末端工具坐标系各轴指向相差较大，会导致机械臂求解逆运动学失败的情况，因此，本发明自定义坐标系，使箱体姿态坐标系与末端工具坐标系指向相近，便于计算机械臂当前位姿与目标位姿的残差求解逆运动学，本发明自定义的坐标系同样适用于瓶子、网球和鼠标点云位姿估计。

31、上述步骤4的具体内容是：定义抓取平面姿态坐标系，最大特征值对应的特征向量为主方向，x轴是此点云平面法向量并使其指向视点，夹爪需要沿着该轴抓取，y轴为箱体短边平行向量，z轴为右手坐标系已知x与y轴后确定的方向，判断目标物体姿态y轴方向是否与机械臂末端执行器初始姿态y轴方向为同一方向，若是同一方向则将当前姿态绕y轴旋转180°，将箱体姿态坐标系x轴与机械臂末端执行器初始姿态x轴方向旋转到同一方向，旋转矩阵为ry；若箱体姿态y轴与机械臂末端执行器初始姿态y轴方向不同，则将当前姿态绕z轴旋转180°，将箱体姿态坐标系x轴与机械臂末端执行器初始姿态x轴方向旋转到同一方向，旋转矩阵为rz；按照绕自身轴旋转，求得最终位姿。

32、在抓取机器人的许多工作场景中，例如随意摆放的多目标物体分拣收纳场景，需要对多目标物体按顺序分拣收纳，为了实现对场景的理解和判断，需要定义一个抓取策略来决定优先抓取的目标，通过合理的抓取顺序进行抓取分拣任务，使抓取机器人依据抓取策略实现稳定安全的抓取任务。针对不同抓取物体的分拣任务，为了实现分拣顺序推理功能，本发明根据目标物体的点云质心在三个坐标轴方向上的不同坐标值实现对场景中物体的分拣顺序判断。瓶子、网球和鼠标直接摆放在工作台上，也考虑堆叠在箱子上，当堆叠在箱子上时，仅对箱子上物体进行点云分割与位姿估计。对于箱体考虑杂乱堆叠摆放在工作台上，一个比较合理的分拣策略就是先将好分拣(处于上表层)的物体分拣出去。那么经过点云分割后所有物体中，不同物体点云的质心是不同的，即物体位置不同，当物体中心的z方向的值越大，说明该物体越堆叠在工作台的上层；抓取目标物体被放置在抓取机器人的前面，所以稍微优先考虑前面的物体，因此定义了一个函数g(b)，它将根据目标物体的不同位置，通过优先级关联起来；

33、上述步骤5中，定义抓取优先级函数，公式如下所示：

34、

35、其中，对目标物体表面的中心点(xb,yb,zb)各个分量进行加权，对位置较高的距离抓取机器人较近的|1-xb|，距离工作台边缘较近的的物体进行优先排序，对每一个目标物体计算得分，优先抓取得分最高的物体。

36、采用本发明的物体识别与位姿估计方法，有益效果如下：

37、(1)针对目标检测算法，采用轻量级的yolov7-tiny算法对抓取物体检测，增加注意力模块，对于遮挡物他也有较好的识别效果，获得抓取目标物体的检测定位框。

38、(2)针对位姿估计算法，通过ransac、聚类等算法对场景点云分割处理得到各个目标物体，结合目标检测算法确定目标物体类型。

39、(3)采用pca位姿估计算法得到每个抓取物体位姿，自定义坐标系，使姿态坐标系与末端工具坐标系指向相近，便于计算机械臂当前位姿与目标位姿的残差求解逆运动学。

40、(4)基于2d-3d结合的视觉检测方法，结合了2d和3d视觉的优点可以识别出不同的抓取物体及位姿，完成对目标物体的识别和位姿估计，使系统的定位精度大幅度提高。