CN114743259A - 位姿估计方法、位姿估计系统、终端、存储介质及应用 - Google Patents

Abstract

本发明属于物体空间位姿估计技术领域，公开一种位姿估计方法、位姿估计系统、终端、存储介质及应用，采用高精度的结构光全局式三维扫描仪来制作物体的点云模板，解决了直接由CAD模型生成模板导致与实际零件存在差异性大的问题。基于两步分割法的分割方式有效的提高目标物体分割提取的效率与准确度。结合法向量约束与邻域数约束的关键点提取算法有效提取模板与场景点云中具有大曲率特征且非噪声的关键点。在关键点处构建FPFH特征描述并利用随机采样一致性，能够有效得到场景中目标物体到模板的一个初始位姿估计。本发明引入对应点法线一致性约束来改进点到平面的ICP算法，避免陷入局部最优解，从而完成初始位姿的精确修正。

Description

位姿估计方法、位姿估计系统、终端、存储介质及应用

技术领域

本发明属于物体空间位姿估计技术领域，尤其涉及一种位姿估计方法、位姿估计系统、终端、存储介质及应用。

背景技术

目前，位姿估计是获取目标物体在当前深度相机所拍摄场景下的位置坐标与姿态信息，其是机器人抓取、搬运、自动驾驶等许多重要应用的基础。

针对目前物体位姿估计的方法，主要可分为基于图像和基于点云两类。基于RGB图像或者RGB-D图像的位姿估计方法在传统方法上依赖于人工设计特征，并且图像的区别特征、纹理等往往会受到光照、反射等外界条件的影响，从而严重影响位姿估计的精度。近年来伴随着深度学习技术的发展，基于图像的位姿估计方法精度虽有一定提升，但是这种方法依赖于数据量的大小，训练模型的泛化能力也有待提高。相比于图像，点云数据能够对物体的形貌、空间位置进行更加细致、准确的描述。因此将点云数据应用于位姿估计领域成为了研究热点。

通过上述分析，现有技术存在的问题及缺陷为：传统的基于点云数据构造点对特征并进行全局投票的位姿估计方式，存在内存浪费、效率不高的问题。为此基于点云配准的位姿估计方法被提了出来，其通过将场景下的物体点云与已知位姿参数的物体模板点云进行高精度配准，便可以实现目标物体的位姿估计。然而，基于点云配准的位姿估计方法应用于低分辨率深度相机采集的场景点云仍存在位姿估计精度较差的问题。

发明内容

针对现有技术存在的低分辨率深度相机获取复杂场景下物体精确位姿信息困难的问题，本发明提供了一种位姿估计方法、位姿估计系统、终端、存储介质及应用。具体涉及一种基于点云场景分割与改进配准算法的位姿估计方法。

本发明是这样实现的，一种位姿估计方法，包括：利用结构光全局式三维扫描仪对待位姿估计的目标物体制作点云模板；对制作的点云模板利用两步分割法的分割方式对目标物体进行分割；

结合法线夹角特征约束与邻域数特征约束对分割的目标物体以及目标物体模板进行关键点提取；

在提取的关键点处构造FPFH特征，并采用随机采样一致性算法完成场景目标物体与目标物体模板之间的粗配准与初始位姿估计；

结合法线一致性约束改进点到面的ICP算法，对场景中目标物体初始位姿估计结果进行修正。

进一步，所述利用两步分割法的分割方式对目标物体进行分割的方法包括：第一步，场景滤波：深度相机采集的原始点云为P＝{p₁,p₂,…,p_n}，根据目标物体所在场景的大致位置设置xyz方向提取的阈值，提取到感兴趣区域的场景点云数据Q＝{q₁,q₂…q_m}；进一步对场景点云数据进行统计滤波去噪；场景点云数据 Q中第i个点的坐标为q_i(x_i,y_i,z_i)，该点到任意一个k近邻点距离为：

根据Q中所有点到各自k邻域点的距离构成高斯分布这一规律，计算所有点与邻域距离的均值μ与标准差σ：

选择合适的标准差倍数λ得到置信度区间(μ-λ·σ,μ+λ·σ)，某点到其k邻域距离的均值位于置信度区间之外则判定为噪声点加以去除，最终得到场景滤波后的点云C＝{c₁,c₂…c_l}；

第二步，聚类分割：采用基于RANSAC聚类与欧式聚类相结合的分割算法，先估计场景点云中存在的平面背景并加以剔除，接着在剩余的点中以欧式距离为标准设置阈值进行聚类，得到的聚类点即为目标物体点云。

