Skip to content
Anima Mundi Anima Mundi

SLAM 原理综述：滤波、优化与 VIO/LIO

I. 引言

Section titled “I. 引言”

A. 定义与核心问题

Section titled “A. 定义与核心问题”

同步定位与地图构建（Simultaneous Localization and Mapping, SLAM）是指移动机器人或智能体在未知环境中，利用自身传感器获取的信息，逐步建立环境地图，并同时利用这张地图来估计自身在环境中的位置与姿态的过程。SLAM 技术的核心在于解决定位与建图之间的相互依赖关系，这通常被称为“鸡生蛋还是蛋生鸡”的问题：精确的地图构建依赖于准确的定位信息，而精确的定位又需要基于准确的环境地图。这种固有的耦合性构成了 SLAM 技术的基础性挑战。

SLAM 技术旨在回答两个基本问题：“我在哪里？”（定位）和“周围环境是什么样子？”（建图）。对于无法依赖外部参考系统（如 GPS）的自主系统而言，SLAM 是实现其在未知环境中自主导航的关键能力。从本质上看，SLAM 是一个状态估计问题，其目标是根据一系列带有噪声的传感器观测数据，估计一个不断增长的状态向量，该向量包含了机器人/传感器的轨迹以及环境地图的描述。

B. 基本挑战

Section titled “B. 基本挑战”

SLAM 的实现面临着一系列固有挑战：

  1. 传感器噪声与不确定性: 现实世界中的传感器（如轮式里程计、惯性测量单元、相机、激光雷达）的测量数据不可避免地受到噪声干扰并存在不确定性。这些误差会随着时间的推移而累积，导致定位和建图的漂移。如何有效管理和量化这种不确定性是 SLAM 系统的核心任务之一。
  2. 数据关联: 将当前的传感器观测数据正确地与地图中已有的地标或部分进行匹配，是 SLAM 中的一个关键且困难的问题。尤其是在特征模糊或环境重复性高的场景下，错误的数据关联可能导致定位失败或地图严重失真，对于滤波方法而言尤其致命。
  3. 闭环检测: 识别机器人是否回到了先前访问过的区域，并利用这一信息来校正累积的定位漂移，确保地图的全局一致性，是 SLAM 中的一个特殊且关键的数据关联问题。闭环检测的失败会导致地图扭曲或出现重影。通常需要借助场景识别（Place Recognition）技术来实现。
  4. 计算复杂度: SLAM 系统需要实时处理大量的传感器数据，并优化可能包含大量状态变量（机器人位姿序列、地图特征点/体素等）的估计问题。尤其是在大规模环境或长期运行时，保持实时性是一个巨大的挑战。
  5. 动态环境: 现实世界往往包含移动的物体（如行人、车辆），这违反了许多传统 SLAM 算法所依赖的静态世界假设。动态物体会对传感器的观测产生干扰，影响定位精度和地图质量。

C. 主要应用领域

Section titled “C. 主要应用领域”

SLAM 技术的不断发展使其在众多领域得到了广泛应用：

  • 自动驾驶: 为车辆提供高精度的定位和环境感知能力。
  • 机器人: 实现机器人在未知环境中的自主导航、探索、操作和人机交互。
  • 增强现实/虚拟现实 (AR/VR): 提供精确的设备位姿跟踪和环境三维重建，实现虚拟内容与现实世界的融合。
  • 无人机 (UAV): 使无人机能够在 GPS 信号受限或拒止的环境（如室内、峡谷）中进行自主飞行和测绘。
  • 环境监测与测绘: 用于地下矿井、森林、农业、考古遗址 等特殊环境的勘探与三维地图构建。

SLAM 应用领域的扩展，例如从基础的机器人导航发展到需要高保真环境模型的 AR/VR 以及大规模、动态场景下的自动驾驶，持续推动着对更高精度、更强鲁棒性（尤其是在动态环境中）以及更丰富地图表示（如稠密地图、语义地图）的需求。这种应用需求的反向拉动，是 SLAM 技术不断演进的重要驱动力。

II. 数学基础：滤波与优化

Section titled “II. 数学基础：滤波与优化”

SLAM 问题本质上是一个状态估计问题，其核心在于如何在充满不确定性的测量下，估计机器人的运动轨迹和环境地图。从概率的角度来看，SLAM 可以被描述为估计机器人路径和地图的后验概率分布，该分布以传感器的测量值和控制输入为条件。解决这一问题的两大主流数学框架是基于滤波的方法和基于优化的方法（也称为平滑法）。

A. 概率 SLAM 基础

Section titled “A. 概率 SLAM 基础”

概率 SLAM 框架通常包含两个关键模型：

  • 运动模型 (Motion Model): 根据机器人的控制输入或里程计测量，预测机器人在下一时刻的状态（位姿）。
  • 观测模型 (Sensor/Measurement Model): 根据传感器（如相机、激光雷达）的观测数据，更新对机器人状态和地图的估计。

B. 基于滤波的 SLAM

Section titled “B. 基于滤波的 SLAM”

滤波方法的核心思想是根据过去的估计和最新的测量数据，顺序地、递归地估计当前的系统状态（机器人位姿和地图）。这类方法通常维护一个关于当前状态的概率分布。

  1. 扩展卡尔曼滤波 (EKF-SLAM):

    • 原理: EKF-SLAM 是最早被广泛应用的 SLAM 算法之一。它使用扩展卡尔曼滤波来处理 SLAM 中固有的非线性运动和观测模型。EKF 通过对非线性函数进行一阶泰勒展开来实现线性化近似。它维护一个高斯分布来表示机器人位姿和所有路标点位置的联合状态，并递归地更新这个分布的均值和协方差矩阵。一个关键特点是 EKF 会显式地维护机器人位姿误差和路标点位置误差之间的相关性。
    • 优点: 理论基础成熟，在满足高斯假设和线性化近似足够好的情况下，可以提供一致性的估计（即估计的协方差能反映真实的误差水平）。
    • 缺点: 计算复杂度高，状态协方差矩阵的更新和存储开销随路标数量呈二次方增长 (O(N2))，难以应用于大规模环境。对非线性模型的线性化近似可能引入误差，导致滤波器发散。对数据关联错误非常敏感，因为系统只维护单一假设，一旦发生错误关联，很难恢复。此外，现实世界中的噪声往往不完全符合高斯分布。由于这些局限性，EKF-SLAM 在现代大规模 SLAM 应用中已较少使用。

  2. 粒子滤波 (Particle Filters / RBPF):

    • 原理: 粒子滤波使用一组带权重的随机样本（称为粒子）来近似状态的后验概率分布，尤其适用于非高斯、非线性的系统。在 SLAM 中，通常采用 Rao-Blackwellized 粒子滤波（RBPF）框架。RBPF 利用 SLAM 问题的条件独立性，将高维的 SLAM 后验概率分解为机器人路径（位姿序列）的估计和给定路径下地图的估计。具体来说，每个粒子代表一个可能的机器人轨迹假设，并独立地维护一个与该轨迹对应的环境地图（例如，每个粒子内部使用 EKF 或维护一个栅格地图）。

    • FastSLAM:

      是 RBPF 在 SLAM 领域最著名的应用之一。它使用粒子表示机器人路径的多种可能性，每个粒子内部使用多个独立的低维 EKF 来估计观测到的路标点的位置。

      • 优点: 通过维护多个假设（粒子），对数据关联错误具有更强的鲁棒性。能够更好地处理非高斯噪声和多模态分布。计算复杂度通常随路标数量线性增长，扩展性优于 EKF-SLAM。
      • 缺点: 存在粒子退化（Particle Impoverishment/Degeneracy）问题，即经过多次重采样后，少量粒子可能占据绝大部分权重，导致样本多样性丧失。可能存在一致性问题，即估计的位姿不确定性可能被低估。计算量与粒子数量成正比，需要足够多的粒子才能较好地近似后验分布。FastSLAM 2.0 通过改进提议分布（Proposal Distribution）提升了性能。HybridSLAM 则尝试结合 FastSLAM（局部）和 EKF-SLAM（全局）的优点。

    • GMapping: 是一种高效的基于 RBPF 的二维激光 SLAM 算法，用于构建栅格地图。它采用了自适应重采样策略（根据粒子权重的有效性判断是否重采样）和改进的提议分布（在预测粒子位姿时考虑了最新的激光扫描数据），显著减少了所需粒子数量。GMapping 曾是 ROS 中广泛使用的 2D SLAM 包，但近年来 Cartographer 等基于优化的方法提供了更优的性能。代码库：openslam_gmapping。

    • UFastSLAM: 即 Unscented FastSLAM，使用无迹卡尔曼滤波（UKF）替代 EKF 来处理每个粒子内部的路标估计，旨在更好地处理系统中的非线性问题。

