FAST-LIVO2 快速总结（相对详细版）

FAST-LIVO2 是香港大学 MaRS 实验室开发的一套激光雷达-惯导-视觉紧耦合 SLAM 框架，在计算效率与精度之间取得了良好平衡。本文在快速总结基础上，结合 ESIKF 数学原理、VoxelMap 地图结构以及鲁棒性设计等方面的分析，进行系统性整理。

1. 系统定位与核心思路

FAST-LIVO2 的核心思路是：以 ESIKF（误差状态迭代卡尔曼滤波） 为统一估计框架，将 IMU 的高频运动预测、LiDAR 的几何观测与 Camera 的光度观测依次融合，输出高频、精确的位姿轨迹与概率体素地图。

相比 FAST-LIO2（仅 LIO），FAST-LIVO2 的主要拓展在于：

  • 引入了统一体素地图（Unified Voxel Map），每个体素同时存储几何信息（概率平面）和视觉信息（图像补丁）；
  • 将 LiDAR 平面的不确定性作为视觉光度误差的几何先验，实现了跨模态误差权重的自适应调节；
  • 新增了逆曝光时间 \(\tau\) 作为状态分量，支持直接法视觉与光度模型联合估计。

2. 系统输入与输出

2.1 输入

传感器 数据内容 典型频率
IMU 角速度 \(\tilde{\boldsymbol{\omega}}\)、加速度 \(\tilde{\mathbf{a}}\)、时间戳 200~1000 Hz
LiDAR 点云（距离 + 强度）、时间戳 10~20 Hz
Camera 灰度/彩色图像、时间戳 10~30 Hz

系统为典型的生产者-消费者架构，三路数据进入独立 buffer 池，由主计算线程按时间戳顺序取出处理。

2.2 输出

  1. 位姿轨迹：带时间戳的 SE(3) 序列，包含世界坐标系下的旋转 \(\mathbf{R}\)、平移 \(\mathbf{p}\) 和速度 \(\mathbf{v}\)；
  2. 概率体素地图：以空间坐标为哈希索引，每个体素存储：
    • 概率平面参数（法向量 \(\mathbf{n}\)、中心点 \(\mathbf{q}\)、协方差 \(\boldsymbol{\Sigma}_{n,q}\)）；
    • 关联的图像补丁（用于视觉更新）；
    • 充分统计量（点数 \(N\)、一阶矩 \(S\)、二阶矩 \(M\)），支持增量更新。

3. 状态定义：19 维误差状态

ESIKF 的名义状态向量共 19 维：

\[\mathbf{x} = \begin{bmatrix} \mathbf{R} \in SO(3),\ \mathbf{p},\ \tau,\ \mathbf{v},\ \mathbf{b}_g,\ \mathbf{b}_a,\ \mathbf{g} \end{bmatrix} \]

分量 维度 含义
\(\mathbf{R}\) 3 (SO(3)) IMU 到世界系的旋转矩阵
\(\mathbf{p}\) 3 世界系位置
\(\tau\) 1 逆曝光时间（视觉模块专用）
\(\mathbf{v}\) 3 世界系速度
\(\mathbf{b}_g\) 3 陀螺仪零偏
\(\mathbf{b}_a\) 3 加速度计零偏
\(\mathbf{g}\) 3 重力向量

为什么使用误差状态而非直接状态？

旋转矩阵 \(\mathbf{R} \in SO(3)\) 不是欧几里得空间的元素，直接对其进行卡尔曼滤波会产生奇异性（例如四元数归一化约束与线性化的矛盾）。ESIKF 的做法是：

  • 名义状态在 SO(3) 等李群流形上传播；
  • 误差状态 \(\delta\mathbf{x} \in \mathbb{R}^{19}\) 定义在切空间（李代数）上，姿态误差用轴角 \(\delta\boldsymbol{\theta} \in \mathbb{R}^3\) 表示，其余分量直接做差。

状态更新使用流形加法 \(\boxplus\)：

\[\mathbf{x} \boxplus \delta\mathbf{x} = \begin{bmatrix} \mathbf{R} \cdot \text{Exp}(\delta\boldsymbol{\theta}) \\ \mathbf{p} + \delta\mathbf{p} \\ \cdots \end{bmatrix} \]

其中 \(\text{Exp}(\boldsymbol{\phi}) = \mathbf{I} + \frac{\sin\|\boldsymbol{\phi}\|}{\|\boldsymbol{\phi}\|}[\boldsymbol{\phi}]_\times + \frac{1-\cos\|\boldsymbol{\phi}\|}{\|\boldsymbol{\phi}\|^2}[\boldsymbol{\phi}]_\times^2\) 是 SO(3) 指数映射（Rodrigues 公式）。

