CN106600639A - 遗传算法结合自适应阈值约束的icp位姿定位技术 - Google Patents

Abstract

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

Description

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

技术领域

本发明涉及一种机器视觉测量技术，具体是指基于编码结构光得到的点云图像，利用点云处理技术计算混乱工件中目标工件点云的六自由度位姿信息。本发明涉及一种ICP算法。

背景技术

从一个随机堆放或未经整理的箱体中以一个正确的姿态抓取目标，这种能力被叫做“随机箱体抓取”(Random Bin Picking，RBP)。1986年，MIT教授Berthold K.P.Horn在《机器人视觉》一书中指出，推广工业机器人应用的障碍之一就是缺乏处理未精确定位工件的能力，一个有效的方法就是将一个模板应用到场景图像中。迭代最近点算法(IterativeClosest Point，ICP)利用最小二乘法计算一个点云到另一个点云的位姿变换，该方法匹配精度高，但易收敛于局部最优点且收敛速度慢。

针对ICP算法易收敛于局部最优的缺点，Andrew R Poirrette等采用遗传算法(GA)计算两幅点云的初始位姿关系以保证匹配结果为全局最优，获得较好的收敛性。钟莹采用主成分分析法(PCA)使点云的三个主方向重合，快速得到粗略配准的点云，该算法设计简洁、响应快速，但要求点云之间存在足够大的重叠区域。钱鹏鹏等提出了一种结合曲率的RANSAC点云初始配准方法，此方法能显著提高初始拼接的精度，但匹配速度满足不了工业抓取的实时性。ChoiC等提出了一种基于投票的位姿粗略估计算法，定位精度达0.3mm，但需要足够多的运算次数。针对ICP算法收敛速度慢的缺点，一般设置距离阈值来剔除错误对应点对。侯东兴等提出了点对坐标约束法和自适应阈值法来剔除错误的同名点对，提高了收敛速度。徐万鑫等利用最长轴的KD树搜索方法，结合曲率特征快速搜索同名点，在不改变准确度的情况下提高了配准速度。Sharp G C等提出一种结合恒定特征的迭代最近点算法(ICPIF)，使用被测物体的欧氏空间不变量如曲率、矩不变量、球谐函数不变量等确定对应点对，提高对应点对匹配的正确率，但该方法对噪声敏感。Bae K H等提出一种结合基元、近邻搜索和位置不确定性的迭代最近点算法(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为需要滤除的离群点，p_i表示第i个符合条件的离群点，表示目标点集中第i个点与第j个其他点的欧氏距离，μ表示所有距离的均值，σ表示距离的方差，d_thresh为大于0的实数。

(2)利用区域增长法对点云进行分割，得到各工件点云集，便于一对一地计算混乱工件位姿。该方法利用点及其邻域特征，利用点法向量夹角作为区域的评判，利用曲率作为种子的评判。首先，计算所有点的曲率，将曲率最小的点作为区域生长起点(种子点)；搜索种子点的邻近点，对于每一个邻近点，计算其法向量与当前种子点法向量的夹角，若该夹角小于阈值θ₁，则将该邻近点归于当前区域，否则舍弃；计算当前区域剩余邻近点的曲率，若曲率小于阈值θ₂，则将该邻近点添加到种子群中，并从种子群中移除当前种子；选择下一个种子点，重复上述步骤。当种子点群为空时，得到按法向量特征得到的点集。

(3)采用最小立方体包围盒的方法对点集进行筛选，去除非目标点集。首先，利用点数阈值去除背景点集和部分遮挡严重的工件点集；求解点集的特征向量，将所有点分别映射到三个特征向量上，求得各点在特征向量上的投影；用投影的最大值减去最小值，得到三个特征方向上的长度，利用三个主方向上的长度阈值选择目标工件。

(4)采用遗传算法计算目标点集相对于参考点云的初始位姿，得到全局最优的粗匹配。

针对目标点集和参考点云的位姿计算问题，可以将其解分为平移和旋转，由于该算法用于点云的粗匹配，因此平移解可利用两片点云质心的相对位置关系求解，即计算目标点集的形心到参考点云形心的相对位置变化，这样只需要优化三个旋转角变量α，β，γ。

α，β，γ的定义域为(0，2π)，取间隔为π/60，则旋转角定义域为可转化为{1，2，…，120}，数值120对应的二进制编码为1111000，故解的编码可用21位二进制编码表示，其中前面7位表示X轴转角、中间7位表示Y轴转角、后面7位表示绕Z轴转角。

设si(i＝1，2，…，m)为问题的m个可能的解，集合S(k)＝{si|s1，s2，…，sm}为第k次操作前的种群，则遗传算法的步骤如下：

1)k＝0，初始化种群数据S(0)；

