CN114092553A - 基于fpfh和icp改进算法的无序抓取姿态估计方法 - Google Patents

基于fpfh和icp改进算法的无序抓取姿态估计方法 Download PDF

Info

Publication number
CN114092553A
CN114092553A CN202111342724.0A CN202111342724A CN114092553A CN 114092553 A CN114092553 A CN 114092553A CN 202111342724 A CN202111342724 A CN 202111342724A CN 114092553 A CN114092553 A CN 114092553A
Authority
CN
China
Prior art keywords
point cloud
attitude
transformation matrix
icp
fpfh
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202111342724.0A
Other languages
English (en)
Inventor
王涛
孙瀚翔
徐荣来
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shanghai Kuling Technology Co ltd
Original Assignee
Shanghai Kuling Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shanghai Kuling Technology Co ltd filed Critical Shanghai Kuling Technology Co ltd
Priority to CN202111342724.0A priority Critical patent/CN114092553A/zh
Publication of CN114092553A publication Critical patent/CN114092553A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G06T7/75—Determining position or orientation of objects or cameras using feature-based methods involving models
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds

Landscapes

  • Engineering & Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Length Measuring Devices By Optical Means (AREA)
  • Image Analysis (AREA)

Abstract

本申请涉及一种基于FPFH和ICP改进算法的无序抓取姿态估计方法，能够基于FPFH和ICP算法在无序抓取中的三维姿态估计方法，用于工业现场三维物体识别和姿态估计，解决无序抓取过程中的物***姿估计精度及实时性问题，采用FPFH算法进行物体识别和三维姿态估计，具有旋转不变性并对噪声具有鲁棒性，能够很好的抑制工业现场噪声的影响。本方法采取ICP迭代最近点算法，针对工业场景中存在一定噪声、遮挡、和反光，容易造成工件点云分布不均匀、存在孔洞的问题，修改了ICP的目标函数，使配准更精确，从而获得更加准确的姿态变换矩阵。

Description

