遗传算法结合自适应阈值约束的ICP位姿定位技术制造技术

一种遗传算法结合自适应阈值约束优化的ICP位姿估计方法，可应用于箱体混乱工件的快速六自由度位姿定位。采用统计学滤波和区域增长分割算法对原始点云进行预处理，去除离群点并得到各混乱工件的点云集；利用遗传算法得到目标点集相对参考点云初始位姿的全局最优解，减少ICP算法迭代的次数；采用自适应阈值约束剔除局部大变形点，利用欧氏距离约束剔除大部分变形点，利用法向量夹角阈值进一步剔除满足距离条件但不满足夹角条件的误匹配点对，保证算法实时性的同时提高位姿定位的精度。

Genetic algorithm combined with adaptive threshold constraint ICP position and orientation technology

A ICP pose estimation method based on genetic algorithm combined with adaptive threshold constraint optimization can be applied to the fast six degree of freedom pose localization of the chaotic workpiece. Using statistical filtering and region growing segmentation of the original point cloud pre-processing, remove the outliers and the confusion of workpiece point gathered; using genetic algorithm to get the global optimal point set relative to the reference point cloud initial pose solution, reduce the number of iterative ICP algorithm; adaptive threshold constraint to eliminate local large deformation. By using the Euclidean distance constraints removed most of the deformation point using the method of vector angle threshold to eliminate the distance but does not meet the conditions to meet the conditions of the angle error of the matching points and ensure real-time and improve the positioning accuracy of the pose.

【技术实现步骤摘要】

