[发明专利]一种基于改进ICP的海底局部环境初步构建方法

权利要求书

1.一种基于改进ICP的海底局部环境初步构建方法，其特征在于，步骤如下：

步骤1：特征点提取：对ISS算法进行改进，使该算法忽略尺度信息带来的影响，由此在点云数据中提取特征点；

设k时刻采集到的一帧三维点云数据为其中p_ki＝{(x_ki,y_ki,z_ki)|1≤i≤n_k}，表示P_k中的第i个点在空间中的坐标位置；

在经典的ISS算法中，对于p_ki，在计算协方差矩阵Cov(p_ki)前，设定给定参数r；计算出p_ki的邻域其中P_ki为以p_ki为半径，以r为半径空间内点的集合，r为给定值；对邻域(||p_ki-p_j||₂＜r)内的每个点p_ki,l，令Δp_ki,l＝p_ki,l-p_ki，由此得到新集和在ΔP_ki的所有元素的各个维度上选择绝对值最大的一个值，令该值为r_max；令利用P_ki,nor进行ISS算法后续的计算，判断p_ki是否为特征点；设从P_k中提取得到的特征点为n_fk表示特征点的数量；

步骤2：特征点描述：对FPFH算法进行改进，减弱该算法对于法向量的依赖性，由此对特征点进行描述；

对于F_k中的第i个特征点f_i，计算SPFH(f_i)时，所使用的参考单位向量为而不直接使用f_i处的法向量设以f_i为圆心，以r_FPFH为半径的空间内点的集合为为中元素数量；f_i距离中所有点距离的最小距离为则有其中K_FPFH为给定值；令描述子χ_i＝FPFH′(f_i)，则有描述子点集

参考单位向量的具体计算方法为：中每个点对应法向量所组成的集合为则有其中K_SPFH是给定参数；对单位化，即可得到

步骤3：特征点匹配：利用RANSAC算法实现两帧点云特征点的匹配，初步估计出两帧点云数据的位姿关系；

步骤4：点云数据匹配及局部环境构建：利用改进的ICP方法，进一步优化当前两帧点云数据的匹配效果。

2.根据权利要求1所述的一种基于改进ICP的海底局部环境初步构建方法，其特征在于，步骤3中特征点匹配具体为：k和k+1时刻所得到的点云数据为P_k和P_k+1，描述子点集为X_k、X_k+1；对X_k和X_k+1中的描述子进行配对操作，去除那些空间中相差较远的特征点配对结果，得到的配对结果记为其中，对于C_k,k+1中的第i个元素有c_i＝{f_k,i,f_k+1,i}，f_k,i∈X_k，f_k+1,i∈X_k+1；计算P_k中每个点到其最近邻居点的距离，则有距离集合：计算D_k,d中距离的平均值d_k,mean；给定距离尺度系数K_scale的值，并确定d_inlier为：d_inlier＝K_scale*d_mean；利用RANSAC来估计P_k和P_k+1间的位姿关系，且使用d_inlier来判断P_k和P_k+1中配对成功的点数，即可得到旋转矩阵R_k→k+1和平移向量t_k→k+1。

3.根据权利要求1所述的一种基于改进ICP的海底局部环境初步构建方法，其特征在于，步骤4中点云数据匹配及局部环境构建为：

利用初始估计的位姿R_k→k+1和t_k→k+1对P_k进行变换得到P_k,k→k+1，即；P_k,k→k+1＝R_k→k+1P_k+t_k→k+1；从P_k,k→k+1中选出位于曲率较低处的点；使用非极大值抑制的方法使选取的点分布均匀；得到的标准点集记为其中n_p,k表示中点的数量；

在P_k+1中搜索距离中的每个点最近的λ个点；从这λ个点中再筛选出距离中相应点小于给定距离D₁的点，得到点集其中，l表示上述筛选操作后中包含点集的数量；

从P_k+1中选出距离点最近的λ个点，记为再从中选出距离点小于给定距离D₁的点集其中，γ_i表示中点的数量；中距离最近的一个点记为p_nst-nbor，比较p_nst-nbor与两点处的法向量，若两法向量的夹角在区间[θ_t,180°-θ_t]，则将从中舍去，其中θ_t为给定值；

计算中所有点与p_nst-nbor的距离，若存在距离大于给定距离R₁的情况，则将从中舍去；

计算中的每个点集中的点是否位于曲率较低处；若没有位于曲率较低处，则将对应的点集剔除出得到其中，l′表示中点集的数量；

对于中的每个点集，计算这些点集中的点在P_k,k→k+1中的最近邻，若最近邻没有位于平面上，或最近邻距离中相应点的距离超过给定距离R₂，则将相应点集从剔除，且R₂＞R₁；得到相应的最近邻点集记为每个点集中的点对应的法向量所组成的集合记为将中的点投影至标准点集的对应点中，得到投影点集

计算与中对应点的距离，得到误差函数error(R_k→k+1,t_k→k+1)；通过非线性优化的方法对error(R_k→k+1,t_k→k+1)进行优化，得到ΔR_k→k+1和Δt_k→k+1；利用ΔR_k→k+1与Δt_k→k+1对R_k→k+1与t_k→k+1进行更新；而后继续对新的R_k→k+1与t_k→k+1进行迭代优化，当到达迭代次数n_t时，停止优化操作，其中，n_t为给定值；通过R_k→k+1与t_k→k+1即可实现第k帧点云数据P_k与第k+1帧点云数据P_k+1间的匹配；通过不断地将各连续帧的点云数据进行匹配，即可逐步实现对海底局部环境的初步构建。