(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210189283.3 (22)申请日 2022.02.28 (71)申请人 华中科技大 学 地址 430074 湖北省武汉市洪山区珞喻路 1037号 申请人 中国航发南方工业有限公司 (72)发明人 杨吉祥　张瑾贤　吴晓峰　叶才铭　 丁汉　 (74)专利代理 机构 北京金智普华知识产权代理 有限公司 1 1401 专利代理师 张晓博 (51)Int.Cl. G06V 40/20(2022.01) G06V 10/26(2022.01) G06V 10/75(2022.01)G06V 10/762(2022.01) G06K 9/62(2022.01) (54)发明名称 位姿估计方法、 位姿估计系统、 终端、 存储介 质及应用 (57)摘要 本发明属于物体空间位姿估计技术领域， 公 开一种位姿估计方法、 位姿估计系统、 终端、 存储 介质及应用， 采用高精度的结构光全局式三维扫 描仪来制作物体的点云模板， 解决了直接由CAD 模型生成模板导致与实际零件存在差异性大的 问题。 基于两步分割法的分割方式有效的提高目 标物体分割提取的效率与准确度。 结合法向量约 束与邻域数约束的关键点提取算法有效提取模 板与场景点云中具有大曲率特征且非噪声的关 键点。 在关键点处构建FPFH 特征描述并利用随机 采样一致性， 能够有效得到场景中目标物体到模 板的一个初始位姿估计。 本发明引入对应点法线 一致性约束来改进点到平面的ICP算法， 避免陷 入局部最优解， 从而完成初始位姿的精确修 正。 权利要求书4页 说明书11页 附图6页 CN 114743259 A 2022.07.12 CN 114743259 A 1.一种位姿估计方法， 其特 征在于， 所述 位姿估计方法包括： 对待位姿估计的目标物体， 提出采用结构光全局式三维扫描仪来制作目标物体的点云 模板， 以消除由理论CAD模型直接生成模板所带来的差异性。 而后， 提出一种基于两步法的 物体分割方式， 能够快速准确的完成场景点云中目标物体的分割。 最后， 提出一种结合法线 夹角约束与邻域数约束的关键点提取算法， 能够有效提取模板与场景点云中具有 大曲率特 征且非噪声的关键点， 紧接着在关键点处构造FPFH特征描述， 并基于随机采样一致性完成 场景目标物体与目标物体模板之间的粗配准与初始位姿估计。 为提高位姿估计精度， 进一 步采用带法线夹角约束的改进ICP算法来完成场景中目标物体位姿估计结果的精确修 正。 2.如权利要求1所述的位姿估计方法， 其特征在于， 所述利用两步分割法的分割方式对 目标物体进行分割的方法包括： 第一步， 场景滤波： 深度相机采集的原始点云为P＝{p1, p2,…,pn}， 根据目标物体所在场景的大致位置设置xyz方向提 取的阈值， 提 取到感兴趣区域 的场景点云数据Q＝{q1,q2…qm}； 进一步对场景点云数据进 行统计滤波去噪； 场景点云数据 Q中第i个点的坐标为qi(xi,yi,zi)， 该点到任意 一个k近邻点距离为： 根据Q中所有点到各自k邻域点的距离构 成高斯分布这一规律， 计算所有点与邻域距离 的均值 μ与标准差σ： 选择合适的标准差倍数λ得到置信度区间( μ ‑λ·σ, μ+λ·σ )， 某点到其k邻域距离 的均 值位于置信度区间之外则判定 为噪声点加以去除， 最终得到场景滤波后的点云C ＝{c1,c2… cl}； 第二步， 聚类分割： 采用基于RANSAC聚类与欧式聚类相结合的分割算法， 先估计场景点 云中存在的平面背景并加以剔除， 接着在剩余的点中以欧式距离为标准设置阈值进行聚 类， 得到的聚类点即为目标物体点云。 3.如权利要求2所述的位姿估计方法， 其特征在于， 所述第 二步聚类分割具体包括： 第1 步， 在滤波后得到的场景区域 点云C中随机 选取3个点并计算对应的平面模型参数； 第2步， 计算余下数据点到所估计平面的距离， 距离小于所给定的阈值则将其划分到该 平面的内点 集合内， 统计集 合内点的数量； 第3步， 重新随机选择三个数据点进行迭代计算， 直到迭代次数达到给定阈值， 停止迭 代； 选取迭代过程中估计的平面模型内点数据最多的一组作为最终所估计的平面， 根据索 引在区域点云C中删除， 得到剔除背景平面后的区域点云D； 在D中随机选取一点p搜索其k个 最近邻， 若p点到近邻点距离小于给定阈值， 则将近邻点加入到聚类集合E中， 聚类集合E的 数量不再增加， 则选取E中除p以外的点重复前面的流程， 直到选取集合内任意一点进行聚 类点数仍 不再增加， 则完成聚类得到 了分割的目标物体点云。权　利　要　求　书 1/4 页 2 CN 114743259 A 24.如权利要求1所述的位姿估计方法， 其特征在于， 所述对分割的目标物体以及目标物 体模板进行关键点 提取包括： 1)计算物体点云法线： 利用主成分分析(PCA)法进行法线计算， 先构建KdTree搜索结 构， 对任意一点p搜索k个最近邻并构造协方差矩阵S： 式中S为点 云中任意一点p的协方差矩阵， pi为点云中任意一点p的第i(1≤i≤k)个近邻 点， 为p点对应的k个邻域 点坐标的均值， 计算方法为： 对于计算的协方差矩阵S， 进行奇异值分解， 求得最小特征值对应的特征向量， 所述特 征向量为p点法线； 2)统计邻域夹角与邻域点个数： 对点云中任意一点p， 选取合适的半径r， 统计r内邻域 点个数x以及 p点与邻域 点法线夹角之和 θs： 式中fp为p点法线， fpi为邻域点pi法线； 3)设置阈值筛选关键点： 设定夹角阈值τθ1与邻域数阈值τx， 当任意一点满足如下条件 时， 则判定为关键点： 其中邻域数阈值取点云中所有点r半径内邻域数均值的 倍。 5.如权利要求1所述的位姿估计方法， 其特征在于， 所述完成场景目标物体与目标物体 模板之间的粗配准与初始位姿估计包括： (i)计算分割的物体点云与模板点云在关键点处的FPFH特征， 在模板点云关键点中搜 索与目标物体相似的关键点， 形成对应点对， 对应点的建立以FPFH的相似性 为标准； (ii)随机选取n组对应点对， 任意对应点对之间的距离应大于给定阈值 τd1； (iii)根据n组对应点对计算两片点云的刚体变换矩阵， 将矩阵应用于物体点云并计算 剩余对应点对之间的距离误差， 距离误差小于阈值 τd2则该点为样本内点并统计内点数量； (iv)重复上述步骤直到最大迭代次数， 将样本内点数最多的刚体变换矩阵作为初始位 姿估计结果Trough， 并将该结果应用于目标物体点云。 6.如权利要求1所述的位姿估计方法， 其特征在于， 所述对场景中目标物体初始位姿估 计结果进行修 正包括： (1)初始对应点对估计， 在源点云与目标点云中构造对应点对， 并形成对应点对集合， 满足对应点对的初始要求 为欧氏距离最短：权　利　要　求　书 2/4 页 3 CN 114743259 A 3