强推！首个全面涵盖LiDAR里程计算法的综述（一）：LiDAR-Only与LIO算法综述

该篇文章是对LiDAR里程计（Odometry）的最新进展和挑战进行的全面综述。是首个全面涵盖LiDAR里程计各类方法的综述，填补了之前研究的空白。为了让读者深入了解该项技术，笔者结合自身的一些经验和理解，将拆分成三个部分为大家详细解读，分别为：LiDAR-Only与LIO算法综述（一），多激光与多传感器融合激光里程计综述（二），激光里程计尚存挑战，公开数据集与评估方法综述（三）。本篇文章为第一讲：LiDAR-Only与LIO算法综述。

激光雷达里程计的发展背景与重要性

最初，里程计主要依赖轮式编码器和航位推算方法。 然而，这种方法由于轮胎打滑和传感器误差，导致了精度不足。为了克服这些问题，研究人员开始探索其他传感器，例如距离传感器和视觉传感器。这一转变也与计算机视觉的重大进展相吻合，尤其是在视觉里程计方面，成为了一个重要的研究领域。

扫描配准算法的开发（如迭代最近点算法ICP）进一步提升了基于距离传感器的里程计能力。这一时期是里程计历史上的一个关键转折点，形成了两个主要的研究流派：视觉里程计和基于距离传感器的里程计。 这些进展为探索3D激光雷达技术奠定了基础，而激光雷达提供了更强大的3D空间测量能力。

尽管视觉里程计取得了长足进展，但其在低光环境（如夜间操作）中的局限性仍然存在。 为应对这些挑战，研究人员逐渐转向激光雷达技术，它具备3D扫描能力，并且不受光照条件的影响。这一转向促成了基于激光雷达的里程计技术的快速发展，使其成为为自主机器人提供精确位置信息的关键工具。

激光雷达传感器简介

| 什么是激光雷达？

激光雷达（LiDAR，全称为“光探测与测距”）是一种利用激光对空间信息进行距离测量和探测的传感器，目前在机器人和智能驾驶领域被广泛运行。激光雷达的基本原理是：向目标区域发射激光脉冲，当这些脉冲遇到障碍物时，部分光会反射回激光雷达传感器。通过测量每个激光脉冲返回所需的时间，并利用光速，激光雷达可以计算到目标的距离。

| 激光雷达的分类

激光雷达可以根据其不同的成像架构和测量原理进行分类。 激光雷达的成像机制主要分为三类：机械激光雷达、固态激光雷达和非扫描架构的Flash激光雷达。关于测量原理，主要有脉冲飞行时间（ToF）、幅度调制连续波（AMCW）和频率调制连续波（FMCW）激光雷达。此外，激光雷达还可以根据探测距离、视场（FOV）和波长等属性进行进一步细分。目前使用较多的是机械激光雷达以及半固态激光雷达，代表性的生产企业有：速腾，禾赛，大疆览沃，Luminar等，代表性的产品有：Avia，AT128等。

LiDAR-only里程计算法综述

LiDAR-only的里程计通过分析连续两帧的激光雷达扫描来确定智能体(包括但不限于移动机器人，车辆，后文将不再强调)的位置。这一过程的核心技术就是扫描匹配技术，可以根据扫描匹配的方式分为三类：(1) 直接匹配，(2) 基于特征的匹配，(3) 基于深度学习的匹配。

| 直接匹配

顾名思义，直接匹配方法就是通过对连续两帧激光雷达扫描数据进行匹配进而得到智能体的位置，这是激光雷达里程计中最简单直接的方法。迭代最近点（ICP）算法是一种常用的技术，通过多次迭代最小化误差函数（通常是匹配点对之间的欧式距离）来估计这种变换。然而，ICP算法存在一些缺点：

• 容易陷入局部最小值，因此需要一个可靠的初始估计

• 对噪声敏感，尤其是动态物体。

• 其迭代性质可能导致较高的计算开销，有时会导致计算速度过慢。

因此，许多研究致力于提高ICP算法的性能，以改善里程计精度。

对于ICP的改进算法包括：

• 修剪ICP（TrimmedICP，TrICP）通过使用最小修剪平方法代替标准的最小二乘法来增强传统的ICP算法，核心思想是通过最小化残差最小的子集点的平方和，提升了计算速度和鲁棒性

• Chen和Medioni提出的点面ICP通过引入现实场景中常见平面的信息，改进了传统的点到点ICP算法。与之对应的还有点线ICP。点点ICP，点线ICP以及点面ICP各有其应用场景，具体可以参考我之前的一篇文章 《彻底弄懂ICP配准算法》，清楚的讲解了上述三种ICP算法的原理与实现以及如何针对不同场景该如何选择。