C. 基于优化的 SLAM (平滑法)

Section titled “C. 基于优化的 SLAM (平滑法)”

优化方法（或称为平滑法）将 SLAM 问题构建为一个非线性最小二乘优化问题。它不是像滤波那样只估计当前时刻的状态，而是同时估计一段时间内（批处理或滑动窗口）的机器人轨迹和地图状态，目标是找到一个状态估计，使得所有传感器测量值与模型预测值之间的误差最小。这种方法通常通过因子图（Factor Graph）来表示。

  1. 图优化 SLAM (Graph SLAM):
    • 原理: 图优化 SLAM 使用图结构来表示 SLAM 问题。图中的节点（Node）通常代表机器人在不同时间点的位姿，有时也包含环境中的路标点。图中的边（Edge）表示节点之间的约束关系，这些约束来源于机器人的运动测量（如里程计）或对环境的观测（如传感器对路标的观测、闭环检测）。优化的目标是调整图中所有节点的位置（位姿和路标坐标），使得所有边所代表的约束的误差总和最小。
    • 优点: 通常比滤波方法更精确，因为它利用了时间段内所有的测量信息进行联合优化。能够很好地处理非线性问题。可以获得全局一致的轨迹和地图估计。随着稀疏非线性优化技术的发展，图优化 SLAM 变得越来越高效。
    • 缺点: 批处理优化（Full Batch Optimization）的计算量可能很大，尤其对于长轨迹和大规模地图。需要一个较好的初始值来保证优化收敛到全局最优解。
    • 关键组成: 前端（Frontend）负责处理传感器数据、进行数据关联、检测闭环，并构建因子图；后端（Backend）负责对构建好的因子图进行优化求解。
    • 优化求解: 通常采用迭代优化算法，如高斯-牛顿法（Gauss-Newton）或列文伯格-马夸尔特法（Levenberg-Marquardt, LM）。这些算法在每次迭代中线性化误差函数，然后求解一个线性方程组来更新状态估计。SLAM 问题的特殊稀疏结构（即每个测量通常只关联少数几个状态变量）使得 Hessian 矩阵或信息矩阵具有稀疏性，利用这种稀疏性可以大大提高优化效率。
    • 常用库: g2o (General Graph Optimization)，GTSAM (Georgia Tech Smoothing and Mapping)，Ceres Solver。
  2. 光束法平差 (Bundle Adjustment, BA): 主要应用于视觉 SLAM 领域。BA 通过最小化重投影误差（Reprojection Error，即地图点投影到图像上的位置与实际观测到的特征点位置之间的差异），联合优化相机位姿和三维地图点的坐标。BA 可以是全局的（优化所有位姿和点），也可以是局部的（在一个滑动窗口内优化）。

D. 滤波与优化的对比

Section titled “D. 滤波与优化的对比”
  • 滤波方法 提供状态的顺序估计，适合需要实时输出当前状态的应用，但由于不断地将过去的状态边缘化（marginalize），可能会累积误差，且精度通常低于优化方法。
  • 优化方法 提供在一个时间窗口或整个数据集上的全局最优估计，通常精度更高，但计算开销可能更大，需要批处理或窗口化处理数据。现代优化技术，如增量平滑与建图（Incremental Smoothing and Mapping, 如 iSAM）或滑动窗口优化，可以在很大程度上实现实时性能。

当前 SLAM 领域的发展趋势清晰地显示，后端处理正从传统的纯滤波方法（尤其是 EKF）转向基于优化的方法（如图优化或 BA）。这一转变在各种传感器模态中都十分明显，例如激光雷达领域的 LIO-SAM 和 Cartographer，视觉领域的 ORB-SLAM3 和 DSO，以及视觉惯性领域的 VINS-Mono 和 OKVIS。这种转变的背后，是计算能力的提升以及稀疏非线性优化技术的成熟，使得原本计算量巨大的优化方法得以在实时应用中落地。相比于滤波方法，优化方法能够更好地利用时间窗口内或全局的数据关联信息，联合优化所有相关变量，从而获得更高的精度和全局一致性，有效克服了滤波方法中误差累积和一致性难以保证的问题。GTSAM、Ceres 等优化库的广泛应用也进一步推动了这一趋势。

算法的演进路径也反映了对早期方法局限性的克服。EKF 对数据关联错误敏感且计算复杂度高，催生了如 FastSLAM 这样的粒子滤波方法。而 FastSLAM 存在的粒子退化和一致性问题，又使得优化方法在计算可行性提高后展现出更大的吸引力。

因此，选择滤波还是优化作为核心数学框架，直接决定了 SLAM 系统的整体架构、性能权衡（精度、速度、一致性之间）以及对不同类型误差（如数据关联错误）的敏感度。滤波系统侧重于状态的实时递推更新，而优化系统则侧重于构建和求解包含所有相关约束的因子图。理解这两种框架的根本差异对于选择或设计合适的 SLAM 系统至关重要。

表 1: 基于滤波的 SLAM 方法对比 (EKF-SLAM vs. FastSLAM)

特征EKF-SLAMFastSLAM
核心原理单一高斯分布表示联合状态粒子滤波 + 每个粒子独立的卡尔曼滤波器
数据关联鲁棒性对错误关联敏感对错误关联鲁棒
计算复杂度路标数量的二次方路标数量和粒子数量的线性
一致性(带相关性时) 相对更一致相对不一致 (可能低估不确定性)
可扩展性对大规模环境扩展性差对大规模环境扩展性更好
处理模糊性高度模糊时性能差能较好处理观测模糊性
优点理论成熟, (带相关性时)一致性较好数据关联鲁棒, 可扩展性好, 能处理非高斯/多模态
缺点计算量大, 线性化误差, 数据关联敏感, 高斯假设粒子退化, 一致性问题, 计算量依赖粒子数
主要参考33

III. SLAM 前端：传感器模态与里程计

Section titled “III. SLAM 前端：传感器模态与里程计”

SLAM 系统的前端（Frontend）负责处理原始传感器数据，估计传感器（机器人）的连续运动，这个过程通常称为里程计（Odometry）。前端还负责提取用于建图和定位的关键信息（如特征点、关键帧），并为后端提供位姿估计的初始值和约束（如里程计约束、闭环约束）。前端的性能直接影响整个 SLAM 系统的精度和鲁棒性。

A. 视觉 SLAM (Visual SLAM, V-SLAM)

Section titled “A. 视觉 SLAM (Visual SLAM, V-SLAM)”

