一种基于改进ICP的海底局部环境初步构建方法技术

本发明专利技术提供一种基于改进ICP的海底局部环境初步构建方法，包括：特征点提取：对ISS算法进行改进，使该算法忽略尺度信息带来的影响，由此在点云数据中提取特征点；特征点描述：对FPFH算法进行改进，减弱该算法对于法向量的依赖性，由此对特征点进行描述；特征点匹配：利用RANSAC算法实现两帧点云特征点的匹配，初步估计出两帧点云数据的位姿关系；点云数据匹配及局部环境构建：利用改进的ICP方法，进一步优化当前两帧点云数据的匹配效果。本发明专利技术对ISS算法所进行的改进，可使该算法在一定程度上忽略点云数据尺度信息带来的影响。点云数据尺度信息带来的影响。点云数据尺度信息带来的影响。

【技术实现步骤摘要】
一种基于改进ICP的海底局部环境初步构建方法


[0001]本专利技术属于同时定位与构图
，具体涉及一种基于改进ICP的海底局部环境初步构建方法。

技术介绍

[0002]随着三维实时成像声呐的出现，使得水下实时稠密点云数据的直接获取成为可能，这在一方面可加快水下SLAM技术的发展。在经典的水下环境建图方法中，一般都是结合运载体的导航系统来确定所采集数据的空间信息的，而如果运载体的导航系统无法正常工作时，那么水下环境构建基本是无法完成的。为了能在运载体的导航系统无法正常工作时仍然能进行局部的环境构建，故设计了一种仅基于点云数据的局部环境初步构建方法，该方法对经典的ISS(Intrinsic Shape Signatures)、FPFH(Fast Point Feature Histograms)和RANSAC(Random Sample Consensus)算法都进行了优化。

技术实现思路

[0003]本专利技术的目的是为了提供一种短时间内不需要依赖惯导导航系统的基于改进ICP的局部环境初步构建方法。
[0004]本专利技术的目的是这样实现的：包括如下步骤：
[0005]步骤1：特征点提取：对ISS算法进行改进，使该算法忽略尺度信息带来的影响，由此在点云数据中提取特征点；
[0006]步骤2：特征点描述：对FPFH算法进行改进，减弱该算法对于法向量的依赖性，由此对特征点进行描述；
[0007]步骤3：特征点匹配：利用RANSAC算法实现两帧点云特征点的匹配，初步估计出两帧点云数据的位姿关系；
[0008]步骤4：点云数据匹配及局部环境构建：利用改进的ICP方法，进一步优化当前两帧点云数据的匹配效果。
[0009]本专利技术还包括：
[0010]1.步骤1中特征点提取具体为：
[0011]设k时刻采集到的一帧三维点云数据为其中p
ki
＝{(x
ki
,y
ki
,z
ki
)|1≤i≤n
k
}，表示P
k
中的第i个点在空间中的坐标位置；
[0012]在经典的ISS算法中，对于p
ki
，在计算协方差矩阵Cov(p
ki
)前，设定给定参数r；
[0013]计算出p
ki
的邻域其中P
ki
为以p
ki
为半径，以r为半径空间内点的集合，r为给定值；
[0014]对邻域(||p
ki
‑
p
j
||2＜r)内的每个点p
ki,l
，令Δp
ki,l
＝p
ki,l
‑
p
ki
，由此得到新集和
[0015]在ΔP
ki
的所有元素的各个维度上选择绝对值最大的一个值，令该值为r
max
；
[0016]令利用P
ki,nor
进行ISS算法后续的计算，判断p
ki
是否为特征点；
[0017]设从P
k
中提取得到的特征点为n
fk
表示特征点的数量。
[0018]2.步骤2中特征点描述具体为：
[0019]对于F
k
中的第i个特征点f
i
，计算SPFH(f
i
)时，所使用的参考单位向量为而不直接使用f
i
处的法向量
[0020]设以f
i
为圆心，以r
FPFH
为半径的空间内点的集合(不包含f
i
)为)为为中元素数量；
[0021]f
i
距离中所有点距离的最小距离为
[0022]则有其中K
FPFH
为给定值；
[0023]令描述子χ
i
＝FPFH
′
(f
i
)，则有描述子点集
[0024]3.参考单位向量的具体计算方法为：
[0025]中每个点对应法向量所组成的集合为
[0026]则有其中K
SPFH
是给定参数；
[0027]对单位化，即可得到
[0028]4.步骤3中特征点匹配具体为：
[0029]k和k+1时刻所得到的点云数据为P
k
和P
k+1
，描述子点集为X
k
、X
k+1
；
[0030]对X
k
和X
k+1
中的描述子进行配对操作，去除那些空间中相差较远的特征点配对结果，得到的配对结果记为
[0031]其中，对于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
；
[0032]计算P
k
中每个点到其最近邻居点(除该点自身外)的距离，则有距离集合：
[0033]计算D
k,d
中距离的平均值d
k,mean
；
[0034]给定距离尺度系数K
scale
的值，并确定d
inlier
为：d
inlier
＝K
scale
*d
mean
；
[0035]利用RANSAC来估计P
k
和P
k+1
间的位姿关系，且使用d
inlier
来判断P
k
和P
k+1
中配对成功的点数，即可得到旋转矩阵R
k
→
k+1
和平移向量t
k
→
k+1
。
[0036]5.步骤4中点云数据匹配及局部环境构建为：
[0037]利用初始估计的位姿R
k
→
k+1
和t
k
→
k+1
对P
k
进行变换得到P
k,k
→
k+1
，即；P
k,k
→
k+1
＝R
k
→
k+1
P
k
+t
k
→
k+1
；
[0038]从P
k,k
→
k+1
中选出位于曲率较低处的点；
[0039]使用非极大值抑制的方法使选取的点分布均匀；
[0040]得到的标准点集记为其中n
p,k
表示中点的数量；...

