激光雷达SLAM入门——如何从激光点云中确定“我在哪里”

自动驾驶汽车顶部的旋转传感器，或是扫地机器人顶部的窗口——这些大多是激光雷达（LiDAR，光探测和测距），它利用激光直接测量与周围环境的距离。LiDAR-SLAM 技术仅使用激光雷达返回的点云数据（可选地借助惯性测量单元 (IMU) 等传感器）即可同时完成自定位和地图构建。视觉 SLAM（参见视觉 SLAM 入门）试图从相机的二维图像中间接恢复三维结构，而 LiDAR-SLAM 则直接从三维距离信息入手——这就是二者的关键区别。本文从激光雷达的测量原理入手，逐步介绍点云对齐的两大支柱算法——ICP 和 NDT，以及基于特征的设计方法 LOAM、回环检测和图 SLAM，最后阐述地标算法的演变及其在实践中的选择。最新进展请参阅激光雷达 SLAM 技术趋势。

Velodyne LiDAR 系列

Livox LiDAR 示例

图片：Velodyne LiDAR 传感器系列 (APJarvis，CC BY-SA 4.0) / Livox Mid-40、Horizon 和Tele-15（Dllu，CC BY-SA 4.0），维基共享资源。此处仅列举具有代表性的产品系列，并非文中提及的最新型号。

0. 本文内容

• 激光雷达实际测量的是什么，以及它为何能“直接”获取距离

• SLAM 的基本问题如何在点云世界中体现

• 两种主要的扫描匹配算法 ICP（迭代最近点）和 NDT（正态分布变换）的工作原理及其区别

• 激光雷达里程计的两种模式：扫描到扫描和扫描到地图

• LOAM 为何要提取特征（边缘和平面），以及其背后的设计理念

• 回环检测和姿态图优化如何协同工作

• 地标算法（ICP / NDT / LOAM / LeGO-LOAM / A-LOAM / Cartographer / LIO-SAM / FAST-LIO2）之间的区别以及如何选择

1. 什么是 LiDAR-SLAM？

一句话概括：激光雷达SLAM技术利用激光飞行时间（或相位偏移）获取的一组高精度距离测量数据（点云），通过反复比对连续扫描数据（扫描匹配），同时估计传感器自身的运动轨迹及其周围环境的三维地图。

输入是激光雷达以固定间隔输出的点云（每次扫描包含几千到几十万个点），输出——与视觉SLAM类似——包含两项内容：传感器在每个时刻的六自由度位姿（三个自由度用于定位，三个自由度用于方向），以及周围环境的三维点云地图。例如，自动驾驶汽车可以在估计自身位置的同时，与周围的车辆、行人以及道路基础设施保持精确的距离关系；自主移动机器人可以在仓库货架间精确导航；无人机在隧道或建筑物内部等几何形状复杂的空间中飞行——在所有这些情况下，激光雷达-SLAM技术都是其幕后功臣。

2. 激光雷达究竟测量什么？

激光雷达测距原理大致分为两类。在飞行时间 (ToF)方法中，传感器发射激光脉冲，测量光束从目标反射返回所需的时间T，然后根据光速c计算距离D。

由于光需要往返传播，因此测量时间T必须减半才能得到单程传播时间。另一种方法是相移（FMCW/AMCW），它发射连续调制的激光束，并通过发射信号和反射信号之间的相位差来计算距离。无论哪种方式，激光雷达不仅能探测到“那里有东西”，还能在一次观测中获取到该物体的实际绝对距离——这是相机完全不具备的能力。正如我们将在第 4 节中看到的，单目相机图像本身永远无法还原真实世界的比例尺，而激光雷达点云从一开始就带有度量比例尺。

激光雷达硬件也分为两大类。机械旋转式激光雷达通过旋转堆叠的多个激光发射器/接收器元件来构建完整的 360 度点云；Velodyne 和 Ouster 是该领域的代表性制造商。固态激光雷达没有旋转的机械部件，而是使用 MEMS 微镜或光学相控阵来扫描有限的视场，从而实现更小的尺寸和更低的成本；Livox 是该领域的代表性产品。固态激光雷达单元虽然视野较窄，但在许多产品中，其非重复扫描模式（每次扫描同一点但路径略有不同）却能带来更高的密度，这意味着随着点云累积时间的增加，点云密度也会随之增加。

