1.一种遗传算法结合自适应阈值约束优化的ICP位姿估计方法，其特征在于，包含如下步骤：(1)采用基于统计学的滤波方法去除离群点噪声；

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

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

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

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

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

所述步骤(1)～(3)对点云图像进行预处理，包含如下步骤：

1)采用基于统计学的滤波方法去除离群点噪声，统计学滤波公式为：其中，p为需要滤除的离群点，pi表示第i个符合条件的离群点， 表示目标点集中第i个点与第j个其他点的欧氏距离，μ表示所有距离的均值，σ表示距离的方差，dthresh为大于0的实数；

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

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

所述步骤(4)对目标点集进行初始位姿估计，包含如下步骤：旋转变量α，β，γ的定义域为(0，2π)，取间隔为π/60，则旋转角定义域为可转化为{1，2，…，120}，数值120对应的二进制编码为1111000，故解的编码可用21位二进制编码表示，其中前面7位表示X轴转角、中间7位表示Y轴转角、后面7位表示绕Z轴转角；

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

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

2)按照下式计算每个个体si的适应值Fi，i＝1，2，…，m：其中， N2为目标点集集P中点的

个数，σ为阈值，||rij||表示目标点集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；

利用全局优化的α，β，γ，结合形心的平移参数tx，ty，tx，可以得到初始位姿变换R0、T0T0＝[tx ty tx]T；

所述步骤(5)～(6)对目标点集进行精确位姿估计，包含如下步骤：设初始目标点集P＝{Pi，i＝1，2，…，N1}和参考点云Q＝{Qi，i＝1，2，…，N2}，其中，P与Q元素间不必存在一一对应关系，元素数目亦不必相同，设第k次匹配后目标点集为Pik，k＝1，

2，…，第k次迭代后的目标函数为

其中，Rk为第k次旋转矩阵，Tk为第k次平移矩阵， 为第k次变换前的目标点集，Qi为参考点云；

具体实现过程如下：

1)利用遗传算法优化的初始变换R0和T0计算目标点集的粗匹配点云 计算F0，令k＝

1，设置阈值ε＞0；

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

3)计算 表示第k次匹配的第i个匹配点对的欧

式距离，得最大偏差 最小偏差 令

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

5)统计偏差值小于σkj的点数目n1；

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

7)若N2-n1＜0.05N2，令N2＝n1，将n1个点作为新的目标点集 转步骤3；否则转步骤

8；

8)计算剩余n1个点中法向量夹角小于α0的点数目n2；

9)若n2＞0.9n1，转步骤10；否则，令α0＝α0/2，转步骤8；

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

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

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

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

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

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

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

计算位姿定位误差E：