4. 数据处理流程

4.1 IMU 数据处理（状态预测）

IMU 以积分方式将状态向前传播（预测步），离散时间传播方程为：

\[\mathbf{R}_{k+1} = \mathbf{R}_k \cdot \text{Exp}(\bar{\boldsymbol{\omega}} \cdot \Delta t) \]

\[\mathbf{p}_{k+1} = \mathbf{p}_k + \mathbf{v}_k \Delta t + \tfrac{1}{2}\mathbf{a}_{imu} \Delta t^2, \quad \mathbf{v}_{k+1} = \mathbf{v}_k + \mathbf{a}_{imu} \Delta t \]

其中 \(\mathbf{a}_{imu} = \mathbf{R}_k(\bar{\mathbf{a}} - \mathbf{b}_a) + \mathbf{g}\)，\(\bar{\boldsymbol{\omega}}, \bar{\mathbf{a}}\) 为中值积分。

与此同时，协方差通过状态转移矩阵 \(\mathbf{F}_x \in \mathbb{R}^{19\times19}\) 传播：

\[\mathbf{P}_{k+1|k} = \mathbf{F}_x \mathbf{P}_{k|k} \mathbf{F}_x^T + \mathbf{Q}_w \]

其中过程噪声 \(\mathbf{Q}_w\) 由陀螺仪噪声 \(\sigma_g^2\)、加速度计噪声 \(\sigma_a^2\) 及零偏随机游走组成。

注意：IMU 去畸变是 LiDAR 数据预处理的前提。旋转式 LiDAR 的每个点对应不同时刻，需利用 IMU 积分为每个点插值出对应位姿，消除运动导致的点云扭曲。

4.2 LiDAR 数据处理（几何观测）

LiDAR 帧到来后依次执行：

① 点云降采样：体素滤波降低数据量，同时避免近处点云过密导致匹配权重失衡。

② 点云协方差建模：每个点在激光雷达坐标系下的不确定性由距离误差和角度误差两部分组成：

\[\boldsymbol{\Sigma}_b = \underbrace{\mathbf{d}\sigma_r^2\mathbf{d}^T}_{\text{距离误差}} + \underbrace{\mathbf{A}\boldsymbol{\Sigma}_\theta\mathbf{A}^T}_{\text{角度误差}} \]

其中 \(\mathbf{d}\) 是归一化方向向量，\(\mathbf{A} = r[\mathbf{d}]_\times\mathbf{N}\) 将切向角度误差投影回笛卡尔坐标系。随后通过旋转矩阵 \(\mathbf{R}\) 和状态协方差 \(\mathbf{P}\) 将 \(\boldsymbol{\Sigma}_b\) 传播到世界坐标系：

\[\boldsymbol{\Sigma}_w = \mathbf{R}\mathbf{R}_{ext}\boldsymbol{\Sigma}_b(\mathbf{R}\mathbf{R}_{ext})^T + [\mathbf{p}_i]_\times\boldsymbol{\Sigma}_R[\mathbf{p}_i]_\times^T + \boldsymbol{\Sigma}_t \]

这意味着：距离越远的点，受姿态不确定性的影响越大（力臂效应）。

③ 点-面匹配与残差计算：在体素地图中搜索与当前点最近的概率平面，计算点到平面距离作为观测残差：

\[z_i = -(\mathbf{n}^T\mathbf{p}_w + d) \]

对应的测量噪声方差同时考虑点云不确定性和平面参数不确定性：

\[R_i = \sigma_{plane} + \mathbf{n}^T\boldsymbol{\Sigma}_w\mathbf{n} + \epsilon \]

通过这一双重不确定性模型，几何质量差的平面（如树丛区域，\(\boldsymbol{\Sigma}_{n,q}\) 大）会被赋予更大的 \(R_i\)，进而在 IEKF 中获得更小的卡尔曼增益，实现自适应降权。

④ 体素地图更新：将降采样后的点云存入对应体素，通过增量更新公式（利用充分统计量的可加性）更新概率平面参数，无需重访历史点云。

4.3 相机数据处理（光度观测）

视觉模块在 LiDAR 更新完成后执行（序列化 ESIKF），其核心思路是直接法——不提取特征点，而是直接最小化灰度残差。

