FAST-LIVO2 深度解析 LiDAR-Inertial-Visual 三源融合里程计

项目概览

核心功能与定位

FAST-LIVO2 是香港大学 MARS 实验室开源的快速直接法 LiDAR-惯性-视觉里程计框架，发表于 IEEE T-RO 2024（arxiv 2408.14035），代码于 2025 年 1 月正式开源。它将 3D LiDAR、IMU 和相机三种传感器通过紧耦合 ESIKF（误差状态迭代卡尔曼滤波）顺序更新机制融合，在严重退化环境中仍能实现高精度定位与建图。

项目核心创新在于直接法统一处理 LiDAR 和视觉测量，无需特征提取，所有传感器共享同一个体素地图（Voxel Map），实现 LiDAR 点对面残差与视觉光度残差在同一个滤波框架中的紧耦合融合。开发者主笔为郑纯然（Chunran Zheng），核心贡献包括 ESIKF 顺序更新、平面先验改进、参考块动态更新、在线曝光时间估计与按需体素射线投射。

目标用户与解决的问题

适用人群包括无人机/无人车开发者、多传感器融合 SLAM 研究人员、退化环境（隧道、走廊、弱纹理）定位工程团队、直接法视觉里程计学习者以及 3D 重建与彩色点云应用开发者。

针对的工程痛点：

• 单一 LiDAR 在几何退化环境（长走廊、隧道）中定位失败
• 单一视觉在弱纹理/弱光照环境中里程计漂移
• 传统多传感器融合系统计算开销大，无法实时运行
• LiDAR 与视觉测量模型不统一，融合效果差
• 特征提取方法依赖手工设计，难以适配不同传感器

一、系统整体架构与分层设计

1.1 架构总览

1.2 核心模块交互关系

1.3 技术栈选型

二、完整数据流处理路径

2.1 从数据采集到决策输出的全流程

2.2 ESIKF 顺序更新时序

2.3 时间同步机制

ESIKF 顺序更新对时间同步要求严格：IMU 提供高频运动先验，LiDAR 与相机则按各自采样时刻进入滤波器。系统通过扫描重组（Scan Recombination）将异步采样的 LiDAR 点重组到相机采样时刻的扫描点，保证 LiDAR 与视觉在同一个 ESIKF 迭代内完成顺序更新。

时间戳对齐策略：

• IMU 持续前向传播，提供 200-1000Hz 的运动预测
• LiDAR 扫描期间使用 IMU 反向传播估计每个点的瞬时位姿
• 相机曝光时刻作为 ESIKF 更新的参考时间锚点
• 同步板（STM32 产生 PPS 脉冲）保证硬件级时间对齐

三、传感器系统与硬件配置

3.1 三种传感器角色分工

3.2 LiDAR 关键参数对比

LiDAR 在系统中的核心作用是提供几何约束（Point-to-Plane 残差）、通过 ikd-Tree 增量构建全局点云地图，并通过点云约束关系检测环境几何退化。

3.3 IMU 关键参数对比

IMU 承担三类任务：400Hz 高频积分预测机器人运动状态、反向传播估计每个 LiDAR 点的瞬时位姿以补偿扫描期间运动、为 ESIKF 提供先验状态估计和协方差传播。

3.4 相机关键参数对比

相机在系统中提供直接法光度残差，补充 LiDAR 在几何退化环境中的约束，同时为点云地图赋予 RGB 颜色信息，在走廊、隧道等 LiDAR 退化场景中提供关键约束。

3.5 硬件同步方案

项目开源了手持设备 LIV_handhold，包含 STM32 同步板（产生 PPS 脉冲同步 LiDAR 和相机）、3D 打印外壳与传感器安装支架的 CAD 文件、LiDAR/IMU/相机/STM32 的完整接线方案、同步采集节点的 ROS 驱动代码。

四、RViz 可视化与数据查看

4.1 RViz 实时可视化话题

FAST-LIVO2 基于 ROS RViz 实现实时数据可视化，提供完整的可视化配置：

4.2 离线分析与评估工具链

五、核心算法 ESIKF 与统一地图

5.1 算法清单与角色

5.2 ESIKF 状态向量与顺序更新

ESIKF 核心思想是将系统状态分解为名义状态+误差状态，在误差状态空间进行线性化，通过迭代更新提高线性化精度。状态向量共 24 维：

x = [R, p, v, b_a, b_g, g, R_LI, p_LI]ᵀ
     │  │  │   │    │   │   │     │
     │  │  │   │    │   │   │     └─ LiDAR-IMU 平移外参(3)
     │  │  │   │    │   │   └─────── LiDAR-IMU 旋转外参(3)
     │  │  │   │    │   └─────────── 重力向量(3)
     │  │  │   │    └─────────────── 陀螺仪偏置(3)
     │  │  │   └──────────────────── 加速度计偏置(3)
     │  │  └──────────────────────── 速度(3)
     │  └─────────────────────────── 位置(3)
     └────────────────────────────── 旋转 SO3(3)

