论文链接：https://arxiv.org/pdf/2504.01451
项目主页：https://github.com/lian-yue0515/D-LI-Init

0. 简介

在现代野外机器人技术中，激光雷达-惯性里程计（LiDAR-Inertial Odometry, LIO）是核心技术之一。然而，LIO系统在紧急场景下的静止初始化特性却面临着严峻挑战，尤其是在高速公路自动驾驶或排爆机器人快速进入目标区域的情况下。本文介绍了一种新方法——D-LI-Init，通过动态初始化解决LIO在运动状态下的初始估计问题，提升了系统的鲁棒性和精度。

图1：展示了机器人在现实场景中需要动态初始化的情况，以及使用最先进算法和我们的方法创建的点云地图。

1. 主要贡献

1. 动态初始化方法：首次提出了一种无需特定运动模式的动态初始化方法，通过迭代对齐激光雷达里程计与IMU测量值，快速且鲁棒地估计系统初始状态。
2. 激光雷达与陀螺仪的紧耦合：基于误差状态迭代卡尔曼滤波（ESIKF）框架，实现了激光雷达与陀螺仪的紧耦合，显著提高了点云质量和状态估计精度。
3. 多平台评估：在多个平台（如车辆、手持设备和无人机）上评估了动态初始化性能，验证了该方法在各种场景下的优异表现。
4. 开源代码与数据集：提供了开源代码和测试数据集，为后续研究提供了基准，这一方法是首个开源的激光雷达-惯性SLAM动态初始化方案。

2. 相关工作

在SLAM系统中，稳健的初始化是一个关键挑战，尤其是对于激光雷达-惯性SLAM（LiDAR-inertial SLAM），该系统依赖于通过分析激光雷达里程计与惯性数据集成之间的关系来估计初始变量。本节将探讨现有的初始化方法和激光雷达里程计相关研究。

2.1 初始化方法

在激光雷达和视觉-惯性SLAM中，关键的初始变量如速度、重力方向和IMU偏差对于系统的准确性能至关重要。现有的激光雷达-惯性SLAM研究主要依赖静态初始化方法，这要求系统在一段时间内保持静止。在此期间，基于IMU的平均角速度估计陀螺仪偏差，而利用平均加速度与局部重力加速度的差异来确定加速度计的偏差，并从中推导出重力方向。虽然这种静态方法在从静止状态开始的车辆系统中表现良好，但对于需要在运动状态下进行初始化的系统，如紧急响应机器人和可穿戴激光雷达SLAM，这种方法显然有限。因此，迫切需要更为稳健的动态初始化方法，以应对系统处于运动状态时的场景。

在视觉-惯性SLAM中，单目相机缺乏度量尺度，因此机器人必须保持非零加速度才能进行初始化。这催生了动态方法的开发，例如一种紧耦合的闭式解法，该方法利用视觉特征和加速度计数据同时估计初始状态变量和特征深度，尽管其计算成本较高。ORB-SLAM3采用最大后验估计（MAP）处理IMU初始化，采用三步法在动态情况下估计初始参数，而无需专门的初始化阶段。此外，He等人提出了一种解耦的视觉-惯性里程计初始化方法，分开了旋转和位移的估计，通过线性平移约束以全局最优的方式求解初始速度和重力方向，从而实现更高的效率和鲁棒性。VINS-Fusion将仅使用视觉的运动结构与IMU预积分测量对齐，以恢复度量尺度、速度、重力方向和陀螺仪偏差。虽然这些视觉SLAM的初始化方法在视觉SLAM中有效，但并不直接适用于激光雷达-惯性系统，因为后者不需要在所有方向上进行尺度恢复或运动激励。然而，这些方法为激光雷达-惯性SLAM中的动态初始化提供了宝贵的见解，可以通过分析激光雷达里程计与惯性数据之间的关系来实现初始状态的估计。

2.2 激光雷达里程计

在激光雷达-惯性SLAM系统的动态初始化中，激光雷达里程计的准确性显著影响初始状态估计的精度。LOAM是激光雷达里程计的经典代表，通过提取线和面特征来实现更高的配准精度和算法效率。然而，这些方法依赖于恒定速度模型进行运动畸变补偿，这限制了它们在真实环境中的性能，尤其是在复杂运动场景下。CT-ICP引入了连续时间传感器运动的概念，将其建模为一个连续函数，确定每个激光雷达帧开始和结束时的车辆状态。尽管CT-ICP展现出高精度，但由于状态变量的维度增加，其计算成本也随之提高，并且偶尔会出现过拟合问题。KISS-ICP则提供了一种轻量级且高效的激光雷达里程计系统，通过点对点的ICP估计状态，确保在不同环境和运动模式下的稳定运行。然而，当使用Livox Mid360等设备生成稀疏点云时，其有效性会降低。I2EKF-LO引入了一种双迭代扩展卡尔曼滤波器用于里程计估计，它在状态估计过程中动态调整过程噪声，并为不同的传感器平台建立运动模型，从而实现准确且高效的状态估计。

尽管现有系统表现尚可，但其状态估计精度略低于激光雷达-惯性系统。激光雷达-惯性系统利用IMU数据来辅助点云运动畸变补偿，并通过ESIKF紧耦合IMU和激光雷达测量值，能够输出高频且高精度的里程计。然而，对于动态初始化系统所需的激光雷达里程计，其初始速度和重力方向未知，使得IMU中的加速度计无法使用。鉴于陀螺仪不受初始值影响，利用ESIKF实现陀螺仪与激光雷达之间的紧耦合，可能是提升激光雷达里程计准确性的一个新颖方法。