激光雷达点云还具有相机图像完全不具备的结构。旋转的激光雷达在水平旋转的同时，发射多束垂直堆叠的激光（例如，16层、32层或128层），从而产生水平方向密度高、垂直方向稀疏且分层的点云——一种各向异性分布。这种结构直接影响着LOAM的特征提取和ICP的法线估计的设计，我们将在下文中详细介绍。

3. SLAM的基本问题：“当前点云在地图中的位置在哪里？”

正如视觉SLAM通过匹配特征点来解决“当前图像在过去图像和地图中的位置”问题一样，激光雷达SLAM通过对齐点云来解决“当前扫描（点云）在过去扫描和地图中的位置”问题。无论使用何种传感器，其根本问题本质上是相同的：单次观测永远无法确定绝对位置；唯一的办法是不断利用与过去观测（地图）的对应关系来估计当前位置。

区别在于这种对应关系的实质内容。视觉SLAM检测图像中的“显著点”（特征），将它们周围的外观以描述符的形式进行数值编码，并通过描述符的相似性来搜索对应关系。相比之下，激光雷达点云几乎不包含亮度或颜色等“外观”信息（虽然反射强度可用，但其区分度远低于图像描述符）。因此，在大多数情况下，点云匹配完全由空间邻近性驱动——即“哪个点在几何上最接近”。寻找两个点云之间刚体变换（旋转R，平移\mathbf{t}）的整个过程称为扫描匹配，它是LiDAR-SLAM的核心。

本节的要点很简单：视觉SLAM采用“特征匹配+极线几何”，而LiDAR-SLAM则用“扫描匹配”取而代之。接下来的两节将介绍实现扫描匹配的两大支柱算法：ICP和NDT。

4. 理解ICP

ICP（迭代最近点）是一种经典的点云配准算法，由Besl和McKay于1992年发表在IEEE模式分析与机器智能汇刊上，至今仍被广泛使用。顾名思义，该算法基于一个简单的思想：将“最近点”视为对应点，然后反复优化。

该算法可以分为三个步骤。

1. 对应点搜索：对于移动点云（源点）中的每个点 p_i，在固定点云（目标点）中找到距离最近的点 q_i——通常使用空间搜索结构，例如 KD 树。

2. 变换估计：给定这组对应点 \{(p_i, q_i)\}，找到使对应点之间距离之和最小的旋转 R 和平移 \mathbf{t}。最简单的版本，即点对点误差，定义如下。

1. 应用并迭代：将得到的 R 和 \mathbf{t} 应用于整个源点云，然后返回步骤 1 并再次搜索对应点。这个对应点搜索 → 变换估计 → 应用的循环会重复进行，直到误差足够小（或不再减小）。

点对点误差很容易理解，但当对齐两个宽阔的平面（例如一面墙）时，从误差的角度来看，点在平面上的确切位置几乎无关紧要（只要垂直偏移量很小，沿平面的小幅滑动几乎不会改变误差），因此收敛速度往往很慢。点对平面误差解决了这个问题：它根据对应点 q_i 周围的局部形状估计法向量 n_i，并仅最小化沿法线方向（点到平面的距离）的偏差。

点到平面算法允许沿平面方向存在偏移，而仅严格惩罚垂直于平面的偏差。众所周知，点到平面算法比点到点算法收敛速度更快、稳定性更高，尤其是在平面丰富的环境中，例如建筑物内部。然而，正如第 2 节所述，旋转激光雷达的点云具有分层且垂直方向稀疏的结构，因此简单地估计法线可能会产生受分层结构影响的噪声法线——法线估计本身需要谨慎处理。

ICP 算法有两个主要缺点。首先，由于对应搜索完全依赖于“最近几何邻域”，较大的初始偏移会导致错误的对应关系，并收敛到局部最小值——因此需要一个良好的初始猜测。其次，对每帧包含数万个点的点云重复运行最近邻搜索计算量巨大，且与点云规模成正比。接下来介绍的 NDT 算法采用了一种完全绕过对应搜索的不同方法。

5. 理解 NDT