【技术保护点】

【技术特征摘要】
1.一种基于改进ICP的海底局部环境初步构建方法，其特征在于，步骤如下：步骤1：特征点提取：对ISS算法进行改进，使该算法忽略尺度信息带来的影响，由此在点云数据中提取特征点；步骤2：特征点描述：对FPFH算法进行改进，减弱该算法对于法向量的依赖性，由此对特征点进行描述；步骤3：特征点匹配：利用RANSAC算法实现两帧点云特征点的匹配，初步估计出两帧点云数据的位姿关系；步骤4：点云数据匹配及局部环境构建：利用改进的ICP方法，进一步优化当前两帧点云数据的匹配效果。2.根据权利要求1所述的一种基于改进ICP的海底局部环境初步构建方法，其特征在于，步骤1中特征点提取具体为：设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
||2＜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
表示特征点的数量。3.根据权利要求1所述的一种基于改进ICP的海底局部环境初步构建方法，其特征在于，步骤2中特征点描述具体为：对于F
k
中的第i个特征点f
i
，计算SPFH(f
i
)时，所使用的参考单位向量为而不直接使用f
i
处的法向量设以f
i
为圆心，以r
FPFH
为半径的空间内点的集合为集合为为中元素数量；f
i
距离中所有点距离的最小距离为则有其中K
FPFH
为给定值；令描述子χ
i
＝FPFH
′
(f
i
)，则有描述子点集4.根据权利要求3所述的一种基于改进ICP的海底局部环境初步构建方法，其特征在于，参考单位向量的具体计算方法为：中每个点对应法向量所组成的集合为则有其中K
SPFH
是给定参数；对单位化，即可得到5.根据权利要求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...

【专利技术属性】
技术研发人员：杜雪，闫泽博，田鸿宇，张伟，张勋，孙岩，郑岩，李娟，
申请(专利权)人：哈尔滨工程大学，
类型：发明
国别省市：