基于SHOT和ICP算法的无序抓取中的三维姿态估计方法技术

本发明专利技术涉及基于SHOT和ICP算法的无序抓取中的三维姿态估计方法，该方法用于无序抓取过程中的物体位姿估计，以SHOT粗匹配方法为ICP精配准提供良好的初始位置，SHOT算法中M矩阵的构建，特征值分解，进而构建局部参考系；计算对应参考系下的拓扑特征，并将结果保存在直方图中，再对模型和场景点云对应的特征提取结果进行霍夫变换，剔除错误点对，解算得到最终的粗配准姿态估计结果；ICP算法最近点的寻找，协方差矩阵的构建以及使目标函数最小化的旋转矩阵和平移向量的求解，解决了传统ICP应用于由于工件反光，采集到存在孔洞的点云时，配准精度下降的问题。

【技术实现步骤摘要】
基于SHOT和ICP算法的无序抓取中的三维姿态估计方法
本专利技术涉及三维姿态估计
，主要涉及一种基于SHOT和ICP算法的无序抓取中的三维姿态估计方法。
技术介绍
目前，基于图像的物体识别与姿态估计已经有很多研究成果，但是二维图像的成像过程是从三维空间映射到二维空间，这个过程会丢失大量的信息；而且一个无法忽视的事实是，最好的视觉系统应该是面向三维世界的。随着自动化程度的提高，机器人导航、工业零件检测及抓取等众多领域对计算机视觉系统的要求越来越高，因此基于二维图像的物体识别与姿态估计已经无法满足人类的要求。而目前三维点云数据的获取已经非常快捷，同时三维点云数据的采集不受光照影响，规避了二维图像遇到的光照、姿态等问题，因此基于点云数据的三维物体识别与姿态估计也引起了人们的重视。在无序抓取的过程中，我们需要寻求一种稳健、快速，并且满足一定精度的姿态估计方法。传统的三维姿态估计一般分为两个阶段：粗匹配阶段和精配准阶段。传统的物体识别和三维姿态估计粗匹配方法，如Rusu提出的点特征直方图PFH(RaduBogdanRusu,NicoBlodow,ZoltanCsabaMarton,etal.AligningPointCloudViewsusingPersistentFeatureHistograms[C]//2008IEEE/RSJInternationalConferenceonIntelligentRobotsandSystems,September22-26,2008,AcropolisConventionCenter,Nice,France.IEEE,2008.)通过计算任意两个顶点间的法向量夹角和距离等特征，映射到直方图中获取统计信息，并求解空间变换关系。该算法耗时较长，无法达到实时性要求。随后，Rusu在保留PFH特征识别能力的基础上，提出了快速点特征直方图FPFH(RaduBogdanRusu,NicoBlodow,MichaelBeetz.FastPointFeatureHistograms(FPFH)for3Dregistration[C]//RoboticsandAutomation,2009.ICRA'09.IEEEInternationalConferenceon.IEEE,2009.)，相比PFH，提高了计算效率。本专利技术针对工业实际应用环境的无序抓取提出基于SHOT和ICP算法的无序抓取中的三维姿态估计方法，解决无序抓取过程中的物体位姿估计精度及实时性问题。
技术实现思路
为了解决无序抓取过程中的物体位姿估计问题，本专利技术提供了一种抗噪声能力强，鲁棒性高，精度较高的基于SHOT和ICP算法的无序抓取中的三维姿态估计方法，用于工业现场三维物体识别和姿态估计。本专利技术解决上述技术问题采用的技术方案是：一种基于SHOT和ICP算法的无序抓取中的三维姿态估计方法，该方法用于无序抓取过程中的物体位姿估计，包括下述步骤：第一步，点云预处理：1-1获取工件所在场景的三维点云数据及同时获取相应的二维图像；1-2点云滤波：采用YOLO-V3目标检测算法，检测工件边框，映射到三维点云，滤除大部分背景；1-3点云下采样：在不影响点云表征能力的基础上，对点云进行下采样，获得场景点云；第二步，应用SHOT物体识别和姿态估计算法：2-1在场景和模型点云上，分别进行多线程法线估计，下采样提取点云关键点；2-2在场景和模型点云上，分别构建局部参考系，获取工件区别于平面的部位的尺寸，局部参考系构建半径r与该尺寸一致，计算关键点在对应局参考系下特征计算半径内的拓扑特征，并将拓扑特征保存在直方图中；2-3霍夫变换进行物体识别，将霍夫变换的区域划分间隔设定为与模型关键点提取下采样尺寸一致，确定正确的特征匹配点对，根据特征匹配点对求解场景和模型点云之间的位姿变换关系，并将变换关系应用于模型点云，使模型点云向场景点云靠拢；用4×4矩阵Ts表示粗匹配姿态变换矩阵，它的左上角的3×3矩阵为旋转矩阵，最后一列的前三个元素为平移向量，第四个元素为1，最后一行的前三个元素为零；第三步，应用ICP算法进行精确姿态估计：kdtree搜索模型点云在场景点云中的最近点，根据找到的对应关系构建协方差矩阵H；根据构建的协方差矩阵利求解使目标函数E最小化的旋转矩阵R和平移向量T；改进的ICP的目标函数E为：其中，为点对应之间距离的均值；pi表示模型点云中的一点，qi表示场景点云中的一点，qi是pi的最邻近点，N为点对的总数；将求得的使目标函数E最小化的旋转矩阵R和平移向量T应用于模型点云，返回搜索模型点云在场景点云中的最近点，进行下一次迭代；迭代的终止条件为计算前后两次迭代之间的旋转角度差值，差值小于差值阈值，则认为问题收敛，终止迭代；得到的姿态变换矩阵Ti；第四步，获得最终的姿态估计结果：将第二步得到的姿态变换矩阵Ts与第三步得到的姿态变换矩阵Ti相乘，得到最终的姿态变换矩阵；将得到的姿态变换矩阵转换成欧拉角和平移向量的形式，即为最终的姿态估计结果。在使用时，针对工业现场不同工件的不同特性，调整局部坐标系和特征描述子的计算半径，调整霍夫参数的区域划分间隔。点云滤波的具体过程是：应用步骤1-1获取的不同种类工件的多张二维图像，使用LabelImg将工件最小外接矩形标注出来；采用YOLO-V3目标检测算法，Adam优化器，训练100个Epoch；调用训练好的模型检测工件边框，利用采集到的二维图像和三维点云各点间一一对应的关系，映射到三维点云。场景点云关键点数量保持为模型点云关键点数量的3到4倍；特征计算半径设定为局部参考系构建半径的4/3。将霍夫参数空间的插值间隔设定为与模型特征点提取下采样尺寸一致，将霍夫投票阈值设定为5.0。第三步中，令则目标函数E的最小化问题转化为寻找旋转矩阵R使Σ2最小化及求解平移向量T＝qi-Rpi；引入改进的目标函数为：采用SVD、单位四元数、对偶四元数、线性最小二乘或非线性最小二乘等方法求解使Σ2最小化的旋转矩阵R，进而求解T。与现有技术相比，本专利技术的有益效果是：本专利技术以SHOT粗匹配方法为ICP精配准提供良好的初始位置，SHOT算法中M矩阵的构建，特征值分解，进而构建局部参考系；计算对应参考系下的拓扑特征，并将结果保存在直方图中，再对模型和场景点云对应的特征提取结果进行霍夫变换，剔除错误点对，解算得到最终的粗配准姿态估计结果；ICP算法最近点的寻找，协方差矩阵的构建以及使目标函数最小化的旋转矩阵和平移向量的求解；根据工业场景中由于噪声、遮挡、和反光，造成工件点云分布不均匀、存在孔洞的问题，改进了目标函数，解决了传统ICP应用于由于工件反光，采集到存在孔洞的点云时，配准精度下降的问题。SHOT算法对噪声的鲁棒性高，具有旋转不变性，识别能力强，能够为ICP算法提供稳定、良好的初始位置；ICP算法进一步提高了位姿估计的精度，使系本文档来自技高网...

【技术保护点】
1.一种基于SHOT和ICP算法的无序抓取中的三维姿态估计方法，该方法用于无序抓取过程中的物体位姿估计，包括下述步骤：/n第一步，点云预处理：/n1-1获取工件所在场景的三维点云数据及同时获取相应的二维图像；/n1-2点云滤波：采用YOLO-V3目标检测算法，检测工件边框，映射到三维点云，滤除大部分背景；/n1-3点云下采样：在不影响点云表征能力的基础上，对点云进行下采样，获得场景点云；/n第二步，应用SHOT物体识别和姿态估计算法：/n2-1在场景和模型点云上，分别进行多线程法线估计，下采样提取点云关键点；/n2-2在场景和模型点云上，分别构建局部参考系，获取工件区别于平面的部位的尺寸，局部参考系构建半径r与该尺寸一致，计算关键点在对应局参考系下特征计算半径内的拓扑特征，并将拓扑特征保存在直方图中；/n2-3霍夫变换进行物体识别，将霍夫变换的区域划分间隔设定为与模型关键点提取下采样尺寸一致，确定正确的特征匹配点对，根据特征匹配点对求解场景和模型点云之间的位姿变换关系，并将变换关系应用于模型点云，使模型点云向场景点云靠拢；用4×4矩阵T

【技术特征摘要】
1.一种基于SHOT和ICP算法的无序抓取中的三维姿态估计方法，该方法用于无序抓取过程中的物体位姿估计，包括下述步骤：
第一步，点云预处理：
1-1获取工件所在场景的三维点云数据及同时获取相应的二维图像；
1-2点云滤波：采用YOLO-V3目标检测算法，检测工件边框，映射到三维点云，滤除大部分背景；
1-3点云下采样：在不影响点云表征能力的基础上，对点云进行下采样，获得场景点云；
第二步，应用SHOT物体识别和姿态估计算法：
2-1在场景和模型点云上，分别进行多线程法线估计，下采样提取点云关键点；
2-2在场景和模型点云上，分别构建局部参考系，获取工件区别于平面的部位的尺寸，局部参考系构建半径r与该尺寸一致，计算关键点在对应局参考系下特征计算半径内的拓扑特征，并将拓扑特征保存在直方图中；
2-3霍夫变换进行物体识别，将霍夫变换的区域划分间隔设定为与模型关键点提取下采样尺寸一致，确定正确的特征匹配点对，根据特征匹配点对求解场景和模型点云之间的位姿变换关系，并将变换关系应用于模型点云，使模型点云向场景点云靠拢；用4×4矩阵Ts表示粗匹配姿态变换矩阵，它的左上角的3×3矩阵为旋转矩阵，最后一列的前三个元素为平移向量，第四个元素为1，最后一行的前三个元素为零；
第三步，应用ICP算法进行精确姿态估计：
kdtree搜索模型点云在场景点云中的最近点，根据找到的对应关系构建协方差矩阵H；根据构建的协方差矩阵利求解使目标函数E最小化的旋转矩阵R和平移向量T；
ICP的目标函数E为：



其中，为点对应之间距离的均值；pi表示模型点云中的一点，qi表示场景点云中的一点，qi是pi的最邻近点，N为点对的总数；
将求得的使目标函数E最小化的旋转矩阵R和平移向量T应用于...

【专利技术属性】
技术研发人员：刘坤，丁皓东，陈鹏，陈海永，黄迪，李龙腾，
申请(专利权)人：河北工业大学，
类型：发明
国别省市：天津;12