一种基于深度学习点云匹配的6DOF物体姿态估计方法

摘要

本发明公开了一种基于深度学习点云匹配的6DOF物体姿态估计方法，涉及机器视觉领域，所述方法包括以下步骤：步骤1、根据基于深度学习的物体关键点检测网络模型对待识别图像进行关键点检测，获得所述待识别图像中物体的关键点的像素坐标，并将所述像素坐标转换为图像坐标，用于后续的PNP算法；步骤2、根据所述关键点的3D坐标和图像坐标，求解所述PNP算法得到所述待识别图像中所述物体的一个初始姿态，并用于后续ICP算法配准的初值；步骤3、根据所述待识别图像中所述物体的所述关键点进行物体分割，获得包含所述物体的姿态信息的局部点云，并采用所述ICP算法将所述局部点云与模板点云进行配准，获取所述待识别图像中所述物体的位姿。

说明书

技术领域

本发明涉及机器视觉领域，尤其涉及一种基于深度学习点云匹配的6DOF物体姿态估计方法。

背景技术

随着机器视觉与人工智能的应用越来越广泛，智能视觉系统成为机器人中最为关键的系统之一。基于视觉的机器人识别与抓取研究越来越得到国内外的广泛关注，并且在上料自动化、焊接机器人、物流机器人等实际生产中智能机器人不仅要能感知环境，还要能与环境互动。

物体抓取是机器人最基本，也是最重要的能力。单一的3维(3D)视觉最主要的缺点是计算量大，场景点云含有的点云数目量巨大，容易产生误匹配；单一的2维(2D)视觉缺乏深度测量或Z方向的信息而不能计算出目标物体的6维(6D)位姿；目前姿态估计的主流方法可以分为：基于对应关系的姿态估计方法、基于模板匹配的方法、基于投票的方法以及基于回归的方法。其中，基于对应关系的方法缺点是需要3D物体精确的3D模型；基于模板匹配的方法精度较为受限；基于投票的方法精度较高，然而运算速度较慢；基于回归的方法精度高，扩展型好，也是目前研究的热点。

ICP算法(Interactive Closest Point)可以获得较高的配准精度，但是对点云的初始位置要求较高，配准效果和点集的初始位置关系很大。物体的6D位姿估计，是指估计相机坐标系下物体的6D位姿，即3D位置和3D姿态，此时原始物体本身的坐标系可以看作是世界坐标系，也即得到原始物体所在世界坐标系到相机坐标系的旋转平移的RT变换。物体的6D位姿估计的意义在于能够获得物体的精确姿态，支撑对于物体的精细操作，主要应用于机器人抓取领域和增强现实领域。

因此，本领域的技术人员致力于开发一种基于深度学习点云匹配的6DOF物体姿态估计方法，解决现有方法中存在的计算量大以及ICP算法缺少较好的初值的缺陷。其中，6DOF为6Degree of Freedom，是六自由度的缩写。

发明内容

有鉴于现有技术的上述缺陷，本发明所要解决的技术问题是如何改进现有的物体姿态估计方法，使得物体姿态估计方法不但可以大大降低计算量提高匹配速度，而且可以大大提高匹配的准确度。

为实现上述目的，本发明提供了一种基于深度学习点云匹配的6DOF物体姿态估计方法，将模板点云与局部点云进行匹配，大大降低了计算量提高了匹配速度。该方法采用基于回归的方法预测3D关键点在2D图像上的投影位置，并用于后续的PNP(Perspective-n-Point)算法，具体是利用深度学习进行关键点检测得到物体的初始姿态，并将物体的初始姿态作为ICP算法的初值，利用该方法得到的初值进而得到的初始位姿接近物体的真实位姿，可以避免算法陷入局部最优，使得迭代可以收敛到正确的配准结果。

本发明提供的一种基于深度学习点云匹配的6DOF物体姿态估计方法，包括以下步骤：