NDT（正态分布变换） 是一种扫描匹配算法，其基本方法与 ICP 算法截然不同。该算法由 Biber 和 Straßer 于 2003 年在 IEEE/RSJ 国际智能机器人与系统会议 (IROS) 上发表。最初，NDT 是为二维激光扫描仪提出的；后来，它被扩展到可以处理三维点云。

NDT 的核心思想是将点云表示为在规则网格上每个体素定义的概率分布，而不是表示为一组单独的点。每个体素内的点分布近似为均值为 \mu、协方差矩阵为 \Sigma 的高斯分布（正态分布）。

通过将整个目标点云表示为一组基于体素的高斯分布，形成分段连续、可微的概率密度函数，不再需要“寻找点对点对应关系”来对齐源点云。相反，对于应用于每个源点 \mathbf{x}_i 的变换 T（旋转和平移），得到 \mathbf{x}_i' = T(\mathbf{x}_i)，然后将该变换后的点在其对应体素的高斯分布下的“合理性”进行求和，作为评分函数，并找到使该评分函数最大化的 T。

此优化问题可使用基于梯度的方法（例如牛顿法）求解。与 ICP 不同，NDT 无需像 ICP 那样将“对应搜索”和“变换估计”作为单独的步骤交替进行——只需评估梯度与预先计算的高斯参数之间的关系即可——因此完全避免了最近邻搜索的开销。而且，由于单个点的噪声会被吸收到每个体素的均值和协方差中，因此结果往往对噪声具有鲁棒性。

然而，NDT 也存在自身的权衡：体素大小的选择。较大的体素计算量较小，但会平均掉细微的形状差异，从而降低对齐精度。较小的体素可以提高形状分辨率，但每个体素的点数较少，导致高斯估计本身不稳定，而且体素数量的增加也会提高计算成本。用户必须正确设置这个可调参数，这使得 NDT 的使用起来相当繁琐。

6. 激光雷达里程计（扫描到扫描/扫描到地图）

利用 ICP 或 NDT 算法计算连续帧之间的相对运动，并将这些估计值随时间累积的过程称为激光雷达里程计。与视觉 SLAM 中的视觉里程计类似，如果没有某种机制将估计值与整个地图进行协调（回环检测），漂移会随着时间的推移而累积，并且无法避免。

激光雷达里程计根据比较对象的不同分为两种类型。扫描到扫描仅将每次扫描与紧邻的前一次扫描进行比对；计算成本低，但由于每次估计误差都会直接影响到下一次扫描的初始估计值，因此漂移容易累积。扫描到地图则不是将当前扫描与单个先前的扫描进行比对，而是与整个累积的局部地图进行比对；利用更多观测数据可以降低噪声敏感性，并且通常比扫描到扫描（Scan-to-Scan）更精确，但代价是计算量更大，因为参考点云更大。

大多数实际的激光雷达SLAM实现方案都结合了这两种方法。首先获得一个快速、粗略的初始估计值——通过扫描到扫描、IMU预测或第5节中基于NDT的粗略配准——然后使用精确的扫描到地图配准来细化该初始估计值。下面的流程图展示了基于这种两阶段思路的通用激光雷达SLAM处理流程。

Diagram 1 · Use the button to switch views

Diagram 2 · Use the button to switch views

图 1 — 每次扫描的快速路径从校正倾斜开始，经过局部配准，更新姿态和局部地图，并将结果作为下一个初始猜测和参考地图。另一个低速率路径会验证关键帧历史记录中的重访，允许循环约束，并使用图优化来校正全局轨迹和地图。

图中所示的Deskew步骤（扫描过程中校正畸变）是激光雷达特有的。旋转的激光雷达的单次扫描并非瞬时完成，而是需要几十到几百毫秒才能完成。如果传感器本身在扫描过程中发生移动，那么在同一次扫描中，先前和之后采集的点最终会代表来自不同时间和位置的观测数据，所有这些数据都混合在一次扫描中（运动畸变）。Deskew使用IMU（或之前的速度估计值）来校正扫描过程中的这种运动，从而重建点云，使其如同从单一时刻采集而成。这种校正与VIO/LIO中介绍的IMU融合密切相关（参见VIO/LIO入门）。

7. 理解LOAM

LOAM（实时激光雷达里程计和地图绘制），由Zhang和Singh发表于LOAM 于 2014 年在机器人：科学与系统 (RSS) 会议上发表，其设计理念至今仍是许多 LiDAR-SLAM 实现的基础。LOAM 的核心创新在于仅提取几何上独特的点，而非使用点云中的所有点。

LOAM 评估每个点周围的局部平滑度（曲率），并将相对于周围环境曲率较高的点提取为边缘特征（物体的尖角或轮廓），将曲率较低的点提取为平面特征（平滑连续表面的一部分，例如墙壁或地板）。与使用包含数万个点的完整扫描数据进行配准不同，LOAM 仅提取这些特征，从而大幅降低了扫描匹配的计算成本。

特征提取完成后，误差计算公式与第 4 节中的 ICP 算法类似，但匹配的几何元素是“线”或“面”，而不是“点”。边缘特征 p_i 的匹配是通过最小化其到直线的距离来实现的。由前一次扫描（或地图）中的两个对应点 p_a 和 p_b 所构成的平面。

平面特征的匹配是通过最小化其到由三个对应点构成的平面的距离来实现的（与第 4 节中的点到平面误差形式相同）。将这些距离相加，并最小化旋转和平移，即可得到帧之间的相对运动。

LOAM 的另一个核心设计选择是其高频激光雷达里程计和低频激光雷达建图的双层结构。激光雷达里程计以高频（每次扫描）对紧邻的前一次扫描进行基于特征的扫描到扫描匹配，从而生成粗略但快速的位姿估计。激光雷达建图以该粗略估计作为初始猜测，并以低于激光雷达里程计的频率对整个累积地图进行扫描到地图匹配，从而生成更精确的位姿和更精细的地图。这两个过程以不同的频率并行运行。通过高频数据，最终利用激光雷达测绘的结果来修正激光雷达里程计的估计值，从而同时实现高频输出和高精度。这种“高频粗略估计+低频精确修正”的设计理念已被第9节中介绍的许多后续的LiDAR-SLAM和LIO实现所继承。

8. 回环检测和图SLAM

与视觉SLAM中的视觉里程计一样，激光雷达里程计本身也会随着时间的推移而累积漂移。即使机器人返回到起点，估计的轨迹也无法反映出这种漂移。修正这种累积误差的机制是回环检测。

在激光雷达领域，回环检测主要分为两个步骤。首先是位置识别：判断当前点云是否与先前访问过的位置的点云相似。由于点云不像图像那样包含亮度信息，因此这依赖于将点云本身的形状分布编码为描述符的方法。例如，像扫描上下文（Scan Context）这样的方法，将扫描分割成扇形单元格，并将每个单元格中的最大高度编码为描述符——或者使用概括整个点云几何特征的方法。其次是几何验证：对于位置识别提出的候选扫描对，实际尝试使用ICP或NDT进行扫描匹配，并检查匹配结果是否足够一致。只有通过这两个阶段，才能将“当前位置”和“先前访问该位置时的位置”联系起来的约束关系建立起来，并被采纳。

使用这种闭环约束来校正累积的位姿并将其映射成一个一致的整体，是位姿图优化（或更一般地，图SLAM框架）的任务。它构建一个图，其节点是传感器在每个时刻的位姿，边是帧之间（或由环路连接的两个遥远时刻之间）相对运动的约束，然后通过非线性优化调整节点（位姿），使所有边约束尽可能相互一致。通过闭环新添加的边起到了……的作用。将漂移（此前漂移仅沿一个方向传播）重新分布到构成环路的整个路径上。这种优化，正如 Visual-SLAM（参见Visual-SLAM 入门，第 10 节）一样，通常也依赖于 LiDAR-SLAM 实现中的 g2o、GTSAM 和 Ceres Solver 等库。

9. 地标算法

LiDAR-SLAM 的发展历程最容易从两个方面来理解：点云的配准方式，以及设计如何将范围缩小到显式特征。

ICP（Besl & McKay，1992），如第 4 节所述，是经典的、至今仍广泛使用的点云配准基础算法。它很少单独用于实时 SLAM，但几乎所有后续方法中都保留了某种基于对应关系的最小化方法。

NDT (Biber & Straßer, 2003)，如第 5 节所述，是一种基于高斯分布的扫描匹配方法，它避免了对应搜索，并与 ICP 一起作为另一种基础选择，广泛应用于从 2D 室内机器人 SLAM 到 3D 自动驾驶的各种应用中。