2)按照下式计算每个个体si的适应值Fi，i＝1，2，…，m：

其中，N₂为目标点集集P中点的个数，σ为阈值，||r_ij||表示目标点集P中的一点i与参考点云Q中的一点j之间的欧式距离；

3)令父代种群S(k)中的适应值较大的0.1m个体不进行相关操作，而直接选入子代种群S(k+1)，而其他u个个体由轮盘赌的方式选出，其复制比例由适应值决定，则个体允许复制的概率为

复制被选中的u个个体；

4)采用两两互相不重复交叉的方式进行交叉操作，生成u个新的个体，交叉操作采用两点交叉，交叉概率为0.7，检验个体码值是否大于1111000，如果大于则要取它与1111000的余码；

5)对选出的u个个体和生成的u个个体进行变异操作，即在父代种群的个体中随机挑选若干基因对其取反，变异概率为0.02，检验个体码值是否大于1111000，如果大于则要取它与1111000的余码；

6)在选择操作后剩余的0.9m个个体和2u个新个体中选择适应度值高的0.9m个个体，结合选择操作中保留的0.1m个个体形成新的种群S(k+1)；

7)当新种群中适应度最大值大于0.9，对该染色体进行解码，得到问题的解α，β，γ；否则，k＝k+1，返回步骤2。

利用全局优化的α，β，γ，结合形心的平移参数t_x，t_v，t_x，可以得到初始位姿变换R₀、T₀

T₀＝[t_x t_y t_x]^T

(5)采用阈值约束优化的ICP算法精确计算点集位姿，设置初始欧氏距离和法向量夹角阈值，根据阈值滤除点群中的局部大变形点，自动更新阈值大小并迭代，直到距离误差满足精度条件。

设初始目标点集P＝{P_i，i＝1，2，…，N₁}和参考点云Q＝{Q_i，i＝1，2，…，N₂}，其中，P与Q元素间不必存在一一对应关系，元素数目亦不必相同。配准过程就是求取2个坐标系间的旋转和平移变换矩阵，使得来自P和Q的同名点间距离最小。

设第k次匹配后目标点集为k＝1，2，…，第k次迭代后的目标函数为

其中，R_k为第k次旋转矩阵，T_k为第k次平移矩阵，为第k次变换前的目标点集，Q_i为参考点云。

具体实现过程如下：

1)利用遗传算法优化的初始变换R₀和T₀计算目标点集的粗匹配点云计算F₀，令k＝1，设置阈值ε＞0；

2)使用KD树搜索最近点点，得到参考点云Q_i与目标点集中距离最近的对应点对；

3)计算i＝1，2，...，N₂，表示第k次匹配的第i个匹配点对的欧式距离，得最大偏差最小偏差令

4)令j＝0，计算欧氏距离初始阈值为法向量夹角初始阈值α₀＝32°；

5)统计偏差值小于σ_kj的点数目n₁；

6)若n₁＞0.8N₂，剔除不满足条件的点，转步骤7；若n₁＜0.8N₂，令j＝j+1，转步骤5；

7)若N₂-n₁＜0.05N₂，令N₂＝n₁，将n₁个点作为新的目标点集转步骤3；否则转步骤8；

8)计算剩余n₁个点中法向量夹角小于α₀的点数目n₂；

9)若n₂＞0.9n₁，转步骤10；否则，令α₀＝α₀/2，转步骤8；

10)取欧氏距离阈值为σ_kj、法向量夹角阈值为α₀，此时无局部大变形点，令N₂＝n₂，剔除局部大变形点，更新目标点集转步骤11；

11)将目标点集和参考点云Q_i代入F_k，利用奇异值分解法使目标函数F_k最小，求出第k次迭代的旋转矩阵R_k和平移矩阵T_k；

12)计算k次迭代后产生的新的目标点集

13)计算点云间的欧式距离F_k

14)判断是否停止迭代，当|F_k-F_k-₁|＞ε时，令k＝k+1，转步骤2；当|F_k-F_k-₁|＜ε时，停止迭代，则ICP迭代次数为k次；

(6)结合遗传算法的初始解和自适应阈值优化ICP的精确解，计算得到目标点集P相对于参考点云Q的六自由度位姿M：

M＝M_kM_k-1...M₀

其中，M_k为第k次变换矩阵，M₀表示遗传优化算法得到的初始位姿变换；

计算位姿定位误差E：

。

Claims (4)

1.一种遗传算法结合自适应阈值约束优化的ICP位姿估计方法，其特征在于，包含如下步骤：

(1)采用基于统计学的滤波方法去除离群点噪声；