进一步，所述第二步聚类分割具体包括：第1步，在滤波后得到的场景区域点云C中随机选取3个点并计算对应的平面模型参数；

第2步，计算余下数据点到所估计平面的距离，距离小于所给定的阈值则将其划分到该平面的内点集合内，统计集合内点的数量；

第3步，重新随机选择三个数据点进行迭代计算，直到迭代次数达到给定阈值，停止迭代；选取迭代过程中估计的平面模型内点数据最多的一组作为最终所估计的平面，根据索引在区域点云C中删除，得到剔除背景平面后的区域点云D；在D中随机选取一点p搜索其k个最近邻，若p点到近邻点距离小于给定阈值，则将近邻点加入到聚类集合E中，聚类集合E的数量不再增加，则选取E中除p以外的点重复前面的流程，直到选取集合内任意一点进行聚类点数仍不再增加，则完成聚类得到了分割的目标物体点云。

进一步，所述对分割的目标物体以及目标物体模板进行关键点提取包括：

1)计算物体点云法线：利用主成分分析(PCA)法进行法线计算，先构建 KdTree搜索结构，对任意一点p搜索k个最近邻并构造协方差矩阵S：

式中S为点云中任意一点p的协方差矩阵，p_i为点云中任意一点p的第 i(1≤i≤k)个近邻点，

为p点对应的k个邻域点坐标的均值，计算方法为：

对于计算的协方差矩阵S，进行奇异值分解，求得最小特征值对应的特征向量，所述特征向量为p点法线；

2)统计邻域夹角与邻域点个数：对点云中任意一点p，选取合适的半径r，统计r内邻域点个数x以及p点与邻域点法线夹角之和θ_s：

式中f_p为p点法线，

为邻域点p_i法线。

3)设置阈值筛选关键点：设定夹角阈值τ_θ1与邻域数阈值τ_x，当任意一点满足如下条件时，则判定为关键点：

其中邻域数阈值取点云中所有点r半径内邻域数均值的

倍。

进一步，所述完成场景目标物体与目标物体模板之间的粗配准与初始位姿估计包括：

(i)计算分割的物体点云与模板点云在关键点处的FPFH特征，在模板点云关键点中搜索与目标物体相似的关键点，形成对应点对，对应点的建立以FPFH的相似性为标准；

(ii)随机选取n组对应点对，任意对应点对之间的距离应大于给定阈值τ_d1；

(iii)根据n组对应点对计算两片点云的刚体变换矩阵，将矩阵应用于物体点云并计算剩余对应点对之间的距离误差，距离误差小于阈值τ_d2则该点为样本内点并统计内点数量；

(iv)重复上述步骤直到最大迭代次数，将样本内点数最多的刚体变换矩阵作为初始位姿估计结果T_rough，并将该结果应用于目标物体点云。

进一步，所述对场景中目标物体初始位姿估计结果进行修正包括：

(1)初始对应点对估计，在源点云与目标点云中构造对应点对，并形成对应点对集合，满足对应点对的初始要求为欧氏距离最短：

f＝min(||m_i-n_i||²)；

式中m_i,n_i为一对对应点，其中m_i为物体点云中一点，n_i为模板点云中一点；

(2)引入法线夹角约束，对对应点集合进行筛选更新，取合适的法线夹角阈值τ_θ2，对应点集中任意一对应点法线夹角小于阈值则保留，大于阈值则进行剔除；

(3)采用更新的对应点集合M,N计算变换矩阵ΔT，先根据源点云对应点集合 M与目标点云对应点集合N构造协方差矩阵T_cov并进行奇异值分解：

式中K为对应点集合M,N中点对数量，

为集合M,N的中心点，即

U、Σ、V分别为协方差矩阵T_cov奇异值分解后对应的特征矩阵；

根据奇异值分解的结果计算得到第一次迭代的位姿变换矩阵ΔT₁：

式中ΔR₁为3×3旋转矩阵，Δt₁为3×1平移向量，计算公式为：

(4)利用位姿矩阵ΔT₁更新物体点云，并计算物体点云与模板点云对应点之间的距离误差：

式中E(ΔT₁)为利用位姿矩阵ΔT₁更新点云后计算的距离误差值。

(5)重复步骤(3)、步骤(4)进行迭代运算，直到满足最大迭代次数或者距离误差小于给定的阈值，便可得到物体位姿精确估计结果T_acc：

T_acc＝ΔT_n·ΔT_n-1…ΔT₂·ΔT₁·T_rough；

式中ΔT₁～ΔT_n分别为1～n次迭代的位姿估计矩阵。