LOAM (Zhang & Singh, 2014)，如第 7 节所述，是一种引入边缘/平面特征提取以及两层高频里程计 + 低频建图设计的方法，至今仍是许多 LiDAR-SLAM 和 LIO 实现的基础。

LeGO-LOAM（轻量级地面优化激光雷达里程计和建图，Shan & Englot，发表于 2018 年 IEEE/RSJ 国际智能机器人与系统会议 (IROS)），专门针对地面车辆扩展了 LOAM。它首先将点云分离为地面点和非地面点，并使用地面点来估计横滚角、俯仰角等参数。以及高程和非地面点，用于估计剩余的自由度（水平位置和偏航角），从而实现轻量级设计，即使在计算能力有限的嵌入式系统上也能实时运行。

A-LOAM（高级 LOAM） 是基于 Ceres Solver 非线性优化算法的 LOAM 思想的简化开源重实现，由香港科技大学空中机器人小组等团队发布。它舍弃了原始 LOAM 的一些细粒度工程调优，转而采用更清晰的代码，并经常被引用为学习和实验 LOAM 系列算法的易用实现。

Cartographer（Hess、Kohler、Rapp 和 Andor，发表于 2016 年 IEEE 国际机器人与自动化会议 (ICRA)，Google） 基于 Ceres Solver 对每个子地图（子地图是多个扫描的集合）进行局部扫描匹配，并结合分治法进行快速回环检测。在候选空间上进行分支定界搜索。它同时支持二维和三维空间，并且由于 ROS 开源实现的广泛应用，已被广泛应用于室内地图构建。

LIO-SAM（基于平滑和地图构建的紧耦合激光雷达惯性里程计，作者：Shan、Englot、Meyers、Wang、Ratti、Rus，发表于 2020 年 IROS 会议） 是一种紧耦合的激光雷达-惯性里程计 (LIO) 方法，它在共享的因子图中联合优化 IMU 预积分因子、激光雷达扫描匹配里程计因子、GPS 因子和回环检测因子（详情请参见 VIO/LIO 入门）。

FAST-LIO2 由香港大学 MARS 实验室的 Xu、Zhang 及其同事发表，是一种基于……的快速 LIO 方法。紧耦合迭代卡尔曼滤波器。它使用顺序最近邻搜索结构（增量式 k-d 树，iKD 树）直接管理点云，无需显式特征提取步骤即可将点云直接与地图配准。其设计优先考虑在计算能力有限的小型平台上进行实时操作。2026 年的最新发展情况已在LiDAR-SLAM 技术趋势中介绍。

10. 方法比较

总的来说，可以将 ICP 和 NDT 视为“基础配准算法”，LOAM 系列（LOAM/LeGO-LOAM/A-LOAM）视为“通过特征设计提高效率”，Cartographer 视为“高效的全局一致性和回环检测”，LIO-SAM/FAST-LIO2 视为“通过紧密耦合 IMU 提高鲁棒性”——每个算法都是在前人基础上发展出的不同改进方向。

11. 激光雷达 SLAM 的不足之处

由于激光雷达主动发射激光来测量距离，因此它在黑暗和逆光环境下表现良好——而这些环境恰恰是视觉 SLAM 的弱项。但是LiDAR-SLAM自身也存在一些缺陷。

• 几何退化：在点云形状沿特定方向变化很小的环境中——例如长隧道或宽阔平坦的停车场——扫描匹配无法唯一确定沿该方向的平移或旋转。这是因为ICP/NDT优化算法会陷入一个“平坦谷”，在该谷中，沿该方向的任何值误差都几乎不变；其根本原因与Visual-SLAM在处理无特征墙体时遇到的缺陷相同。

• 恶劣天气：雨、雪、雾和灰尘会散射和吸收激光，衰减原本会返回的反射光，或产生噪声点（出现在实际不存在物体位置的虚假反射）。

• 镜子和透明物体：玻璃和镜子会镜面反射激光，在实际物体位置之外（镜像位置）产生虚假点。