步骤1、根据基于深度学习的物体关键点检测网络模型对待识别图像进行关键点检测，获得所述待识别图像中物体的关键点的像素坐标，并将所述像素坐标转换为图像坐标，用于后续的PNP算法；

步骤2、根据所述关键点的3D坐标和图像坐标，求解所述PNP算法得到所述待识别图像中所述物体的一个初始姿态，并用于后续ICP算法配准的初值；

步骤3、根据所述待识别图像中所述物体的所述关键点进行物体分割，获得包含所述物体的姿态信息的局部点云，并采用所述ICP算法将所述局部点云与模板点云进行配准，获取所述待识别图像中所述物体的位姿。

进一步地，在所述步骤1中根据基于深度学习的所述物体关键点检测网络模型对所述待识别图像进行关键点检测之前，还包括以下步骤：

步骤1.1、获取多幅待识别目标物体的参考图像；

步骤1.2、对所述参考图像进行关键点的标记，得到对应的参考标签；

步骤1.3、根据各所述参考图像及对应的所述参考标签进行建模，得到基于深度学习的所述物体关键点检测网络模型。

进一步地，在所述步骤1.3中根据各所述参考图像及对应的所述参考标签进行建模之前，还包括以下步骤：

步骤1.3.1、将所述参考图像调整为480x640大小；

步骤1.3.2、随机更改所述参考图像的亮度和饱和度；

步骤1.3.3、随机旋转所述参考图像；

步骤1.3.4、将所述参考图像和所述参考标签转换为PyTorch张量，并在[-1,1]之间进行归一化处理。

进一步地，在所述步骤1.3中使用ResNet18作为基本框架建立基于深度学习的所述物体关键点检测网络模型，并修改所述ResNet18的第一层和最后一层。

进一步地，在所述ResNet18的所述第一层中，将神经网络接受灰度图像的输入通道数设置为1。

进一步地，所述步骤1.2中的所述参考标签的数量为16个。

进一步地，在所述ResNet18的所述最后一层中，将所述物体关键点检测网络模型的输出通道数设定为32，以预测所述物体的16个所述参考标签的X和Y坐标。

进一步地，所述物体关键点检测网络模型的损失函数为所述参考标签的预测值和真实值之间的均方误差，每当所述损失函数达到新的最小值时，保存网络权重。

进一步地，利用LableImg标注工具获取所述物体的外接包围框，得到所述外接包围框的长和宽以及左上角的像素坐标值，并和所述参考标签的坐标值一并存入XML文件中。

进一步地，所述待识别图像是利用相机拍照得到。

本发明提供的一种基于深度学习点云匹配的6DOF物体姿态估计方法至少具有以下技术效果：

1、本发明提供的6DOF物体姿态估计方法将模板点云与局部点云进行匹配，大大降低了计算量提高了匹配速度；

2、本发明提供的6DOF物体姿态估计方法利用深度学习进行关键点检测得到物体的初始姿态，并将物体的初始姿态作为ICP算法的初值，利用该方法得到的初值进而得到的初始位姿接近物体的真实位姿，可以避免算法陷入局部最优，使得迭代可以收敛到正确的配准结果。

以下将结合附图对本发明的构思、具体结构及产生的技术效果作进一步说明，以充分地了解本发明的目的、特征和效果。

附图说明

图1是PNP算法示意图；

图2是本发明的一个较佳实施例的整体流程图；

图3是图2所示实施例的ICP算法流程图。

具体实施方式

以下参考说明书附图介绍本发明的多个优选实施例，使其技术内容更加清楚和便于理解。本发明可以通过许多不同形式的实施例来得以体现，本发明的保护范围并非仅限于文中提到的实施例。

本发明所要解决的技术问题是如何改进现有的物体姿态估计方法，使得物体姿态估计方法不但可以大大降低计算量提高匹配速度，而且可以大大提高匹配的准确度。