V-SLAM 使用相机作为主要传感器来感知环境和估计运动。

  • 传感器类型:
    • 单目相机 (Monocular): 使用单个相机。优点是成本最低、体积最小。缺点是存在尺度不确定性（无法仅从单张图像恢复绝对距离），需要充分的运动才能初始化。
    • 双目相机 (Stereo): 使用两个具有固定基线的相机。优点是可以通过三角测量原理直接计算特征点的深度，解决了单目的尺度问题。缺点是计算量相对更大，需要精确的相机标定和图像校正。
    • RGB-D 相机: 同时提供彩色图像（RGB）和深度图像（Depth）。优点是直接获取稠密的深度信息，简化了建图和初始化过程。缺点是通常测量范围有限，易受光照和物体表面材质影响，功耗和成本相对较高，多用于室内环境。
  • 主要方法对比: V-SLAM 前端的核心在于如何利用图像信息估计相机运动。主要有三类方法：
    1. 基于特征点的方法 (Feature-Based / Indirect):
      • 原理: 首先从图像中提取稀疏但具有辨识度的特征点（如角点、斑点），如 SIFT, SURF, ORB, FAST, GFTT 等。然后计算这些特征点的描述子（Descriptor）。通过匹配相邻帧之间或当前帧与地图之间的特征点描述子来建立数据关联。最后，根据匹配好的特征点对，利用几何约束（如对极几何、PnP 算法）来估计相机的相对运动。
      • 优点: 对光照变化具有较好的鲁棒性（如果特征描述子具有不变性）。技术成熟，研究广泛。在纹理丰富的环境中表现良好。计算效率相对较高（因为处理的是稀疏特征）。
      • 缺点: 在纹理稀疏或重复纹理区域容易失败（特征点不足或匹配模糊）。丢弃了图像中除特征点外的其他信息。特征提取和匹配本身也有一定的计算开销。
      • 代表算法: PTAM, MonoSLAM, ORB-SLAM 系列, RTAB-Map。
    2. 直接法 (Direct):
      • 原理: 不依赖特征提取和匹配，而是直接利用图像中像素的灰度（光度）信息。基于光度不变性假设（即空间中同一点在不同视角下观测到的亮度保持不变），通过最小化相邻帧之间对应像素块的光度误差来估计相机运动。
      • 优点: 可以利用图像中所有的梯度信息，即使在纹理稀疏的区域也能工作。能够构建稠密或半稠密的地图。在光度不变性假设满足时，可能达到很高的精度。
      • 缺点: 对光照变化、相机曝光参数变化非常敏感（违反光度不变性假设）。稠密直接法的计算量非常大。需要精确的光度标定（考虑相机响应函数、曝光时间、暗角等）。
      • 代表算法: DTAM, LSD-SLAM (Large-Scale Direct Monocular SLAM), DSO (Direct Sparse Odometry)。
    3. 半直接法 (Semi-Direct):
      • 原理: 试图结合特征点法和直接法的优点。通常会提取特征点（如 FAST 角点），但在跟踪这些特征点时，不依赖描述子匹配，而是采用直接法，通过优化特征点周围小块像素的光度误差来估计运动和深度。
      • 优点: 通常速度非常快，因为它结合了稀疏特征点和直接法的跟踪方式。在鲁棒性和地图密度之间取得了一定的平衡。
      • 缺点: 可能仍然对光照变化敏感。可能继承两类方法的缺点，如在极低纹理区域遇到困难。一些早期的半直接法为了追求速度牺牲了后端的优化和闭环检测，导致长期运行时漂移较大。
      • 代表算法: SVO (Semi-direct Visual Odometry)。
  • 关键 V-SLAM 算法与代码库:
    • ORB-SLAM3: 基于 ORB 特征点的优秀 SLAM 系统。支持单目、双目、RGB-D 以及视觉惯性传感器。采用最大后验概率（MAP）估计，引入 Atlas 多地图系统用于长期运行和重定位，改进了基于 DBoW2 的场景识别（先几何验证后共视一致性检查，提高召回率），包含局部 BA、闭环修正和全局 BA 等模块。代码库: UZ-SLAMLab/ORB_SLAM3。注意：该算法在同一数据集上多次运行结果可能不同，具有一定的随机性。ORB-SLAM3AB 是针对颠簸路面场景的改进版本。
    • DSO (Direct Sparse Odometry): 稀疏直接法里程计的代表。通过最小化选定像素点的光度误差，联合优化相机运动和点的逆深度。特别强调了光度标定的重要性（考虑曝光时间、暗角、响应函数）。以高精度著称。主要针对单目相机，但也有双目扩展。代码库: JakobEngel/dso (原始单目), alexwu-bayarea/stereo-dso (双目)。
    • SVO (Semi-direct Visual Odometry): 半直接法的代表。通过直接法跟踪 FAST 特征点，进行稀疏建图和概率深度估计，并通过 BA 进行优化。以速度快闻名。SVO Pro 是其后续版本，增加了对鱼眼/全向相机的支持、主动曝光控制、基于滑动窗口的后端优化、全局 BA 和闭环检测等功能。代码库: uzh-rpg/rpg_svo (原始版本), uzh-rpg/rpg_svo_pro_open (SVO Pro)。PL-SLAM 是在 SVO 基础上增加了线特征的版本。

表 2: 视觉 SLAM 前端方法对比

方法原理优点缺点代表系统
特征点法提取、匹配稀疏特征点，基于几何约束估计位姿对光照变化鲁棒 (依赖描述子), 技术成熟, 纹理丰富时效果好纹理缺失时失败, 丢弃非特征信息, 特征提取/匹配开销ORB-SLAM, PTAM
直接法直接使用像素灰度，最小化光度误差估计位姿利用所有梯度信息, 纹理缺失时可用, 可建稠密/半稠密图对光照/曝光敏感, 稠密计算量大, 需光度标定DSO, LSD-SLAM
半直接法提取特征点，使用直接法跟踪特征点周围像素块速度快, 在鲁棒性和密度间取得平衡可能仍对光照敏感, 可能牺牲后端/闭环SVO

B. 激光雷达 SLAM (LiDAR SLAM)

Section titled “B. 激光雷达 SLAM (LiDAR SLAM)”

LiDAR SLAM 使用激光雷达（LiDAR）作为主要传感器来获取环境的三维结构信息。

  • 传感器类型:
    • 2D LiDAR: 在一个平面内进行扫描，获取该平面的距离和角度信息。成本较低，扫描速度快，常用于室内机器人。
    • 3D LiDAR: 通过旋转多线激光束（旋转式）或采用其他扫描机制（如固态 LiDAR）来获取环境的三维点云数据。
  • 相比视觉的优势: 测距精度高，对光照变化不敏感，在缺乏纹理的环境中也能稳定工作。
  • 相比视觉的劣势: 成本通常更高，功耗更大，数据处理（尤其是稠密点云）计算量大，对透明或高反光表面效果差，在特征丰富的场景下获取的信息密度可能不如相机。
  • 扫描匹配技术对比: LiDAR SLAM 前端的核心是扫描匹配（Scan Matching），即计算连续两次扫描（Scan-to-Scan）之间或当前扫描与已构建地图（Scan-to-Map）之间的相对位姿变换。主要技术包括：
    1. 基于 ICP (Iterative Closest Point) 的方法:
      • 原理: 通过迭代寻找两个点云之间的最近对应点，并最小化这些对应点之间的距离（通常是平方和），从而计算出最优的刚体变换（旋转和平移）。
      • 优点: 算法原理简单。通过改进（如点到平面 ICP、GICP、NICP 等）可以达到较高的配准精度。
      • 缺点: 对初始位姿估计敏感，容易陷入局部最优。对噪声和离群点敏感。计算量较大，尤其对于大规模点云。
      • 实现: PCL (Point Cloud Library), libpointmatcher。代码示例: cwchen1102/icp_localization, casychow/Iterative-Closest-Point, ToshikiNakamura0412/icp_matching_ros, YuePanEdward/GH-ICP (Global Optimal Matching), navoday01/Point-cloud-alignment-with-ICP, ori-drs/aicp_mapping (Auto-tuned ICP)。
    2. 基于 NDT (Normal Distributions Transform) 的方法:
      • 原理: 将参考点云所占据的空间划分为网格（Voxel）。在每个网格内，用一个正态分布（高斯分布）来描述其中包含的点的分布情况（均值和协方差）。然后，将待配准点云中的点变换到参考点云坐标系下，通过优化这些点在其所属网格的正态分布下的概率（或得分）来找到最佳变换。
      • 优点: 对于大规模点云，配准速度通常比 ICP 快。对初始位姿和离群点不太敏感。
      • 缺点: 配准精度通常低于 ICP 及其变种。精度受网格划分大小的影响。
      • 实现: PCL。ndt_omp 使用 OpenMP 加速了 NDT 计算。代码示例: koide3/ndt_omp, jumpercc/python-pcl-ndt。
    3. 基于特征的方法 (LOAM 风格):
      • 原理: 从点云中提取具有区分性的几何特征，如边缘点（Edge points，局部曲率大的点）和平面点（Planar points，局部曲率小的点）。然后通过匹配这些对应类型的特征（如点到线距离、点到面距离）来估计位姿变换。
      • 优点: 通常比直接处理所有点的 ICP/NDT 更快。在结构化环境中表现鲁棒。
      • 缺点: 特征提取本身有计算开销。在缺乏明显几何结构（如长廊、空旷场地）或点云稀疏的环境中可能失败。配准精度依赖于特征提取的质量和稳定性。
      • 代表算法: LOAM, LeGO-LOAM, LIO-SAM。
  • 关键 LiDAR SLAM 算法与代码库:
    • LOAM (LiDAR Odometry and Mapping): 里程碑式的基于特征的激光 SLAM 方法。采用高频率、低精度的里程计（Scan-to-Scan）和低频率、高精度的地图优化（Scan-to-Map）相结合的策略。提取边缘和平面特征进行匹配。代码库: laboshinl/loam_velodyne。
    • LeGO-LOAM (Lightweight and Ground-Optimized LOAM): 针对地面移动机器人（UGV）对 LOAM 进行的优化。增加了地面点分割步骤，在特征提取前进行点云分割，采用两步 LM 优化。算法轻量化，适合嵌入式系统。代码库: RobustFieldAutonomyLab/LeGO-LOAM。存在多个改进分支，如LeGO-LOAM-SR (ROS2 移植), LeGO-LOAM-BOR (代码重构优化), ayushgargdroid/lego-loam。SC-LeGO-LOAM 增加了基于 Scan Context 的闭环检测。
    • Cartographer: Google 开发的基于图优化的 SLAM 系统。支持 2D 和 3D 建图，可融合多种传感器（LiDAR, IMU, Odometry, GPS）。前端实时构建子图（Submap），后端进行全局位姿图优化和闭环检测。代码库: googlecartographer/cartographer (官方已不再积极维护)。ROS 社区维护有活跃的 fork 版本。
    • GMapping: 基于 RBPF 的 2D 激光 SLAM 算法，用于构建栅格地图，需要激光雷达和里程计数据。代码库: openslam_gmapping。
    • BLAM (Berkeley Localization and Mapping): 基于 LiDAR 的 3D SLAM 系统，后端使用 GTSAM 进行因子图优化。代码库: erik-nelson/blam。
    • BreezySLAM: 简单的激光 SLAM 实现，侧重 Python 接口，基于 tinySLAM/CoreSLAM。可集成里程计数据，使用粒子滤波。代码库: simondlevy/BreezySLAM。

