一种基于精简特征迭代最近点SF-ICP的激光里程计方法技术

本发明专利技术属于3D激光同时定位与建图SLAM技术领域，公开了一种基于精简特征迭代最近点SF

【技术实现步骤摘要】
一种基于精简特征迭代最近点SF
‑
ICP的激光里程计方法


[0001]本专利技术属于3D激光同时定位与建图(SLAM)
，特别是涉及一种基于精简特征迭代最近点SF
‑
ICP的激光里程计方法。

技术介绍

[0002]激光里程计一直以来都是3D激光SLAM邻域一项基础而重要的课题。其任务是根据当前时刻激光雷达扫描获得的点云信息和历史点云信息，对平台当前的位姿信息进行估计。激光里程计是3D激光SLAM系统实现功能最基础也是最重要的模块之一，它的输出结果是3D激光SLAM系统其他模块工作的基础，它的性能也在很大程度上决定了整个3D激光SLAM系统的性能。
[0003]然而，3D激光雷达单次扫描获取的信息量相当丰富，在室外建图的场景中，也存在着大量的轻、小、不稳定或运动中的物体，不能作为位姿估计和建立环境地图的参照物。传统的迭代最近点(ICP)算法，正态分布变换(NDT)算法等方法，在处理这些问题的时候，一方面没有很好的剔除这些无效信息的手段，另一方面，也因为参与计算的数据量过大，对算力的需求过高，很难满足移动平台对SLAM系统实时性的需求。目前对于这些问题，存在2种不同的研究方法，一种方法是通过对点云进行预处理，剔除无效特征，只保留最重要的特征，通过对特征点的精简，提高位姿估计的效率和精度，本专利技术采用这种方法；另一种方法是将传感器数据和标注好的地图进行匹配以生成数据集，通过深度学习训练一个高精度的端到端模型，之后对模型进行蒸馏以提高估计效率，这种方法目前常被应用于视觉SLAM。

技术实现思路