关键步骤：

  1. 根据当前位姿估计，将体素地图中的 3D 点投影到图像平面；

  2. 计算投影区域的灰度匹配误差作为观测残差；

  3. 深度估计利用体素内的概率平面作为几何先验：

    \[\text{depth} = \frac{\mathbf{n}^T\mathbf{q}}{\mathbf{n}^T K^{-1}\mathbf{u}} \]

    从而避免了传统直接法中耗时且不稳定的极线深度搜索；

  4. 几何先验的不确定性 \(\boldsymbol{\Sigma}_{plane}\) 通过误差传播链传递到光度残差的权重中，LiDAR 平面越精确，对应的视觉约束权重越高。

5. IEKF 更新

5.1 信息矩阵形式的迭代更新

为了高效处理大量观测点，FAST-LIVO2 采用信息矩阵形式（避免直接求大维度的卡尔曼增益）。设 \(\mathbf{H}_{sub} \in \mathbb{R}^{m\times6}\) 为堆叠的观测雅可比矩阵（每行对应一个匹配点，仅旋转和平移的 6 个分量非零），则每次迭代的更新为：

\[\mathbf{K}_1 = \left(\mathbf{H}^T\mathbf{R}^{-1}\mathbf{H} + \mathbf{P}^{-1}\right)^{-1} \]

\[\delta\mathbf{x}^{(j+1)} = \mathbf{K}_1\mathbf{H}^T\mathbf{R}^{-1}\mathbf{z}^{(j)} + (\mathbf{x}_{prop} \boxminus \mathbf{x}^{(j)}) - \mathbf{G}(\mathbf{x}_{prop} \boxminus \mathbf{x}^{(j)})_{0:5} \]

迭代直至姿态增量 \(\|\delta\boldsymbol{\theta}\| < 0.01°\) 且平移增量 \(\|\delta\mathbf{t}\| < 0.015\) cm，且需完成重匹配（rematch）确认收敛。

5.2 退化问题处理

退化（Degeneracy）是 LiDAR SLAM 的核心挑战，典型场景包括长直走廊（沿轴向无约束）、空旷广场等。

退化检测：通过对信息矩阵 \(\mathbf{H}^T\mathbf{H}\) 做特征值分解，若某方向的特征值 \(\lambda_{min}\) 低于阈值，则判定该方向发生退化。

退化补偿：由于速度与角速度在状态方程中存在耦合（加速度对速度的影响，角速度对姿态的影响），直接对全状态增益矩阵分析容易被耦合项干扰。正确做法是对增益矩阵按物理意义拆分（旋转、平移、速度等子块分别分析），再针对退化方向将对应的状态更新回退到 IMU 预测值，而非简单地放弃整次更新：

\[\delta\mathbf{x}' = \delta\mathbf{x} - (v_{degen}^T\delta\mathbf{x})v_{degen} \]

这样可以在退化方向信任 IMU，而在有效方向继续使用 LiDAR 的约束。

6. 整体流程回顾

flowchart LR A[IMU] -->|积分预测| B[状态预测 x_prop, P] C[LiDAR] -->|去畸变 + 降采样| D[点云预处理] D -->|协方差建模| E[点-面匹配] E -->|LiDAR 更新| F[IEKF 第一步] G[Camera] -->|灰度残差| H[视觉更新] F -->|状态初值| H H --> I[最终位姿 + 更新 P] I -->|写入| J[统一体素地图] J -->|概率平面先验| H

7. 与同类系统的横向比较

对比维度 FAST-LIO2 LIO-SAM FAST-LIVO2
传感器模态 LiDAR + IMU LiDAR + IMU (+ GPS/回环) LiDAR + IMU + Camera
估计器 IESKF 因子图 (iSAM2) 序列化 IESKF
地图结构 ikd-Tree 全局体素 + 因子图 统一概率体素
视觉约束 无 无 直接法（几何先验引导）
抗退化 被动（依赖点云密度） 被动（依赖回环） 主动（几何+纹理互补）
计算量 低 中 中高

核心优势体现在极端退化场景（如长隧道）：FAST-LIO2 在沿隧道轴向无法提供约束，而 FAST-LIVO2 可利用墙面纹理特征（直接法光度误差）补充轴向约束，从而抑制漂移。

参考文献

  1. Zheng, C., et al. (2024). "FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry." IEEE Transactions on Robotics.
  2. Xu, W., et al. (2022). "FAST-LIO2: Fast Direct LiDAR-Inertial Odometry." IEEE TRO.
  3. Yuan, C., et al. (2022). "Efficient and Probabilistic Adaptive Voxel Mapping for Accurate Online LiDAR Odometry." (VoxelMap)
  4. Sola, J. (2017). "Quaternion kinematics for the error-state Kalman filter." arXiv:1711.02508.
posted @ 2026-03-03 23:45  HanDoMind  阅读(0)  评论(0)    收藏  举报