表 3: LiDAR 扫描匹配技术对比

技术原理优点缺点典型应用/系统
ICP 及其变种迭代最小化对应点距离原理简单, 精度可较高 (尤其 Point-to-Plane 等变种)计算量大, 对初始值/离群点敏感, 易陷局部最优闭环验证, 精细配准
NDT 及其变种划分体素, 匹配点云在体素内的高斯分布速度快, 对初始值/离群点不敏感精度通常低于 ICP, 受栅格大小影响实时里程计, 初步配准
基于特征(LOAM)提取边缘/平面等几何特征, 匹配特征速度快 (处理稀疏特征), 结构化场景鲁棒特征提取开销, 非结构化/稀疏场景困难, 精度依赖特征质量LOAM, LeGO-LOAM

C. 视觉惯性 SLAM (Visual-Inertial SLAM, VI-SLAM / VINS)

Section titled “C. 视觉惯性 SLAM (Visual-Inertial SLAM, VI-SLAM / VINS)”

VI-SLAM 融合视觉（相机）和惯性测量单元（IMU）的数据来进行定位和建图。

  • 融合动机: 相机提供丰富的环境外观信息，但单目相机存在尺度模糊，且易受运动模糊、低纹理区域的影响。IMU 能够以高频率提供载体的加速度和角速度信息，从中可以解算出姿态、获取重力方向、提供度量尺度信息，但 IMU 自身存在快速漂移的问题。将两者融合可以利用其互补性：IMU 为视觉提供尺度、重力方向和高频运动估计，视觉为 IMU 提供漂移修正。
  • 融合方式:
    • 松耦合 (Loosely-coupled): 分别独立运行视觉 SLAM 和 IMU 积分，然后融合各自的位姿估计结果（如使用卡尔曼滤波）。实现简单，但没有充分利用传感器间的约束关系，精度和鲁棒性通常不如紧耦合。
    • 紧耦合 (Tightly-coupled): 将相机观测（如特征点）和 IMU 的原始测量（或预积分量）同时纳入一个统一的优化框架中，联合估计系统状态（位姿、速度、IMU 偏置、地图点等）。能够充分利用传感器间的约束，通常精度和鲁棒性更高。
  • 关键挑战:
    • 初始化: 系统启动时需要估计初始的尺度、重力方向、速度以及 IMU 偏置等未知量，这通常需要传感器有足够的运动激励。
    • 时间同步: 相机和 IMU 数据之间精确的时间戳对齐至关重要。
    • 计算量: 紧耦合优化通常计算量较大。
  • 关键 VI-SLAM 算法与代码库:
    • VINS-Mono / VINS-Fusion: 由香港科技大学团队开发的紧耦合、基于优化的 VIO/VI-SLAM 系统（后端采用滑动窗口因子图优化）。核心技术包括 IMU 预积分（有效处理高频 IMU 数据）、鲁棒的在线初始化、相机与 IMU 外参/时间戳在线标定、闭环检测（DBoW2）和全局位姿图优化。VINS-Fusion 是 VINS-Mono 的扩展，支持双目相机，并提供了与 GPS 融合的示例。代码库: HKUST-Aerial-Robotics/VINS-Mono, HKUST-Aerial-Robotics/VINS-Fusion。
    • OKVIS (Open Keyframe-based Visual-Inertial SLAM): 由苏黎世联邦理工学院团队开发的紧耦合、基于优化的 VIO 系统（后端采用基于关键帧的非线性优化）。支持多相机配置。以高精度著称。早期版本主要关注里程计功能，缺乏全局闭环优化。代码库: ethz-asl/okvis, ethz-asl/okvis_ros (ROS 版本)。
    • ORB-SLAM3 (VI 模式): ORB-SLAM3 集成了强大的视觉惯性 SLAM 功能。采用紧耦合、基于 MAP 估计的方法，包含鲁棒的 IMU 初始化流程。支持单目惯性和双目惯性配置。代码库: UZ-SLAMLab/ORB_SLAM3。
    • MSCKF (Multi-State Constraint Kalman Filter): 一种基于 EKF 的 VIO 方法，其特点是不将特征点加入状态向量，而是利用特征点在多个相机帧中的观测来构建约束，从而提高了滤波器的可扩展性。该方法被用于多机器人 SLAM。

D. 激光雷达惯性 SLAM (LiDAR-Inertial SLAM, LIO / LINS)

Section titled “D. 激光雷达惯性 SLAM (LiDAR-Inertial SLAM, LIO / LINS)”

LIO 融合 LiDAR 和 IMU 的数据。

  • 融合动机: LiDAR 提供精确的几何结构信息，但在几何特征稀疏或退化的环境（如长廊、大平面）中性能会下降，且快速运动会导致点云畸变。IMU 提供高频的运动估计，可以补偿 LiDAR 运动畸变（去畸变，Deskewing），并在 LiDAR 信息不足时辅助状态估计，提高系统的鲁棒性。
  • 融合方式: 同样分为松耦合和紧耦合。紧耦合将 LiDAR 点云（或提取的特征）和 IMU 测量（或预积分量）在一个统一的框架下联合优化，通常能获得更好的鲁棒性和精度。
  • 关键 LIO 算法与代码库:
    • LOAM (含 IMU): 原始 LOAM 算法利用 IMU 数据进行运动补偿（去畸变）。
    • LeGO-LOAM (含 IMU): 同样使用 IMU 进行去畸变。
    • LIO-SAM (LiDAR Inertial Odometry via Smoothing and Mapping): 紧耦合（也有观点认为其将 LiDAR 里程计结果而非原始点放入因子图，因此可视为松耦合）、基于优化的 LIO 系统（后端采用 GTSAM 因子图）。融合 LiDAR（边缘/平面特征）、IMU（通过预积分提供去畸变、里程计约束、初始化信息）以及可选的 GPS（提供全局约束）。包含基于 ICP 的闭环检测模块。代码库: TixiaoShan/LIO-SAM。
    • FAST-LIO / FAST-LIO2: 紧耦合、基于滤波的 LIO 系统（后端采用迭代扩展卡尔曼滤波 IEKF）。FAST-LIO 使用 LiDAR 特征点，而 FAST-LIO2 可以直接处理原始点云，并引入了 ikd-Tree 数据结构来加速地图更新和最近邻搜索，支持多种 LiDAR 类型（旋转式和固态）。以计算效率高和鲁棒性强著称。代码库: hku-mars/FAST_LIO。
    • FAST_LIO_SAM: 结合了 FAST-LIO2 的前端（处理原始点云）和 LIO-SAM 的后端（基于 GTSAM 的图优化），旨在提升全局一致性。增加了关键帧保存和改进的 GPS 因子约束。代码库: kahowang/FAST_LIO_SAM。
    • R3Live: 一个紧耦合的 LiDAR-惯性-视觉融合框架。其核心是基于 FAST-LIO 的激光惯性里程计（LIO）提供几何结构和运动估计，同时利用视觉惯性里程计（VIO）为 LIO 构建的点云地图进行实时 RGB 着色。代码库: hku-mars/r3live。
    • LINS (LiDAR-Inertial Navigation System): 通常指基于紧耦合 IEKF 的 LIO 系统。FF-LINS 是其一个变种，采用基于因子图优化的帧到帧（Frame-to-Frame）策略。