为实现上述目的，本发明提供了一种基于深度学习点云匹配的6DOF物体姿态估计方法，将模板点云与局部点云进行匹配，大大降低了计算量提高了匹配速度。该方法还利用深度学习进行关键点检测得到物体的初始姿态，并将物体的初始姿态作为ICP算法的初值，利用该方法得到的初值进而得到的初始位姿接近物体的真实位姿，可以避免算法陷入局部最优，使得迭代可以收敛到正确的配准结果。

本发明中要用到的算法是PNP算法和ICP算法。

PNP问题的描述以及定义相对简单，它的目的就是求解2D-3D点对运动的方法，解决的是根据图像中特征点的二维像素坐标和其对应的三维空间坐标来估计相机在参考坐标系中的位姿。直观来讲，当相机观察到空间中的某一物体时，已知观察物体上n个三维空间点坐标(相对于某个指定的坐标系O)及其二维投影位置的情况下，如何估计相机的位姿(即相机在坐标系O下的姿态)正是PNP要解决的问题，即利用已知三维结构与图像的对应关系求解相机与参考坐标系的相对关系。比如，在一幅图像中，知道其中至少四个图像中确定的点在3D空间下的相对坐标位置，我们就可以估计出相机相对于这些点的姿态，或者说估计出这些3D点在相机坐标系下的姿态。上述说的姿态或者位姿，包括位置以及方向，即一个6自由度的状态，即为需要求解的旋转矩阵与平移向量，两者统称为相机的外参。所以，可以总结出PNP算法所需的条件是：(1)物体在世界坐标系下的n个参考点坐标；(2)n个3D坐标投影在图像上的2D参考点坐标；(3)相机的内参K。根据PNP算法特性，可以将物体姿态估计问题转化为物体的关键点预测问题。

PNP问题是一类问题。针对不同的情况有不同的解法，常见的PNP算法有：P3P、DLS、EPnP、UPnP等。

如图1所示，考虑空间中某个点P，它在世界坐标系O

其中，

即为我们要求的旋转矩阵与平移向量，也就是相机的外参。通过相机的外参可以将世界坐标系下的点转换到相机坐标系下表示，通过相机的内参可以将相机坐标系下的点转换到像素坐标系下表示。

因为相机的内参K是已知且不变的，令：

则上式可以表示为：

