买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：上海库灵科技有限公司

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

主权项：1.一种基于FPFH和ICP改进算法的无序抓取姿态估计方法，其特征在于，包括如下步骤：S100、获取工件表面点云数据并进行预处理；以及，S200、采用FPFH物体识别和三维姿态估计算法对预处理后的点云数据进行处理，获得粗匹配姿态变换矩阵；以及，S300、采用ICP迭代最近点改进算法，对模型点云在场景点云中的最近点进行精确姿态估计，获取姿态变换矩阵；以及，S400、根据所述粗匹配姿态变换矩阵和所述姿态变换矩阵，计算获取最终姿态变换矩阵，并对所述最终姿态变换矩阵进行转换处理，得到最终姿态估计结果；在步骤S200中，所述采用FPFH物体识别和三维姿态估计算法对预处理后的点云数据进行处理，获得粗匹配姿态变换矩阵，包括：S210、在场景点云和模型点云上分别进行多线程法线估计，并通过下采样分别提取点云关键点；以及，S220、在场景点云和模型点云上，分别构建局部参考系，在对应局部参考系下，计算所述点云关键点在特征计算半径内的拓扑特征，并将所述拓扑特征保存在直方图中；以及，S230、通过霍夫变换进行物体识别，确定正确的特征匹配点对，根据所述特征匹配点对，求解场景点云和模型点云之间的位姿变换关系，并将变换关系应用于模型点云，使模型点云向场景点云靠拢，构建并获得粗匹配姿态变换矩阵；在步骤S300，所述采用ICP迭代最近点改进算法，对模型点云在场景点云中的最近点进行精确姿态估计，获取姿态变换矩阵，包括：S310、搜索所述模型点云在所述场景点云中的最近点并计算对应关系，根据所述对应关系构建协方差矩阵H；以及，S320、根据构建的所述协方差矩阵H以及引入的ICP改进目标函数，求解使目标函数最小化的旋转矩阵R和平移向量；以及，S330、将所述旋转矩阵R和平移向量应用于所述模型点云，并返回搜索模型点云在场景点云中的最近点，根据预设差值阈值和预设终止条件进行迭代，得到姿态变换矩阵；在步骤S400中，所述根据所述粗匹配姿态变换矩阵和所述姿态变换矩阵，计算获取最终姿态变换矩阵，并对所述最终姿态变换矩阵进行转换处理，得到最终姿态估计结果，包括：S410、获取所述粗匹配姿态变换矩阵和所述姿态变换矩阵；以及，S420、将得到的所述姿态变换矩阵与所述姿态变换矩阵相乘，计算得到最终姿态变换矩阵；以及，S430、将得到的所述最终姿态变换矩阵转换成欧拉角和平移向量的形式，并根据转换结果展示最终姿态估计结果。

全文数据：

权利要求：

百度查询： 上海库灵科技有限公司 基于FPFH和ICP改进算法的无序抓取姿态估计方法