传感器融合，特别是视觉-惯性（VI-SLAM）和激光-惯性（LIO）的融合，正成为实现鲁棒 SLAM 的主流趋势。这是由单一传感器的固有局限性以及不同传感器之间的互补性所驱动的。纯视觉 SLAM 在处理尺度模糊、弱纹理和光照变化方面存在困难，而纯激光 SLAM 则在几何退化环境和缺乏外观信息方面表现不佳。IMU 虽然自身漂移严重，但能提供关键的度量尺度、重力方向和高频运动信息。通过紧密融合这些传感器（如 VINS-Mono, OKVIS, LIO-SAM, FAST-LIO），可以显著克服各自的弱点，获得更鲁棒、更精确的 SLAM 系统。大量专用 VI 和 LIO 算法的涌现印证了这一趋势。

同时，在单一传感器模态内部，并不存在绝对最优的前端方法。无论是视觉领域的特征点法、直接法、半直接法，还是激光雷达领域的 ICP、NDT、基于特征的方法，各种对比研究都持续揭示了它们在不同场景下的性能权衡。例如，特征点法对光照鲁棒但需要纹理；直接法能处理弱纹理但对光照敏感；ICP 精度高但慢且敏感；NDT 快但精度稍低；LOAM 类方法在结构化场景快但在非结构化场景可能失败。这意味着算法的选择必须紧密结合具体的应用需求、环境特性和可用的计算资源。

此外，前端在整个 SLAM 流程中扮演着至关重要的角色。它不仅负责初步的运动估计（里程计），还承担着数据筛选（如关键帧选择）、特征/点云处理、以及为后端提供优化所需的约束（如里程计约束、闭环候选）等任务。前端输出的质量，如扫描匹配的精度 或数据关联的正确性，直接决定了后端优化的效果乃至整个系统的成败。因此，拥有一个鲁棒的前端（如 ORB-SLAM3, LIO-SAM, FAST-LIO 中的前端模块）对于构建高性能的 SLAM 系统是不可或缺的。

IV. SLAM 后端：建图与全局一致性

Section titled “IV. SLAM 后端：建图与全局一致性”

SLAM 系统的后端（Backend）主要负责处理前端输出的信息（如位姿估计、关键帧、特征点、闭环候选），进行全局优化，以消除累积误差，获得全局一致的机器人轨迹和环境地图。

A. 位姿图优化 (Pose Graph Optimization, PGO)

Section titled “A. 位姿图优化 (Pose Graph Optimization, PGO)”

PGO 是现代 SLAM 后端的核心技术之一。它将 SLAM 问题抽象为一个图，其中节点代表机器人的关键位姿，边代表位姿之间的相对运动约束（来源于里程计）或观测约束（来源于闭环检测）。

  • 目标: 通过调整图中所有位姿节点，使得所有约束边的误差总和最小，从而获得全局最优的轨迹估计。这通常被构建为一个非线性最小二乘问题。
  • 离群点抑制 (Outlier Rejection): 由于前端提供的约束（尤其是闭环约束）可能存在错误（离群点），后端优化必须具备鲁棒性。常用的方法包括使用鲁棒代价函数（如 Huber Loss, Cauchy Loss），或者采用显式的离群点剔除机制，如 RANSAC、最大一致性集（Maximum Clique）方法、以及更先进的技术如 Graduated Non-Convexity (GNC，用于逐步逼近非凸优化问题的解) 或 Incremental Consistency Maximization (ICM，在将约束加入图之前检查其与其他约束的一致性)。
  • 求解库: 常用的后端优化库包括 g2o, GTSAM, 和 Ceres Solver。这些库提供了因子图的构建、优化算法的实现以及稀疏线性代数求解器接口。

B. 闭环检测 (Loop Closure Detection)

Section titled “B. 闭环检测 (Loop Closure Detection)”

闭环检测是识别机器人是否回到先前已访问区域的过程，对于校正长期运行中累积的漂移、保证地图全局一致性至关重要。

  • 流程: 通常包括三个步骤：

    1. 候选帧生成: 根据当前位姿，在历史位姿中搜索可能的回环候选帧（通常基于距离或外观相似度）。
    2. 候选帧验证: 对候选的回环对进行几何一致性验证，计算它们之间的精确相对位姿变换，并判断是否为真回环。
    3. 约束添加: 如果验证成功，则将计算出的相对位姿变换作为一个新的约束添加到后端的位姿图（或因子图）中。

  • 常用技术:

    • 基于外观的方法 (视觉 SLAM):

      利用图像的外观信息进行场景识别。

      • 词袋模型 (Bag-of-Words, BoW): 将图像表示为视觉词汇的直方图，通过比较直方图的相似度来快速检索相似场景。DBoW2 是广泛使用的 BoW 库，被 ORB-SLAM、SVO Pro 等系统采用。传统 DBoW2 为提高精度，通常需要连续多帧匹配成功才进行几何验证，这可能降低召回率。ORB-SLAM3 提出先进行几何验证，再检查局部共视一致性，以提高召回率。
      • 序列匹配: 如 SeqSLAM，通过比较图像序列的相似性来识别回环，对视角变化不敏感。
      • 挑战: 外观方法易受光照、视角、季节变化、动态物体遮挡等因素影响。

    • 基于几何的方法 (LiDAR SLAM):

      利用点云的几何结构信息进行场景识别。

      • 扫描匹配: 在非连续的扫描帧之间进行 ICP 或 NDT 等配准。
      • 全局描述子: 为每个扫描（或局部地图）计算一个全局描述子，通过比较描述子来快速查找回环候选。例如 Scan Context、SOLiD 等。
      • 基于特征的方法: 匹配从不同扫描中提取的几何特征。
      • 挑战: 累积的里程计漂移会影响回环候选的搜索范围。全局描述子可能对视角变化敏感，且在重复结构区域区分度不高。

    • 多机器人闭环: 需要检测不同机器人之间的回环，这增加了匹配的复杂性和对鲁棒性的要求。LAMP 2.0 系统为此设计了候选帧优先级排序和鲁棒的两阶段配准流程。

C. 地图表示 (Map Representation)

Section titled “C. 地图表示 (Map Representation)”

SLAM 系统构建的地图用于定位、路径规划和人机交互等任务。地图的表示方式影响着系统的效率、精度和功能。

  • 基于特征的地图 (Feature-based): 主要由稀疏的三维点（路标点）组成，每个点通常关联着一个或多个特征描述子。这种地图表示存储高效，但缺乏环境的稠密几何细节。常见于基于特征点的视觉 SLAM 系统，如 ORB-SLAM, PTAM。

  • 稠密地图 (Dense):

    • 体素栅格 (Voxel Grid): 将空间离散化为三维栅格（体素），每个体素存储占据信息、颜色、距离场值等。例如 OctoMap 使用八叉树结构高效存储稀疏和稠密区域。
    • 截断符号距离场 (Truncated Signed Distance Field, TSDF): 在体素栅格中存储每个体素中心到最近表面的有符号距离（截断在一定范围内）。常用于实时三维重建，可以方便地提取表面网格。KinectFusion 及其变种是代表。
    • 面元地图 (Surfel Map): 将环境表面表示为一系列小的椭圆形面片（Surfel），每个面元包含位置、法线、颜色、半径等信息。
    • 稠密点云 (Dense Point Cloud): 直接存储大量的原始或处理后的三维点。几何信息丰富，但存储和处理开销大。
    • 优点: 提供详细的环境几何结构。缺点: 计算和存储开销大。代表系统: LSD-SLAM, ElasticFusion。

  • 栅格地图 (Grid Map): 将环境（通常是 2D 平面）离散化为规则的二维网格，每个单元格存储其被占据的概率。计算简单，广泛应用于基于 2D LiDAR 的 SLAM（如 GMapping）和路径规划。通常假设单元格之间统计独立。

  • 拓扑地图 (Topological Map): 用图结构表示环境的连通关系，节点代表环境中的地点（Place）或区域，边代表地点之间的可达路径。不关注精确的几何细节，而是环境的结构。可以作为度量地图的补充，用于全局路径规划和场景识别。

  • 隐式神经表示 (Implicit Neural Representations): 使用神经网络来表示场景。

    • NeRF (Neural Radiance Fields): 使用多层感知机（MLP）学习从空间坐标和视角方向到体积密度和颜色的映射，通过体积渲染生成新视角图像。
    • 高斯溅射 (Gaussian Splatting, 3DGS): 使用大量三维高斯基元来表示场景，每个高斯包含位置、形状（协方差）、颜色和不透明度等参数。渲染速度快。
    • 优点: 能够表示复杂的几何和外观，实现照片级的渲染效果。缺点: 训练和查询可能计算量大，与传统 SLAM 框架的结合仍在发展中。将在第五节详细讨论。

  • 语义地图 (Semantic Map): 在几何地图的基础上，为地图元素（点、体素、面片、物体）赋予语义标签（如“桌子”、“椅子”、“墙壁”）。提供更高层次的环境理解。