用最后一行把s消去(s＝τ

于是有：

可以看到，每个特征点提供了两个关于T

根据PNP算法的特性，我们将物体姿态估计问题转化为物体关键点预测问题。

ICP问题(3D-3D)：假设有一组配对好的3D点，对两个RGB-D图像进行匹配：

P＝{p

现在，想要找一个欧式变换R，t，使得：

这个问题可以用迭代最近点对ICP问题进行求解。3D-3D位姿估计问题中，并没有出现相机模型，也就是说，仅考虑两组3D点之间的变换时，和相机没有关系。和PNP类似，ICP的求解也分为两种方式：利用线性代数的求解SVD方法以及利用非线性优化方式的求解BA(Bundle Adjustment)。

如图2所示，为本发明提供的一种基于深度学习点云匹配的6DOF物体姿态估计方法，从图中可以看出，物体姿态估计方法分为物体关键点检测、物体分割和点云匹配3个步骤。

具体包括以下步骤：

步骤1、根据基于深度学习的物体关键点检测网络模型对待识别图像进行关键点检测，获得待识别图像中物体的关键点的像素坐标，并将像素坐标转换为图像坐标，用于后续的PNP算法；

步骤2、根据关键点的3D坐标和图像坐标，求解PNP算法得到待识别图像中物体的一个初始姿态，并用于后续ICP算法配准的初值；

步骤3、根据待识别图像中物体的关键点进行物体分割，获得包含物体的姿态信息的局部点云，并采用ICP算法将局部点云与模板点云进行配准，获取待识别图像中物体的位姿。

ICP算法的第一步是要确定一个迭代初值，即初始姿态，选取的初值将对最后配准结果产生重要的影响，配准效果和点集的初始姿态关系很大。如果初值选择不合适，算法可能就会限入局部最优，使得迭代不能收敛到正确的配准结果。本发明实施例提出的一种基于深度学习点云匹配的6DOF物体姿态估计方法是先基于深度学习神经网络获得物体的一个较好的初始姿态，再将初始姿态作为ICP算法的初值用来提升配准效果。

其中，在步骤1中根据基于深度学习的物体关键点检测网络模型对待识别图像进行关键点检测之前，还包括以下步骤：

步骤1.1、获取多幅待识别目标物体的参考图像；

步骤1.2、对参考图像进行关键点的标记，得到对应的参考标签；

步骤1.3、根据各参考图像及对应的参考标签进行建模，得到基于深度学习的物体关键点检测网络模型。

其中，为了增加模型的泛化性，对图像进行数据增强。在步骤1.3中根据各参考图像及对应的参考标签进行建模之前，还包括以下步骤：

步骤1.3.1、将参考图像调整为480x640大小；

步骤1.3.2、随机更改参考图像的亮度和饱和度；

步骤1.3.3、随机旋转参考图像；

步骤1.3.4、将参考图像和参考标签转换为PyTorch张量，并在[-1,1]之间进行归一化处理。

其中，在步骤1.3中使用ResNet18作为基本框架建立基于深度学习的物体关键点检测网络模型，并修改ResNet18的第一层和最后一层。

其中，在ResNet18的第一层中，将神经网络接受灰度图像的输入通道数设置为1。

其中，步骤1.2中的参考标签的数量为16个。

其中，在ResNet18的最后一层中，将物体关键点检测网络模型的输出通道数设定为32(16x2＝32)，以预测物体的16个参考标签的X和Y坐标。

其中，物体关键点检测网络模型的损失函数为参考标签的预测值和真实值之间的均方误差，学习率应保持较低以避免梯度爆炸。每当损失函数达到新的最小值时，保存网络权重。训练至少20个epochs以获得最佳性能。

其中，利用LableImg标注工具获取物体的外接包围框，得到外接包围框的长和宽以及左上角的像素坐标值，并和参考标签的坐标值一并存入XML文件中，从而得到基于深度学习的物体关键点检测网络模型。

其中，待识别图像是利用相机拍照得到。

其中，在步骤3中，根据检测到的物体的关键点得到场景中物体的包络框将物体分割出来，根据分割出来的区域得到待识别物体的局部点云，该局部点云包含物体的姿态信息，然后将场景区域的局部点云与获取的模板点云进行配准，由于点云数量小，可以直接采用ICP算法对其进行配准，求出了其旋转和平移矩阵，初值就使用步骤2中PNP算法计算得到的初始姿态，最终获取物体精确的位姿。ICP算法通过搜寻模板点云中某点与场景点云中的最近点来确定需要配准的点对关系，根据点对之间欧氏距离的均方根误差值对配准进行评估。

ICP配准算法优点众多，如配准结果准确，不必对点集进行特征提取与分割、初值好的情况下可以获得很好的算法收敛性。

图3为ICP算法流程图，具体为：

获取模板点云和场景点云；

设置迭代次数N和误差阈值e0；

利用KD-Tree搜索最近点，确定对应点对集，并记录实际迭代次数n；

计算点对之间的旋转矩阵R和平移矩阵T；

执行点云变换，计算变化后的点云与ROI点云的误差e；

判断误差e与误差阈值e0的大小；

如果误差e小于误差阈值e0成立，则输出旋转矩阵R和平移矩阵T，结束6DOF物体姿态估计；

如果误差e小于误差阈值e0不成立，则判断实际迭代次数n与迭代次数N的大小；

如果实际迭代次数n小于等于迭代次数N成立，则回到步骤“利用KD-Tree搜索最近点，确定对应点对集，并记录实际迭代次数n”，继续进行点云匹配；

如果实际迭代次数n小于等于迭代次数N不成立，则ICP配准失败，结束完成6DOF物体姿态估计。

以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。