本发明的另一目的在于提供一种实施所述位姿估计方法的位姿估计系统，所述位姿估计系统包括：

结构光全局式三维扫描仪，对进行位姿估计的目标物体进行全方面的表面轮廓扫描，获取目标物体完整的表面三维点云数据并将表面三维点云数据作为模板数据；

目标物体分割模块，用于使用低分率深度相机采集当前包含目标物体的场景点云数据；然后采用基于两步分割的方法先通过滤波算法过滤噪声并提取场景中感兴趣区域点云，接着对感兴趣区域点云结合使用RANSAC聚类与欧式聚类完成目标物体分割；

关键点提取模块，用于结合法线夹角特征约束与邻域数特征约束对分割的目标物体以及目标物体模板进行关键点提取，提取的关键点具有高度辨别力且非噪声的数据点；

初始位姿估计模块，用于在提取的关键点处构造FPFH特征描述，并采用随机采样一致性算法完成场景目标物体与目标物体模板之间的粗配准与初始位姿估计；

位姿修正模块，用于引入法线一致性约束来改进点到面的ICP算法，对场景中目标物体初始位姿估计结果的修正。

本发明的另一目的在于提供一种信息数据处理终端，所述信息数据处理终端设备包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行所述位姿估计方法。

本发明的另一目的在于提供一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，使得所述处理器执行所述位姿估计方法。

本发明的另一目的在于提供一种所述位姿估计方法在机器人抓取、搬运、自动驾驶领域中位姿定位上的应用。

结合上述的技术方案和解决的技术问题，请从以下几方面分析本发明所要保护的技术方案所具备的优点及积极效果为：

第一、针对上述现有技术存在的技术问题以及解决该问题的难度，紧密结合本发明的所要保护的技术方案以及研发过程中结果和数据等，详细、深刻地分析本发明技术方案如何解决的技术问题，解决问题之后带来的一些具备创造性的技术效果。具体描述如下：

本发明对待位姿估计的目标物体，采用高精度的结构光全局式三维扫描仪来制作物体的点云模板，解决了直接由CAD模型生成模板导致与实际零件存在差异性大的问题。

本发明基于两步分割法的分割方式，即首先根据目标物体大致位置提取感兴趣区域，而后结合RANSAC聚类与欧式聚类对目标物体进行精确分割，能够有效的提高目标物体分割提取的效率与准确度。

本发明结合法向量约束与邻域数约束的关键点提取算法，能够有效提取模板与场景点云中具有大曲率特征且非噪声的关键点。

本发明在关键点处构建FPFH特征描述并利用随机采样一致性，能够有效得到场景中目标物体到模板的一个初始位姿估计。

本发明引入对应点法线一致性约束来改进点到平面的ICP算法，能够提高原ICP算法的鲁棒性，避免陷入局部最优解，从而完成初始位姿的精确修正。

第二，把技术方案看做一个整体或者从产品的角度，本发明所要保护的技术方案具备的技术效果和优点，具体描述如下：

本发明通过结合深度相机与三维扫描仪设备各自的优势来采集点云数据、制作点云模板，后续利用所提出的位姿估计算法，能够实现对工业场景下零件位姿的自动识别，为后续对零件进行抓取，搬运，加工等操作提供了精确的先验结果。

本发明围绕三维点云数据提出的一种工业零件位姿识别技术，相比传统的基于图像位姿识别技术，不易受环境、光照等条件的干扰，具有更强的鲁棒性。

本发明所提方案相比于现有基于深度学习的位姿估计技术，无需制作大量的目标物体数据集来进行训练，节省了大量时间，实现较为简单。

本发明所提出的基于改进配准的位姿估计方法，对比于已有基于配准的位姿估计技术能够有更高的位姿估计精度。

本发明只需要前期利用高精度的三维扫描仪制作好模板库，后期仅仅只需要使用低分率深度相机配合位姿估计算法即可以实现零件位姿的估计，不仅具有高的估计精度，而且成本较低。

第三，作为本发明的权利要求的创造性辅助证据，还体现在以下几个重要方面：

本发明的技术方案转化后的预期收益和商业价值为：

本发明所提供的一种工业零件位姿估计方法结合工业机器人，能够配套的运用到诸如抓取、搬运、码垛等工业场景之下，有效提高企业生产的自动化水平，降低劳动力成本，逐步实现企业无人化、智能化的管理目标，为企业进行提质增益。