• 广义ICP（Generalized-ICP） 在概率框架内结合了点到点ICP和点到面ICP，在最小化步骤中利用了点的协方差。该方法在保持标准ICP算法速度和简单性的同时，展现了对噪声和异常值的更强鲁棒性。

• NICP扩展了广义ICP，通过在6维空间中评估距离，包括3D点坐标及其相应的表面法线信息，从而进一步提升性能。

• LiTAMIN和LiTAMIN2通过减少点云数量并修改传统ICP的代价函数，支持更快速的配准，并增强了配准的鲁棒性。

除ICP外，另外一种被广泛使用的直接配准算法就是NDT配准算法。正态分布变换（NDT）算法是一种无需建立点对应关系的匹配方法。 NDT算法通过为体素化目标点云，并假设其满足正态分布，基于空间概率函数中的可能性来对齐两个点云。Hong和Lee通过引入概率NDT表示改进了传统的NDT算法。为点样本分配了概率，通过结合计算的协方差来解决退化效应。研究表明，概率NDT在里程计估计方面优于传统的NDT。

尽管scan-to-scan匹配算法取得了进展，其精度仍然受到限制。因此，最新的激光雷达里程计工作大多通过结合scan-to-scan匹配和scan-to-map匹配(这里的map主要指local map)来估计机器人的位姿。IMLS-SLAM通过基于隐式移动最小二乘（IMLS）表示的scan-to-map匹配来估计里程计，而DLO通过结合选定的关键帧子集中的点云创建子地图以进行scan-to-map匹配。

传统的激光雷达里程计通常在接收到新的激光雷达点云时计算离散的里程计估计。相比之下，一些方法旨在模拟机器人实际的连续运动，并进行连续轨迹的建模，即连续时间LiDAR Odometry。代表性的工作CT-ICP通过在激光雷达扫描的起始和结束位姿之间对各个点的位置进行插值，从而实现这一点。随后，通过scan-to-map匹配对每个点进行配准，从而获得连续时间的里程计估计。连续时间LiDAR Odometry相比与传统里程计的一个核心区别就是考虑了时间属性，该属性也是天然就有的，因此精度上的确有所提高。

| 基于特征的匹配

基于特征的激光雷达里程计算法的核心思想是：通过从激光雷达点云中提取特征点，并利用这些特征点来估计智能体位姿。该类方法在进行点云配准时仅使用提取到的特征点，而不是当前帧整个点云，可以消除噪声等异常值，从而提高计算速度和整体性能。基于特征的方法的主要挑战在于选择“优质”的特征点，以提升点云配准性能。

其中极具代表性和开创性的算法就是大名鼎鼎的LOAM及其衍生改进方法。LOAM通过评估局部表面平滑度来识别尖锐边缘和平面表面特征点，并通过这些特征点的匹配来估计机器人的运动。LOAM框架的后续发展着重于通过优化特征点的选择来提升性能。

• LeGO-LOAM通过点云分割将点分类为地面点或分割点，确保特征提取的准确性。它利用来自地面点的平面特征和来自分割点的边缘特征，逐步确定6自由度（DOF）的变换。LeGO-LOAM的优化思想值得大家借鉴，后续的很多算法也采用了类似方法，即：将六自由度的位姿估计结合提取到的特征点分解成两个部分进行逐步优化，地面点用于约束roll,pitch,z，边缘特征用于优化yaw,x,y。

• R-LOAM和RO-LOAM通过结合已知全局坐标的参考物体的3D三角网格特征来优化机器人的轨迹。

• SuMa通过比较当前扫描的顶点和法线图与基于surfel的地图渲染出的顶点和法线图，使用表面法线进行里程计估计。

• SuMa++ 将来自RangeNet++的语义信息集成到surfel-based地图中，并应用语义ICP，将语义约束添加到ICP算法的目标函数中。

• F-LOAM着重于从机械激光雷达的点云中提取明显的水平特征，其中垂直方向的数据稀疏，而水平方向的数据较密集。这种方法减少了水平面中虚假特征检测的风险。

• Zhou等人和π-LSAM在室内环境中联合优化关键帧姿态和平面参数，称为平面调整（PA）。

• MULLS提取多种特征点（地面、立面、柱子、梁）并使用scan-to-map的多度量线性最小二乘ICP（MULLS-ICP）。

