一种高效率三维点云地图构建方法

本发明适用于移动机器人slam领域，具体涉及一种高效率三维点云地图构建方法。

背景技术：

1、随着自动驾驶、机器人导航及环境感知等领域的快速发展，对三维环境信息的准确构建与高效处理成为了关键技术之一。激光雷达因其能够提供丰富的三维空间信息而被广泛用于这些领域。在众多激光雷达三维点云地图构建算法中，lego loam轻量级地面优化激光雷达里程计和建图方法因其轻量级、高效率和地面优化特性而备受关注。然而，传统的lego loam算法在地面提取过程中，直接依赖于多线激光雷达的激光线对地面进行扫描，这种方法在复杂环境中容易导致地面点云中混杂着大量非地面杂点，进而影响地图的准确性和后续导航定位的精度。

2、此外，三维点云数据的庞大性和复杂性要求算法在计算效率和准确性之间取得良好平衡。传统算法在处理大量数据时，往往计算量巨大，难以实时处理，特别是在高动态环境下，算法的实时性和鲁棒性面临巨大挑战。同时，为了剔除杂点并保持有效点云的完整性，传统方法可能需要复杂的计算模型，这不仅增加了计算负担，还可能导致有效信息的丢失。

3、因此，为了克服lego loam算法在地面提取方面的不足，提高三维点云地图的准确性和构建效率，有必要对该算法进行优化改进。具体来说，需要设计一种能够有效剔除地面杂点、同时保留有效点云信息，并在保证精度的前提下提高处理速度的算法。此外，还应考虑如何更好地处理大规模点云数据，降低计算复杂度，提高算法在实时性和鲁棒性方面的表现。

4、基于上述背景，本发明提出了一种基于lego loam算法的三维点云地图构建方法，即l-lego loam算法。该算法在保留lego loam原始算法地面提取模块的基础上，通过引入高效的动态杂点剔除机制和体素滤波方式，显著提高了地面点云的提取精度和地图的清晰度。同时，结合最近邻插值法对地面点云进行扩充，补全了结构不明显的有效点云，进一步提升了地图的完整性和准确性。此外，该算法还通过优化处理流程，降低了点云的发布运算时间，提高了地图构建的实时性。

技术实现思路

1、本发明旨在至少解决现有技术中存在的技术问题之一；为此，本发明提出了一种高效率三维点云地图构建方法，用于解决以下技术问题：

2、现有的lego loam算法在地面提取过程中，直接依赖于多线激光雷达的激光线对地面进行扫描，这种方法在复杂环境中容易导致地面点云中混杂着大量非地面杂点，进而影响地图的准确性和后续导航定位的精度，传统算法在处理大量数据时，往往计算量巨大，难以实时处理，特别是在高动态环境下，算法的实时性和鲁棒性面临巨大挑战。同时，为了剔除杂点并保持有效点云的完整性，传统方法可能需要复杂的计算模型，这不仅增加了计算负担，还可能导致有效信息的丢失。

3、为解决上述问题，本发明的第一方面提供了一种高效率三维点云地图构建方法，包括以下步骤：

4、s1：提取原始lego loam算法，将提取出的地面点云放到一个容器中；

5、s2：引入动态杂点剔除机制，对点云数据的高度差异、密度变化和激光雷达扫描的连续性进行分析，对该容器中的点作最近邻插值处理并覆盖原始容器；

6、s3：在插值处理后对新的容器中点云作体素滤板处理并覆盖上一级容器；

7、s4：衔接原始lego loam算法架构，将地面点云和非地面点云发布出去；

8、s5：在kitti_06公开数据集下复现原始得到的点云图，并与改进后的l-lego loam点云图作分析对比。

9、作为本发明进一步的方案，所述s1，包括以下步骤：

10、采用原始的lego loam算法进行地面提取，将提取出的地面点云放到一个容器中，其中包含点云数据。

11、作为本发明进一步的方案，所述s2，包括以下步骤：

12、引入点云粗配准算法剔除动态点，通过比较相邻帧中同一簇对象的移动距离，当大于阈值时将该点作为动态点并删除，对分割后的点云进行边缘点和面点特征提取，在连续帧之间进行特征匹配，找到连续帧之间的位姿变换矩阵，对点云数据的局部特征和激光雷达扫描的连续性进行分析；

13、其中，局部特征包括高度差异和密度变化；

14、从点云容器中读取激光雷达点云数据，初始化数据结构，包括地面点的标记矩阵和标签矩阵，用于标记地面点并为每个点分配一个标签，将采集到的点云数据投影为距离图像，对地面点与非地面点进行分离，在分离后的地面点云中，计算全部点的均值得到地面点云的质心的坐标作为参考点，通过计算两点之间的垂直距离得到其他地面点相对于参考点的高度差，将高度差数据进行记录，形成高度差异数据集；