本发明的技术方案是否解决了人们一直渴望解决、但始终未能获得成功的技术难题：本发明有效的解决了当前运用低分辨率深度相机进行位姿估计困难的难题，低分辨率相机所采集的点云数据往往存在很大的噪声，因此现有的基于低分辨率相机进行位姿估计的精度较低，本发明利用高精度点云模板库，以及物体精确分割和改进配准算法，能够有效的提高运用低分辨率深度相机进行位姿估计的精度。

附图说明

图1是本发明实施例提供的位姿估计方法原理图。

图2是本发明实施例提供的位姿估计方法流程图。

图3是本发明实施例提供的物体点云聚类分割流程图。

图4是本发明实施例提供的物体模板点云示意图。其中，图4(a)方块模板；图4(b)圆柱体模板；图4(c)复杂深腔零件模板。

图5是本发明实施例提供的方块物体分割实例图。其中，图5(a)场景点云；图5(b)区域滤波；图5(c)聚类分割。

图6是本发明实施例提供的圆柱物体分割实例图。其中，图6(a)场景点云；图6(b)区域滤波；图6(c)聚类分割。

图7是本发明实施例提供的复杂腔体零件分割实例图。其中，图7(a)场景点云；图7(b)区域滤波；图7(c)聚类分割。

图8是本发明实施例提供的物体初始位姿估计结果示意图(深色为模板点云，浅色为物体点云)。其中，图8(a)方块；图8(b)圆柱体；图8(c)复杂深腔零件。

图9是本发明实施例提供的物体初始位姿精确修正结果示意图(深色为模板点云，浅色为物体点云)。其中，图9(a)方块；图9(b)圆柱体；图9(c)复杂深腔零件。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

一、解释说明实施例。为了使本领域技术人员充分了解本发明如何具体实现，该部分是对权利要求技术方案进行展开说明的解释说明实施例。

在本发明中图1是本发明实施例提供的位姿估计方法原理。

如图2所示，本发明实施例提供的位姿估计方法包括：

S101，利用结构光全局式三维扫描仪对要进行位姿估计的物体进行全方面的表面轮廓扫描，扫描精度控制在0.025mm以内，获取其完整的表面三维点云数据并将其作为模板数据。

S102，使用低分率深度相机采集当前包含目标物体的场景点云数据。然后采用基于两步分割的方法，即首先通过滤波算法过滤噪声并提取场景中感兴趣区域点云，接着对感兴趣区域点云结合使用RANSAC聚类与欧式聚类完成目标物体精确分割。

S103，结合法线夹角特征约束与邻域数特征约束对分割的目标物体以及目标物体模板进行关键点提取，提取的关键点是具有高度辨别力且非噪声的数据点。

S104，在提取的关键点处构造FPFH特征描述，并采用随机采样一致性算法完成场景目标物体与目标物体模板之间的粗配准与初始位姿估计。

S105，引入法线一致性约束来改进点到面的ICP算法，实现对场景中目标物体初始位姿估计结果的精确修正。

作为进一步优选的，具体采用如下方法完成步骤S102场景点云中目标物体的精确分割：

点云场景分割是通过采用一系列分割算法将目标物体从场景点云中提取出来，后续将其分别与事先制作好的目标物体模板进行配准和位姿估计，能够有效减少背景、噪声的干扰，提高位姿估计的效率与精度。为了实现场景中目标物体的精确分割，本发明提出了一种基于两步分割的方法，具体操作如下：

(1)场景滤波

场景滤波主要结合了直通滤波与统计滤波，设深度相机采集的原始点云为 P＝{p₁,p₂,…,p_n}，根据目标物体所在场景的大致位置设置xyz方向提取的阈值，便可以提取到感兴趣区域的场景点云数据Q＝{q₁,q₂…q_m}。由于环境、光照等因素影响，所提取的区域仍包含大量噪声点、离群点，进一步对其进行统计滤波去噪。设点云Q中第i个点的坐标为q_i(x_i,y_i,z_i)，该点到其任意一个k近邻点距离为：

根据Q中所有点到其各自k邻域点的距离构成高斯分布这一规律，计算所有点与其邻域距离的均值μ与标准差σ：

选择合适的标准差倍数λ得到置信度区间(μ-λ·σ,μ+λ·σ)，若某点到其k邻域距离的均值位于置信度区间之外则判定为噪声点加以去除，最终得到了场景滤波后的点云C＝{c₁,c₂…c_l}。

(2)聚类分割

为了能够在感兴趣区域精确分割出目标物体，本发明进一步采用基于 RANSAC聚类与欧式聚类相结合的分割算法，首先估计场景点云中存在的平面背景并加以剔除，接着在剩余的点中以欧式距离为标准设置阈值进行聚类，得到的聚类点即为目标物体点云。