[0004]本专利技术目的在于解决3D激光SLAM系统传感器数据量巨大、室外建图场景复杂多变，传统的位姿估计方法无法满足精度和实时性要求的问题。提出一种基于精简特征迭代最近点(SF
‑
ICP)的激光里程计算法，它的精度和响应速度均优于传统算法，未来有希望运用于更多的安装3D激光SLAM系统的移动平台。
[0005]为了实现本专利技术目的，本专利技术公开了一种基于精简特征迭代最近点SF
‑
ICP的激光里程计方法，包括以下步骤：
[0006]步骤1、对传感器获得的原始激光点云数据进行预处理；
[0007]步骤2、将预处理完的点云进行点集聚类精简；
[0008]步骤3、将精简完成的点云进行特征提取；
[0009]步骤4、综合IMU传感器数据、历史点云和当前已提取的特征，进行点云配准运算，输出里程计估计位姿。
[0010]进一步地，步骤1具体为，将传感器返回的当前帧点云按照雷达扫描角速度和IMU记录的移动速度进行点云畸变校正；将畸变校正完的点云数据进行地面分割，剔除地面点云数据，输出预处理完的点云数据。
[0011]进一步地，步骤2具体为，将步骤1预处理完的点云进行点集聚类运算，之后将点数
低于阈值的点集作为离群点剔除，输出精简完的点云数据。
[0012]进一步地，点集聚类运算采用基于图像的分割方法，对于点云P
k
中没有分类的某个点p，从该点出发向上下左右四个方向进行广度遍历，如果遍历到的点p
’
并没有被分类，则计算雷达坐标系下原点O与p和p
’
中较远的点的连线与两点之间连线的夹角；α是激光雷达的角分辨率，如果p与p
’
两点在同一个扫描列，则使用竖直角分辨率，反之如果在同一个扫描行则使用水平角分辨率；若夹角α大于设定阈值θ
th
，则认为两点位于同一个物体上，归为一类并将p
’
点加入到广度搜索队列中，否则放弃处理该点；在对队列中的点全部搜索完成后，如果类中点的数量小于设定阈值n
th
，就说明这些点可能来自诸如树叶此类小的物体上，由于这些物体相比于大型物体连续出现在两个前后扫描帧中的可能性较小，即它们有可能会在后续的特征提取步骤中产生不可靠的特征，所以将这些类中的所有点标记为离群点以拒绝其参加后续的特征提取步骤。
[0013]进一步地，步骤3具体为，将步骤2精简完的点云作为备选特征点，通过计算备选特征点与近邻点的平滑程度，将备选特征点划分为备选边缘特征点和备选平面特征点，之后在每一扫描行内划分4个子区域，在每个子区域内挑选2个边缘特征点和4个平面特征点，最后输出边缘特征点集与平面特征点集；对于平滑程度的计算，定义了一个参量c；对于第k个扫描帧所得到的点云P
k
中的某一个点p
i
，令S表示与点p
i
位于相同扫描行且与点p
i
相邻的连续点集，点p
i
两侧取相同数量的点；则点p
i
位置的局部光滑程度c的计算公式为：
[0014][0015]其中X
(k，i)
表示点p
i
在里程计坐标系下的坐标，点p
j
为连续点集S中的点，表示点p
i
在体坐标系下的坐标，|S|表示点集S中点的数量，||
·
||表示点坐标的二范数。
[0016]进一步地，在选择特征点时需遵循下述规则：为了使得特征点能够在整个扫描平面上均匀分布，将扫描行分为四个子区域，每个子区域选出两个边缘特征点以及四个平面特征点；在同一扫描行上的点将根据计算出来的c值进行排序，具有大的c值的点会被标记为边缘特征点，而具有小的c值的点会被标记为平面特征点；需要排除周围已经存在特征点的点，使特征点均匀分布；需要排除扫描激光束平行或者被前面物体遮挡的几个点，因为它们会因为视角的变化而导致特征的剧烈变化，通过计算沿着扫描行周围点坐标的变化梯度来判断它们是否不可靠。
[0017]进一步地，步骤4具体为，通过对固定帧数的历史点云的融合，生成当前时刻的局部特征地图，之后对IMU数据进行预积分，产生一个对当前位姿的粗略估计，通过这个估计对步骤3挑选的特征点集的点云数据进行做坐标系转换，获得重映射后的当前特征点集，将重映射后的当前特征点集按照边缘特征点和平面特征点的分类，分别与局部特征地图，进行点到边迭代最近点算法和点到面迭代最近点算法配准，最后综合配准结果，生成里程计的位姿估计结果；这里定义第k个扫描帧中获得的点云P
k
，其提取得到的边缘特征点集为E
k
，平面特征点集为H
k
。
[0018]进一步地，在步骤4过程中，采用单帧扫描与局部地图相匹配的特征匹配方法，保持长度为n的滑动时间窗口的特征点云数据，当需要组建第k帧的局部地图M
k
时，将时间窗口内的所有点云进行合并：
[0019][0020][0021]其中M
(k，E)
与M
(k，H)
分别局部边缘特征地图以及局部平面特征地图，与分别是第i帧投影到里程计坐标系下的边缘特征点集以及平面特征点集；
[0022]进一步地，在步骤4过程中，对IMU数据进行预积分以对当前位姿进行本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于精简特征迭代最近点SF
‑
ICP的激光里程计方法，其特征在于，包括以下步骤：步骤1、对传感器获得的原始激光点云数据进行预处理；步骤2、将预处理完的点云进行点集聚类精简；步骤3、将精简完成的点云进行特征提取；步骤4、综合IMU传感器数据、历史点云和当前已提取的特征，进行点云配准运算，输出里程计估计位姿。2.根据权利要求1所述的一种基于精简特征迭代最近点SF
‑
ICP的激光里程计方法，其特征在于，步骤1具体为，将传感器返回的当前帧点云按照雷达扫描角速度和IMU记录的移动速度进行点云畸变校正；将畸变校正完的点云数据进行地面分割，剔除地面点云数据，输出预处理完的点云数据。3.根据权利要求1所述的一种基于精简特征迭代最近点SF
‑
ICP的激光里程计方法，其特征在于，步骤2具体为，将步骤1预处理完的点云进行点集聚类运算，之后将点数低于阈值的点集作为离群点剔除，输出精简完的点云数据。4.根据权利要求3所述的一种基于精简特征迭代最近点SF
‑
ICP的激光里程计方法，其特征在于，点集聚类运算采用基于图像的分割方法，对于点云P
k
中没有分类的某个点p，从该点出发向上下左右四个方向进行广度遍历，如果遍历到的点p
’
并没有被分类，则计算雷达坐标系下原点O与p和p
’
中较远的点的连线与两点之间连线的夹角；α是激光雷达的角分辨率，如果p与p
’
两点在同一个扫描列，则使用竖直角分辨率，反之如果在同一个扫描行则使用水平角分辨率；若夹角α大于设定阈值θ
th
，则认为两点位于同一个物体上，归为一类并将p
’
点加入到广度搜索队列中，否则放弃处理该点；在对队列中的点全部搜索完成后，如果类中点的数量小于设定阈值n
th
，将这些类中的所有点标记为离群点以拒绝其参加后续的特征提取步骤。5.根据权利要求1所述的一种基于精简特征迭代最近点SF
‑
ICP的激光里程计方法，其特征在于，步骤3具体为，将步骤2精简完的点云作为备选特征点，通过计算备选特征点与近邻点的平滑程度，将备选特征点划分为备选边缘特征点和备选平面特征点，之后在每一扫描行内划分4个子区域，在每个子区域内挑选2个边缘特征点和4个平面特征点，最后输出边缘特征点集与平面特征点集；对于平滑程度的计算，定义了一个参量c；对于第k个扫描帧所得到的点云P
k
中的某一个点p
i
，令S表示与点p
i
位于相同扫描行且与点p
i
相邻的连续点集，点p
i
两侧取相同数量的点；则点p
i
位置的局部光滑程度c的计算公式为：其中X
(k，i)
表示点p
i
在里程计坐标系下的坐标，点p
j
为连续点集S中的点，表示点p
i
在体坐标系下的坐标，|S|表示点集S中点的数量，||
·
||表示点坐标的二范数。6.根据权利要求5所述的一种基于精简特征迭代最近点SF
‑
ICP的激光里程计方法，其特征在于，在选择特征点时需遵循下述规则：为了使得特征点能够在整个扫描平面上均匀分布，将扫描行分为四个子区域，每个子区域选出两个边缘特征点以及四个平面特征点；在同一扫描行上的点将根据计算出来的c值进行排序，具有大的c值的点会被标记为边缘特征
点，而具有小的c值的点会被标记为平面特征点；需要排除周围已经存在特征点的点，使特征点均匀分布；需要排除扫描激光束平行或者被前面物体遮挡的几个点，因为它们会因为视角的变化而导致特征的剧烈变化，通过计算沿着扫描行周围点坐标的变化梯度来判断它们是否不可靠。7.根据权利要求1所述的一种基于精简特征迭代最近点SF
‑
ICP的激光里程计方法，其特征在于，步骤4具体为，通过对固定帧数的历史点云的融合，生成当前时刻的局部特征地图，之后对IMU数据进行预积分，产生一个对当前位姿的粗略估计，通过这个估计对步骤3挑选的特征点集的点云数据进行做坐标系转换，获得重映射后的当前特征点集，将重映射后的当前特征点集按照边缘特征点和平面特征点的分类，分别与局部特征地图，进行点...

【专利技术属性】
技术研发人员：郭剑辉，顾捷，刘宇轩，罗旭丽，濮存来，李伦波，
申请(专利权)人：南京理工大学，
类型：发明
国别省市：