74、基于3D或2D激光雷达的统一3D映射框架

基于3D或2D激光雷达的统一3D映射框架

1. 引言

同时定位与地图构建（SLAM）是自主机器人领域的基本问题之一。近年来，随着激光雷达传感和SLAM技术的进步，机器人在许多工业领域的应用迅速增长，如土木工程设施的自主检查。通常，不同的应用场景需要开发专门的系统，例如在隧道检查项目中，根据作业环境的规模不同，会使用不同的无人机（UAV），这些无人机配备了不同的激光雷达，往往需要不同的SLAM算法。然而，维护多种算法需要大量的精力，这在实际应用中是不可取的。

因此，本文提出了一个统一的映射框架，该框架可以处理多种类型的激光雷达，包括固定3D激光雷达和旋转3D/2D激光雷达。其目标是给定任何类型激光雷达传感器收集的一系列激光扫描数据，算法能够计算传感器的运动并同时构建3D地图。

2. 相关工作

过去几十年，关于基于激光雷达的SLAM有大量的研究。经典的概率方法，如卡尔曼滤波器和粒子滤波器，基于传感器噪声模型的测量值来推断机器人状态和地图的分布。这些方法为SLAM问题奠定了理论基础，在鲁棒性、准确性和效率方面取得了巨大成功，但大多数方法仅限于使用固定2D激光雷达解决平面SLAM问题。虽然理论上这些算法可以推广到3D，但随着维度的增加，计算成本可能变得难以承受。

在3D场景中，2D激光雷达可以安装在旋转电机或弹簧上以构建3D地图。额外的自由度显著扩大了传感器的视野（FOV），但由于缺乏重叠，顺序扫描匹配变得不可能。为了解决这个问题，一些方法使用平滑连续轨迹来表示机器人的运动，但平滑运动假设并不总是成立。

近年来，随着3D测距技术的广泛应用，使用精确3D激光雷达实现实时、大规模和低漂移的SLAM方法不断涌现。例如，Martín等人开发了基于差分进化的扫描匹配算法，Zhang和Singh等人提取边缘和平面特征并采用ICP方法进行特征配准，Hess等人引入了Cartographer SLAM，通过扫描匹配估计位姿并定期优化稀疏位姿图（SPG）以生成一致的地图。

3. 方法

3.1 定位

定位模块结合了误差状态卡尔曼滤波器（ESKF）和高斯粒子滤波器（GPF）来估计先验地图内的机器人状态。GPF将原始激光扫描数据转换为位姿测量值，使ESKF无需直接处理2D或3D距离数据，这是确保兼容性的关键因素。

具体来说，ESKF对惯性测量单元（IMU）的测量值进行数值积分以预测机器人状态，并使用伪位姿测量值更新预测。在GPF中，根据预测采样一组粒子（位姿假设），然后对其进行加权并最终求平均值以找到后验信念。通过从后验信念中减去预测值，恢复伪位姿测量值并用于更新ESKF。

以下是定位算法的流程图：

graph TD;
    A[传感器读数和距离图] --> B[ESKF预测机器人状态];
    B --> C[GPF采样粒子];
    C --> D[粒子加权];
    D --> E[求平均值得到后验信念];
    E --> F[恢复伪位姿测量值];
    F --> G[ESKF更新预测];
    G --> H[输出位姿估计];

3.2 子地图和距离地图

Cartographer将局部占用网格地图定义为子地图。由于使用了不同的定位方法，需要调整子地图管理方案，以便定位模块可以访问子地图。

扫描积累分为两个阶段：
1. 第一阶段，N次扫描积累形成3D扫描，然后将其匹配并插入到活动子地图中。活动子地图包括匹配子地图和增长子地图，3D扫描会同时插入到这两个子地图中。
2. 第二阶段，如果M次3D扫描插入到匹配子地图中，增长子地图将变为新的匹配子地图，旧的匹配子地图将被删除，同时创建一个新的子地图并开始增长。

在这两个阶段中，每当形成3D扫描或创建新子地图时，都会向SPG添加新的位姿节点。

为了实现这些调整，在原始网格地图旁边添加了一个八叉树地图（Octomap）。形成的3D扫描插入到八叉树地图中，并立即更新相应的距离地图。八叉树地图库提供了一种有效的方法来检测变化，从而可以高效地计算距离地图。此外，更新八叉树地图和距离地图使用多线程技术，以避免距离地图转换引起的时间延迟。