• VoxelMap采用自适应大小的粗到精体素构建，以应对不同的环境结构和稀疏、不规则的激光雷达点云。它通过概率平面表示解决了激光雷达测量噪声和姿态估计误差的不确定性。该算法本人测试过，精度还是非常高的，但是可能涉及到一些调参过程。

除了ICP算法的各种变体之外，NDT算法也可以在使用特征时应用。

• NDT-LOAM最初通过加权NDT（wNDT）算法获得近似的里程计估计，然后通过结合角点和表面特征来精炼该估计。

• E-LOAM提取几何和强度特征，并通过局部结构信息增强这些特征，使用D2D-NDT匹配估算里程计。

• Wang等人提出了一种结合NDT和PLICP（点到线ICP） 的粗到精配准度量。通过NDT粗略估计的位姿作为PLICP的初始猜测，从而获得更准确的位姿估计。

| 基于深度学习的匹配

虽然直接匹配和基于特征的方法在多种环境中表现良好，但它们在对应关系匹配方面往往遇到困难。为了解决这一挑战，保持特征的一致性并找到每次扫描之间的关系至关重要。一些研究人员开始探索深度学习方法，这些方法在解决这些问题方面展现出巨大的潜力。

• LO-Net提出了一种scan-to-scan的激光雷达里程计网络，能够预测法线，识别动态区域，并结合时空几何一致性约束来改进连续扫描之间的交互。

• LodoNet利用了一种将匹配的关键点对（MKPs）从激光雷达范围图回投到三维点云的过程，这涉及使用受PointNet启发的MKPs选择模块，以帮助识别最佳匹配点用于估计旋转和平移。

• Cho等人将无监督学习技术利用在激光雷达里程计中，采用VertexNet量化点的不确定性，并使用PoseNet预测帧间的相对姿态。该网络通过估计法向量来结合几何信息，并使用不确定性加权的ICP损失。在监督训练过程中，他们通过FOV损失解决了平凡解的问题。

| 总结

LiDAR-inertial里程计(LIO)算法综述

仅基于激光雷达的里程计在计算效率上非常高，因为它不需要额外的传感器。但是，也存在一些缺陷，比如在特征退化场景下的定位问题。因此，更为实用的激光雷达里程计通常将激光雷达与IMU结合使用。IMU提供角速度和线性加速度的测量，适合用于估算粗略的机器人运动，在结合激光雷达时提高位姿估计的准确性。

激光雷达-惯性里程计(LIO，后续用LIO代替)将激光雷达和IMU数据融合对智能体实时位姿进行估计，该融合方法主要分为两大类，分别为：松耦合融合估计和紧耦合融合估计。

• 基于松耦合的LIO：独立估算每个传感器的状态，随后将这些状态按权重组合，最终确定机器人的状态。这种方法具有很高的灵活性，因为它能够分别估算每个传感器的状态。只要为新传感器模态创建合适的里程计模块，它可以很容易地适应传感器系统的变化，而不需要对现有框架进行大幅修改。此外，它允许为特定的传感器分配权重，确保在某个传感器性能欠佳的情况下依然能够利用其他传感器的数据，保持系统的鲁棒性。

• 基于紧耦合的LIO：同时利用所有传感器的测量数据来估计机器人的状态。相比松耦合方法，这种方法能够在位姿估计过程中引入更多的约束，从而可能带来更高的里程计精度。然而，这种方法的计算负荷较大，因为必须同时处理所有的观测数据。此外，如果某个传感器提供的观测数据质量较差，系统的鲁棒性可能会受到更大的影响。

至于这两种融合方式的选择，个人经验是： 想要更高的精度可以优先选择紧耦合融合方式，但这并不是说紧耦合精度一定高于松耦合。这种方法特别适用于只有LiDAR和IMU这两种传感器的框架，并想得到高精度定位结果。如果融合框架有多种传感器，比如还有轮速，RTK等，可以优化考虑松耦合，因为调试起来更方便。

| 基于松耦合的LIO算法

在现有的仅基于激光雷达的方法中，LOAM和LeGO-LOAM通过引入IMU传感器来校正激光雷达扫描中的畸变并提供初始运动估计，取得了重要进展。在这些改进的基础上：

• Zhou等人通过INS和编码器数据估计机器人的粗略位姿，随后使用NDT算法进行激光雷达里程计的精细化处理。

• Tang等人使用扩展卡尔曼滤波器（EKF）来融合来自激光雷达和IMU传感器的独立位置结果。

• Zhen等人采用误差状态卡尔曼滤波器（ESKF），将来自IMU的先验运动模型与激光雷达推导的部分后验信息融合，以提高系统的鲁棒性和精度。