具体的分割流程如附图3所示，首先在滤波后得到的场景区域点云C中随机选取3个点并计算对应的平面模型参数，接着计算余下数据点到所估计平面的距离，若距离小于所给定的阈值则将其划分到该平面的内点集合内，统计集合内点的数量。重新随机选择三个数据点进行迭代计算，直到迭代次数达到给定阈值，停止迭代。选取迭代过程中估计的平面模型内点数据最多的一组作为最终所估计的平面，根据其索引在区域点云C中删除，得到剔除背景平面后的区域点云D。在D中随机选取一点p搜索其k个最近邻，若p点到其近邻点距离小于给定阈值，则将该近邻点加入到聚类集合E中，若聚类集合E的数量不再增加，则选取E中除p以外的点重复前面的流程，直到选取集合内任意一点进行聚类点数仍不再增加，则完成聚类得到了分割的目标物体点云。

作为进一步优选的，具体采用如下方法提取步骤S3目标物体点云与模板点云的关键点：

(1)计算物体点云法线：法线计算主要采用主成分分析(PCA)法，首先构建 KdTree搜索结构，对任意一点p搜索其k个最近邻并构造协方差矩阵S：

式中S为点云中任意一点p的协方差矩阵，p_i为点云中任意一点p的第 i(1≤i≤k)个近邻点，

为p点对应的k个邻域点坐标的均值，计算方法为：

对于计算的协方差矩阵S，将其进行奇异值分解，求得最小特征值对应的特征向量即为p点法线。

(2)统计邻域夹角与邻域点个数：对点云中任意一点p，选取合适的半径r，统计r内邻域点个数x以及p点与其邻域点法线夹角之和θ_s：

式中f_p为p点法线，

为邻域点p_i法线。

(3)设置阈值筛选关键点：法线夹角反应了物体点云局部曲率变化情况，邻域个数则反应了该点为真实有效点的概率。取合适的夹角阈值τ_θ1与邻域数阈值τ_x，当任意一点满足如下条件时，则判定其为关键点：

其中邻域数阈值取点云中所有点r半径内邻域数均值的

倍。其中物体模板点云如图4所示，图4(a)方块模板；图4(b)圆柱体模板；图4(c)复杂深腔零件模板。

在本发明中，图5是方块物体分割实例图。其中，图5(a)场景点云；图5(b) (b)区域滤波；图5(c)聚类分割。

图6是本发明实施例提供的圆柱物体分割实例图。其中，图6(a)场景点云；图6(b)区域滤波；图6(c)聚类分割。

图7是本发明实施例提供的复杂腔体零件分割实例图。其中，图7(a)场景点云；图7(b)区域滤波；图7(c)聚类分割。

作为进一步优选的，具体采用如下方法完成步骤S104目标物体的初始位姿估计：

(1)计算分割的物体点云与模板点云在关键点处的FPFH(快速点特征直方图)特征，在模板点云关键点中搜索与目标物体相似的关键点，形成对应点对，其中对应点的建立以FPFH的相似性为标准。

(2)随机选取n组对应点对，为保证对应点对的有效性，其任意对应点对之间的距离应大于给定阈值τ_d1。

(3)根据n组对应点对计算两片点云的刚体变换矩阵，将矩阵应用于物体点云并计算剩余对应点对之间的距离误差，若距离误差小于阈值τ_d2则该点视为样本内点并统计内点数量。

(4)重复上述步骤直到最大迭代次数，将样本内点数最多的刚体变换矩阵作为初始位姿估计结果T_rough，并将该结果应用于目标物体点云。

在本发明中，图8是本发明实施例提供的物体初始位姿估计结果示意图(深色为模板点云，浅色为物体点云)。其中，图8(a)方块；图8(b)圆柱体；图 8(c)复杂深腔零件。

作为进一步优选的，具体采用如下方法完成步骤S105对目标物体初始位姿估计的精确修正：

(1)初始对应点对估计，在源点云(应用初始位姿变换后的目标物体点云)与目标点云(制作的模板点云)中构造对应点对，并形成对应点对集合，满足对应点对的初始要求为欧氏距离最短：

f＝min(||m_i-n_i||²)

式中m_i,n_i为一对对应点，其中m_i为物体点云中一点，n_i为模板点云中一点。

(2)引入法线夹角约束，对对应点集合进行筛选更新，取合适的法线夹角阈值τ_θ2，若对应点集中任意一对应点法线夹角小于阈值则保留，反之进行剔除。

(3)采用更新的对应点集合M,N计算变换矩阵ΔT，首先根据源点云对应点集合M与目标点云对应点集合N构造协方差矩阵T_cov并进行奇异值分解：