基于FPFH和ICP改进算法的无序抓取姿态估计方法
技术领域
本公开涉及三维姿态估计技术领域，尤其涉及一种基于FPFH和ICP改进算法的无序抓取姿态估计方法、装置、控制***和可读存储介质。
背景技术
目前，基于图像的物体识别与姿态估计已经有很多研究成果，但是二维图像的成像过程是从三维空间映射到二维空间，这个过程会丢失大量的信息；而且一个无法忽视的事实是，最好的视觉***应该是面向三维世界的。随着自动化程度的提高，机器人导航、工业零件检测及抓取等众多领域对计算机视觉***的要求越来越高，因此基于二维图像的物体识别与姿态估计已经无法满足人类的要求。而目前三维点云数据的获取已经非常快捷，同时三维点云数据的采集不受光照影响，规避了二维图像遇到的光照、姿态等问题，因此基于点云数据的三维物体识别与姿态估计也引起了人们的重视。
在无序抓取的过程中，需要寻求一种基于FPFH和ICP改进算法的无序抓取姿态估计方法稳健、快速，并且满足一定精度的姿态估计方法。传统的三维姿态估计一般分为两个阶段：粗匹配阶段和精配准阶段。传统的物体识别和三维姿态估计粗匹配方法，如Rusu提出的点特征直方图PFH(Radu Bogdan Rusu，Nico Blodow，Zoltan Csaba Marton，etal.Aligning Point Cloud Views using Persistent Feature Histograms[C]//2008IEEE/RSJ International Conference on Intelligent Robots and Systems，September 22-26，2008，Acropolis Convention Center，Nice，France.IEEE，2008.)，通过计算任意两个顶点间的法向量夹角和距离等特征，映射到直方图中获取统计信息，并求解空间变换关系。该算法耗时较长，无法达到实时性要求。随后，Tombari提出SHOT(TombariF，Salti S，Di Stefano L.Unique signatures of histograms for local surfacedescription[C]//European conference on computer vision.Springer，Berlin，Heidelberg，2010:356-369.)，相比PFH，虽然提高了计算效率，但是其物***姿估计精度及实时性低，不适用于现阶段工业中无序抓取的三维姿态估计。
发明内容
有鉴于此，本公开提出了一种基于FPFH和ICP改进算法的无序抓取姿态估计方法和***，解决无序抓取过程中的物***姿估计精度及实时性问题。
根据本公开的一方面，提供了一种基于FPFH和ICP改进算法的无序抓取姿态估计方法，包括如下步骤：
S100、获取工件表面点云数据并进行预处理；以及，
S200、采用FPFH物体识别和三维姿态估计算法对预处理后的点云数据进行处理，获得粗匹配姿态变换矩阵Ts；以及，
S300、采用ICP迭代最近点改进算法，对模型点云在场景点云中的最近点进行精确姿态估计，获取姿态变换矩阵Ti；以及，
S400、根据所述粗匹配姿态变换矩阵Ts和所述姿态变换矩阵Ti，计算获取最终姿态变换矩阵，并对所述最终姿态变换矩阵进行转换处理，得到最终姿态估计结果。
在一种可能的实现方式中，可选地，在步骤S200中，所述采用FPFH物体识别和三维姿态估计算法对预处理后的点云数据进行处理，获得粗匹配姿态变换矩阵Ts，包括：
S210、在场景点云和模型点云上分别进行多线程法线估计，并通过下采样分别提取点云关键点；以及，
S220、在场景点云和模型点云上，分别构建局部参考系，在对应局部参考系下，计算所述点云关键点在特征计算半径内的拓扑特征，并将所述拓扑特征保存在直方图中；以及，
S230、通过霍夫变换进行物体识别，确定正确的特征匹配点对，根据所述特征匹配点对，求解场景点云和模型点云之间的位姿变换关系，并将变换关系应用于模型点云，使模型点云向场景点云靠拢，构建并获得粗匹配姿态变换矩阵Ts。
在一种可能的实现方式中，可选地，在步骤S300，所述采用ICP迭代最近点改进算法，对模型点云在场景点云中的最近点进行精确姿态估计，获取姿态变换矩阵Ti，包括：
S310、搜索所述模型点云在所述场景点云中的最近点并计算对应关系，根据所述对应关系构建协方差矩阵H；以及，
S320、根据构建的所述协方差矩阵H以及引入的ICP改进目标函数，求解使目标函数最小化的旋转矩阵R和平移向量T；以及，
S330、将所述旋转矩阵R和平移向量T应用于所述模型点云，并返回搜索模型点云在场景点云中的最近点，根据预设差值阈值和预设终止条件进行迭代，得到姿态变换矩阵Ti。
在一种可能的实现方式中，可选地，在步骤S400中，所述根据所述粗匹配姿态变换矩阵Ts和所述姿态变换矩阵Ti，计算获取最终姿态变换矩阵，并对所述最终姿态变换矩阵进行转换处理，得到最终姿态估计结果，包括：
S410、获取所述粗匹配姿态变换矩阵Ts和所述姿态变换矩阵Ti；以及，
S420、将得到的所述姿态变换矩阵Ts与所述姿态变换矩阵Ti相乘，计算得到最终姿态变换矩阵；以及，
S430、将得到的所述最终姿态变换矩阵转换成欧拉角和平移向量的形式，并根据转换结果展示最终姿态估计结果。
根据本公开的另一方面，提供了一种实现所述基于FPFH和ICP改进算法的无序抓取姿态估计方法的装置，包括点云数据获取模块、FPFH算法处理模块、ICP算法处理模块和姿态估算模块，其中，
所述点云数据获取模块：用于获取工件表面点云数据并进行预处理；
所述FPFH算法处理模块：用于采用FPFH物体识别和三维姿态估计算法对预处理后的点云数据进行处理，获得粗匹配姿态变换矩阵Ts
所述ICP算法处理模块：用于采用ICP迭代最近点改进算法，对模型点云在场景点云中的最近点进行精确姿态估计，获取姿态变换矩阵Ti
所述姿态估算模块：用于根据所述粗匹配姿态变换矩阵Ts和所述姿态变换矩阵Ti，计算获取最终姿态变换矩阵，并对所述最终姿态变换矩阵进行转换处理，得到最终姿态估计结果。
在一种可能的实现方式中，可选地，所述FPFH算法处理模块包括：
点云关键点提取模块：用于在场景点云和模型点云上分别进行多线程法线估计，并通过下采样分别提取点云关键点；
拓扑特征计算模块：用于在场景点云和模型点云上，分别构建局部参考系，在对应局部参考系下，计算所述点云关键点在特征计算半径内的拓扑特征，并将所述拓扑特征保存在直方图中；
位姿变换模块：用于通过霍夫变换进行物体识别，确定正确的特征匹配点对，根据所述特征匹配点对，求解场景点云和模型点云之间的位姿变换关系，并将变换关系应用于模型点云，使模型点云向场景点云靠拢，构建并获得粗匹配姿态变换矩阵Ts。
在一种可能的实现方式中，可选地，所述ICP算法处理模块包括：
协方差矩阵构建模块：用于搜索所述模型点云在所述场景点云中的最近点并计算对应关系，根据所述对应关系构建协方差矩阵H；
求解计算模块：用于根据构建的所述协方差矩阵H以及引入的ICP改进目标函数，求解使目标函数最小化的旋转矩阵R和平移向量T；
迭代模块：用于将所述旋转矩阵R和平移向量T应用于所述模型点云，并返回搜索模型点云在场景点云中的最近点，根据预设差值阈值和预设终止条件进行迭代，得到姿态变换矩阵Ti。
在一种可能的实现方式中，可选地，所述姿态估算模块包括：
估算数据获取模块：用于获取所述粗匹配姿态变换矩阵Ts和所述姿态变换矩阵Ti；
计算模块：用于将得到的所述姿态变换矩阵Ts与所述姿态变换矩阵Ti相乘，计算得到最终姿态变换矩阵；
可视化模块：用于将得到的所述最终姿态变换矩阵转换成欧拉角和平移向量的形式，并根据转换结果展示最终姿态估计结果。
根据本公开的另一方面，还提供了一种控制***，包括：
处理器；
用于存储处理器可执行指令的存储器；
其中，所述处理器被配置为执行所述可执行指令时实现所述的基于FPFH和ICP改进算法的无序抓取姿态估计方法。
根据本公开的另一方面，还提供了一种非易失性计算机可读存储介质，其上存储有计算机程序指令，所述计算机程序指令被处理器执行时实现所述的基于FPFH和ICP改进算法的无序抓取姿态估计方法。
本申请的技术效果：
本发明通过获取工件表面点云数据并进行预处理；以及，采用FPFH物体识别和三维姿态估计算法对预处理后的点云数据进行处理，获得粗匹配姿态变换矩阵Ts；以及，采用ICP迭代最近点改进算法，对模型点云在场景点云中的最近点进行精确姿态估计，获取姿态变换矩阵Ti；以及，根据所述粗匹配姿态变换矩阵Ts和所述姿态变换矩阵Ti，计算获取最终姿态变换矩阵，并对所述最终姿态变换矩阵进行转换处理，得到最终姿态估计结果。能够基于FPFH和ICP算法在无序抓取中的三维姿态估计方法，用于工业现场三维物体识别和姿态估计，解决无序抓取过程中的物***姿估计精度及实时性问题，采用FPFH算法(快速点特征直方图方法，fast point feature histograms,FPFH)物体识别和三维姿态估计方法，具有旋转不变性并对噪声具有鲁棒性，能够很好的抑制工业现场噪声的影响。为了进一步提高位姿估计的精度，需要在物体识别和三维姿态估计粗配准方法后，应用精配准算法，本方法采取(ICP)迭代最近点算法，并针对工业场景中存在一定噪声、遮挡、和反光，容易造成工件点云分布不均匀、存在孔洞的问题，修改了ICP的目标函数，使配准更精确，从而获得更加准确的姿态变换矩阵。
根据下面参考附图对示例性实施例的详细说明，本公开的其它特征及方面将变得清楚。
附图说明
包含在说明书中并且构成说明书的一部分的附图与说明书一起示出了本公开的示例性实施例、特征和方面，并且用于解释本公开的原理。
图1示出为本发明基于FPFH和ICP改进算法的无序抓取姿态估计方法的实施流程示意图；
图2示出为一次识别结果可视化示意图；
图3示出为原始ICP算法和引入改进目标函数的ICP算法的位姿估计精度对比示意图；
图4示出为本发明通过FPFH算法和改进的ICP算法的位姿估计精度对比示意图。
具体实施方式
以下将参考附图详细说明本公开的各种示例性实施例、特征和方面。附图中相同的附图标记表示功能相同或相似的元件。尽管在附图中示出了实施例的各种方面，但是除非特别指出，不必按比例绘制附图。
在这里专用的词“示例性”意为“用作例子、实施例或说明性”。这里作为“示例性”所说明的任何实施例不必解释为优于或好于其它实施例。
另外，为了更好的说明本公开，在下文的具体实施方式中给出了众多的具体细节。本领域技术人员应当理解，没有某些具体细节，本公开同样可以实施。在一些实例中，对于本领域技术人员熟知的方法、手段、元件和电路未作详细描述，以便于凸显本公开的主旨。
本发明以FPFH粗匹配方法为ICP精配准提供良好的初始位置，FPFH算法中M矩阵的构建，特征值分解，进而构建局部参考系；计算对应参考系下的拓扑特征，并将结果保存在直方图中，再使用随机采样一致性算法，对模型和场景点云对应的特征提取结果剔除错误点对，解算得到最终的粗配准姿态估计结果；ICP算法最近点的寻找，协方差矩阵的构建以及使目标函数最小化的旋转矩阵和平移向量的求解；根据工业场景中由于噪声、遮挡、和反光，造成工件点云分布不均匀、存在孔洞的问题，改进了目标函数，解决了传统ICP应用于由于工件反光，采集到存在孔洞的点云时，配准精度下降的问题。FPFH算法对噪声的鲁棒性高，具有旋转不变性，识别能力强，能够为ICP算法提供稳定、良好的初始位置；ICP算法进一步提高了位姿估计的精度，使***能够满足抓取任务中的精度要求，二者相互作用提高位姿估计的精度和保证实时性。
本申请在使用时，针对工业现场不同工件的不同特性，调整局部坐标系和特征描述子的计算半径，调整霍夫参数空间的插值间隔，方法的运行速度能够满足实时性要求。
实施例1
如图1所示，根据本公开的一方面，提供了一种基于FPFH和ICP改进算法的无序抓取姿态估计方法，包括如下步骤：
S100、获取工件表面点云数据并进行预处理；
工件表面点云数据的获取方式可以不限制。
本实施例优先采用Photoneo面结构光三维相机获取工件表面点云数据(同时获取了相应的二维图像)。
获取后，因为工业现场工件表面光滑，会存在遮挡、反光等，工件点云中存在较大的孔洞，所以需要对获取的点云数据预处理，进行点云滤波。如图2所示的一次识别结果可视化示意图，待测对象为汽车发动机的某个工件，图中主视图窗口(左边的大窗口)中，白色点云即为应用最终的姿态变换矩阵将模型点云变换到场景中的效果；黑色和灰色内容为场景点云；模型视图窗口(右边的小窗口)中为显示的初始的模型点云(即没有经过姿态变换矩阵处理的点云)，工业现场工件表面光滑，会存在遮挡、反光等，工件点云中存在较大的孔洞。窗口中还包括载入模型、识别测试、无序抓取、停止及运行按钮，通过载入模型加载待测工件模型点云；识别测试，用于测试待测工件是否能够识别成功，完成三维姿态估计；无序抓取，将根据识别测试结果控制机械手进行相应的抓取动作。
使用面结构光相机采集工件二维图像(生产线上需要装配组装的每类工件都获取多张二维图像，本实施例中选择每类5000张)，并滤除大部分背景，降低背景干扰，减少运算量；在不影响点云表征能力的基础上，对点云进行下采样，进一步减少运算量。
S200、采用FPFH物体识别和三维姿态估计算法对预处理后的点云数据进行处理，获得粗匹配姿态变换矩阵Ts；
应用FPFH物体识别和三维姿态估计算法对预处理后的点云数据进行处理，在场景和模型点云(模型点云为软件模拟生成或者在场景点云将工件部分完全截选构成模型点云)上，分别进行多线程法线估计，下采样提取点云关键点，下采样尺度的选取保证算法运行时间满足实际应用要求即可。采样后，在场景和模型点云上，分别构建局部参考系，计算关键点在对应参考系下特征计算半径内的拓扑特征，并将特征保存在直方图中；最后经过霍夫变换而得到场景和模型点云之间的位姿变换关系，并将变换关系应用于模型点云，使模型点云向场景点云靠拢，用4×4矩阵表示粗匹配姿态变换矩阵Ts。
FPFH具有旋转不变性并对噪声具有鲁棒性，能够很好的抑制工业现场噪声的影响。
S300、采用ICP迭代最近点改进算法，对模型点云在场景点云中的最近点进行精确姿态估计，获取姿态变换矩阵Ti；
本实施例，采用kdtree来搜索模型点云在场景点云中的最近点，根据找到的对应关系构建协方差矩阵H，并根据构建的协方差矩阵H，求解使目标函数最小化的旋转矩阵R和平移向量T，返回搜索模型点云在场景点云中的最近点，进行下一次迭代；计算对应点之间的均方误差，若小于阈值，则迭代停止；实际上在程序实现时，迭代的终止条件为计算前后两次迭代之间的旋转角度差值，差值小于差值阈值，则认为问题收敛，终止迭代；得到的姿态变换矩阵Ti。
ICP迭代最近点改进算法，可以针对工业场景中存在一定噪声、遮挡、和反光，容易造成工件点云分布不均匀、存在孔洞的问题，修改了ICP的目标函数，使配准更精确，从而获得更加准确的姿态变换矩阵。如图3所示，为原始的ICP算法和引入了改进的目标函数的ICP算法的精度对比图，横坐标为围成孔洞的点数量，纵坐标为均方误差。图中实验是在相同工件，相同场景下，在工件点云中人为制造不同大小的孔洞，在此基础上对原始的ICP算法和改进的ICP算法，在工件点云存在不同大小孔洞时的均方误差(RMSE)进行对比，图中。可见，随着孔洞的增大，改进的ICP算法相较于原始ICP算法配准精度显著提高，更适用于工业现场，且无论是大孔洞还是小孔洞，本申请的ICP算法均方误差均小于原始的ICP算法。
S400、根据所述粗匹配姿态变换矩阵Ts和所述姿态变换矩阵Ti，计算获取最终姿态变换矩阵，并对所述最终姿态变换矩阵进行转换处理，得到最终姿态估计结果。
将得到的姿态变换矩阵Ts与姿态变换矩阵Ti相乘，得到最终的姿态变换矩阵；将得到的姿态变换矩阵转换成欧拉角和平移向量的形式，即为最终的姿态估计结果。
FPFH算法对噪声的鲁棒性高，具有旋转不变性，识别能力强；能够为ICP算法提供稳定、良好的初始位置；ICP算法进一步提高了位姿估计的精度，使***能够满足抓取任务中的精度要求。
如图4所示，为FPFH算法和改进的ICP算法的位姿估计精度对比，图中实验是在工业现场中，每次随机调整工件的位姿，采集工件点云，进行点云预处理。根据工件的特性(如存在凹陷或凸起的尺寸)，调整FPFH特征描述子的局部参考系构建半径、特征计算半径、迭代次数；将ICP算法前后两次迭代之间的旋转角度差值的阈值设为1×e-8，最大迭代次数限定为50次。在此基础上采用FPFH物体识别和初始配准算法与ICP精配准算法，估计工件位姿。进行100次独立实验，每次计算模型点云与目标点云(场景点云)之间点对应距离的平均值，以此作为度量误差的标准；计算100次实验的平均精度来观察整体效果。
如图4所示，可以看出在FPFH初配准算法提供初始位姿的情况下，ICP总能使误差减小；横坐标表示实验次数，纵坐标表示精度，单位mm。针对初始位姿误差较大的情况，ICP减小误差的效果尤为明显。FPFH初始配准的平均精度为2.59543mm，ICP精配准的平均精度(mean ICP)为2.26341mm，由于拍摄位置和角度不同，模型点云和场景点云之间往往存在较大差异，实际误差要比实验求得小一些。实验中100次实验总能使姿态估计问题收敛并达到较高的精度，证明该算法鲁棒性较强，精度较高，能够满足一般的无序抓取需求。本实施例中使用联想拯救者笔记本电脑，七代i5，2.5GHz，一次识别结果的检测时间为1200ms。
在一种可能的实现方式中，可选地，在步骤S200中，所述采用FPFH物体识别和三维姿态估计算法对预处理后的点云数据进行处理，获得粗匹配姿态变换矩阵Ts，包括：
S210、在场景点云和模型点云上分别进行多线程法线估计，并通过下采样分别提取点云关键点；
在场景和模型点云(模型点云为软件模拟生成或者在场景点云将工件部分完全截选构成模型点云)上，分别进行多线程法线估计，下采样提取点云关键点，下采样尺度的选取保证算法运行时间满足实际应用要求即可；为了提高识别成功率，使场景点云关键点数量保持为模型点云关键点数量的3到4倍。
S220、在场景点云和模型点云上，分别构建局部参考系，在对应局部参考系下，计算所述点云关键点在特征计算半径内的拓扑特征，并将所述拓扑特征保存在直方图中；
在场景和模型点云上，分别构建局部参考系，计算关键点在对应参考系下特征计算半径内的拓扑特征，并将特征保存在直方图中；局部参考系构建半径r的选取，获取工件区别于平面的部位(如凹陷、凸起、孔洞)的尺寸，将构建半径保持与之一致即可；
将特征计算半径设定为参考系构建半径的4/3；
对于每一个关键点p，及以p为球心，半径为r的球体Sp内的点q，计算：
ds＝||p-q||2
M＝0,
M＝M+(r-ds)(p-q)(p-q)T
对矩阵M做特征值分解：
M＝VDV-1,V＝[x+y+z+]
根据求得的特征向量构建局部参考系，其中V是由矩阵M的三个特征向量构成的矩阵，x+，y+，z+是取球体Sp中以点密度高的方向作为正方向的三个特征向量,分别对应着局部参考系的x，y，z轴。
在局部参考系下，在特征计算半径内，按照方位8个，俯仰2个，径向2个划分为32个区域；计算点q在局部参考系下的坐标，并计算坐标系原点到该点的向量的夹角θ，同时对处在边缘的点，对其俯仰、径向、和方位角以及到原点的欧式距离进行四线性插值，将结果保存在直方图中。
S230、通过霍夫变换进行物体识别，确定正确的特征匹配点对，根据所述特征匹配点对，求解场景点云和模型点云之间的位姿变换关系，并将变换关系应用于模型点云，使模型点云向场景点云靠拢，构建并获得粗匹配姿态变换矩阵Ts。
霍夫变换进行物体识别，确定正确的特征匹配点对，根据特征匹配点对，求解场景和模型点云之间的位姿变换关系，并将变换关系应用于模型点云，使模型点云向场景点云靠拢；用4×4矩阵Ts表示粗匹配姿态变换矩阵，它的坐上角的3×3矩阵为旋转矩阵，最后一列的前三个元素为平移向量，第四个元素为1，最后一行的前三个元素为零。将霍夫参数空间的插值间隔设定为与模型特征点提取下采样尺寸一致，将霍夫投票阈值设定为5.0。针对工业现场不同工件的不同特性，调整局部坐标系和特征计算半径，调整霍夫参数的区域划分间隔。
在一种可能的实现方式中，可选地，在步骤S300，所述采用ICP迭代最近点改进算法，对模型点云在场景点云中的最近点进行精确姿态估计，获取姿态变换矩阵Ti，包括：
S310、搜索所述模型点云在所述场景点云中的最近点并计算对应关系，根据所述对应关系构建协方差矩阵H；
利用kdtree搜索模型点云在场景点云中的最近点，根据找到的对应关系构建协方差矩阵H；
令pi表示模型点云中的一点，qi表示场景点云中的一点，qi是pi的最邻近点，则：
Figure BDA0003352660180000121
Figure BDA0003352660180000122
Figure BDA0003352660180000123
其中，
Figure BDA0003352660180000124
为模型点云中心点，
Figure BDA0003352660180000125
为模型对应的场景点云(可能是一部分点)的中心点，N为点对的总数，矩阵H为构建的协方差矩阵。
S320、根据构建的所述协方差矩阵H以及引入的ICP改进目标函数，求解使目标函数最小化的旋转矩阵R和平移向量T；
根据构建的协方差矩阵H，求解使目标函数最小化的旋转矩阵R和平移
Figure BDA0003352660180000126
令
Figure BDA0003352660180000127
则该目标函数的最小化问题可以转化为：
Figure BDA0003352660180000128
(1)寻找旋转矩阵R使Σ2最小化；
(2)则平移向量T＝qi-Rpi；
为了求解(1)，可用的方法有SVD、单位四元数、对偶四元数、线性最小二乘、非线性最小二乘，这里以SVD为例求解：
对步骤S310中构造的协方差矩阵H进行奇异值分解：
H＝UΛVT，
其中U为左奇异向量，V为右奇异向量，Λ的主对角线为奇异值，其余元素都为0。
则
R＝VUT，
再应用求解(2)：
T＝qi-Rpi，
旋转矩阵R和平移向量T即为当前迭代中使目标函数最小化的结果。
用4×4矩阵Ti表示精配准姿态变换矩阵，它的坐上角的3×3矩阵为旋转矩阵R，最后一列的前三个元素为平移向量T，第四个元素为1，最后一行的前三个元素为零。
S330、将所述旋转矩阵R和平移向量T应用于所述模型点云，并返回搜索模型点云在场景点云中的最近点，根据预设差值阈值和预设终止条件进行迭代，得到姿态变换矩阵Ti。
将求得的变换关系(即旋转矩阵R和平移矩阵T)应用于模型点云，返回搜索模型点云在场景点云中的最近点，进行下一次迭代；计算对应点之间的均方误差，若小于阈值，则迭代停止；实际上在程序实现时，迭代的终止条件为计算前后两次迭代之间的旋转角度差值，差值小于差值阈值，则认为问题收敛，终止迭代；得到的姿态变换矩阵Ti；实施例中，差值阈值取值为1×e-8。若大于差值阈值则回到步骤3-1继续迭代，直至满足终止条件。
预设差值阈值和预设终止条件皆根据实际需求自行选择设定即可。
在一种可能的实现方式中，可选地，在步骤S400中，所述根据所述粗匹配姿态变换矩阵Ts和所述姿态变换矩阵Ti，计算获取最终姿态变换矩阵，并对所述最终姿态变换矩阵进行转换处理，得到最终姿态估计结果，包括：
S410、获取所述粗匹配姿态变换矩阵Ts和所述姿态变换矩阵Ti；以及，
S420、将得到的所述姿态变换矩阵Ts与所述姿态变换矩阵Ti相乘，计算得到最终姿态变换矩阵；以及，
S430、将得到的所述最终姿态变换矩阵转换成欧拉角和平移向量的形式，并根据转换结果展示最终姿态估计结果。
将第二步得到的姿态变换矩阵Ts与第三步得到的姿态变换矩阵Ti相乘，得到最终的姿态变换矩阵；将得到的姿态变换矩阵转换成欧拉角和平移向量的形式，即为最终的姿态估计结果。
需要说明的是，尽管以kdtree作为示例介绍了如上搜索模型点云在场景点云中的最近点的搜索方法以及霍夫变换，但本领域技术人员能够理解，本公开应不限于此。事实上，用户完全可根据个人喜好和/或实际应用场景灵活设定上述方法，只要获取对应的结果即可。
这样，以FPFH粗匹配方法为ICP精配准提供良好的初始位置，FPFH算法中M矩阵的构建，特征值分解，进而构建局部参考系；计算对应参考系下的拓扑特征，并将结果保存在直方图中，再使用随机采样一致性算法，对模型和场景点云对应的特征提取结果剔除错误点对，解算得到最终的粗配准姿态估计结果；ICP算法最近点的寻找，协方差矩阵的构建以及使目标函数最小化的旋转矩阵和平移向量的求解；根据工业场景中由于噪声、遮挡、和反光，造成工件点云分布不均匀、存在孔洞的问题，改进了目标函数，解决了传统ICP应用于由于工件反光，采集到存在孔洞的点云时，配准精度下降的问题。FPFH算法对噪声的鲁棒性高，具有旋转不变性，识别能力强，能够为ICP算法提供稳定、良好的初始位置；ICP算法进一步提高了位姿估计的精度，使***能够满足抓取任务中的精度要求，二者相互作用提高位姿估计的精度和保证实时性。
实施例2
在实施例1的实施原理上，本实施例对应提供一种装置，用来实现上述方法，其中，装置的各个模块的功能和实施原理，具体参见实施例1的表述，本处不再过多赘述。
根据本公开的另一方面，提供了一种实现所述基于FPFH和ICP改进算法的无序抓取姿态估计方法的装置，包括点云数据获取模块、FPFH算法处理模块、ICP算法处理模块和姿态估算模块，其中，
所述点云数据获取模块：用于获取工件表面点云数据并进行预处理；
所述FPFH算法处理模块：用于采用FPFH物体识别和三维姿态估计算法对预处理后的点云数据进行处理，获得粗匹配姿态变换矩阵Ts
所述ICP算法处理模块：用于采用ICP迭代最近点改进算法，对模型点云在场景点云中的最近点进行精确姿态估计，获取姿态变换矩阵Ti
所述姿态估算模块：用于根据所述粗匹配姿态变换矩阵Ts和所述姿态变换矩阵Ti，计算获取最终姿态变换矩阵，并对所述最终姿态变换矩阵进行转换处理，得到最终姿态估计结果。
本实施例，点云数据获取模块优选Photoneo面结构光三维相机，利用Photoneo面结构光三维相机获取工件表面点云数据。使用面结构光相机采集工件二维图像(生产线上需要装配组装的每类工件都获取多张二维图像，本实施例中选择每类5000张)，滤除大部分背景，降低背景干扰，减少运算量；在不影响点云表征能力的基础上，对点云进行下采样，进一步减少运算量。
在一种可能的实现方式中，可选地，所述FPFH算法处理模块包括：
点云关键点提取模块：用于在场景点云和模型点云上分别进行多线程法线估计，并通过下采样分别提取点云关键点；
拓扑特征计算模块：用于在场景点云和模型点云上，分别构建局部参考系，在对应局部参考系下，计算所述点云关键点在特征计算半径内的拓扑特征，并将所述拓扑特征保存在直方图中；
位姿变换模块：用于通过霍夫变换进行物体识别，确定正确的特征匹配点对，根据所述特征匹配点对，求解场景点云和模型点云之间的位姿变换关系，并将变换关系应用于模型点云，使模型点云向场景点云靠拢，构建并获得粗匹配姿态变换矩阵Ts。
在一种可能的实现方式中，可选地，所述ICP算法处理模块包括：
协方差矩阵构建模块：用于搜索所述模型点云在所述场景点云中的最近点并计算对应关系，根据所述对应关系构建协方差矩阵H；
求解计算模块：用于根据构建的所述协方差矩阵H以及引入的ICP改进目标函数，求解使目标函数最小化的旋转矩阵R和平移向量T；
迭代模块：用于将所述旋转矩阵R和平移向量T应用于所述模型点云，并返回搜索模型点云在场景点云中的最近点，根据预设差值阈值和预设终止条件进行迭代，得到姿态变换矩阵Ti。
在一种可能的实现方式中，可选地，所述姿态估算模块包括：
估算数据获取模块：用于获取所述粗匹配姿态变换矩阵Ts和所述姿态变换矩阵Ti；
计算模块：用于将得到的所述姿态变换矩阵Ts与所述姿态变换矩阵Ti相乘，计算得到最终姿态变换矩阵；
可视化模块：用于将得到的所述最终姿态变换矩阵转换成欧拉角和平移向量的形式，并根据转换结果展示最终姿态估计结果。
显然，本领域的技术人员应该明白，上述的本发明的各模块或各步骤可以用通用的计算装置来实现，它们可以集中在单个的计算装置上，或者分布在多个计算装置所组成的网络上，可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。这样，本发明不限制于任何特定的硬件和软件结合。
实施例3
更进一步地，根据本公开的另一方面，还提供了一种控制***，包括：
处理器；
用于存储处理器可执行指令的存储器；其中，所述处理器被配置为执行所述可执行指令时实现实施例1所述的基于FPFH和ICP改进算法的无序抓取姿态估计方法。
本公开实施例控制***包括处理器以及用于存储处理器可执行指令的存储器。其中，处理器被配置为执行可执行指令时实现前面任一所述的一种基于FPFH和ICP改进算法的无序抓取姿态估计方法。
此处，应当指出的是，处理器的个数可以为一个或多个。同时，在本公开实施例的控制***中，还可以包括输入装置和输出装置。其中，处理器、存储器、输入装置和输出装置之间可以通过总线连接，也可以通过其他方式连接，此处不进行具体限定。
存储器作为一种计算机可读存储介质，可用于存储软件程序、计算机可执行程序和各种模块，如：本公开实施例的一种基于FPFH和ICP改进算法的无序抓取姿态估计方法所对应的程序或模块。处理器通过运行存储在存储器中的软件程序或模块，从而执行控制***的各种功能应用及数据处理。
输入装置可用于接收输入的数字或信号。其中，信号可以为产生与设备/终端/服务器的用户设置以及功能控制有关的键信号。输出装置可以包括显示屏等显示设备。
实施例4
根据本公开的另一方面，还提供了一种非易失性计算机可读存储介质，其上存储有计算机程序指令，所述计算机程序指令被处理器执行时实现实施例1所述的基于FPFH和ICP改进算法的无序抓取姿态估计方法。
以上已经描述了本公开的各实施例，上述说明是示例性的，并非穷尽性的，并且也不限于所披露的各实施例。在不偏离所说明的各实施例的范围和精神的情况下，对于本技术领域的普通技术人员来说许多修改和变更都是显而易见的。本文中所用术语的选择，旨在最好地解释各实施例的原理、实际应用或对市场中的技术的技术改进，或者使本技术领域的其它普通技术人员能理解本文披露的各实施例。

Claims (10)

1.一种基于FPFH和ICP改进算法的无序抓取姿态估计方法，其特征在于，包括如下步骤：
S100、获取工件表面点云数据并进行预处理；以及，
S200、采用FPFH物体识别和三维姿态估计算法对预处理后的点云数据进行处理，获得粗匹配姿态变换矩阵Ts；以及，
S300、采用ICP迭代最近点改进算法，对模型点云在场景点云中的最近点进行精确姿态估计，获取姿态变换矩阵Ti；以及，
S400、根据所述粗匹配姿态变换矩阵Ts和所述姿态变换矩阵Ti，计算获取最终姿态变换矩阵，并对所述最终姿态变换矩阵进行转换处理，得到最终姿态估计结果。
2.根据权利要求1所述的基于FPFH和ICP改进算法的无序抓取姿态估计方法，其特征在于，在步骤S200中，所述采用FPFH物体识别和三维姿态估计算法对预处理后的点云数据进行处理，获得粗匹配姿态变换矩阵Ts，包括：
S210、在场景点云和模型点云上分别进行多线程法线估计，并通过下采样分别提取点云关键点；以及，
S220、在场景点云和模型点云上，分别构建局部参考系，在对应局部参考系下，计算所述点云关键点在特征计算半径内的拓扑特征，并将所述拓扑特征保存在直方图中；以及，
S230、通过霍夫变换进行物体识别，确定正确的特征匹配点对，根据所述特征匹配点对，求解场景点云和模型点云之间的位姿变换关系，并将变换关系应用于模型点云，使模型点云向场景点云靠拢，构建并获得粗匹配姿态变换矩阵Ts。
3.根据权利要求1或2所述的基于FPFH和ICP改进算法的无序抓取姿态估计方法，其特征在于，在步骤S300，所述采用ICP迭代最近点改进算法，对模型点云在场景点云中的最近点进行精确姿态估计，获取姿态变换矩阵Ti，包括：
S310、搜索所述模型点云在所述场景点云中的最近点并计算对应关系，根据所述对应关系构建协方差矩阵H；以及，
S320、根据构建的所述协方差矩阵H以及引入的ICP改进目标函数，求解使目标函数最小化的旋转矩阵R和平移向量T；以及，
S330、将所述旋转矩阵R和平移向量T应用于所述模型点云，并返回搜索模型点云在场景点云中的最近点，根据预设差值阈值和预设终止条件进行迭代，得到姿态变换矩阵Ti。
4.根据权利要求3所述的基于FPFH和ICP改进算法的无序抓取姿态估计方法，其特征在于，在步骤S400中，所述根据所述粗匹配姿态变换矩阵Ts和所述姿态变换矩阵Ti，计算获取最终姿态变换矩阵，并对所述最终姿态变换矩阵进行转换处理，得到最终姿态估计结果，包括：
S410、获取所述粗匹配姿态变换矩阵Ts和所述姿态变换矩阵Ti；以及，
S420、将得到的所述姿态变换矩阵Ts与所述姿态变换矩阵Ti相乘，计算得到最终姿态变换矩阵；以及，
S430、将得到的所述最终姿态变换矩阵转换成欧拉角和平移向量的形式，并根据转换结果展示最终姿态估计结果。
5.一种实现权利要求1-4任一项所述基于FPFH和ICP改进算法的无序抓取姿态估计方法的装置，其特征在于，包括点云数据获取模块、FPFH算法处理模块、ICP算法处理模块和姿态估算模块，其中，
所述点云数据获取模块：用于获取工件表面点云数据并进行预处理；
所述FPFH算法处理模块：用于采用FPFH物体识别和三维姿态估计算法对预处理后的点云数据进行处理，获得粗匹配姿态变换矩阵Ts
所述ICP算法处理模块：用于采用ICP迭代最近点改进算法，对模型点云在场景点云中的最近点进行精确姿态估计，获取姿态变换矩阵Ti
所述姿态估算模块：用于根据所述粗匹配姿态变换矩阵Ts和所述姿态变换矩阵Ti，计算获取最终姿态变换矩阵，并对所述最终姿态变换矩阵进行转换处理，得到最终姿态估计结果。
6.根据权利要求5所述的装置，其特征在于，所述FPFH算法处理模块包括：
点云关键点提取模块：用于在场景点云和模型点云上分别进行多线程法线估计，并通过下采样分别提取点云关键点；
拓扑特征计算模块：用于在场景点云和模型点云上，分别构建局部参考系，在对应局部参考系下，计算所述点云关键点在特征计算半径内的拓扑特征，并将所述拓扑特征保存在直方图中；
位姿变换模块：用于通过霍夫变换进行物体识别，确定正确的特征匹配点对，根据所述特征匹配点对，求解场景点云和模型点云之间的位姿变换关系，并将变换关系应用于模型点云，使模型点云向场景点云靠拢，构建并获得粗匹配姿态变换矩阵Ts。
7.根据权利要求5或6所述的装置，其特征在于，所述ICP算法处理模块包括：
协方差矩阵构建模块：用于搜索所述模型点云在所述场景点云中的最近点并计算对应关系，根据所述对应关系构建协方差矩阵H；
求解计算模块：用于根据构建的所述协方差矩阵H以及引入的ICP改进目标函数，求解使目标函数最小化的旋转矩阵R和平移向量T；
迭代模块：用于将所述旋转矩阵R和平移向量T应用于所述模型点云，并返回搜索模型点云在场景点云中的最近点，根据预设差值阈值和预设终止条件进行迭代，得到姿态变换矩阵Ti。
8.根据权利要求7所述的装置，其特征在于，所述姿态估算模块包括：
估算数据获取模块：用于获取所述粗匹配姿态变换矩阵Ts和所述姿态变换矩阵Ti；
计算模块：用于将得到的所述姿态变换矩阵Ts与所述姿态变换矩阵Ti相乘，计算得到最终姿态变换矩阵；
可视化模块：用于将得到的所述最终姿态变换矩阵转换成欧拉角和平移向量的形式，并根据转换结果展示最终姿态估计结果。
9.一种控制***，其特征在于，包括：
处理器；
用于存储处理器可执行指令的存储器；
其中，所述处理器被配置为执行所述可执行指令时实现权利要求1至4中任意一项所述的基于FPFH和ICP改进算法的无序抓取姿态估计方法。
10.一种非易失性计算机可读存储介质，其上存储有计算机程序指令，其特征在于，所述计算机程序指令被处理器执行时实现权利要求1至4中任意一项所述的基于FPFH和ICP改进算法的无序抓取姿态估计方法。
CN202111342724.0A 2021-11-12 2021-11-12 基于fpfh和icp改进算法的无序抓取姿态估计方法 Pending CN114092553A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111342724.0A CN114092553A (zh) 2021-11-12 2021-11-12 基于fpfh和icp改进算法的无序抓取姿态估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111342724.0A CN114092553A (zh) 2021-11-12 2021-11-12 基于fpfh和icp改进算法的无序抓取姿态估计方法

Publications (1)

Publication Number Publication Date
CN114092553A true CN114092553A (zh) 2022-02-25

Family

ID=80300519

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111342724.0A Pending CN114092553A (zh) 2021-11-12 2021-11-12 基于fpfh和icp改进算法的无序抓取姿态估计方法

Country Status (1)

Country Link
CN (1) CN114092553A (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114743259A (zh) * 2022-02-28 2022-07-12 华中科技大学 位姿估计方法、位姿估计***、终端、存储介质及应用
CN114783068A (zh) * 2022-06-16 2022-07-22 深圳市信润富联数字科技有限公司 姿态识别方法、装置、电子装置和存储介质
CN117934734A (zh) * 2024-03-25 2024-04-26 北京思路智园科技有限公司 基于cv和vr的企业危险工艺动态场景模拟方法及***

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114743259A (zh) * 2022-02-28 2022-07-12 华中科技大学 位姿估计方法、位姿估计***、终端、存储介质及应用
CN114783068A (zh) * 2022-06-16 2022-07-22 深圳市信润富联数字科技有限公司 姿态识别方法、装置、电子装置和存储介质
CN117934734A (zh) * 2024-03-25 2024-04-26 北京思路智园科技有限公司 基于cv和vr的企业危险工艺动态场景模拟方法及***
CN117934734B (zh) * 2024-03-25 2024-06-07 北京思路智园科技有限公司 基于cv和vr的企业危险工艺动态场景模拟方法及***

Similar Documents

Publication Publication Date Title
CN111784770B (zh) 基于shot和icp算法的无序抓取中的三维姿态估计方法
CN114092553A (zh) 基于fpfh和icp改进算法的无序抓取姿态估计方法
CN109685848B (zh) 一种三维点云与三维传感器的神经网络坐标变换方法
CN111738261A (zh) 基于位姿估计和校正的单图像机器人无序目标抓取方法
CN113223091B (zh) 三维目标检测方法、抓取方法、装置及电子设备
CN109886124B (zh) 一种基于线束描述子图像匹配的无纹理金属零件抓取方法
CN111598946B (zh) 一种物***姿测量方法、装置及存储介质
WO2022017131A1 (zh) 点云数据的处理方法、智能行驶控制方法及装置
JP5063776B2 (ja) 幾何学変換に基づく一般化統計的テンプレートマッチング
CN114556268B (zh) 一种姿势识别方法及装置、存储介质
Kanezaki et al. High-speed 3d object recognition using additive features in a linear subspace
Pettersson et al. Online stereo calibration using FPGAs
Sun et al. Robotic grasping using semantic segmentation and primitive geometric model based 3D pose estimation
Figueiredo et al. Robust cylinder detection and pose estimation using 3D point cloud information
JP4921847B2 (ja) 対象物の三次元位置推定装置
Lee et al. Robust recognition and pose estimation of 3d objects based on evidence fusion in a sequence of images
JP6955081B2 (ja) オブジェクト姿勢を決定するための電子デバイス、システムおよび方法
Avidar et al. Local-to-global point cloud registration using a dictionary of viewpoint descriptors
Grossmann et al. Fast view-based pose estimation of industrial objects in point clouds using a particle filter with an icp-based motion model
CN117372480A (zh) 一种利用自适应下采样与曲率增强点对特征的点云配准方法
Hashimoto et al. Current status and future trends on robot vision technology
CN111986299B (zh) 点云数据处理方法、装置、设备及存储介质
Azad et al. Accurate shape-based 6-dof pose estimation of single-colored objects
Barros et al. Real-time human pose estimation from body-scanned point clouds
Ninomiya et al. Deep manifold embedding for 3D object pose estimation

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination