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

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

【技术实现步骤摘要】
位姿估计方法、位姿估计系统、终端、存储介质及应用


[0001]本专利技术属于物体空间位姿估计
，尤其涉及一种位姿估计方法、位姿估计系统、终端、存储介质及应用。

技术介绍

[0002]目前，位姿估计是获取目标物体在当前深度相机所拍摄场景下的位置坐标与姿态信息，其是机器人抓取、搬运、自动驾驶等许多重要应用的基础。
[0003]针对目前物体位姿估计的方法，主要可分为基于图像和基于点云两类。基于RGB图像或者RGB
‑
D图像的位姿估计方法在传统方法上依赖于人工设计特征，并且图像的区别特征、纹理等往往会受到光照、反射等外界条件的影响，从而严重影响位姿估计的精度。近年来伴随着深度学习技术的发展，基于图像的位姿估计方法精度虽有一定提升，但是这种方法依赖于数据量的大小，训练模型的泛化能力也有待提高。相比于图像，点云数据能够对物体的形貌、空间位置进行更加细致、准确的描述。因此将点云数据应用于位姿估计领域成为了研究热点。
[0004]通过上述分析，现有技术存在的问题及缺陷为：传统的基于点云数据构造点对特征并进行全局投票的位姿估计方式，存在内存浪费、效率不高的问题。为此基于点云配准的位姿估计方法被提了出来，其通过将场景下的物体点云与已知位姿参数的物体模板点云进行高精度配准，便可以实现目标物体的位姿估计。然而，基于点云配准的位姿估计方法应用于低分辨率深度相机采集的场景点云仍存在位姿估计精度较差的问题。

技术实现思路