式中K为对应点集合M,N中点对数量，

为集合M,N的中心点，即

U、Σ、V分别为协方差矩阵T_cov奇异值分解后对应的特征矩阵；

根据奇异值分解的结果可计算得到第一次迭代的位姿变换矩阵ΔT₁：

式中ΔR₁为3×3旋转矩阵，Δt₁为3×1平移向量，其具体的计算公式为：

(4)利用位姿矩阵ΔT₁更新物体点云，并计算物体点云与模板点云对应点之间的距离误差：

式中E(ΔT₁)为利用位姿矩阵ΔT₁更新点云后计算的距离误差值。

(5)重复步骤(3)、步骤(4)进行迭代运算，直到满足最大迭代次数或者距离误差小于给定的阈值，便可得到物体位姿精确估计结果T_acc：

T_acc＝ΔT_n·ΔT_n-1…ΔT₂·ΔT₁·T_rough

式中ΔT₁～ΔT_n分别为1～n次迭代的位姿估计矩阵。

在本发明中，图9是本发明实施例提供的物体初始位姿精确修正结果示意图(深色为模板点云，浅色为物体点云)。其中，图9(a)方块；图9(b)圆柱体；图9(c)复杂深腔零件。

二、应用实施例。为了证明本发明的技术方案的创造性和技术价值，该部分是对权利要求技术方案进行具体产品上或相关技术上的应用实施例。

本发明可应用于航空发动机机匣自动化冲洗技术，航空发动机机匣在加工后需要对其表面污垢、铁屑进行冲洗，传统的冲洗方式往往由人工进行，存在费时费力，劳动成本大的缺陷。本发明能够精确识别放置在待冲洗工位上航空发动机机匣的实时位姿，并将结果发送给机器人，机器人对待冲洗工位上的零件进行精确夹取，进入到冲洗舱内进行自动化冲洗。

本发明也可应用于移动式服务机器人的3D场景感知，通过在移动式服务机器人四周安装好深度相机，采集其四周环境物体的点云数据，后续对采集的点云数据进行分割，并与指定的物体模板库进行配准得到其位置与姿态，从而实现对机器人对周围事物的一个精确感知。

本发明还可应用于散乱摆放零件的整理工作，对于零件散乱摆放的工业场景，结合本发明所提供的位姿识别技术，能够准确识别出场景中各个零件所处的位姿，并将结果发送给机器人，从而实现对散乱摆放零件的精确抓取与整理。

本发明同样可应用于自动驾驶过程中，对汽车周围的物体位姿进行实时估计，其通过在汽车四周安装对应的深度相机，采集其周围环境的点云数据，对不同类型物体进行分割与配准，从而获取其位姿信息并采取对应的避让措施。

三、实施例相关效果的证据。本发明实施例在研发或者使用过程中取得了一些积极效果，和现有技术相比的确具备很大的优势，下面内容结合试验过程的数据、图表等进行描述。

本发明所提出的基于两步分割法相较于直接分割方法，具有更高的分割效率与准确度。直接分割法是直接对采集的原始场景点云进行采样、滤波以及聚类分割。将本发明所提基于两步分割法与直接分割法进行对比，其在目标物体分割时间以及分割点数之间的差异如下表1和表2所示，可以看出相较于直接分割技术，本发明在分割效率上平均提升了42.6％，并且由于两步法分割无需对原始点云进行降采样以寻求更合理的速度，因此分割出的点云更多，结果更准确。

表1物体分割时间比较

表2物体分割点数比较

本发明所提出的基于改进配准位姿估计方案，在提升位姿估计精度方面相较于4PCS、SAC-IA、点到点ICP等几种主流的基于点云配准位姿估计算法，具有更高的优势。对于以上所提技术方案，从位姿估计结果最大偏差(Max-error)、均方根误差(RMSE)这两个指标开展了分析比较，其结果如下表3和表4所示，可以看出本发明所提方案在位姿估计精度方面有着很大的改善，相比于基于 4PCS+ICP配准的位姿估计，最大误差降低了42.96％，均方根误差降低了45.75，同时相比于基于SAC-IA+ICP配准的位姿估计，最大误差降低了50.65％，均方根误差降低了17.80％。

表3物体位姿估计最大误差比较

表4物体位姿估计均方根误差比较