• Hening等人利用自适应EKF，在INS与GPS、INS与激光雷达的残差中进行估计，进一步优化结果。

• Yang等人选择姿态图优化，通过结合INS和基于激光雷达扫描匹配的估计来实现准确且可靠的状态估计。

尽管松耦合方法相比仅使用激光雷达的方法在精度上有所提升，并且具有模块化的灵活性，但它们未能充分发挥传感器之间的协同作用。这促使了对紧耦合方法的研究增加，这些方法旨在最大限度地整合传感器，以提高里程计性能。

| 基于紧耦合的LIO算法

与松耦合技术相比，紧耦合方法在统一框架中处理多个传感器的数据。这种集成处理利用了不同传感器模态之间的相互依赖性，旨在提高状态估计过程的准确性和鲁棒性。

这种方法的开端是Zebedee，这是3D LIO领域的开创性工作。 Zebedee通过优化表面对应误差和IMU测量偏差来进行里程计估计。最初，直接将IMU测量集成到因子图中由于6D位姿参数的高频输出而带来了计算挑战。IMU预积分方法的出现解决了这个问题，它将关键帧之间的数百个IMU测量数据压缩为一个IMU预积分因子。这使得每个传感器测量数据都能包含在因子图中，加速了基于图优化的激光雷达里程计方法的发展。

在这些进展的基础上，进一步的创新也随之而来。

• LIPS通过构建包含连续IMU预积分因子和来自激光雷达测量的3D平面因子的因子图，解决图优化问题以获取机器人里程计。

• IN2LAMA使用IMU的上采样预积分测量（UPM）对激光雷达扫描进行去畸变，并结合激光雷达因子、IMU偏差因子和传感器时间偏移因子进行批量流形优化。

• 后续版本IN2LAAMA引入IMU预积分因子，进一步精确去畸变所有激光雷达测量。尽管这种先进的去畸变过程提高了精度，但可能会影响实时操作。

• LIO-SAM中，通过IMU预积分估计的运动不仅用于去畸变激光雷达扫描，还作为因子图中的一个因子，用来构建帧间约束。

• LIO-Mapping利用激光雷达扫描和预积分IMU测量进行联合优化，并引入旋转约束优化。

在紧耦合方法的进一步发展中，研究重点转向了特征选择和全局优化。

• KFS-LIO引入了一个度量标准，用于选择最有效的激光雷达特征子集，从而简化现有的基于图的算法。

• Li等人通过层次化姿态图优化和固态激光雷达的创新特征提取方法，解决了不规则扫描模式的问题，并使用度量加权函数来量化每个激光雷达特征的残差。

• Koide等人利用GPU加速的体素化广义ICP匹配代价因子和IMU预积分因子，并采用基于关键帧的固定延迟平滑技术高效估算低漂移轨迹，创建最小化全局注册误差的因子图。

• Setterfield等人直接将激光雷达测量中的特征对应关系纳入因子图中。

与LiDAR-only里程计算法类似，用了使用天然具有的时间信息，基于连续时间的紧耦合LIO应运而生。

• CLINS采用连续时间框架，利用三次B样条进行轨迹估计，通过优化控制点和节点实现任意时间点的轨迹估计。CLINS擅长处理来自激光雷达和IMU传感器的异步数据，并在高动态场景中表现出色，这使其能够有效应对由于不同采集时间导致的点云畸变问题。

• Wildcat通过滑动窗口方式整合异步的激光雷达和IMU测量，并使用连续时间轨迹表示。

• DLIO则利用层次化几何观测器代替滤波器进行状态估计，并提出了一种新的粗到精连续轨迹方法，使用恒定加加速度模型大幅减少计算开销。

伴随着基于图优化的方法得到发展，各种因子被集成到因子图中以提高里程计性能。然而，这类方法的计算需求日益增加，引发了对计算负荷的担忧。因此，基于滤波器的多种方法应运而生，通常基于经典的卡尔曼滤波器。

• LINS使用迭代误差状态卡尔曼滤波器（iESKF），相比基于图的算法实现了更快的里程计估算。尽管为提高计算效率做了许多尝试，LINS系统在计算负荷和处理速度上仍面临挑战，尤其是在计算大量激光雷达测量的卡尔曼增益时。

• FAST-LIO通过引入一种新的卡尔曼增益公式成功解决了这一问题。FAST-LIO2通过直接将原始激光雷达测量与地图配准，不再需要特征提取过程，进一步提高了精度，并通过一种称为ikd-Tree的数据结构提升了计算速度。