(2)利用区域增长法对点云进行分割，得到各工件点云集，便于一对一地计算混乱工件位姿；

(3)采用最小立方体包围盒的方法对点集进行筛选，去除非目标点集；

(4)采用遗传算法计算目标点集相对于参考点云的初始位姿，得到全局最优的粗匹配；

(5)采用阈值约束优化的ICP算法精确计算点集位姿，设置初始欧氏距离和法向量夹角阈值，根据阈值滤除点群中的局部大变形点，自动更新阈值大小并迭代，直到距离误差满足精度条件；

(6)结合遗传算法的初始解和自适应阈值优化ICP的精确解，计算得到目标点集相对于参考点云的位姿变化。

2.根据权利要求1所述的采用统计学滤波和区域增长分割算法对原始点云进行预处理，去除离群点并得到各混乱工件的点云集，其特征是：所述步骤(1)～(3)对点云图像进行预处理，包含如下步骤：

1)采用基于统计学的滤波方法去除离群点噪声，统计学滤波公式为：

$p=\{p_i\∈P_{raw}|\stackrel{\‾}{dist}(p_i,p_j)>(\mathrm{\μ}+d_{thresh}\·\mathrm{\σ})\}$

其中，p为需要滤除的离群点，p_i表示第i个符合条件的离群点，表示目标点集中第i个点与第j个其他点的欧氏距离，μ表示所有距离的均值，σ表示距离的方差，d_thresh为大于0的实数；

2)利用区域增长法对点云进行分割，得到各工件点云集，便于一对一地计算混乱工件位姿计；利用点法向量夹角作为区域的评判，利用曲率作为种子的评判；首先，计算所有点的曲率，将曲率最小的点作为区域生长起点(种子点)；搜索种子点的邻近点，对于每一个邻近点，计算其法向量与当前种子点法向量的夹角，若该夹角小于阈值θ₁，则将该邻近点归于当前区域，否则舍弃；计算当前区域剩余邻近点的曲率，若曲率小于阈值θ₂，则将该邻近点添加到种子群中，并从种子群中移除当前种子；选择下一个种子点，重复上述步骤；当种子点群为空时，得到按法向量特征得到的点集；

3)采用最小立方体包围盒的方法对点集进行筛选，去除非目标点集；首先，利用点数阈值去除背景点集和部分遮挡严重的工件点集；求解点集的特征向量，将所有点分别映射到三个特征向量上，求得各点在特征向量上的投影；用投影的最大值减去最小值，得到三个特征方向上的长度，利用三个主方向上的长度阈值选择目标工件。

3.根据权利要求1所述的采用遗传算法计算目标点集相对于参考点云的初始位姿，得到粗匹配下的全局最优解，其特征是：所述步骤(4)对目标点集进行初始位姿估计，包含如下步骤：

旋转变量α，β，γ的定义域为(0，2π)，取间隔为π/60，则旋转角定义域为可转化为{1，2，…，120}，数值120对应的二进制编码为1111000，其中前面7位表示X轴转角、中间7位表示Y轴转角、后面7位表示绕Z轴转角；

设si(i＝1,2，…，m)为问题的m个可能的解，集合S(k)＝{si|s1，s2，…，sm}为第k次操作前的种群，则遗传算法的步骤如下：

1)k＝0，初始化种群数据S(0)；

2)按照下式计算每个个体si的适应值Fi，i＝1,2，…，m：

$Fitness=\frac{{\mathrm{\Σ}}_{i=1}^{N_1}{W}_i}{N_2}$

其中，N₂为目标点集集P中点的个数，σ为阈值，||r_ij||表示目标点集P中的一点i与参考点云Q中的一点j之间的欧式距离；

3)令父代种群S(k)中的适应值较大的0.1m个体不进行相关操作，而直接选入子代种群S(k+1)，而其他u个个体由轮盘赌的方式选出，其复制比例由适应值决定，则个体允许复制的概率为

$P_i=\frac{F_i}{{\mathrm{\Σ}}_{i=1}^n{F}_i},i=1,2,\mathrm{...},0.9m$

复制被选中的u个个体；

4)采用两两互相不重复交叉的方式进行交叉操作，生成u个新的个体，交叉操作采用两点交叉，交叉概率为0.7，检验个体码值是否大于1111000，如果大于则要取它与1111000的余码；

5)对选出的u个个体和生成的u个个体进行变异操作，即在父代种群的个体中随机挑选若干基因对其取反，变异概率为0.02，检验个体码值是否大于1111000，如果大于则要取它与1111000的余码；

6)在选择操作后剩余0.9m个个体和2u个新个体中选择适应度值高的0.9m个个体，结合选择操作中保留的0.1m个个体形成新的种群S(k+1)；