应当注意，本发明的实施方式可以通过硬件、软件或者软件和硬件的结合来实现。硬件部分可以利用专用逻辑来实现；软件部分可以存储在存储器中，由适当的指令执行系统，例如微处理器或者专用设计硬件来执行。本领域的普通技术人员可以理解上述的设备和方法可以使用计算机可执行指令和/或包含在处理器控制代码中来实现，例如在诸如磁盘、CD或DVD-ROM的载体介质、诸如只读存储器(固件)的可编程的存储器或者诸如光学或电子信号载体的数据载体上提供了这样的代码。本发明的设备及其模块可以由诸如超大规模集成电路或门阵列、诸如逻辑芯片、晶体管等的半导体、或者诸如现场可编程门阵列、可编程逻辑设备等的可编程硬件设备的硬件电路实现，也可以用由各种类型的处理器执行的软件实现，也可以由上述硬件电路和软件的结合例如固件来实现。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，都应涵盖在本发明的保护范围之内。

Claims (10)

1.一种位姿估计方法，其特征在于，所述位姿估计方法包括：

对待位姿估计的目标物体，提出采用结构光全局式三维扫描仪来制作目标物体的点云模板，以消除由理论CAD模型直接生成模板所带来的差异性。而后，提出一种基于两步法的物体分割方式，能够快速准确的完成场景点云中目标物体的分割。最后，提出一种结合法线夹角约束与邻域数约束的关键点提取算法，能够有效提取模板与场景点云中具有大曲率特征且非噪声的关键点，紧接着在关键点处构造FPFH特征描述，并基于随机采样一致性完成场景目标物体与目标物体模板之间的粗配准与初始位姿估计。为提高位姿估计精度，进一步采用带法线夹角约束的改进ICP算法来完成场景中目标物体位姿估计结果的精确修正。

2.如权利要求1所述的位姿估计方法，其特征在于，所述利用两步分割法的分割方式对目标物体进行分割的方法包括：第一步，场景滤波：深度相机采集的原始点云为P＝{p₁,p₂,…,p_n}，根据目标物体所在场景的大致位置设置xyz方向提取的阈值，提取到感兴趣区域的场景点云数据Q＝{q₁,q₂…q_m}；进一步对场景点云数据进行统计滤波去噪；场景点云数据Q中第i个点的坐标为q_i(x_i,y_i,z_i)，该点到任意一个k近邻点距离为：

根据Q中所有点到各自k邻域点的距离构成高斯分布这一规律，计算所有点与邻域距离的均值μ与标准差σ：

选择合适的标准差倍数λ得到置信度区间(μ-λ·σ,μ+λ·σ)，某点到其k邻域距离的均值位于置信度区间之外则判定为噪声点加以去除，最终得到场景滤波后的点云C＝{c₁,c₂…c_l}；

第二步，聚类分割：采用基于RANSAC聚类与欧式聚类相结合的分割算法，先估计场景点云中存在的平面背景并加以剔除，接着在剩余的点中以欧式距离为标准设置阈值进行聚类，得到的聚类点即为目标物体点云。

3.如权利要求2所述的位姿估计方法，其特征在于，所述第二步聚类分割具体包括：第1步，在滤波后得到的场景区域点云C中随机选取3个点并计算对应的平面模型参数；

第2步，计算余下数据点到所估计平面的距离，距离小于所给定的阈值则将其划分到该平面的内点集合内，统计集合内点的数量；

第3步，重新随机选择三个数据点进行迭代计算，直到迭代次数达到给定阈值，停止迭代；选取迭代过程中估计的平面模型内点数据最多的一组作为最终所估计的平面，根据索引在区域点云C中删除，得到剔除背景平面后的区域点云D；在D中随机选取一点p搜索其k个最近邻，若p点到近邻点距离小于给定阈值，则将近邻点加入到聚类集合E中，聚类集合E的数量不再增加，则选取E中除p以外的点重复前面的流程，直到选取集合内任意一点进行聚类点数仍不再增加，则完成聚类得到了分割的目标物体点云。

4.如权利要求1所述的位姿估计方法，其特征在于，所述对分割的目标物体以及目标物体模板进行关键点提取包括：

1)计算物体点云法线：利用主成分分析(PCA)法进行法线计算，先构建KdTree搜索结构，对任意一点p搜索k个最近邻并构造协方差矩阵S：

式中S为点云中任意一点p的协方差矩阵，p_i为点云中任意一点p的第i(1≤i≤k)个近邻点，

为p点对应的k个邻域点坐标的均值，计算方法为：

对于计算的协方差矩阵S，进行奇异值分解，求得最小特征值对应的特征向量，所述特征向量为p点法线；

2)统计邻域夹角与邻域点个数：对点云中任意一点p，选取合适的半径r，统计r内邻域点个数x以及p点与邻域点法线夹角之和θ_s：

