[论文解读] FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter

Wei Xu, Fu Zhang|arXiv (Cornell University)|Oct 16, 2020

Robotics and Sensor-Based Localization参考文献 26被引用 27

一句话总结

FAST-LIO 提出了一种计算高效的紧耦合激光雷达-惯性里程计系统，采用迭代扩展卡尔曼滤波器（iEKF）融合激光雷达特征与惯性测量单元（IMU）数据。该方法提出了一种新颖的卡尔曼增益计算公式，将复杂度从测量维数降低至状态维数，从而实现在四旋翼飞行器机载计算机上以不到25毫秒处理超过1,200个激光雷达特征的实时性能。

ABSTRACT

This paper presents a computationally efficient and robust LiDAR-inertial odometry framework. We fuse LiDAR feature points with IMU data using a tightly-coupled iterated extended Kalman filter to allow robust navigation in fast-motion, noisy or cluttered environments where degeneration occurs. To lower the computation load in the presence of large number of measurements, we present a new formula to compute the Kalman gain. The new formula has computation load depending on the state dimension instead of the measurement dimension. The proposed method and its implementation are tested in various indoor and outdoor environments. In all tests, our method produces reliable navigation results in real-time: running on a quadrotor onboard computer, it fuses more than 1,200 effective feature points in a scan and completes all iterations of an iEKF step within 25 ms. Our codes are open-sourced on Github.

研究动机与目标

• 解决无人机运行中常见的高速运动、高噪声或特征退化环境下的激光雷达-惯性里程计挑战。
• 克服在嵌入式平台上融合大量激光雷达特征点（例如每帧超过1,200个）与IMU数据时带来的高计算成本问题。
• 补偿由于非瞬时扫描和无人机动力学引起的激光雷达扫描中的运动畸变。
• 在小型无人机上利用低成本固态激光雷达和IMU实现实时、精确且鲁棒的状态估计。
• 开发一种与测量维数无关的计算高效卡尔曼增益计算方法，以降低处理负载。

提出的方法

• 采用紧耦合的迭代扩展卡尔曼滤波器（iEKF）联合估计状态并校正激光雷达扫描中的运动畸变。
• 实施前向与后向状态传播，以建模激光雷达扫描期间的时变运动，实现对运动畸变的精确补偿。
• 提出一种新颖的卡尔曼增益公式，其仅依赖于状态维数而非测量维数，显著降低计算复杂度。
• 利用矩阵求逆引理证明新卡尔曼增益公式与传统公式的等价性，确保理论正确性。
• 将iEKF与考虑IMU偏差和动力学特性的状态转移模型相结合，采用误差状态形式以提升数值稳定性。
• 采用点到平面与点到边距离度量进行激光雷达特征匹配，实现在稀疏或杂乱环境中的鲁棒扫描配准。

实验结果

研究问题

• RQ1 紧耦合iEKF框架是否能提升在高速运动与高振动条件下的激光雷达-惯性里程计鲁棒性？
• RQ2 新颖的卡尔曼增益计算公式是否能在不牺牲估计精度的前提下降低计算负载？
• RQ3 通过前向与后向状态传播，能否有效补偿激光雷达扫描中的运动畸变？
• RQ4 该系统是否能在嵌入式无人机平台上实现每帧处理超过1,200个激光雷达特征的实时性能？
• RQ5 与现有激光雷达-惯性系统相比，该方法在精度与计算效率方面表现如何？

主要发现

• FAST-LIO每帧处理超过1,200个有效激光雷达特征点，并在四旋翼飞行器机载计算机上于25毫秒内完成所有iEKF迭代。
• 所提出的卡尔曼增益公式将计算复杂度从O(m²)降低至O(n²)，其中n为状态维数，m为测量维数，从而实现实时性能。
• 该系统在室内外环境中均实现了精确且无漂移的建图，包括高速运动与强烈振动等挑战性场景。
• 在无人机飞行测试中，FAST-LIO在传统激光雷达仅方法失效的特征退化环境中表现出卓越的鲁棒性。
• 与LINS的对比结果表明，FAST-LIO在计算负载显著更低的前提下，实现了相当或更优的建图精度。
• 该系统已在GitHub开源，并使用Livox AVIA激光雷达与DJI Manifold 2-C计算单元在真实无人机平台上完成验证。