本专利技术涉及一种机器视觉测量技术，具体是指基于编码结构光得到的点云图像，利用点云处理技术计算混乱工件中目标工件点云的六自由度位姿信息。本专利技术涉及一种ICP算法。
技术介绍
从一个随机堆放或未经整理的箱体中以一个正确的姿态抓取目标，这种能力被叫做“随机箱体抓取”(RandomBinPicking，RBP)。1986年，MIT教授BertholdK.P.Horn在《机器人视觉》一书中指出，推广工业机器人应用的障碍之一就是缺乏处理未精确定位工件的能力，一个有效的方法就是将一个模板应用到场景图像中。迭代最近点算法(IterativeClosestPoint，ICP)利用最小二乘法计算一个点云到另一个点云的位姿变换，该方法匹配精度高，但易收敛于局部最优点且收敛速度慢。针对ICP算法易收敛于局部最优的缺点，AndrewRPoirrette等采用遗传算法(GA)计算两幅点云的初始位姿关系以保证匹配结果为全局最优，获得较好的收敛性。钟莹采用主成分分析法(PCA)使点云的三个主方向重合，快速得到粗略配准的点云，该算法设计简洁、响应快速，但要求点云之间存在足够大的重叠区域。钱鹏鹏等提出了一种结合曲率的RANSAC点云初始配准方法，此方法能显著提高初始拼接的精度，但匹配速度满足不了工业抓取的实时性。ChoiC等提出了一种基于投票的位姿粗略估计算法，定位精度达0.3mm，但需要足够多的运算次数。针对ICP算法收敛速度慢的缺点，一般设置距离阈值来剔除错误对应点对。侯东兴等提出了点对坐标约束法和自适应阈值法来剔除错误的同名点对，提高了收敛速度。徐万鑫等利用最长轴的KD树搜索方法，结合曲率特征快速搜索同名点，在不改变准确度的情况下提高了配准速度。SharpGC等提出一种结合恒定特征的迭代最近点算法(ICPIF)，使用被测物体的欧氏空间不变量如曲率、矩不变量、球谐函数不变量等确定对应点对，提高对应点对匹配的正确率，但该方法对噪声敏感。BaeKH等提出一种结合基元、近邻搜索和位置不确定性的迭代最近点算法(GP-ICPR)，该算法利用点的曲率变化与其对应点法向量夹角确定对应点，匹配的欧氏距离平均误差为0.25mm，但该方法需要手动校准初始位姿。张蕾等提出一种距离约束改进的迭代邻近点算法，采用最近原则排除含相同点的点对，并利用点对距离约束排除误配准点，相对于传统ICP，该方法显著提高配准的速度和精度。除ICP算法及其改进算法外，还有其他的位姿计算方法，主要是对点云特征进行提取，然后利用基于投票或者图理论的方法进行位姿计算。基于特征提取的位姿计算方法，只能应用于具有明显几何特征的点云，适用范围受限。本专利技术提出的遗传算法结合自适应阈值约束的ICP优化算法既克服了ICP算法易陷于局部最优的缺点，又提高了算法的收敛速度，从而满足位姿计算的速度和精度。
技术实现思路
本专利技术的目的是提供一种遗传算法结合自适应阈值约束的ICP位姿计算方法，利用遗传算法计算点云初始位姿，抑制ICP算法陷于局部最优解，利用欧氏距离和法向量夹角的自适应阈值方法，提高ICP算法的收敛速度。为达到此目的，本专利技术的技术方案如下：(1)采用基于统计学的滤波方法去除离群点噪声。(2)利用区域增长法对点云进行分割，得到各工件点云集，便于一对一地计算混乱工件位姿。(3)采用最小立方体包围盒的方法对点集进行筛选，去除非目标点集。(4)采用遗传算法计算目标点集相对于参考点云的初始位姿，得到全局最优的粗匹配。(5)采用阈值约束优化的ICP算法精确计算点集位姿，设置初始欧氏距离和法向量夹角阈值，根据阈值滤除点群中的局部大变形点，自动更新阈值大小并迭代，直到距离误差满足精度条件。(6)结合遗传算法的初始解和自适应阈值优化ICP的精确解，计算得到目标点集相对于参考点云的位姿变化。本专利技术提供了一种遗传算法结合自适应阈值约束优化的ICP位姿估计方法。该方法采用统计学滤波和区域增长分割算法对原始点云进行预处理，去除离群点并得到各混乱工件的点云集；针对ICP易限于局部最优的问题，利用遗传优化算法对点云进行粗匹配，得到目标点集相对于参考点云的初始位姿；针对迭代速度较慢的缺点，提出了一种自适应阈值约束法，利用点对距离约束和法向量夹角约束去除局部大变形点，在保证实时性的同时提高了位姿估计的精度。附图说明图1是本专利技术的整体算法流程图。图2是遗传算法优化的ICP算法流程图。图3是自适应阈值约束算法流程图。图4是自适应阈值效果示意图。具体实施方式为使本专利技术的目的、技术方案和优点更加清楚明白，下面结合具体实施例，并参照附图，对本专利技术作进一步详细说明。本专利技术通过对初始点云进行预处理，得到没有噪声的单工件点云，从而对混乱箱体工件进行快速位姿定位，整个算法流程主要由点云去噪、点云分割、点云筛选、位姿粗匹配、位姿精确定位等构成。进一步的，具体实现步骤为：(1)采用基于统计学的滤波方法去除离群点噪声。统计学滤波公式为：其中，p为需要滤除的离群点，pi表示第i个符合条件的离群点，表示目标点集中第i个点与第j个其他点的欧氏距离，μ表示所有距离的均值，σ表示距离的方差，dthresh为大于0的实数。(2)利用区域增长法对点云进行分割，得到各工件点云集，便于一对一地计算混乱工件位姿。该方法利用点及其邻域特征，利用点法向量夹角作为区域的评判，利用曲率作为种子的评判。首先，计算所有点的曲率，将曲率最小的点作为区域生长起点(种子点)；搜索种子点的邻近点，对于每一个邻近点，计算其法向量与当前种子点法向量的夹角，若该夹角小于阈值θ1，则将该邻近点归于当前区域，否则舍弃；计算当前区域剩余邻近点的曲率，若曲率小于阈值θ2，则将该邻近点添加到种子群中，并从种子群中移除当前种子；选择下一个种子点，重复上述步骤。当种子点群为空时，得到按法向量特征得到的点集。(3)采用最小立方体包围盒的方法对点集进行筛选，去除非目标点集。首先，利用点数阈值去除背景点集和部分遮挡严重的工件点集；求解点集的特征向量，将所有点分别映射到三个特征向量上，求得各点在特征向量上的投影；用投影的最大值减去最小值，得到三个特征方向上的长度，利用三个主方向上的长度阈值选择目标工件。(4)采用遗传算法计算目标点集相对于参考点云的初始位姿，得到全局最优的粗匹配。针对目标点集和参考点云的位姿计算问题，可以将其解分为平移和旋转，由于该算法用于点云的粗匹配，因此平移解可利用两片点云质心的相对位置关系求解，即计算目标点集的形心到参考点云形心的相对位置变化，这样只需要优化三个旋转角变量α，β，γ。α，β，γ的定义域为(0，2π)，取间隔为π/60，则旋转角定义域为可转化为{1，2，…，120本文档来自技高网...

【技术保护点】
一种遗传算法结合自适应阈值约束优化的ICP位姿估计方法，其特征在于，包含如下步骤：(1)采用基于统计学的滤波方法去除离群点噪声；(2)利用区域增长法对点云进行分割，得到各工件点云集，便于一对一地计算混乱工件位姿；(3)采用最小立方体包围盒的方法对点集进行筛选，去除非目标点集；(4)采用遗传算法计算目标点集相对于参考点云的初始位姿，得到全局最优的粗匹配；(5)采用阈值约束优化的ICP算法精确计算点集位姿，设置初始欧氏距离和法向量夹角阈值，根据阈值滤除点群中的局部大变形点，自动更新阈值大小并迭代，直到距离误差满足精度条件；(6)结合遗传算法的初始解和自适应阈值优化ICP的精确解，计算得到目标点集相对于参考点云的位姿变化。

【技术特征摘要】
1.一种遗传算法结合自适应阈值约束优化的ICP位姿估计方法，其特征在于，包含如下步骤：(1)采用基于统计学的滤波方法去除离群点噪声；(2)利用区域增长法对点云进行分割，得到各工件点云集，便于一对一地计算混乱工件位姿；(3)采用最小立方体包围盒的方法对点集进行筛选，去除非目标点集；(4)采用遗传算法计算目标点集相对于参考点云的初始位姿，得到全局最优的粗匹配；(5)采用阈值约束优化的ICP算法精确计算点集位姿，设置初始欧氏距离和法向量夹角阈值，根据阈值滤除点群中的局部大变形点，自动更新阈值大小并迭代，直到距离...

【专利技术属性】
技术研发人员：白瑞林，石爱军，田青华，
申请(专利权)人：江南大学，
类型：发明
国别省市：江苏;32