后端优化（如 PGO）的成功极大地依赖于前端提供的输入质量，包括里程计估计的精度和闭环候选的可靠性。虽然鲁棒的后端优化算法（如使用 GNC）可以在一定程度上容忍前端的误差，但它们无法从根本上修复由严重错误（如持续的错误数据关联或大量错误的闭环）导致的偏差。因此，高质量的前端输出是后端成功优化的前提。

闭环检测仍然是 SLAM 领域，尤其是在大规模、动态变化或外观变化剧烈的环境下面临的主要挑战之一。如何在确保准确性（避免错误闭环，即高精度）的同时，尽可能多地找到真实的回环（高召回率）是一个困难的权衡。里程计漂移、描述子的鲁棒性、验证的计算成本 以及多机器人场景下的复杂性 都是当前研究需要解决的问题。ORB-SLAM3 对 DBoW2 流程的改进 正是针对这一权衡的尝试。

地图表示方式也在不断演进。早期的 SLAM 系统主要使用稀疏特征点 或二维栅格地图。随着应用需求（如 AR/VR 中的真实感渲染和交互，机器人与环境的精细交互）的发展，对更丰富地图表示的需求日益增长，推动了稠密几何地图（如网格、面元）、隐式神经表示（NeRF/3DGS） 以及语义地图 的研究。

V. 前沿进展与研究方向

Section titled “V. 前沿进展与研究方向”

随着基础 SLAM 理论和技术的成熟，研究重点逐渐转向应对更复杂、更真实的场景，以及赋予系统更高级的能力。

A. 动态环境下的 SLAM

Section titled “A. 动态环境下的 SLAM”
  • 挑战: 现实世界中普遍存在的移动物体（行人、车辆等）违反了传统 SLAM 的静态世界假设，会对定位精度和地图构建产生严重干扰。
  • 主要方法:
    1. 检测并剔除动态物体: 通过几何约束（如对极约束、多视图几何）、运动一致性检查或语义信息（利用深度学习进行物体检测或分割）来识别场景中的动态元素，然后将与这些动态元素相关的传感器测量数据视为离群点，从 SLAM 的状态估计中剔除。代表系统有 DynaSLAM, DS-SLAM, Detect-SLAM, CFP-SLAM, SD-SLAM。深度学习方法（目标检测、语义分割）在此类方法中应用广泛。
    2. 同时跟踪相机与动态物体 (DATMO / Dynamic SLAM): 不仅估计相机（机器人）自身的位姿和静态环境地图，还显式地对场景中的动态物体进行建模和运动状态估计。这可以提供更完整的场景理解，支持预测和规划。不同的方法在如何表示动态物体（如在世界坐标系下表示点的轨迹，还是在物体自身坐标系下表示点）上有所不同。代表系统有 SLAMMOT, DynaSLAM II, DynoSAM。
    3. 鲁棒估计: 采用对离群点不敏感的鲁棒优化技术或代价函数。
    4. 四维表示: 将地图表示扩展到时间维度（4D），如使用 4D 高斯溅射，直接对动态场景进行建模。代表系统有 D4DGS-SLAM, DGS-SLAM。

B. 长期 SLAM / 终身 SLAM (Long-Term / Lifelong SLAM)

Section titled “B. 长期 SLAM / 终身 SLAM (Long-Term / Lifelong SLAM)”
  • 挑战: 使 SLAM 系统能够在广阔空间或长时间（跨越数天、数月甚至数年）内持续稳定运行，并保持地图的一致性。这需要应对环境外观的剧烈变化（如光照、天气、季节、场景布局改变）、地图规模的不断增长带来的存储和计算压力，以及避免在新环境中学习时遗忘旧环境（灾难性遗忘）。
  • 主要方法:
    1. 鲁棒的场景识别: 开发能够在外观发生显著变化时仍能准确识别出曾到访地点的技术。
    2. 高效的地图管理: 设计能够高效存储、更新、检索大规模地图的数据结构和策略。例如，使用多地图系统（如 ORB-SLAM3 的 Atlas、LAMP 2.0）来管理不同区域或时间的地图。
    3. 持续学习 (Continual Learning): 使系统能够增量式地学习新环境的外观和结构，同时保留对旧环境的记忆，避免灾难性遗忘。BioSLAM 借鉴生物记忆机制，设计了双内存系统（静态内存区 SMZ 和动态内存区 DMZ）来实现终身场景识别。Continual SLAM 结合了终身 SLAM（同一环境长期运行）和域适应（跨环境知识迁移）的概念。

C. 语义 SLAM (Semantic SLAM)

Section titled “C. 语义 SLAM (Semantic SLAM)”
  • 挑战: 将高层次的语义理解（如物体识别、场景分类、物体属性和关系理解）融入 SLAM 框架。
  • 动机: 构建包含语义信息的地图，使机器人能够更好地理解环境，进行更智能的交互和任务规划。语义信息也有助于改善数据关联（如利用物体作为路标或约束）、闭环检测和场景识别。
  • 主要方法:
    1. 语义标注: 利用深度学习模型（如语义分割、物体检测网络）对图像或点云进行处理，为地图元素（点、体素、面元、物体）赋予语义标签。
    2. 基于对象的 SLAM: 将环境中的物体（如门、窗、家具）作为地图的基本单元或路标进行建模和跟踪。
    3. 联合优化: 将语义一致性（如物体类别约束、空间关系约束）作为额外的约束项加入 SLAM 的后端优化框架中。
    4. 分层语义表示: 使用层次化结构来组织语义信息，以提高效率和表达能力。例如，Hier-SLAM++ 118 利用 3D 高斯溅射作为基础表示，并结合大型语言模型（LLM）和三维生成模型构建语义类别的层次树，实现了端到端的语义信息编码和优化。

D. 深度学习在 SLAM 中的应用

Section titled “D. 深度学习在 SLAM 中的应用”

深度学习（DL）和强化学习（RL）技术正越来越多地被集成到 SLAM 系统中，以提升性能或实现新功能。

  • 应用方式:

    1. 替代传统模块:

      使用 DL 模型替代 SLAM 流水线中的特定模块，例如：

      • 基于学习的特征提取与描述。
      • 基于学习的深度估计（尤其用于单目 SLAM）。
      • 基于学习的光流估计。
      • 基于学习的视觉里程计或激光里程计。

    2. 端到端 SLAM: 尝试使用单个或多个深度网络直接从传感器输入学习到相机位姿和地图表示（目前较少见，且可解释性和鲁棒性保证是挑战）。

    3. 提升感知能力: 用于语义分割、物体检测，以支持语义 SLAM 或动态 SLAM。

    4. 增强鲁棒性: 学习环境先验知识，处理困难场景（如弱纹理、光照变化）。

    5. 神经地图表示: 利用 NeRF 或 3D 高斯溅射 (3DGS) 等神经表示方法构建稠密、照片级的环境地图。这是当前的研究热点，但仍面临可扩展性、动态场景处理、与鲁棒跟踪/闭环的集成等挑战。代表系统包括 iMAP, NICE-SLAM, Point-SLAM, GO-SLAM (NeRF)；GS-SLAM, Photo-SLAM, SplaTAM, Gaussian-SLAM (3DGS)。GigaSLAM 尝试将此类方法应用于大规模室外场景。Scaffold-SLAM 旨在提高跨相机类型（单目、双目、RGB-D）的渲染质量。

    6. 强化学习 (RL): 用于学习 SLAM 相关的决策过程，如路径规划、探索策略、闭环决策、避障等。RL 的优势在于其自适应性、对环境变化的鲁棒性以及通过与环境交互学习优化策略的能力。

E. 多传感器融合 (Multi-Sensor Fusion)

Section titled “E. 多传感器融合 (Multi-Sensor Fusion)”
  • 动机: 通过融合来自不同类型传感器的信息（如相机、LiDAR、IMU、GPS、雷达、轮式里程计、热成像等），利用它们各自的优势，弥补单一传感器的不足，从而提高 SLAM 系统的整体精度、鲁棒性和适用范围。视觉惯性(VIO)和激光惯性(LIO)融合已在第三节详细讨论。
  • 挑战: 不同传感器之间精确的空间标定（外参）和时间同步至关重要。选择合适的融合架构（松耦合、紧耦合或两者的结合）也是关键。

F. 多机器人 SLAM (Multi-Robot SLAM)