• Faster-LIO则用增量体素（iVox）替代了ikd-Tree以实现更快的搜索。

• Shi等人利用不变扩展卡尔曼滤波器（Invariant EKF）来减轻基于EKF的里程计估计中的线性化误差，这种误差可能会显著影响估计性能。与标准EKF相比，不变EKF表现出更好的收敛性和一致性，能够提供更可靠的结果。基于该理论提出了两种新方法：Inv-LIO1和Inv-LIO2。Inv-LIO1通过scan-to-scan匹配初步估计状态，并使用地图模块进行优化；而Inv-LIO2通过scan-to-map匹配执行里程计，并集成全局地图更新，尽管精度提高了，但计算时间也增加了。

基于图优化和滤波器的方法的进步极大地增强了LIO在典型环境中的可靠性。现如今，许多方法专门设计用于在复杂场景中（如动态和退化环境中）进行鲁棒的里程计估计。

• Ding等人基于贝叶斯网络的因子图优化，考虑了如城市区域等高动态场景。

• RF-LIO以IMU预积分进行初始位姿估计，利用IMU预积分与扫描匹配之间的误差生成距离图，并去除动态点。此外，RF-LIO还使用图优化进一步提高了位姿估计的准确性。

• Hu等人在FAST-LIO2中结合了基于分割的运动物体检测和验证，以处理动态环境中的数据关联不准确问题。

• LIMOT通过基于轨迹的多目标跟踪估计自车辆和动态目标物体的位姿，通过分离动态和静态物体的位姿因子，整个因子图可以同时过滤动态物体并进行位姿估计。

• Kim等人提出了一种自适应关键帧生成方案，该方案考虑了周围环境，从而在极端环境中实现更高的里程计精度。

在上述基础上，一些算法结合多种约束和度量标准，以进一步优化里程计精度。

• LION引入了可观测性度量，用于预测估计里程计质量的潜在下降风险。该可观测性分数指导系统切换到通过监督算法（如HeRo）辅助的备用里程计算法。

• LIO-Vehicle通过将地面车辆的运动约束扩展到2自由度车辆动力学，处理几何退化的环境。

• Zeng等人提出了一种基于单线深度变化的特征提取方案，专门设计用于固态激光雷达的不均匀采样点云特性。

• Chen等人通过对地面车辆使用SE(2)约束位姿估计来解决非SE(2)车辆运动扰动问题。

• Li等人通过在几何平面特征中结合强度边缘特征，改进了特征提取过程。他们还使用基于残差和配准一致性的多加权函数在位姿优化过程中评估每个特征的质量。

• RI-LIO在其状态估计过程中结合了两种残差类型：来自反射率图像的光度误差和来自几何点的点到平面距离。这些图像是使用真实角度修正投影（CPBRA）方法生成的，解决了激光雷达投影偏差问题。

另一种提高精度的方法是高频里程计，通过分割激光雷达扫描进而改进运动估计。这些高频方法为快速变化场景中实现更灵敏和精确的里程计提供了一条路径。

• LoLa-SLAM通过分割激光雷达扫描，确保有足够的测量数据进行准确匹配，从而实现了低延迟、高时间更新率的定位。这种方法对于高频里程计至关重要，因为它允许更频繁和及时地更新车辆的位置。

• FR-LIO通过自适应将激光雷达扫描分为多个子帧，以应对动态环境中的激进运动，从而提高估计的鲁棒性，尤其是在动态环境中。- Zhao等人引入了迭代ESKFS，以缓解由于子帧增多引起的退化问题。

• Point-LIO通过逐点框架实现高频里程计。这种方法涉及在每个点级别处理激光雷达扫描，自然消除了运动畸变。

此外，深度学习方法在提升里程计估计中也起到了关键作用。

• Chen等人将因子图用于状态估计，并结合基于学习的点云网络进行闭环检测。

• Liu利用自适应粒子群滤波器结合高效重采样策略来处理环境多样性，并集成轻量化学习的闭环检测。

• Liu和Ou提出了基于学习的闭环检测FG-LC-Net，并使用数据结构S-Voxel来提高系统速度。

| 总结

每日更新自动驾驶与机器人领域最新研究成果，星球内容包括：人形机器人，端到端自动驾驶，优秀论文深度透析，高精定位，大规模建图，三维重建，nerf与3DGS，感知与人工智能，规控决策等数十个领域优秀技术方案分享，点云配准、传感器标定、特征提取等工程性技术方案分享，行业招聘信息，国内外行业热点新闻

点击下方卡片

第一时间获取最热行业热点资讯，最新智驾机器人行业技术