式中f_p为p点法线，f_pi为邻域点p_i法线；

3)设置阈值筛选关键点：设定夹角阈值τ_θ1与邻域数阈值τ_x，当任意一点满足如下条件时，则判定为关键点：

其中邻域数阈值取点云中所有点r半径内邻域数均值的

倍。

5.如权利要求1所述的位姿估计方法，其特征在于，所述完成场景目标物体与目标物体模板之间的粗配准与初始位姿估计包括：

(i)计算分割的物体点云与模板点云在关键点处的FPFH特征，在模板点云关键点中搜索与目标物体相似的关键点，形成对应点对，对应点的建立以FPFH的相似性为标准；

(ii)随机选取n组对应点对，任意对应点对之间的距离应大于给定阈值τ_d1；

(iii)根据n组对应点对计算两片点云的刚体变换矩阵，将矩阵应用于物体点云并计算剩余对应点对之间的距离误差，距离误差小于阈值τ_d2则该点为样本内点并统计内点数量；

(iv)重复上述步骤直到最大迭代次数，将样本内点数最多的刚体变换矩阵作为初始位姿估计结果T_rough，并将该结果应用于目标物体点云。

6.如权利要求1所述的位姿估计方法，其特征在于，所述对场景中目标物体初始位姿估计结果进行修正包括：

(1)初始对应点对估计，在源点云与目标点云中构造对应点对，并形成对应点对集合，满足对应点对的初始要求为欧氏距离最短：

f＝min(||m_i-n_i||²)

式中m_i,n_i为一对对应点，其中m_i为物体点云中一点，n_i为模板点云中一点；

(2)引入法线夹角约束，对对应点集合进行筛选更新，取合适的法线夹角阈值τ_θ2，对应点集中任意一对应点法线夹角小于阈值则保留，大于阈值则进行剔除；

(3)采用更新的对应点集合M,N计算变换矩阵ΔT，先根据源点云对应点集合M与目标点云对应点集合N构造协方差矩阵T_cov并进行奇异值分解：

式中K为对应点集合M,N中点对数量，

为集合M,N的中心点，即

U、Σ、V分别为协方差矩阵T_cov奇异值分解后对应的特征矩阵；

根据奇异值分解的结果计算得到第一次迭代的位姿变换矩阵ΔT₁：

式中ΔR₁为3×3旋转矩阵，Δt₁为3×1平移向量，计算公式为：

(4)利用位姿矩阵ΔT₁更新物体点云，并计算物体点云与模板点云对应点之间的距离误差：

式中E(ΔT₁)为利用位姿矩阵ΔT₁更新点云后计算的距离误差值；

(5)重复步骤(3)、步骤(4)进行迭代运算，直到满足最大迭代次数或者距离误差小于给定的阈值，便可得到物体位姿精确估计结果T_acc：

T_acc＝ΔT_n·ΔT_n-1…ΔT₂·ΔT₁·T_rough；

式中ΔT₁～ΔT_n分别为1～n次迭代的位姿估计矩阵。

7.一种实施权利要求1～6任意一项所述位姿估计方法的位姿估计系统，其特征在于，所述位姿估计系统包括：

结构光全局式三维扫描仪，对进行位姿估计的目标物体进行全方面的表面轮廓扫描，获取目标物体完整的表面三维点云数据并将表面三维点云数据作为模板数据；

目标物体分割模块，用于使用低分率深度相机采集当前包含目标物体的场景点云数据；然后采用基于两步分割的方法先通过滤波算法过滤噪声并提取场景中感兴趣区域点云，接着对感兴趣区域点云结合使用RANSAC聚类与欧式聚类完成目标物体分割；

关键点提取模块，用于结合法线夹角特征约束与邻域数特征约束对分割的目标物体以及目标物体模板进行关键点提取，提取的关键点具有高度辨别力且非噪声的数据点；

初始位姿估计模块，用于在提取的关键点处构造FPFH特征描述，并采用随机采样一致性算法完成场景目标物体与目标物体模板之间的粗配准与初始位姿估计；

位姿修正模块，用于引入法线一致性约束来改进点到面的ICP算法，对场景中目标物体初始位姿估计结果的修正。

8.一种信息数据处理终端，其特征在于，所述信息数据处理终端设备包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行权利要求1～6任意一项所述位姿估计方法。

9.一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，使得所述处理器执行权利要求1～6任意一项所述位姿估计方法。

10.一种如权利要求1～6任意一项所述位姿估计方法在机器人抓取、搬运、自动驾驶领域中位姿定位上的应用。

Images