子地图管理的主要参数如下表所示：
| 定位相关参数 | 映射相关参数 |
| — | — |
| 加速器噪声 σa | 每次积累的扫描次数 N |
| 陀螺仪噪声 σg | 每个子地图的扫描次数 M |
| 距离地图分辨率 | 每次优化的扫描次数 I |
| 距离地图的最大范围 | |

4. 实验

4.1 模拟隧道测试

模拟机器人的主要任务是飞过火车车厢隧道并绘制其内部地图。飞行过程约需15分钟，因为要保持相对较低的速度（最大1.13 m/s）并频繁停留，以收集足够的激光点来构建子地图。移动速度过快会导致定位不稳定，而3D激光雷达可以快速扫描环境中的许多点，从而解决这个问题。

在模拟中，可以将估计的位姿与地面真值进行比较。从实验结果来看，在飞行接近结束时，三个轴上的最大位置误差分别为2.0 m、0.37 m和1.11 m，其中x轴的漂移最大，这是因为用于估计x的火车车厢上的点数相对较少。总行驶距离为165 m，平移漂移率为1.34%。旋转估计则更加一致，滚动、俯仰和偏航的平均误差分别为0.14°、0.15°和0.24°，偏航误差偶尔会出现峰值，但可以快速恢复，旋转漂移率为0.003°/m。

以下是模拟隧道测试的部分结果：
- 构建的隧道地图可视化（体素滤波，分辨率0.1 m）。
- 地面真值（蓝色）和估计轨迹（红色）的比较。
- 位姿估计误差图。

4.2 真实世界测试

真实世界实验在多个平台上进行：
1. 配备i7（2.5 GHz）计算机的固定VLP - 16（10 Hz，范围100 m）。
2. 旋转VLP - 16。
3. 配备2.32 GHz ARM处理器的旋转Hokuyo（40 Hz，范围30 m）。

第一个实验在走廊环路内进行，VLP - 16水平放置且旋转速度为零。尽管VLP - 16可以测量3D结构，但由于其20°的视野不足以可靠地估计高度，在机器人返回原点时会观察到较大的高度漂移，并且需要更多时间来检测闭环。

第二个实验在卡内基梅隆大学校园的庭院周围进行，VLP - 16的电机旋转速度设置为30 rpm。由于区域较大，用于定位的距离地图分辨率较粗（0.3 m），并限制在40 m × 40 m × 40 m的边界框内。

最后一个实验中，机器人绘制了一个最小宽度为1.1 m的狭窄走廊地图。为了确保收集到足够的激光点，机器人被手动缓慢移动（约0.5 m/s），在闭环之前仅观察到较小的高度漂移。这是因为旋转激光雷达扩大了视野，显著提高了映射性能。

需要注意的是，在上述三种情况下，只需要更改少数参数。对于定位相关参数，σa和σg表征IMU的噪声水平，距离地图分辨率根据环境规模选择，距离地图的最大范围限制了机器人的可见距离。对于映射相关参数，M和N如子地图管理阶段所述，I控制后台SPG的优化频率。

5. 结论

本文提出的算法展示了在统一框架中处理不同激光雷达配置的能力，只需调整少数参数，简化了开发和应用过程。从实验中得到的一些关键见解如下：
- 激光雷达的视野很重要 ：固定3D激光雷达设置简单，但垂直视野有限，导致高度估计不可靠，可能需要向下倾斜以捕获更多地面点。旋转激光雷达的视野明显更宽，对不同环境更具鲁棒性，但旋转电机需要精心设计以确保连续数据流和准确同步，例如需要昂贵的滑环机制来实现连续旋转。
- 移动速度在旋转2D激光雷达的情况下至关重要 ：在测试中，低速是必要的，以便激光扫描仪能够积累足够的点来更新子地图。移动速度过快可能导致定位不稳定。而对于3D激光雷达，低速不是关键要求。
- 子地图分辨率的选择会影响内存使用、计算复杂度和映射精度 ：低分辨率子地图内存使用低，查询数据速度快，但会牺牲地图精度；高分辨率子地图消耗更多内存，但不一定能提高地图精度。因此，需要通过反复试验仔细选择分辨率。

基于3D或2D激光雷达的统一3D映射框架

6. 技术优势与意义

该统一3D映射框架具有多方面的技术优势和重要意义。