15、高度差计算公式：

16、

17、其中，为第个地面点的高度差，（，，）为地面点云质心的坐标，为地面点的总数量，（，，）为第个地面点的坐标；

18、对点云数据进行预处理，构建kd树，设置搜索半径阈值和最小点密度阈值，对于点云中的每个点，使用radiussearch函数搜索在半径阈值内的邻近点，将找到的邻近点的数量作为该点的局部密度值，并存储在向量densities中；创建一个visited向量，记录每个点是否已经被访问过，遍历每个点的密度，若点的密度小于最小点密度阈值则跳过；否则，使用广度优先搜索算法对该点及其邻近点进行聚类；将不小于最小密度阈值的点放入同一个聚类中，得到点云满足阈值条件的密度聚类，计算密度聚类的平均值、标准差、最大值和最小值，使用pclvisualizer可视化类对聚类结果进行可视化；

19、从激光雷达传感器中收集原始扫描数据，对原始扫描数据进行预处理操作，包括去噪和滤波，从预处理后的扫描数据中提取时间戳信息并按照时间顺序排序，对于连续的扫描数据，计算整个时间序列的平均时间间隔作为时间连续性指标；创建一个pcl，使用pcl库中的voxelgrid结构对点云数据进行体素化划分，设置每个体素的大小，调用函数根据设置的体素大小将点云数据分割成多个体素，对于每一对连续的帧分别计算在体素网格中的占用情况，通过比较两个帧中相同体素的占用情况计算重叠的比例作为空间连续性指标；对时间连续性指标和空间连续性指标赋予相应的权重得到综合连续性评价值；

20、综合连续性评价值计算公式：

21、

22、其中，为综合连续性评价值，为第次扫描的时间戳，为第次扫描的时间戳，为扫描次数，为两个帧中同时被占用的体素，为两个帧中的总的体素，为时间连续性指标对应的权重系数，为空间连续性指标对应的权重系数；

23、从点云容器中读取激光雷达点云数据，初始化数据结构，包括地面点的标记矩阵和标签矩阵，将采集到的点云数据投影为距离图像，对地面点与非地面点进行分离，并将地面点存储在点云容器中，采用广度优先搜索算法对点云进行聚类分割，将地面点和非地面点分开处理，采用最近邻插值法对提取出的地面云进行扩充后得到新的点云容器。

24、作为本发明进一步的方案，所述s3，包括以下步骤：

25、创建体素滤波器对象，设置体素滤波器的参数，对新的点云容器进行体素滤波处理，得到过滤后的点云容器，将过滤后的点云容器中的点云添加到原始点云容器中进行覆盖操作，覆盖上一级容器。

26、作为本发明进一步的方案，所述s4，包括以下步骤：

27、衔接原始lego loam算法架构，根据算法处理将点云数据分为地面点云和非地面点云，在处理点云的代码中，为地面点云和非地面点云创建两个不同的ros发布者，使用创建的发布者将地面点云和非地面点云分别发布出去。

28、作为本发明进一步的方案，所述s5，包括以下步骤：

29、在kitti_06公开数据集下复现原始得到的点云图，对原始点云数据进行预处理操作后进行点云分割，预处理操作包括滤波和重采样，将处理后的原始点云数据进行可视化展示，并与改进后的l-lego loam点云图作分析对比。

30、本发明的有益效果：

31、本发明通过引入高效的动态杂点剔除机制，本发明能够智能分析点云数据的局部特征和激光雷达扫描的连续性，其中，局部特征包括高度差异和密度变化，精确区分地面点云与非地面点云，有效剔除地面杂点，显著提升地面点云提取的精度，为构建高质量的三维地图奠定基础；

32、本发明采用体素滤波方式，本发明在保留点云结构特征的同时，进一步剔除了离群点，减少了噪声干扰，从而提高了构建的三维点云地图的准确性和清晰度。这有助于提升后续导航定位、路径规划等应用的精度和可靠性；

33、本发明针对三维点云数据庞大的问题，本发明通过优化算法实现，采用计算量较小的最近邻插值法对地面点云进行扩充，以补全结构不明显的有效点云，同时减少了不必要的计算量。此外，通过整体优化处理流程，降低了点云的发布运算时间，提高了算法的实时性和处理效率，使得在大规模数据集上也能实现快速构建三维地图；

34、本发明不仅适用于复杂多变的户外环境，还能有效应对不同光照条件、天气变化等外部因素的干扰，提高了算法的鲁棒性和适应性。这使得本发明在自动驾驶、机器人导航等领域具有更广泛的应用前景；

35、本发明的提出，为三维点云地图构建技术提供了一种新的思路和方法，推动了该领域技术的进一步发展和完善。通过不断优化和改进，本发明有望在未来成为三维地图构建领域的重要技术手段之一。