• 动态物体：使用移动物体（例如行人、车辆）上的点，就好像它们本身是静止的一样。在配准过程中，静态环境会引入误差，影响传感器自身运动的估计。因此，检测并排除动态物体，或对其进行显式建模，就显得尤为必要。

• 点云密度与计算成本的权衡：更高分辨率、更高频率的点云往往能带来更精确的配准，但每帧点数越多，扫描匹配的计算成本就越高。在计算能力有限的嵌入式系统或无人机中，这种密度与速度之间的权衡成为一项重要的设计约束。

• 硬件成本：高分辨率、远距离旋转激光雷达的价格仍然远高于相机，成本往往是其普及应用的一大障碍。

这些弱点与相机的弱点方向不同，因此，将相机（视觉SLAM）和激光雷达（激光雷达SLAM）相结合——即传感器融合（参见传感器融合入门）——是弥补这些弱点的有效方法。单一传感器的局限性。

12. 实践选择

如何选择激光雷达-SLAM方法很大程度上取决于可安装的传感器类型、可用计算资源、所需精度以及环境的几何特征。

• 室内服务机器人和扫地机器人：低成本的二维激光雷达仍然是强有力的选择，而像Cartographer这样稳健的二维SLAM实现方案也得到了广泛应用。室内空间结构丰富，例如墙壁和家具，很少出现几何退化，因此是激光雷达-SLAM的理想环境。

• 自动驾驶汽车：高分辨率三维激光雷达结合GPS、IMU和摄像头等多传感器融合是基本假设。为了应对隧道和立交桥等几何退化区域，像LIO-SAM/FAST-LIO2这样紧密耦合IMU的配置方案具有显著优势。

• 无人机：在重量和功耗限制严格的情况下，固态激光雷达是理想选择。 （例如 Livox）与计算效率高的直接方法（例如 FAST-LIO2）相结合往往更受欢迎。

• 仓库和工厂的 AGV/AMR：具有规则的走廊式结构的环境很常见，因此基于二维激光雷达的扫描到地图 (Scan-to-Map) 方法（例如 Cartographer）是一个实用的选择。在货架布局频繁变化的环境中，地图更新频率也是选择的一个因素。

• 退化环境——隧道、地下空间、长直线走廊：单独使用激光雷达往往会导致配准不稳定，因此紧密耦合惯性测量单元 (IMU)（LIO-SAM/FAST-LIO2）或与独立数据源（例如轮式里程计）相结合实际上至关重要。

• 室内与室外：室内空间结构丰富，几何线索众多，更适合使用激光雷达 SLAM；而室外空间在开阔区域和恶劣天气条件下会带来退化问题，因此 IMU/GNSS 融合更为重要。

作为一条实用规则简述：对于计算资源有限且需要高频输出的情况，可以选择 FAST-LIO2 等直接方法；对于全局一致性要求较高且集成 GPS 的系统，可以选择 LIO-SAM；而 Cartographer 则因其在二维环境下的良好表现以及与 ROS 生态系统的紧密集成而备受青睐。鉴于如今对环境恶化适应性的要求，完全不使用 IMU 的配置（例如 ICP/NDT/LOAM 独立系统）几乎已成为不切实际的选择，大多数运行中的系统现在都以紧密耦合 IMU 作为基本假设。

13. 参考文献

• Besl & McKay，“三维形状配准方法”（IEEE TPAMI，1992）

• Zhang & Singh，“LOAM：实时激光雷达里程计和地图构建”（RSS，2014）

• Xu 等，“FAST-LIO2：快速直接激光雷达惯性里程计”（IEEE T-RO，2022）

14. 总结

LiDAR-SLAM 获取从激光飞行时间获得的度量尺度距离信息（点云），使用 ICP（最近点迭代配准）或 NDT（拟合体素高斯分布）进行扫描匹配，并累积……通过结合扫描到扫描和扫描到地图的激光雷达里程计进行轨迹定位，并通过回环检测和姿态图优化不断修正累积误差，最终实现同步自定位和建图。地标算法的演变历程——LOAM系列（通过特征提取提高效率）、Cartographer（提高全局一致性）以及LIO-SAM/FAST-LIO2（通过紧密IMU耦合提高鲁棒性）——各自针对不同的挑战而发展，在应对几何退化环境这一关键点上，大多数运行系统都趋向于以紧密IMU耦合为基线的设计。