3. 核心算法

本节将详细介绍D-LI-Init方法的核心算法，该方法旨在为激光雷达-惯性SLAM提供一种稳健且快速的动态初始化方案。D-LI-Init的框架假设传感器间的外部参数已知，无需特定的运动模式，适用于多种平台。该方法通过迭代对齐激光雷达里程计与IMU预积分，利用最小二乘法（LSM）精确估计运动或静止机器人的初始状态。

图2：D-LI-Init 概述

3.1 激光雷达-陀螺仪里程计（LGO）

LGO模块采用陀螺仪数据来预测旋转运动，从而补偿扫描过程中的旋转畸变。具体而言，LGO利用恒定速度模型预测平移运动，并将平移运动的畸变补偿集成到迭代更新过程中，以提升点云的质量。为了应对恒定速度模型与实际运动之间的不匹配，采用了输入点云的帧分割方法。这一策略能够减小由于恒定速度假设导致的误差，确保高精度状态估计，进而提高动态初始化的准确性。

3.2 前向传播和反向传播

在接收到陀螺仪测量值后，进行前向传播，更新状态和协方差矩阵。前向传播过程通过设置过程噪声为零，按以下公式更新状态和协方差。反向传播过程则主要用于计算激光雷达帧内每个点的位姿，重点关注旋转分量。这一过程将所有点投影到扫描结束时的坐标系，从而为后续的迭代更新提供基础。

3.3 迭代更新

每次迭代都包括运动畸变补偿和观测更新。运动补偿后的扫描提供对预测位姿的隐式测量，基于点到平面的距离残差函数进行观测更新，直至状态估计收敛。在这一过程中，LGO模块的退化检测机制能够确保输出的激光雷达里程计的高精度，同时也为数据可靠性提供判别标准。

图3：运动失真补偿方法在简单室内环境中的效果展示。

3.4 退化检测

通过分析协方差矩阵的旋转和平移子块，D-LI-Init方法能够检测数据的可靠性。这一机制确保初始化过程的稳健性，避免因不可靠的数据导致初始化失败。协方差矩阵的局部子矩阵更好地考虑了旋转和位移的约束信息，便于建立退化检测标准。

3.5 动态初始化

D-LI-Init方法的核心目标是通过迭代对齐激光雷达里程计与IMU预积分，精确估计初始速度、重力方向和IMU偏差。在动态初始化过程中，准确估计加速度计偏差需要平台至少进行30度的旋转，以便将其与重力加速度解耦。因此，该方法故意省略了加速度计偏差的估计，将其推迟到后续的估计系统中。

图4：不同算法在不同数据集上生成的轨迹。LGO表现优异，超越了LO，并接近LIO。子图展示了以下内容：（a）-（h）分别为NEW quad、MCD night 04、MCD night 13、Gnd MBLR 1、NEW sloitter、NEW math、MCD day 02和UbanLoco测试中各种算法的轨迹。

3.6 数据收集与初始化

在动态初始化模块启动之前，首先根据退化检测标准收集足够数量的数据。为确保初始值估计的准确性，选择20帧数据进行处理。在此基础上，通过LGO模块获取相应的位姿，并同时收集与同一时间间隔对应的IMU预积分数据。

3.6.1 陀螺仪偏差初始化

为了解决陀螺仪偏差的问题，D-LI-Init方法对连续帧之间的旋转约束进行对齐。通过构建线性方程，求解出陀螺仪的初始偏差，并利用该偏差重新传播IMU预积分，以便解决后续变量。

3.6.2 初始速度与重力方向初始化

通过对齐来自里程计数据和IMU预积分的速度和位移约束，D-LI-Init方法估计初始速度和重力方向。构建离散形式的状态增量方程，结合激光雷达数据与IMU的外部参数，以线性最小二乘问题的形式求解初始状态。

3.6.3 迭代更新

在迭代过程中，初始值的精确估计直接影响激光雷达里程计生成的点云质量，因此，D-LI-Init方法使用通过LGO和IMU预积分对齐所获得的粗略初始值，在LIO系统中增强激光雷达里程计的精度。通过不断重复这一过程，直到收敛为止，确保初始速度的准确性，以实现高质量的位姿预测和点云运动畸变补偿。

4. 实验结论

实验结果表明，D-LI-Init在动态初始化过程中能够精确估计初始状态，确保后续状态估计的精度。该方法在多种平台上进行了评估，表现出色，展示了其在实际应用中的广泛适用性。通过开源代码与测试数据集的发布，D-LI-Init为动态初始化研究提供了重要基准，推动了激光雷达-惯性SLAM技术的发展。

图4：不同算法在不同数据集上生成的轨迹。LGO表现优异，超越了LO，并接近LIO。子图展示了以下内容：（a）-（h）分别为NEW quad、MCD night 04、MCD night 13、Gnd MBLR 1、NEW sloitter、NEW math、MCD day 02和UbanLoco测试中各种算法的轨迹。

5. 总结

D-LI-Init方法的提出为激光雷达-惯性里程计的动态初始化提供了新的思路，解决了传统静态初始化方法在运动状态下的局限性。未来的研究可以在此基础上进一步探索更多实际应用场景，推动无人驾驶、机器人导航等领域的发展。