[论文解读] FAST-LIO2: Fast Direct LiDAR-inertial Odometry

Wei Xu, Yixi Cai+3 more|arXiv (Cornell University)|Jul 14, 2021

Robotics and Sensor-Based Localization参考文献 57被引用 12

一句话总结

FAST-LIO2 提出了一种快速、直接的激光雷达-惯性里程计框架，通过使用增量式 k-d 树（ikd-Tree）直接将原始激光雷达点注册到地图中，绕过了特征提取步骤，从而实现在多种激光雷达类型和环境下的高精度、实时导航。该方法在性能上最高可达 100 Hz，具有优异的精度和鲁棒性，尤其在稀疏或高速运动场景中表现突出，同时支持动态地图更新和在嵌入式系统上的高效计算。

ABSTRACT

This paper presents FAST-LIO2: a fast, robust, and versatile LiDAR-inertial odometry framework. Building on a highly efficient tightly-coupled iterated Kalman filter, FAST-LIO2 has two key novelties that allow fast, robust, and accurate LiDAR navigation (and mapping). The first one is directly registering raw points to the map (and subsequently update the map, i.e., mapping) without extracting features. This enables the exploitation of subtle features in the environment and hence increases the accuracy. The elimination of a hand-engineered feature extraction module also makes it naturally adaptable to emerging LiDARs of different scanning patterns; The second main novelty is maintaining a map by an incremental k-d tree data structure, ikd-Tree, that enables incremental updates (i.e., point insertion, delete) and dynamic re-balancing. Compared with existing dynamic data structures (octree, R*-tree, nanoflann k-d tree), ikd-Tree achieves superior overall performance while naturally supports downsampling on the tree. We conduct an exhaustive benchmark comparison in 19 sequences from a variety of open LiDAR datasets. FAST-LIO2 achieves consistently higher accuracy at a much lower computation load than other state-of-the-art LiDAR-inertial navigation systems. Various real-world experiments on solid-state LiDARs with small FoV are also conducted. Overall, FAST-LIO2 is computationally-efficient (e.g., up to 100 Hz odometry and mapping in large outdoor environments), robust (e.g., reliable pose estimation in cluttered indoor environments with rotation up to 1000 deg/s), versatile (i.e., applicable to both multi-line spinning and solid-state LiDARs, UAV and handheld platforms, and Intel and ARM-based processors), while still achieving higher accuracy than existing methods. Our implementation of the system FAST-LIO2, and the data structure ikd-Tree are both open-sourced on Github.

研究动机与目标

• 开发一种计算高效、鲁棒且精确的激光雷达-惯性里程计系统，适用于嵌入式平台的实时部署。
• 消除对手工设计特征提取的依赖，该方法对扫描模式和环境条件敏感，尤其在稀疏或无结构场景中表现不佳。
• 支持多种激光雷达类型，包括多线旋转式和固态激光雷达，适用于各类平台（无人机、手持设备）和处理器（Intel、ARM）。
• 维持一个密集的、动态的三维地图，支持增量式更新、高效的最近邻搜索以及树上降采样，以实现实时性能。

提出的方法

• 系统采用紧耦合的迭代扩展卡尔曼滤波器（IEKF）融合原始激光雷达点云和惯性测量单元（IMU）数据，实现状态估计。
• 直接将每次激光雷达扫描的原始 3D 点注册到地图中，无需提取边缘或平面特征，从而保留细微的环境特征并提升精度。
• 提出一种新型的增量式 k-d 树数据结构——ikd-Tree，支持低计算成本的动态点插入、删除以及并行再平衡。
• ikd-Tree 支持高效的 k-最近邻（k-NN）搜索，并支持树上降采样，可在不损失地图质量的前提下降低内存和计算负载。
• 该框架通过将 IMU 数据集成到状态估计过程中，有效处理运动畸变，从而在高速运动中提升精度。
• 系统经过优化，实现了高达 100 Hz 的里程计与制图性能，在大型室外环境中具备低延迟处理能力。

实验结果

研究问题

• RQ1 与基于特征的方法相比，直接将点注册到动态地图是否能提升激光雷达-惯性里程计的精度和鲁棒性？
• RQ2 何种数据结构能够高效支持实时激光雷达制图中的增量式地图更新、动态再平衡以及快速 k-NN 搜索？
• RQ3 无特征方法是否能在包括视场角小、扫描模式非均匀的固态激光雷达在内的多种激光雷达类型中保持高精度和高性能？
• RQ4 在室内杂乱或室外植被等复杂环境中，直接激光雷达注册与基于特征的注册在计算效率与精度之间的权衡如何？
• RQ5 在极端运动条件（如 1000 deg/s 旋转）以及特征提取失败的稀疏场景中，系统性能如何？

主要发现

• FAST-LIO2 在大型室外环境中实现了高达 100 Hz 的里程计与制图性能，其计算效率显著优于现有最先进方法。
• 在旋转速度高达 1000 deg/s 的复杂室内环境中，系统表现出鲁棒的姿态估计能力，而基于特征的方法通常会失效。
• 在多个公开激光雷达数据集的 19 个序列基准测试中，FAST-LIO2 始终保持比现有最先进激光雷达-惯性里程计系统更高的精度。
• ikd-Tree 数据结构在 k-NN 搜索的整体性能上优于现有动态数据结构（八叉树、R*-树、nanoflann k-d 树），支持高效的增量更新和树上降采样。
• 在三个真实环境（城市、植被、室内）中，每帧的平均处理时间分别为 19.6 ms、23.9 ms 和 23.7 ms，证实了其实时可行性。
• 系统在航拍实验中成功映射了树冠、道路边缘和车道线等复杂场景，轨迹在视觉上与 GPS/IMU 估计结果一致，尽管算法未使用任何 GPS 输入。