[0005]针对现有技术存在的低分辨率深度相机获取复杂场景下物体精确位姿信息困难的问题，本专利技术提供了一种位姿估计方法、位姿估计系统、终端、存储介质及应用。具体涉及一种基于点云场景分割与改进配准算法的位姿估计方法。
[0006]本专利技术是这样实现的，一种位姿估计方法，包括：利用结构光全局式三维扫描仪对待位姿估计的目标物体制作点云模板；对制作的点云模板利用两步分割法的分割方式对目标物体进行分割；
[0007]结合法线夹角特征约束与邻域数特征约束对分割的目标物体以及目标物体模板进行关键点提取；
[0008]在提取的关键点处构造FPFH特征，并采用随机采样一致性算法完成场景目标物体与目标物体模板之间的粗配准与初始位姿估计；
[0009]结合法线一致性约束改进点到面的ICP算法，对场景中目标物体初始位姿估计结果进行修正。
[0010]进一步，所述利用两步分割法的分割方式对目标物体进行分割的方法包括：第一步，场景滤波：深度相机采集的原始点云为P＝{p1,p2,
…
,p
n
}，根据目标物体所在场景的大致位置设置xyz方向提取的阈值，提取到感兴趣区域的场景点云数据Q＝{q1,q2…
q
m
}；进一
步对场景点云数据进行统计滤波去噪；场景点云数据 Q中第i个点的坐标为q
i
(x
i
,y
i
,z
i
)，该点到任意一个k近邻点距离为：
[0011][0012]根据Q中所有点到各自k邻域点的距离构成高斯分布这一规律，计算所有点与邻域距离的均值μ与标准差σ：
[0013][0014][0015]选择合适的标准差倍数λ得到置信度区间(μ
‑
λ
·
σ,μ+λ
·
σ)，某点到其k邻域距离的均值位于置信度区间之外则判定为噪声点加以去除，最终得到场景滤波后的点云C＝{c1,c2…
c
l
}；
[0016]第二步，聚类分割：采用基于RANSAC聚类与欧式聚类相结合的分割算法，先估计场景点云中存在的平面背景并加以剔除，接着在剩余的点中以欧式距离为标准设置阈值进行聚类，得到的聚类点即为目标物体点云。
[0017]进一步，所述第二步聚类分割具体包括：第1步，在滤波后得到的场景区域点云C中随机选取3个点并计算对应的平面模型参数；
[0018]第2步，计算余下数据点到所估计平面的距离，距离小于所给定的阈值则将其划分到该平面的内点集合内，统计集合内点的数量；
[0019]第3步，重新随机选择三个数据点进行迭代计算，直到迭代次数达到给定阈值，停止迭代；选取迭代过程中估计的平面模型内点数据最多的一组作为最终所估计的平面，根据索引在区域点云C中删除，得到剔除背景平面后的区域点云D；在D中随机选取一点p搜索其k个最近邻，若p点到近邻点距离小于给定阈值，则将近邻点加入到聚类集合E中，聚类集合E的数量不再增加，则选取E中除p以外的点重复前面的流程，直到选取集合内任意一点进行聚类点数仍不再增加，则完成聚类得到了分割的目标物体点云。
[0020]进一步，所述对分割的目标物体以及目标物体模板进行关键点提取包括：
[0021]1)计算物体点云法线：利用主成分分析(PCA)法进行法线计算，先构建 KdTree搜索结构，对任意一点p搜索k个最近邻并构造协方差矩阵S：
[0022][0023]式中S为点云中任意一点p的协方差矩阵，p
i
为点云中任意一点p的第 i(1≤i≤k)个近邻点，为p点对应的k个邻域点坐标的均值，计算方法为：
[0024][0025]对于计算的协方差矩阵S，进行奇异值分解，求得最小特征值对应的特征向量，所述特征向量为p点法线；
[0026]2)统计邻域夹角与邻域点个数：对点云中任意一点p，选取合适的半径r，统计r内邻域点个数x以及p点与邻域点法线夹角之和θ
s
：
[0027][0028]式中f
p
为p点法线，为邻域点p
i
法线。
[0029]3)设置阈值筛选关键点：设定夹角阈值τ
θ1
与邻域数阈值τ
x
，当任意一点满足如下条件时，则判定为关键点：
[0030][0031]其中邻域数阈值取点云中所有点r半径内邻域数均值的倍。
[0032]进一步，所述完成场景目标物体与目标物体模板之间的粗配准与初始位姿估计包括：
[0033](i)计算分割的物体点云与模板点云在关键点处的FPFH特征，在模板点云关键点中搜索与目标物体相似的关键点，形成对应点对，对应点的建立以FPFH的相似性为标准；
[0034](ii)随机选取n组对应点对，任意对应点对之间的距离应大于给定阈值τ
d1
；
[0035](iii)根据n组对应点对计算两片点云的刚体变换矩阵，将矩阵应用于物体点云并计算剩余对应点对之间的距离误差，距离误差小于阈值τ
d2
则该点为样本内点并统计内点数量；
[0036](iv)重复上述步骤直到最大迭代次数，将样本内点数最多的刚体变换矩阵作为初始位姿估计结果T
rough
，并将该结果应用于目标物体点云。
[0037]进一步，所述对场景中目标物体初始位姿估计结果进行修正包括：
[0038](1)初始对应本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种位姿估计方法，其特征在于，所述位姿估计方法包括：对待位姿估计的目标物体，提出采用结构光全局式三维扫描仪来制作目标物体的点云模板，以消除由理论CAD模型直接生成模板所带来的差异性。而后，提出一种基于两步法的物体分割方式，能够快速准确的完成场景点云中目标物体的分割。最后，提出一种结合法线夹角约束与邻域数约束的关键点提取算法，能够有效提取模板与场景点云中具有大曲率特征且非噪声的关键点，紧接着在关键点处构造FPFH特征描述，并基于随机采样一致性完成场景目标物体与目标物体模板之间的粗配准与初始位姿估计。为提高位姿估计精度，进一步采用带法线夹角约束的改进ICP算法来完成场景中目标物体位姿估计结果的精确修正。2.如权利要求1所述的位姿估计方法，其特征在于，所述利用两步分割法的分割方式对目标物体进行分割的方法包括：第一步，场景滤波：深度相机采集的原始点云为P＝{p1,p2,
…
,p
n
}，根据目标物体所在场景的大致位置设置xyz方向提取的阈值，提取到感兴趣区域的场景点云数据Q＝{q1,q2…
q
m
}；进一步对场景点云数据进行统计滤波去噪；场景点云数据Q中第i个点的坐标为q
i
(x
i
,y
i
,z
i
)，该点到任意一个k近邻点距离为：根据Q中所有点到各自k邻域点的距离构成高斯分布这一规律，计算所有点与邻域距离的均值μ与标准差σ：的均值μ与标准差σ：选择合适的标准差倍数λ得到置信度区间(μ
‑
λ
·
σ,μ+λ
·
σ)，某点到其k邻域距离的均值位于置信度区间之外则判定为噪声点加以去除，最终得到场景滤波后的点云C＝{c1,c2…
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)计算分割的物体点云...

【专利技术属性】
技术研发人员：杨吉祥，张瑾贤，吴晓峰，叶才铭，丁汉，
申请(专利权)人：中国航发南方工业有限公司，
类型：发明
国别省市：