7)当新种群中适应度最大值大于0.9，对该染色体进行解码，得到问题的解α，β，γ；否则，k＝k+1，返回步骤2；

利用全局优化的α，β，γ，结合形心的平移参数t_x，t_v，t_x，可以得到初始位姿变换R₀、T₀

$R_0=\left[\begin{array}{ccc}\cos \mathrm{\β}\cos \mathrm{\γ}& \cos \mathrm{\β}\sin \mathrm{\γ}& -\sin \mathrm{\β}\\ \sin \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\γ}-\cos \mathrm{\α}\sin \mathrm{\γ}& \sin \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ}+\cos \mathrm{\α}\cos \mathrm{\γ}& \sin \mathrm{\α}\cos \mathrm{\β}\\ \cos \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\γ}+\sin \mathrm{\α}\sin \mathrm{\γ}& \cos \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ}-\sin \mathrm{\α}\cos \mathrm{\γ}& \cos \mathrm{\α}\cos \mathrm{\β}\end{array}\right]$

T₀＝[t_x t_y t_x]^T。

4.根据权利要求1所述的设置初始欧氏距离和法向量夹角阈值，根据阈值滤除点群中的局部大变形点，自动更新阈值大小并迭代，直到ICP满足精度条件，其特征是：所述步骤(5)～(6)对目标点集进行精确位姿估计，包含如下步骤：

设初始目标点集P＝{P_i，i＝1,2，…，N₁}和参考点云Q＝{Q_i，i＝1,2，…，N₂}。其中，P与Q元素间不必存在一一对应关系，元素数目亦不必相同，设第k次匹配后目标点集为P_i ^k，k＝1,2，…，第k次迭代后的目标函数为

$F_k=\underset{i=1}{\overset{N_2}{\Σ}}\left|\right|R_k{P}_i^{k-1}+T_k-Q_i\left|\right|$

其中，R_k为第k次旋转矩阵，T_k为第k次平移矩阵，P_i ^k-1为第k次变换前的目标点集，Q_i为参考点云；

具体实现过程如下：

1)利用遗传算法优化的初始变换R₀和T₀计算目标点集的粗匹配点云P_i ⁰，计算F₀，令k＝1，设置阈值ε＞0；

2)使用KD树搜索最近点点，得到参考点云Q_i与目标点集P_i ^k-1中距离最近的对应点对；

3)计算 表示第k次匹配的第i个匹配点对的欧式距离，得最大偏差最小偏差令

4)令j＝0，计算欧氏距离初始阈值为法向量夹角初始阈值α₀＝32°；

5)统计偏差值小于σ_kj的点数目n₁；

6)若n₁＞0.8N₂，剔除不满足条件的点，转步骤7；若n₁＜0.8N₂，令j＝j+1，转步骤5；

7)若N₂-n₁＜0.05N₂，令N₂＝n₁，将n₁个点作为新的目标点集P_i ^k-1，转步骤3；否则转步骤8；

8)计算剩余n₁个点中法向量夹角小于α₀的点数目n₂；

9)若n₂＞0.9n₁，转步骤10；否则，令α₀＝α₀/2，转步骤8；

10)取欧氏距离阈值为σ_kj、法向量夹角阈值为α₀，此时无局部大变形点，令N₂＝n₂，剔除局部大变形点，更新目标点集p_i ^k-1，转步骤11；

11)将目标点集P_i ^k-1和参考点云Q_i代入F_k，利用奇异值分解法使目标函数F_k最小，求出第k次迭代的旋转矩阵R_k和平移矩阵T_k；

12)计算k次迭代后产生的新的目标点集P_i ^k

P_i ^k＝R_kP_i ^k-1+T_k；

13)计算点云间的欧式距离F_k

$F_k={\mathrm{\Σ}}_{i=1}^{N_2}\left|\right|P_i^k-Q_i\left|\right|;$

14)判断是否停止迭代，当|F_k-F_k-1|＞ε时，令k＝k+1，转步骤2；当|F_k-F_k-1|＜ε时，停止迭代，则ICP迭代次数为k次；

15)结合遗传算法的初始解和自适应阈值优化ICP的精确解，计算得到目标点集P相对于参考点云Q的六自由度位姿M：

M＝M_kM_k-1...M₀

$M_k=\left[\begin{array}{cc}{R}_k& T_k\\ 0& 1\end{array}\right]$

其中，M_k为第k次变换矩阵，M₀表示遗传优化算法得到的初始位姿变换；计算位姿定位误差E：

$E=\frac{1}{N_2}\·{\Σ}_{i=1}^{N_2}\left|\right|{\mathrm{MQ}}_i^0-P_i\left|\right|.$