Section titled “F. 多机器人 SLAM (Multi-Robot SLAM)”
  • 目标: 让多个机器人协同工作，更快、更鲁棒地完成大范围环境的地图构建和定位任务。
  • 挑战: 如何在有限的通信带宽和时延下进行机器人间的有效协作？如何设计系统架构（集中式 vs 分布式）？如何进行地图融合？如何可靠地检测和验证机器人之间的闭环（Inter-robot Loop Closure）？如何维护整个系统的一致性？8。
  • 系统架构:
    • 集中式 (Centralized): 所有机器人都将传感器数据或局部地图发送到一个中央处理站，由中央站负责全局的地图融合、闭环检测和优化（例如 LAMP 2.0）。优点是协调相对简单，易于保证全局一致性。缺点是存在单点故障风险，对通信带宽要求高，可扩展性受限。
    • 分布式/去中心化 (Decentralized): 每个机器人独立进行局部 SLAM，并通过与邻近机器人的机会性通信来交换信息（如相对位姿、地图片段、闭环信息），共同维护一个一致的全局地图。优点是鲁棒性强（无单点故障），可扩展性好。缺点是需要设计复杂的分布式数据融合和一致性维护算法（共识算法）。代表系统有 DOOR-SLAM, Kimera-Multi，以及基于分布式变分推断的方法。

当前 SLAM 研究的趋势表明，研究者们正努力将 SLAM 系统从实验室环境推向更复杂、更真实的现实世界应用。这体现在对动态环境、长期运行 和语义理解 等问题的日益关注上。传统的静态世界、短期运行的几何 SLAM 已不能满足这些需求。

深度学习，特别是 NeRF 和 3D 高斯溅射等神经表示方法，正在深刻改变 SLAM 中的地图构建环节，使得创建高保真、照片级的环境模型成为可能。这对于 AR/VR 等应用极具价值。然而，将这些新颖的表示方法与鲁棒、高效的实时跟踪和全局优化（如闭环）相结合，并扩展到大规模、无约束的室外场景，仍然是当前研究的重点和难点。

解决高级 SLAM 问题往往需要融合多个前沿领域的技术。例如，处理动态环境通常需要借助语义分割；实现长期 SLAM 则需要鲁棒的场景识别和持续学习能力；而多机器人 SLAM 的可靠运行离不开鲁棒的机器人间闭环检测。这表明 SLAM 领域的进展常常是通过跨子领域的交叉创新实现的。

这也意味着“SLAM”的内涵正在扩展。它不再仅仅是关于几何定位和建图，而是越来越多地包含场景理解（动态、语义）和长期自适应能力，与计算机视觉、机器学习、终身学习等领域的关系日益紧密。最终目标是为机器人构建持久、信息丰富且能够自适应的空间智能。

VI. 资源：开源软件与数据集

Section titled “VI. 资源：开源软件与数据集”

SLAM 领域的发展极大地受益于开放共享的文化。大量的开源软件库和公开数据集为研究人员和开发者提供了便利，加速了技术的迭代和验证。

A. 关键开源库与框架

Section titled “A. 关键开源库与框架”
  • ROS (Robot Operating System): 是机器人领域事实上的标准中间件。它提供了一套用于编写机器人软件的框架，包括硬件抽象、设备驱动、库函数、可视化工具、消息传递、包管理等。许多 SLAM 算法都提供了 ROS 接口或基于 ROS 实现，便于集成和在真实机器人上部署。
  • PCL (Point Cloud Library): 是一个功能强大的开源库，专注于 2D/3D 点云数据的处理。它提供了点云获取、滤波、分割、配准（如 ICP, NDT）、特征提取、表面重建、可视化等大量算法和工具，是 LiDAR SLAM 和三维重建领域的基础库之一。
  • OpenCV (Open Source Computer Vision Library): 是计算机视觉领域最流行的开源库。它包含了大量的图像处理、特征检测与匹配、目标跟踪、机器学习等算法，是视觉 SLAM 系统不可或缺的组成部分。
  • 优化库:
    • g2o (General Graph Optimization): 一个用于优化基于图的非线性误差函数的 C++框架，广泛用于 SLAM 后端的位姿图优化和 BA。
    • GTSAM (Georgia Tech Smoothing and Mapping): 另一个流行的用于平滑（优化）和建图的 C++库，基于因子图，提供了高效的稀疏非线性优化求解器。
    • Ceres Solver: Google 开发的用于求解大规模非线性最小二乘问题的 C++库，也被许多 SLAM 系统（尤其是视觉 SLAM）用作后端优化器。
  • OpenSLAM.org: 曾是 SLAM 领域重要的开源代码和数据集共享平台。虽然其网站本身可能不再活跃更新，但它托管的许多经典算法（如 GMapping, g2o, TORO, tinySLAM）的代码库已被迁移到 GitHub 上的 OpenSLAM-org 组织下。
  • 特定算法代码库: 本综述在第三节和第五节中提及的许多关键 SLAM 算法都提供了开源实现，例如：ORB-SLAM3, VINS-Mono, LIO-SAM, FAST-LIO, Cartographer, LeGO-LOAM, DSO, SVO, OKVIS 等。这些代码库是学习、使用和改进这些算法的重要资源。
  • 深度学习 SLAM 框架: 随着深度学习在 SLAM 中的应用增多，也出现了一些旨在统一开发和评估流程的框架，如 XRDSLAM 和与 NeRF 相关的 NeRFStudio。

B. 标准基准数据集

Section titled “B. 标准基准数据集”

公开数据集对于算法的评估、比较和复现至关重要。以下是一些 SLAM 领域常用的基准数据集：

  • KITTI: 自动驾驶领域最著名的数据集之一。包含在德国卡尔斯鲁厄市区、乡村和高速公路上采集的多传感器数据（LiDAR, 双目/单目相机, IMU, GPS）和高精度地面真值。广泛用于评估各种 SLAM/里程计算法的性能。
  • EuRoC MAV: 专为视觉惯性 SLAM 设计的无人机数据集。包含在室内环境中飞行的微型飞行器采集的双目相机图像、IMU 数据以及高精度的运动捕捉系统提供的地面真值。是评估 VIO/VI-SLAM 算法（如 VINS-Mono, ORB-SLAM3, OKVIS）的标准基准。
  • TUM RGB-D: 包含使用 RGB-D 相机（如 Kinect）在不同室内场景采集的数据序列，提供高精度的地面真值。常用于评估 RGB-D SLAM 算法以及新兴的基于神经表示的 SLAM 方法。
  • TUM VI: 另一个用于视觉惯性里程计基准测试的数据集。
  • ScanNet: 大规模室内场景 RGB-D 数据集，包含语义和实例分割标注，常用于语义 SLAM 研究。
  • Replica: 高质量的室内场景合成数据集，提供逼真的渲染效果和完整的地面真值（位姿、深度、语义等），常用于评估基于神经表示（NeRF/3DGS）的 SLAM 算法的建图质量。
  • 其他: 还存在许多其他数据集，如 LeGO-LOAM 使用的 Stevens 数据集，用于模拟飞行的 TartanAir 数据集，以及一些专门针对动态场景 或长期运行等特定挑战设计的数据集。此外，还有如 SLAMBench 和 evo 等评估工具用于轨迹精度和性能的量化评估。

SLAM 领域丰富的开源软件和公开数据集构成了其繁荣发展的重要基石。ROS、PCL、OpenCV 等基础库，以及 g2o, GTSAM, Ceres 等优化库 提供了强大的工具支撑。OpenSLAM.org 等平台和众多算法的开源代码库 促进了知识的传播和技术的迭代。标准数据集如 KITTI 和 EuRoC 则为算法性能的量化比较提供了统一的平台。

这种开放的生态系统极大地降低了研究人员和开发者的入门门槛，使得他们能够站在前人的肩膀上进行创新，并通过标准化的基准测试进行公平的比较。然而，工具和依赖的多样性（如特定版本的 ROS、OpenCV、Ceres、Eigen 等，常在代码库中注明）有时也会给环境配置和系统集成带来挑战。像 XRDSLAM 这样的框架正试图缓解这些问题。

VII. 结论与未来展望

Section titled “VII. 结论与未来展望”

A. 技术现状总结

Section titled “A. 技术现状总结”