从兼容性角度来看，它打破了传统基于激光雷达的SLAM算法只能适配特定类型负载的局限。以往不同的平台往往需要不同的算法，这使得算法的开发、部署和维护成本极高。而此框架能够处理多种类型的激光雷达，包括固定3D、旋转3D/2D激光雷达，大大提高了算法的通用性和可扩展性。例如在实际的隧道检查项目中，不同规模的作业环境可能需要不同的无人机搭载不同的激光雷达，使用该框架就可以避免为每种配置单独开发和维护算法。

在定位精度方面，通过结合误差状态卡尔曼滤波器（ESKF）和高斯粒子滤波器（GPF）的定位模块，实现了更准确的位姿估计。GPF将原始激光扫描数据转换为位姿测量值，让ESKF无需直接处理复杂的2D或3D距离数据，有效提高了定位的准确性和稳定性。特别是在模拟隧道测试和真实世界测试中，都能较好地跟踪机器人的运动轨迹，虽然存在一定的误差，但整体表现符合预期。

从资源利用和效率角度，子地图管理方案的调整以及八叉树地图（Octomap）的引入，使得距离地图的计算更加高效。八叉树地图库能够有效检测变化，并且采用多线程技术更新地图，避免了时间延迟，提高了系统的实时性。同时，只需要调整少数参数就能适应不同的平台和环境，减少了参数调优的工作量，提高了开发和应用的效率。

7. 应用场景拓展

该统一3D映射框架在多个领域具有广泛的应用前景。

在工业巡检领域，如石油化工、电力等行业的管道巡检。可以使用搭载激光雷达的无人机或移动机器人，通过该框架构建管道的3D地图，检测管道的损坏、泄漏等情况。不同的管道环境可能需要不同类型的激光雷达，该框架的兼容性可以满足多样化的需求。

在智能建筑领域，可用于建筑物的室内外建模。在建筑物的建设和维护过程中，需要精确的3D地图来进行规划和管理。无论是大型商业建筑还是小型住宅，都可以利用该框架快速、准确地构建地图。例如在室内装修过程中，通过构建精确的室内3D地图，可以更好地进行设计和施工。

在农业领域，可用于农田的地形测绘和作物生长监测。使用搭载激光雷达的无人机对农田进行扫描，构建农田的3D地图，有助于了解农田的地形起伏、土壤湿度等信息，为精准农业提供支持。同时，还可以监测作物的生长情况，及时发现病虫害等问题。

8. 挑战与改进方向

尽管该框架取得了一定的成果，但仍然面临一些挑战，需要进一步改进。

在激光雷达视野方面，虽然旋转激光雷达可以扩大视野，但旋转电机的设计和同步仍然是一个难题。目前需要昂贵的滑环机制来实现连续旋转，增加了成本和系统的复杂性。未来可以研究更经济、可靠的旋转机构，提高旋转激光雷达的性能和稳定性。

在移动速度方面，旋转2D激光雷达对移动速度的要求较高，限制了其应用范围。可以探索更高效的扫描和数据处理算法，使旋转2D激光雷达在较高速度下也能准确地更新子地图，提高定位的稳定性。

在子地图分辨率选择方面，目前需要通过反复试验来确定合适的分辨率，缺乏一种科学、系统的方法。可以研究基于环境特征和任务需求的自适应分辨率选择算法，根据实际情况自动调整子地图分辨率，在保证地图精度的同时，优化内存使用和计算复杂度。

9. 总结与展望

基于3D或2D激光雷达的统一3D映射框架为解决不同类型激光雷达的SLAM问题提供了一种有效的解决方案。通过创新的定位模块和子地图管理方案，实现了在多种平台上的实时3D地图构建，并且只需要调整少数参数。实验结果验证了该框架的可行性和有效性，同时也揭示了一些需要改进的方面。

未来，随着激光雷达技术和算法的不断发展，该框架有望进一步优化和完善。可以结合更多的传感器数据，如视觉传感器、超声波传感器等，提高地图的准确性和完整性。同时，将该框架应用于更多的实际场景中，推动机器人技术在各个领域的广泛应用。

以下是该框架的整体工作流程总结流程图：

graph LR;
    A[激光雷达数据采集] --> B[定位模块（ESKF+GPF）];
    B --> C[子地图管理与距离地图更新];
    C --> D[构建3D地图];
    D --> E[输出结果（地图和位姿估计）];
    F[参数调整] --> B;
    F --> C;

综上所述，该统一3D映射框架在机器人领域具有重要的研究价值和应用前景，为实现更智能、高效的机器人导航和地图构建提供了有力支持。