顺序更新流程：

LiDAR 更新步骤：

1. 从 ikd-Tree 查询每个点的 5 个近邻
2. 拟合局部平面，计算法向量 n 和中心点 q
3. 计算点对面残差 z = nᵀ(R·p_L + t - q)
4. 计算雅可比矩阵 H = ∂z/∂δx
5. 卡尔曼更新 K = P̂Hᵀ(HP̂Hᵀ + R)⁻¹，状态更新 δx = K·z
6. 协方差更新 P = (I - KH)P̂(I - KH)ᵀ + KRKᵀ

视觉更新步骤：

1. 从体素地图查询当前帧可见的视觉地图点
2. 对每个地图点提取参考块（w×w 像素）和当前块
3. 计算光度残差 z = I_cur(warp(p_ref, δx)) - I_ref(warp(p_ref))
4. 仿射变换补偿光照变化，在线估计曝光时间
5. 梯度加权：高梯度像素权重更大
6. 同样的卡尔曼更新流程

5.3 ikd-Tree 增量式 k-d 树

ikd-Tree 突破传统 k-d 树不支持动态插入/删除的限制，通过增量式平衡策略实现 O(log n) 操作：

与 Octree/nanoflann KD-Tree 对比：

5.4 直接法视觉里程计

直接法与特征点法对比：

视觉地图点选择策略采用可见体素查询、按需光线投射（Raycasting）确认可见性、基于残差大小和梯度方向剔除异常点、视角变化超过阈值时更新参考块图像。

5.5 体素地图统一表示

LiDAR 和视觉共享同一个体素地图，每个体素同时承载几何信息与视觉信息：

Voxel {
    ├── 几何信息
    │   ├── 点集 {p₁, p₂, ..., pₙ}     # 体素内 3D 点
    │   ├── 法向量 n                      # 平面法线
    │   ├── 平面中心 q                    # 平面中心点
    │   └── 平面拟合协方差                # 拟合不确定性
    │
    └── 视觉信息
        ├── 视觉地图点 p_v               # 3D 坐标
        ├── 参考块 I_ref                 # w×w 像素参考图像
        ├── 参考位姿 T_ref               # 参考帧位姿
        └── 仿射参数 a, b                # 光照补偿参数
}

六、导航定位与退化环境鲁棒性

6.1 ROS 话题输出

FAST-LIVO2 输出的定位信息通过 ROS 话题发布，可直接接入导航系统：

6.2 Nav2 导航系统集成

6.3 退化环境互补策略

FAST-LIVO2 的核心优势在于退化环境中的鲁棒性，通过三传感器互补实现：

6.4 UAV 自主导航应用

论文中展示了 FAST-LIVO2 在无人机自主导航中的完整应用：定位（实时 6DoF 位姿）→ 建图（实时 3D 点云地图）→ 规划（基于地图的 Bubble Planner 路径规划）→ 控制（MPC 轨迹跟踪）→ 避障（动态障碍物检测与规避）。所有计算均在机载计算机上实时完成。

6.5 精度评估数据

七、快速上手与编译运行

7.1 环境依赖

# Ubuntu 16.04~20.04 + ROS Noetic
sudo apt install ros-noetic-desktop-full

# 核心依赖库
sudo apt install libpcl-dev libeigen3-dev libopencv-dev

7.2 Sophus 与 Vikit 编译

# Sophus（non-templated/double-only 分支）
git clone https://github.com/strasdat/Sophus.git
cd Sophus && git checkout a621ff
mkdir build && cd build && cmake ..
make && sudo make install

# Vikit（与 fast-livo1 不同，需用 rpg_vikit 仓库）
cd ~/catkin_ws/src
git clone https://github.com/xuankuzcr/rpg_vikit.git

7.3 主仓编译与运行

# 克隆并编译
cd ~/catkin_ws/src
git clone https://github.com/hku-mars/FAST-LIVO2
cd ~/catkin_ws
catkin_make
source devel/setup.bash

# 启动并播放数据集
roslaunch fast_livo mapping_avia.launch
rosbag play YOUR_DOWNLOADED.bag

7.4 推荐学习路径

八、项目资源信息

FAST-LIVO2 用统一的体素地图和 ESIKF 顺序更新框架，将 LiDAR 的几何约束、IMU 的运动预测和相机的纹理约束融为一体。在走廊里 LiDAR 退化时视觉补位，在弱纹理时 LiDAR 主导，三传感器互补让机器人在任何环境中都不迷失方向。

注：基于 hku-mars/FAST-LIVO2 仓库 README 与 arxiv 2408.14035 论文校正技术参数：PCL 最低版本为 1.6（非 1.8）、OpenCV 最低版本为 3.2（非 4.2）、Sophus 使用 non-templated/double-only 分支、代码于 2025-01-23 正式开源。GitHub Stars 数据截至 2025 年 4 月约 2,197，原文 2,600+ 为预估值已删除。