经过数十年的发展，SLAM 技术取得了长足的进步。基于优化的后端（如图优化、BA）已成为主流，取代了早期的滤波方法，显著提升了精度和一致性。传感器融合，特别是视觉-惯性（VIO）和激光-惯性（LIO）的紧耦合，已成为提高系统鲁棒性和精度的标准范式。几何 SLAM（即关注定位和几何地图构建）技术相对成熟，涌现出如 ORB-SLAM3, VINS-Mono, LIO-SAM, FAST-LIO, Cartographer 等一系列优秀的开源系统。同时，深度学习正以前所未有的深度和广度渗透到 SLAM 的各个环节，不仅用于改进传统模块（如特征提取、深度估计），还催生了新的地图表示方法（NeRF/3DGS），并被用于应对动态环境、语义理解和长期运行等挑战性问题。

B. 持续存在的挑战

Section titled “B. 持续存在的挑战”

尽管取得了显著进展，但实现普适、鲁棒、高效的 SLAM 系统仍面临诸多挑战：

  • 鲁棒性: 在各种具有挑战性的现实环境中（如弱纹理、剧烈光照变化、恶劣天气、大规模重复结构、传感器数据缺失或退化、存在大量动态干扰等）保持稳定可靠的定位和建图仍然困难。
  • 可扩展性: 随着环境规模增大和运行时间延长，如何有效管理地图数据、控制计算复杂度和内存消耗，以支持大规模、长期 SLAM，仍然是一个关键问题。
  • 数据关联与闭环检测: 在存在感知混淆（Perceptual Aliasing，即不同地点看起来相似）或环境外观随时间显著变化的情况下，实现高精度、高召回率且低误报率的数据关联和闭环检测仍然极具挑战性。
  • 地图表示: 如何构建既能精确表达环境几何结构，又能包含丰富语义信息，同时存储高效、易于更新和查询，并能满足下游任务（如规划、交互）需求的地图表示，是一个持续探索的方向。
  • 动态环境: 如何在存在大量不可预测动态物体的环境中，既能精确估计自身运动，又能有效感知、跟踪甚至预测动态物体的行为，仍然是一个开放性难题。
  • 深度学习集成: 如何更好地将基于学习的方法（尤其是端到端方法或神经表示）与传统的几何 SLAM 框架相结合，以确保系统的可解释性、泛化能力、实时性能和鲁棒性保证，仍需深入研究。

C. 未来研究方向

Section titled “C. 未来研究方向”

未来 SLAM 的研究预计将在以下几个方向重点发展：

  • 增强鲁棒性与自适应性: 开发能够主动感知环境挑战（如传感器退化、动态干扰强度）并自适应调整策略的 SLAM 系统，提高对各种干扰和传感器故障的内在鲁棒性。
  • 终身学习与环境适应: 研究能够持续学习、适应环境变化（光照、季节、布局等）而无需人工干预，并能长期稳定运行的终身 SLAM 系统。
  • 更丰富的场景理解: 将几何信息、语义信息、动态信息更紧密地融合，构建更全面、更高层次的环境模型，实现真正的场景理解。
  • 神经表示的深化应用: 进一步提升基于 NeRF/3DGS 等神经表示的 SLAM 系统的实时性、可扩展性（尤其对于大规模室外场景和纯单目输入）和鲁棒性，并探索其在地图更新、闭环检测等方面的潜力。
  • 分布式多智能体 SLAM: 研发更具可扩展性、鲁棒性和通信效率的分布式多机器人协同 SLAM 技术。
  • 可解释性与安全性: 提高 SLAM 系统（尤其是基于学习的系统）的可解释性，并为其在安全关键应用（如自动驾驶）中的部署提供可靠性与安全性保证。
  • 资源效率: 针对计算能力和功耗受限的平台（如小型无人机、移动设备、边缘计算设备），开发更轻量级、更高效的 SLAM 算法。

总结来说，尽管 SLAM 技术在过去几十年中取得了巨大的进步，但要实现能够在复杂、动态、非结构化的真实世界中长期自主运行的、真正智能的 SLAM 系统，仍然面临诸多挑战。未来的研究趋势似乎指向更紧密地结合传统几何方法、深度学习（用于感知、表示和学习）以及可能的强化学习（用于决策），朝着构建更全面的空间人工智能系统迈进。这种融合有望最终克服当前技术的局限，推动 SLAM 在更广泛领域的应用。

VIII. 附录：关键 SLAM 算法概览

Section titled “VIII. 附录：关键 SLAM 算法概览”

表 4: 关键 SLAM 算法汇总

算法名称类型 (主要)关键传感器核心思想/贡献主要开源代码库 (示例)
GMapping2D LiDAR SLAM (滤波, RBPF)2D LiDAR, Odometry高效 Rao-Blackwellized 粒子滤波构建 2D 栅格地图, 改进提议分布和自适应重采样ros-perception/openslam_gmapping
Cartographer2D/3D SLAM (优化, 图优化)LiDAR, IMU, Odo, GPS基于子图构建的实时前端和全局位姿图优化后端googlecartographer/cartographer (及 ROS forks)
LOAM3D LiDAR Odometry & Mapping (优化, 特征)3D LiDAR, (IMU)高频里程计+低频建图优化, 提取边缘/平面特征laboshinl/loam_velodyne
LeGO-LOAM3D LiDAR O&M (优化, 特征)3D LiDAR, IMU轻量级, 针对地面车辆优化, 增加地面分割和点云分割RobustFieldAutonomyLab/LeGO-LOAM
LIO-SAMLiDAR-Inertial SLAM (优化, 图优化)3D LiDAR, IMU, GPS紧耦合 LiDAR(特征)+IMU(预积分)+GPS 因子图优化 (GTSAM), 含闭环TixiaoShan/LIO-SAM
FAST-LIO2LiDAR-Inertial Odometry (滤波, IEKF)3D LiDAR, IMU紧耦合 LiDAR(原始点)+IMU 迭代 EKF 滤波, 计算高效, ikd-Tree 加速hku-mars/FAST_LIO
R3LiveLiDAR-Inertial-Visual SLAM (混合)3D LiDAR, IMU, RGB基于 FAST-LIO 的 LIO 提供几何+VIO 提供颜色, 实时 RGB 稠密建图hku-mars/r3live
ORB-SLAM3Visual / VI-SLAM (优化, 特征)Mono/Stereo/RGB-D, (IMU)基于 ORB 特征, 支持多种传感器, Atlas 多地图系统, 改进场景识别, MAP 估计UZ-SLAMLab/ORB_SLAM3
DSOVisual Odometry (优化, 直接稀疏)Monocular, (Stereo)稀疏直接法, 最小化光度误差, 联合优化位姿和逆深度, 含光度标定JakobEngel/dso
SVOVisual Odometry (优化, 半直接)Monocular, (Multi-Cam)半直接法, 跟踪 FAST 特征点周围像素块, 速度快；SVO Pro 增加鱼眼、闭环等uzh-rpg/rpg_svo, uzh-rpg/rpg_svo_pro_open
VINS-MonoVisual-Inertial SLAM (优化, 混合)Monocular, IMU紧耦合优化 VIO (滑动窗口因子图), IMU 预积分, 鲁棒初始化, 在线标定, 闭环HKUST-Aerial-Robotics/VINS-Mono
OKVISVisual-Inertial Odometry (优化, 混合)Multi-Camera, IMU紧耦合优化 VIO (关键帧非线性优化), 支持多相机ethz-asl/okvis
BLAM3D LiDAR SLAM (优化, 图优化)3D LiDAR基于 GTSAM 后端的 LiDAR SLAMerik-nelson/blam
BreezySLAM2D/3D LiDAR SLAM (滤波, 粒子)LiDAR, (Odometry)基于 tinySLAM 的简单 SLAM 实现, Python 为主simondlevy/BreezySLAM
Hier-SLAM++Semantic 3DGS SLAM (优化, 神经)RGB-D / Monocular基于 3D 高斯溅射, 引入分层语义表示 (LLM+生成模型), 支持单目Hier-SLAM/Hier-SLAM-PlusPlus (预期)
DynoSAMDynamic SLAM (优化, 图优化)Visual / VI统一优化框架估计相机、静态/动态结构和物体运动RoboticImaging/DynoSAM
BioSLAMLifelong SLAM (学习, 场景识别)Visual / LiDAR模拟生物记忆的双内存系统 (SMZ/DMZ) 实现终身场景识别, 对抗灾难性遗忘(未明确提供, 需查阅论文)
LAMP 2.0Multi-Robot LiDAR SLAM (优化, 集中式)LiDAR, Odometry集中式多机器人 SLAM, 鲁棒闭环检测 (优先级+GNC 优化)nebula-autonomy/LAMP

(注: 此表仅列出部分代表性算法，SLAM 领域算法众多，开源状态和链接可能随时间变化。类型分类主要依据其核心特点，部分系统可能融合多种技术。)

参考文献

Section titled “参考文献”