【LIO】FAST-LIO论文详解

FAST-LIO论文详解

- [1. 摘要](#1. 摘要)
- [2. 简介](#2. 简介)
- - [1. 相关工作](#1. 相关工作)
  - - [A. LiDAR 里程计和地图绘制](#A. LiDAR 里程计和地图绘制)
  - [2. 实现方法](#2. 实现方法)
  - - [A. 基础知识](#A. 基础知识)
    - - [1. 连续模型](#1. 连续模型)
      - 在这里插入图片描述
    - [B. 激光雷达测量的预处理](#B. 激光雷达测量的预处理)
    - [C. 状态估计](#C. 状态估计)
    - 1) 前向传播：前向传播：)
    - 2) 反向传播与运动补偿：反向传播与运动补偿：)
    - 3) 残差计算：残差计算：)

1. 摘要

提出了一种计算效率高且稳健的 LiDAR-惯性里程计框架。我们使用紧密耦合的迭代扩展卡尔曼滤波器将 LiDAR 特征点与 IMU 数据融合，以在发生退化的快速运动、嘈杂或混乱环境中实现稳健导航。为了在存在大量测量的情况下降低计算负荷，我们提出了一种计算卡尔曼增益的新公式。新公式的计算负荷取决于状态维度而不是测量维度。所提方法所有测试中都能实时产生可靠的导航结果：在四旋翼机载计算机上运行，它在一次扫描中融合了 1,200 多个有效特征点，并在 25 毫秒内完成 iEKF 步骤的所有迭代.

2. 简介

SLAM是移动机器人（例如无人机 (UAV)）的基本先决条件。视觉（惯性）里程计 (VO)，例如立体 VO [1] 和单目 VO [2, 3]，由于其重量轻且成本低廉而常用于移动机器人。尽管提供了丰富的 RGB 信息，但视觉解决方案缺乏直接深度测量，并且需要大量计算资源来重建轨迹规划的 3D 环境。此外，它们对光照条件非常敏感。光检测和测距 (LiDAR) 传感器可以克服所有这些困难，但对于小型移动机器人来说成本太高（而且体积太大）。固态 LiDAR 近来成为 LiDAR 开发的主要趋势，例如基于微机电系统 (MEMS) 扫描 [4] 和旋转棱镜 [5] 的 LiDAR。这些 LiDAR 非常经济高效（成本范围与全局快门相机相似）、重量轻（可由小型无人机携带）且性能高（可进行远程和高精度的主动和直接 3D 测量）。这些特性使得此类 LiDAR 适用于无人机，尤其是工业无人机，需要获取环境的精确 3D 地图（例如，航空测绘）或可能在照明变化剧烈的杂乱环境中运行（灾后搜索检查）。尽管潜力巨大，但固态 LiDAR 也给 SLAM 带来了新的挑战：1) LiDAR 测量中的特征点通常是环境中的几何结构（例如，边缘和平面）。当无人机在没有强大特征的杂乱环境中运行时，基于 LiDAR 的解决方案很容易退化。

当 LiDAR 的视场较小时，这个问题更加明显。2) 由于沿扫描方向的分辨率很高，LiDAR 扫描通常包含许多特征点（例如几千个）。虽然这些特征点不足以在退化的情况下可靠地确定姿势，但将如此大量的特征点紧密融合到 IMU 测量中需要大量的计算资源，而这些资源是无人机机载计算机无法承受的。3) 由于 LiDAR 使用几个激光/接收器对按顺序采样点，因此扫描中的激光点总是在不同的时间采样，从而导致运动失真，这将严重降低扫描配准 [6]。无人机螺旋桨和电动机的不断旋转也会给 IMU 测量带来显着的噪音。为了使 LiDAR 导航适用于无人机等小型移动机器人，提出了 FAST-LIO，一种计算效率高且强大的 LiDAR-惯性里程计包。贡献如下：1）为了应对发生退化的快速运动、嘈杂或混乱的环境，我们采用紧耦合迭代卡尔曼滤波器将 LiDAR 特征点与 IMU 测量值融合。提出一个反向传播过程来补偿运动失真；2）为了降低大量 LiDAR 特征点造成的计算负担，提出了一种计算卡尔曼增益的新公式，并证明其与传统卡尔曼增益公式等价。新公式的计算复杂度取决于状态维度而不是测量维度。3）将公式实现到快速而强大的 LiDAR 惯性里程计软件包中。该系统能够在小型四旋翼机载计算机上运行。 4) 各种室内和室外环境中进行实验，并进行实际无人机飞行测试（图 1），以验证系统在快速运动或强烈振动噪声存在时的稳健性。

论文组织如下：在第二节中，我们讨论了相关的研究工作。在第三节中概述了完整的系统流程和每个关键组件的详细信息。实验在第 IV 节中介绍，然后在第 V 节中得出结论。

1. 相关工作

A. LiDAR 里程计和地图绘制

Besl 等人 [6] 提出了一种用于扫描配准的迭代最近点 (ICP) 方法，为 LiDAR 里程计奠定了基础。ICP 在密集的 3D 扫描中表现良好。然而，对于 LiDAR 测量的稀疏点云，ICP 所需的精确点匹配很少存在。为了解决这个问题Segal [7] 提出了一种基于点到平面距离的广义 ICP。然后，Zhang 等人 [8] 将此 ICP 方法与点到边距离相结合，开发了 LiDAR 里程计和地图绘制 (LOAM) 框架。开发了许多 LOAM 变体，例如 LeGO-LOAM [9] 和 LOAM-Livox [10]。虽然这些方法适用于结构化环境和大 FoV 的 LiDAR，但它们很容易受到无特征环境或小 FoV LiDAR 的影响 [10]。

B. 松耦合 LiDAR-惯性里程计 IMU 测量通常用于缓解无特征环境中 LiDAR 退化的问题。

松耦合 LiDAR-惯性里程计 (LIO) 方法通常分别处理 LiDAR 和 IMU 测量，然后融合它们的结果。例如，IMU 辅助 LOAM [8] 将 IMU 测量中集成的姿势作为 LiDAR 扫描配准的初始估计。Zhen 等人 [11] 使用误差状态 EKF 融合 IMU 测量和 LiDAR 测量的高斯粒子滤波器输出。 Balazadegan 等人 [12] 添加了 IMU 重力模型来估计 6 自由度自运动，以辅助 LiDAR 扫描配准。Zuo 等人 [13] 使用多状态约束卡尔曼滤波器 (MSCKF) 将扫描配准结果与 IMU 和视觉测量值融合。松耦合方法的常见程序是通过配准新扫描然后将姿势测量值与 IMU 测量值融合来获得姿势测量值。扫描配准和数据融合之间的分离减少了计算负荷。然而，它忽略了系统的其他状态（例如速度）与新扫描姿势之间的相关性。此外，在无特征环境中，扫描配准可能会在某些方向上退化，并导致后期融合不可靠。

C. 紧耦合 LiDAR-惯性里程计

与松耦合方法不同，紧耦合 LiDAR-惯性里程计方法通常将 LiDAR 的原始特征点（而不是扫描配准结果）与 IMU 数据融合。紧耦合 LIO 主要有两种方法：基于优化和滤波器。Geneva 等人 [14] 使用图形优化，其中 IMU 预积分约束 [15] 和 LiDAR 特征点的平面约束 [16]。最近，Ye [17] 提出了 LIOM 包，它使用类似的图形优化，但基于边缘和平面特征。对于基于滤波的方法，Bry [18] 使用高斯粒子滤波器 (GPF) 融合 IMU 和平面 2D LiDAR 的数据。这种方法也已用于波士顿动力 Atlas 人形机器人。由于粒子滤波器的计算复杂度随着特征点数量和系统维度的增加而快速增长，因此通常更倾向于使用卡尔曼滤波器及其变体，例如EKF [19]、无迹KF [20] 和迭代卡KF[21]。我们的方法属于紧耦合方法。采用类似于 [21] 的迭代EKF来减轻线性化误差。KF（及其变体）的时间复杂度为 Om2，其中 m 是测量维度 [22]，这可能会导致在处理大量 LiDAR 测量时计算负荷非常高。简单的下采样会减少测量次数，但代价是信息丢失。[21] 通过提取和拟合地面来减少测量次数，类似于 [9]。然而，这并不适用于空中应用，因为地面可能并不总是存在

2. 实现方法

A. 基础知识

状态表示与流形定义

系统状态 M \mathcal{M} M 定义为流 M ∈ S O ( 3 ) × R 15 \mathcal{M} \in SO(3) \times \mathbb{R}^{15} M∈SO(3)×R15形，其维度为 18（其中 15 来自 IMU 状态，3 来自 SO(3)）。这种流形允许在最小状态空间中进行变换，从而提高卡尔曼滤波更新过程的效率。
流形上的变换操作由运算符 ⊞ \boxplus ⊞ 和$ \boxminus$ 定义，它们描述如何在流形上进行增量操作$ \mathcal{M} \boxplus \Delta \mathcal{M} = \text{SO(3) 的指数映射 + 欧几里得空间中的增量}$
M ⊟ N = SO(3) 的对数映射 + 欧几里得空间中的减法 \mathcal{M} \boxminus \mathcal{N} = \text{SO(3) 的对数映射 + 欧几里得空间中的减法} M⊟N=SO(3) 的对数映射 + 欧几里得空间中的减法

1. 连续模型

假设IMU刚性地附着在LiDAR上，具有已知的外参$ ITL = \begin{bmatrix} I_{RL} & I_{pL} \end{bmatrix} 。以 I M U 帧（记为。以IMU帧（记为。以IMU帧（记为I$作为参考体帧，得运动学模型：
p ˙ I G = v I G , v ˙ I G = R I G ( a m − b a − n a ) + g G , g ˙ G = 0 R ˙ I G = R I G ω ^ m , b ˙ ω G = n b ω , b ˙ a G = n b a \dot{p}^G_I = v^G_I, \quad \dot{v}^G_I = R^G_I (\mathbf{a}m - \mathbf{b}a - \mathbf{n}a) + \mathbf{g}^G, \quad \dot{\mathbf{g}}^G = 0 \\\dot{R}^G{I} = R^G{I} \hat{\omega}m, \quad \dot{b}^G{\omega} = n{b\omega}, \quad \dot{b}^G_{a} = n_{b_a} p˙IG=vIG,v˙IG=RIG(am−ba−na)+gG,g˙G=0R˙IG=RIGω^m,b˙ωG=nbω,b˙aG=nba

其中 G p I , G R I ^G p_I, ^G R_I GpI,GRI是IMU在全局帧（即第一个IMU帧，记为 G G G 中的位置和姿态， G g ^G \mathbf{g} Gg是全局帧中未知的重力向量， a m \mathbf{a}_m am和$ \omega_m 是 I M U 测量值，是IMU测量值，是IMU测量值，\mathbf{n}a 和和和\mathbf{n} \omega$ 是IMU测量的白噪声， b a \mathbf{b}a ba和 b ω \mathbf{b}\omega bω是IMU偏置，建模为具有高斯噪声$ \mathbf{n}{ba}$ 和$ \mathbf{n} {b\omega}$的随机游走过程，符号 ⋅ ^ \hat{\cdot} ⋅^表示反对称矩阵。

离散模型：
基于上述定义的 ⊞ \boxplus ⊞操作，我们可以使用零阶保持器在IMU采样周期 Δ t \Delta t Δt 下对连续模型(1)进行离散化。得到的离散模型为：
x i + 1 = x i ⊞ ( Δ t f ( x i , u i , w i ) ) ( 2 ) x_{i+1} = x_i \boxplus \left(\Delta t f\left(x_i, u_i, w_i\right)\right) \quad (2) xi+1=xi⊞(Δtf(xi,ui,wi))(2)
其中 i i i 是IMU测量的索引，函数 f、状态 x、输入 u u u 和噪声 w w w 定义如下：
M = S O ( 3 ) × R 15 , dim ⁡ ( M ) = 18 x ≐ [ G R I T G p I T G v I T b ω T b a T G g T ] T ∈ M u ≐ [ ω m T a m T ] , w ≐ [ n ω T n a T n b ω T n b a T ] f ( x i , u i , w i ) = [ ω m i − b ω i − n ω i G R I i ( a m i − b a i − n a i ) + G g i n b ω i n b a i 0 3 × 1 ] \mathcal{M} = SO(3) \times \mathbb{R}^{15}, \quad \operatorname{dim}(\mathcal{M}) = 18 \\ x \doteq \begin{bmatrix} {}^G R_I^T & {}^G p_I^T & {}^G v_I^T & b_\omega^T & b_a^T & {}^G g^T \end{bmatrix}^T \in \mathcal{M} \\ u \doteq \begin{bmatrix} \omega_m^T \\ a_m^T \end{bmatrix}, \quad w \doteq \begin{bmatrix} n_\omega^T \\ n_a^T \\ n_{b\omega}^T \\ n_{b a}^T \end{bmatrix} \\ f(x_i, u_i, w_i) = \begin{bmatrix} \omega_{m_i} - b_{\omega_i} - n_{\omega_i} \\ {}^G R_{I_i} \left(a_{m_i} - b_{a_i} - n_{a_i}\right) + {}^G g_i \\ n_{b\omega_i} \\ n_{b a_i} \\ 0_{3\times 1} \end{bmatrix} M=SO(3)×R15,dim(M)=18x≐[GRITGpITGvITbωTbaTGgT]T∈Mu≐[ωmTamT],w≐ nωTnaTnbωTnbaT f(xi,ui,wi)= ωmi−bωi−nωiGRIi(ami−bai−nai)+Gginbωinbai03×1
在这段描述中， M \mathcal{M} M表示状态空间， x x x表示状态向量，它包含了IMU相对于全局参考帧的姿态、位置、速度、陀螺仪和加速度计的偏置以及重力向量。 u u u表示外部输入，包括角速度和加速度测量值。 w w w表示噪声向量，包括角速度、加速度、偏置的噪声。函数 f f f 描述了状态的动态变化，包括角速度、加速度、偏置的随机游走模型和重力的影响。

B. 激光雷达测量的预处理

激光雷达测量是在其局部坐标系中的点坐标。由于原始激光雷达点以非常高的速率（例200kHz）采样，通常不可能在接收到每个新点后立即处理。一种更实际的方法是将这些点累积一段时间，然后一次性处理它们。FAST-LIO中，最小累积间隔设置为20毫秒，这导致高达50Hz的全状态估计（即里程计输出）和地图更新，如图2(a)所示。这样的累积点集被称为扫描，处理它的时间记为 t k t_{k} tk 见图2(b)）。从原始点中，我们提取具有高局部平滑度的平面点和具有低局部平滑度的边缘点，如文献[8]和[10]中所述。假设特征点的数量为 m，每个点在时间 ρ j ∈ ( t k − 1 , t k ] \rho_{j}\in (t_{k-1}, t_{k}] ρj∈(tk−1,tk] 被采样，并记为 L j p f j {}^{L_{j}}p_{f_{j}} Ljpfj ，其中 L j L_{j} Lj 是时间 ρ j \rho_{j} ρj处的激光雷达局部帧。在一次激光雷达扫描期间，还有多个IMU测量。

C. 状态估计

为了估计状态公式(2)中的状态，我们使用迭代扩展卡尔曼滤波器。此外，我们按照[23, 24]中的方法在状态估计的切空间中描述估计协方差。假设在 t k − 1 t_{k-1} tk−1 时最后一次激光雷达扫描的最优状态估计为 x ‾ k − 1 \overline{x}{k-1} xk−1，协方差矩阵为 P ‾ k − 1 \overline{P}{k-1} Pk−1。那么 P ‾ k − 1 \overline{P}{k-1} Pk−1 表示下面定义的随机误差状态向量的协方差：
x ~ k − 1 ≐ x k − 1 ⊟ x ‾ k − 1 = [ δ θ T G p ~ I T G v ~ I T b ~ ω T b ~ a T G g ~ T ] T \widetilde{x}{k-1} \doteq x_{k-1} \boxminus \overline{x}_{k-1} = \left[\begin{array}{llllll} \delta\theta^T & ^G\widetilde{p}_I^T & ^G\widetilde{v}I^T & \widetilde{b}\omega^T & \widetilde{b}_a^T & {}^G\widetilde{g}^T \end{array}\right]^T x k−1≐xk−1⊟xk−1=[δθTGp ITGv ITb ωTb aTGg T]T

其中 δ θ = log ⁡ ( G R ‾ I T G R I ) \delta\theta = \log\left({}^G\overline{R}{I}^{T G} R{I}\right) δθ=log(GRITGRI) 是姿态误差，其余为标准加性误差（即某个量 x x x 的估计 x ‾ \overline{x} x 的误差为 x ~ = x − x ‾ \widetilde{x} = x - \overline{x} x =x−x）。直观地说，姿态误差 δ θ \delta\theta δθ 描述了真实姿态和估计姿态之间的（小）偏差。这种误差定义的主要优点是它允许我们通过 3 × 3 3 \times 3 3×3 协方差矩阵 E { δ θ δ θ T } E\left\{\delta\theta\delta\theta^T\right\} E{δθδθT} 来表示姿态不确定性。由于姿态有3个自由度（DOF），这是一种最小的表示。

1) 前向传播：

前向传播在接收到IMU输入后执行（见图2）。更具体地说，状态是按照(2)通过将过程噪声 w i w_{i} wi 设置为零来传播的：
x ^ i + 1 = x ^ i ⊞ ( Δ t f ( x ^ i , u i , 0 ) ) ; x ^ 0 = x ‾ k − 1 . \widehat{x}{i+1} = \widehat{x}{i} \boxplus \left(\Delta t f\left(\widehat{x}{i}, u{i}, 0\right)\right); \widehat{x}{0} = \overline{x}{k-1}. x i+1=x i⊞(Δtf(x i,ui,0));x 0=xk−1.

其中 Δ t = τ i + 1 − τ i \Delta t = \tau_{i+1} - \tau_{i} Δt=τi+1−τi。为了传播协方差，我们使用下面得到的错误状态动态模型：

x ~ i + 1 = x i + 1 ⊟ x ^ i + 1 = ( x i ⊞ Δ t f ( x i , u i , w i ) ) ⊟ ( x ^ i ⊞ Δ t f ( x ^ i , u i , 0 ) ) ≃ ( 23 ) F x ~ x ~ i + F w w i （ 5 ） \widetilde{x}{i+1} = x{i+1} \boxminus \widehat{x}{i+1} \\ = \left(x{i} \boxplus \Delta t f\left(x_{i}, u_{i}, w_{i}\right)\right) \boxminus \left(\widehat{x}{i} \boxplus \Delta t f\left(\widehat{x}{i}, u_{i}, 0\right)\right) \\ \stackrel{(23)}{\simeq} F_{\widetilde{x}} \widetilde{x}{i} + F{w} w_{i} （5） x i+1=xi+1⊟x i+1=(xi⊞Δtf(xi,ui,wi))⊟(x i⊞Δtf(x i,ui,0))≃(23)Fx x i+Fwwi（5）

矩阵 F x ~ F_{\widetilde{x}} Fx 和 F w F_{w} Fw 在(5)中的计算遵循附录A。结果如(7)所示，其中 ω ^ i = ω m i − b ^ ω i , a ^ i = a m i − b ^ a i \widehat{\omega}{i} = \omega{m_{i}} - \widehat{b}{\omega{i}}, \widehat{a}{i} = a{m_{i}} - \widehat{b}{a{i}} ω i=ωmi−b ωi,a i=ami−b ai 并且 A ( u ) − 1 A(u)^{-1} A(u)−1 遵循[25]中相同的定义如下：
A ( u ) − 1 = I − 1 2 ⌊ u ⌋ ∧ + ( 1 − α ( ∥ u ∥ ) ) ⌊ u ⌋ ∧ 2 ∥ u ∥ 2 α ( m ) = m 2 cot ⁡ ( m 2 ) = m 2 cos ⁡ ( m / 2 ) sin ⁡ ( m / 2 ) A\left(u\right)^{-1} = I - \frac{1}{2} \lfloor u \rfloor_{\wedge} + (1 - \alpha(\|u\|)) \frac{\lfloor u \rfloor_{\wedge}^{2}}{\|u\|^{2}} \\ \alpha(m) = \frac{m}{2} \cot\left(\frac{m}{2}\right) = \frac{m}{2} \frac{\cos(m/ 2)}{\sin(m/ 2)} A(u)−1=I−21⌊u⌋∧+(1−α(∥u∥))∥u∥2⌊u⌋∧2α(m)=2mcot(2m)=2msin(m/2)cos(m/2)

将白噪声 w 的协方差表示为Q，那么传播的协方差 P ^ i \widehat{P}{i} P i可以按照以下方程迭代计算：
P ^ i + 1 = F x ~ P ^ i F x ~ T + F w Q F w T ; P ^ 0 = P ‾ k − 1 . \widehat{P}{i+1} = F_{\widetilde{x}} \widehat{P}{i} F{\widetilde{x}}^T + F_{w} Q F_{w}^T; \quad \widehat{P}{0} = \overline{P}{k-1}. P i+1=Fx P iFx T+FwQFwT;P 0=Pk−1.

传播过程持续进行，直到达到新扫描的结束时间 t k t_{k} tk，此时传播的状态和协方差分别记为 x ^ k , P ^ k \widehat{x}{k}, \widehat{P}{k} x k,P k。然后 P ^ k \widehat{P}{k} P k 表示真实状态 x k x{k} xk与状态传播 x ^ k \widehat{x}{k} x k 之间的误差协方差（即， x k ⊟ x ^ k x{k} \boxminus \widehat{x}_{k} xk⊟x k ）。

2) 反向传播与运动补偿：

当在时间 t k t_{k} tk达到点积累时间间隔时，新的特征点扫描应该与传播的状态 x ^ k \widehat{x}{k} x k 和协方差 P ^ k \widehat{P}{k} P k 融合，以产生最优的状态更新。然而，尽管新扫描在时间 t k t_{k} tk ，特征点是在它们各自的采样时间 ρ j ≤ t k \rho_{j} \leq t_{k} ρj≤tk 被测量的（见第III-B.4节和图2(b)），导致在参考坐标系中的不匹配。

为了补偿时间 ρ j \rho_{j} ρj 和时间 t k t_{k} tk之间的相对运动（即运动畸变），我们从 x ^ k \widehat{x}{k} x k 开始，反向传播（2）如下：
x ˇ j − 1 = x ˇ j ⊞ ( − Δ t f ( x ˇ j , u j , 0 ) ) \check{x}{j-1} = \check{x}{j} \boxplus \left(-\Delta t f\left(\check{x}{j}, u_{j}, 0\right)\right) xˇj−1=xˇj⊞(−Δtf(xˇj,uj,0))

反向传播以特征点的频率执行，这通常比IMU的速率高得多。对于在两个IMU测量之间采样的所有特征点，我们使用左侧的IMU测量作为反向传播的输入。此外，注意到 f ( x j , u j , 0 ) f(x_{j}, u_{j}, 0) f(xj,uj,0)（见（3））的最后三个块元素（对应于陀螺仪偏置、加速度计偏置和外参）为零，因此反向传播可以简化为：

其中 ρ j − 1 ∈ [ τ i − 1 , τ i ) ， Δ t = ρ j − ρ j − 1 \rho_{j-1} \in [\tau_{i-1}, \tau_{i}) ， \Delta t = \rho_{j} - \rho_{j-1} ρj−1∈[τi−1,τi)，Δt=ρj−ρj−1，s.f. 表示"从...开始"。反向传播将产生时间 ρ j \rho_{j} ρj 和扫描结束时间 t k t_{k} tk 之间的相对姿态： I k T ˇ I j = ( I k R ˇ I j , I k p ˇ I j ) . {}^{I_{k}}\check{T}{I{j}} = \left({}^{I_{k}}\check{R}{I{j}}, {}^{I_{k}}\check{p}{I{j}}\right). IkTˇIj=(IkRˇIj,IkpˇIj).这个相对姿态使我们能够将局部测量$ {}^{L_{j}} p_{f_{j}}$ 投影到扫描结束测量 $ {}^{L_{k}} p_{f_{j}}$如下（见图2）：
L k p f j = I T L − 1 I k T ˇ I j I T L L j p f j . ( 10 ) {}^{L_k} p_{f_j} = {}^{I} T_L^{-1} I_k\check{T}{I_j} {}^{I} T_L {}^{L_j} p{f_j}. \quad (10) Lkpfj=ITL−1IkTˇIjITLLjpfj.(10)

其中 I T L {}^{I} T_{L} ITL 是已知的外参（见第III-B.2节）。然后，投影点 L k p f j {}^{L_{k}} p_{f_{j}} Lkpfj用于构建后续部分中的残差。

3) 残差计算：

有了（10）中的运动补偿，我们可以将特征点扫描 { L k p f j } \left\{ {}^{L_{k}} p_{f_{j}} \right\} {Lkpfj} 视为在相同时间 t k t_{k} tk采样，并用它来构建残差。假设当前迭代的迭代卡尔曼滤波器是$ \kappa$ ，相应的状态估计是 x ^ k κ \widehat{x}{k}^{\kappa} x kκ 。当 κ = 0 \kappa=0 κ=0时， x ^ k κ = x ^ k \widehat{x}{k}^{\kappa} = \widehat{x}{k} x kκ=x k，即（4）中传播的预测状态。然后，特征点 { L k p f j } \left\{ {}^{L{k}} p_{f_{j}} \right\} {Lkpfj} 可以转换到全局帧如下：
G p ^ f j κ = G T ^ I k κ I T L L k p f j ; j = 1 , ⋯ , m . ( 11 ) {}^{G}\widehat{p}{f{j}}^{\kappa} = {}^{G}\widehat{T}{I{k}}^{\kappa}{}^{I} T_{L}{}^{L_{k}} p_{f_{j}}; \quad j = 1, \cdots, m. \quad (11) Gp fjκ=GT IkκITLLkpfj;j=1,⋯,m.(11)

对于每个激光雷达特征点，假设其在地图中最近的由附近特征点定义的平面或边缘是该点真正所属的位置。也就是说，残差定义为特征点的估计全局帧坐标 G p ^ f j κ {}^{G}\widehat{p}{f{j}}^{\kappa} Gp fjκ与地图中最近平面（或边缘）的距离。记 u j u_{j} uj 为相应平面（或边缘）的法向量（或边缘方向），在该平面（或边缘）上有一个点 G q j {}^{G} q_{j} Gqj ，那么残差 z j κ z_{j}^{\kappa} zjκ 计算如下：
z j κ = G j ( G p ^ f j κ − G q j ) ( 12 ) z_j^\kappa = G_j \left( {}^G\widehat{p}_{f_j}^\kappa - {}^G q_j \right) \quad (12) zjκ=Gj(Gp fjκ−Gqj)(12)

其中 G j = u j T G_{j} = u_{j}^{T} Gj=ujT 对于平面特征， G j = ⌊ u j ⌋ ∧ G_{j} = \left\lfloor u_{j} \right\rfloor\wedge Gj=⌊uj⌋∧对于边缘特征。 u j u_{j} uj 的计算和在地图中搜索定义相应平面或边缘的附近点是通过构建最新地图中点的KD树来实现的[10]。此外，我们只考虑范数低于特定阈值（例如，0.5米）的残差。超过此阈值的残差要么是离群点，要么是新观测到的点。

为了将计算得到的残差 z j κ z_{j}^{\kappa} zjκ与从IMU数据传播得到的状态预测 x ^ k \widehat{x}{k} x k 和协方差 P ^ k \widehat{P}{k} P k 融合，我们需要将残差 z j κ z_{j}^{\kappa} zjκ 与真实状态 x k x_{k} xk 和测量噪声的测量模型线性化。测量噪声来自于在测量点 L j p f j {}^{L_{j}} p_{f_{j}} Ljpfj时的激光雷达测距和波束导向噪声 L j n f j {}^{L_{j}} n_{f_{j}} Ljnfj 。从点测量中移除这个噪声可以得到真实点位置：
L j p f j g t = L j p f j − L j n f j . ( 13 ) {}^{L_j} p_{f_j}^{gt} = {}^{L_j} p_{f_j} - {}^{L_j} n_{f_j}. \quad (13) Ljpfjgt=Ljpfj−Ljnfj.(13)

这个真实点，通过(10)投影到帧 L k L_{k} Lk，然后使用真实状态$ x_{k}$（即位姿）投影到全局帧，应该正好位于地图中的平面（或边缘）上。也就是说，将(13)代入(10)，然后代入(11)，再进一步代入(12)，应该得到零。即，
0 = h j ( x k , L j n f j ) = G j ( G T I k I k T ˇ I j I T L ( L j p f j − L j n f j ) − G q j ) 0 = h_{j}\left(x_{k},{}^{L_{j}} n_{f_{j}}\right) = G_{j}\left({}^{G} T_{I_{k}}{}^{I_{k}}\check{T}{I{j}}{}^{I} T_{L}\left({}^{L_{j}} p_{f_{j}} - {}^{L_{j}} n_{f_{j}}\right) - {}^{G} q_{j}\right) 0=hj(xk,Ljnfj)=Gj(GTIkIkTˇIjITL(Ljpfj−Ljnfj)−Gqj)

通过在$\widehat{x}{k}^{\kappa} $ 处对上述方程进行一阶近似，可以得到：
0 = h j ( x k , L j n f j ) ≃ h j ( x ^ k κ , 0 ) + H j κ x ~ k κ + v j = z j κ + H j κ x ~ k κ + v j 0 = h_j\left(x_k, {}^{L_j} n{f_j}\right) \simeq h_j\left(\widehat{x}_k^\kappa, 0\right) + H_j^\kappa \widetilde{x}_k^\kappa + v_j \\= z_j^\kappa + H_j^\kappa \widetilde{x}_k^\kappa + v_j 0=hj(xk,Ljnfj)≃hj(x kκ,0)+Hjκx kκ+vj=zjκ+Hjκx kκ+vj

这里， H j κ H_j^\kappa Hjκ是残差 h j h_j hj 关于状态 x k x_k xk 的雅可比矩阵， x ~ k κ \widetilde{x}_k^\kappa x kκ是状态误差， v j v_j vj是测量噪声。

在将残差 z j κ z_{j}^{\kappa} zjκ与从IMU数据传播得到的状态预测 x ^ k \widehat{x}{k} x k和协方差 P ^ k \widehat{P}{k} P k融合时，我们需要将残差 z j κ z_{j}^{\kappa} zjκ 与真实状态 x k x_{k} xk 和测量噪声的测量模型线性化。测量噪声来自于在测量点 L j p f j {}^{L_{j}} p_{f_{j}} Ljpfj时的激光雷达测距和波束导向噪声 L j n f j {}^{L_{j}} n_{f_{j}} Ljnfj。从点测量中移除这个噪声可以得到真实点位置：
L j p f j g t = L j p f j − L j n f j . ( 13 ) {}^{L_j} p_{f_j}^{gt} = {}^{L_j} p_{f_j} - {}^{L_j} n_{f_j}. \quad (13) Ljpfjgt=Ljpfj−Ljnfj.(13)

这个真实点，通过(10)投影到帧 L k L_{k} Lk，然后使用真实状态 x k x_{k} xk即位姿）投影到全局帧，应该正好位于地图中的平面（或边缘）上。也就是说，将(13)代入(10)，然后代入(11)，再进一步代入(12)，应该得到零。即，
0 = h j ( x k , L j n f j ) = G j ( G T I k I k T ˇ I j I T L ( L j p f j − L j n f j ) − G q j ) 0 = h_{j}\left(x_{k},{}^{L_{j}} n_{f_{j}}\right) = G_{j}\left({}^{G} T_{I_{k}}{}^{I_{k}}\check{T}{I{j}}{}^{I} T_{L}\left({}^{L_{j}} p_{f_{j}} - {}^{L_{j}} n_{f_{j}}\right) - {}^{G} q_{j}\right) 0=hj(xk,Ljnfj)=Gj(GTIkIkTˇIjITL(Ljpfj−Ljnfj)−Gqj)

通过在 x ^ k κ \widehat{x}_{k}^{\kappa} x kκ 处对上述方程进行一阶近似，可以得到：

这里， x ~ k κ = x k ⊟ x ^ k κ \widetilde{x}{k}^{\kappa} = x{k} \boxminus \widehat{x}{k}^{\kappa} x kκ=xk⊟x kκ（或者等价地 x k = x ^ k κ ⊞ x ~ k κ x{k} = \widehat{x}{k}^{\kappa} \boxplus \widetilde{x}{k}^{\kappa} xk=x kκ⊞x kκ 是 h j ( x ^ k κ ⊞ x ~ k κ , L j n f j ) h_{j}\left(\widehat{x}{k}^{\kappa} \boxplus \widetilde{x}{k}^{\kappa}, {}^{L_{j}} n_{f_{j}}\right) hj(x kκ⊞x kκ,Ljnfj) 关于 x ~ k κ \widetilde{x}{k}^{\kappa} x kκ 的雅可比矩阵，评估为零， v j ∈ N ( 0 , R j ) v{j} \in \mathcal{N}\left(0, R_{j}\right) vj∈N(0,Rj)来自于原始测量噪声 L j n f j {}^{L_{j}} n_{f_{j}} Ljnfj。

注意，从第III-C.1节中的前向传播得到的 x k x_{k} xk的先验分布是：
x k ⊟ x ^ k = ( x ^ k κ ⊞ x ~ k κ ) ⊟ x ^ k = x ^ k κ ⊟ x ^ k + J κ x ~ k κ （ 15 ） x_k \boxminus \widehat{x}_k = \left(\widehat{x}_k^\kappa \boxplus \widetilde{x}_k^\kappa\right) \boxminus \widehat{x}_k = \widehat{x}_k^\kappa \boxminus \widehat{x}_k + J^\kappa \widetilde{x}_k^\kappa （15） xk⊟x k=(x kκ⊞x kκ)⊟x k=x kκ⊟x k+Jκx kκ（15）

其中 J κ J^\kappa Jκ 是 ( x ^ k κ ⊞ x ~ k κ ) ⊟ x ^ k \left(\widehat{x}k^\kappa \boxplus \widetilde{x}k^\kappa\right) \boxminus \widehat{x}k (x kκ⊞x kκ)⊟x k 关于 x ^ k κ \widehat{x}{k}^{\kappa} x kκ的偏导数，评估为零：
J κ = [ A ( G R ^ I k κ ⊟ G R ^ I k ) − T 0 3 × 15 0 15 × 3 I 15 × 15 ] J^{\kappa} = \left[\begin{array}{cc} A\left({}^{G}\widehat{R}{I{k}}^{\kappa} \boxminus {}^{G}\widehat{R}{I{k}}\right)^{-T} & 0_{3\times 15} \\ 0_{15\times 3} & I_{15\times 15} \end{array}\right] Jκ=[A(GR Ikκ⊟GR Ik)−T015×303×15I15×15]

其中 A ( ⋅ ) − 1 A(\cdot)^{-1} A(⋅)−1在(6)中定义。对于第一次迭代（即扩展卡尔曼滤波器的情况）， x ^ k κ = x ^ k \widehat{x}{k}^{\kappa} = \widehat{x}{k} x kκ=x k，那么 J κ = I J^{\kappa} = I Jκ=I 。

将(15)中的先验与(14)中的后验分布结合，得到最大后验估计（MAP）：
min ⁡ x ~ k κ ( ∥ x k ⊟ x ^ k ∥ P ^ k − 1 2 + ∑ j = 1 m ∥ z j κ + H j κ x ~ k κ ∥ R j − 1 2 ) ( 17 ) \min_{\widetilde{x}_k^\kappa} \left( \left\|x_k \boxminus \widehat{x}k \right\|{\widehat{P}k^{-1}}^2 + \sum{j=1}^m \left\| z_j^\kappa + H_j^\kappa \widetilde{x}k^\kappa \right\|{R_j^{-1}}^2 \right) \quad (17) x kκmin(∥xk⊟x k∥P k−12+j=1∑m zjκ+Hjκx kκ Rj−12)(17)

其中 ∥ x ∥ M 2 = x T M x \|x\|{M}^2 = x^T M x ∥x∥M2=xTMx。将(15)中的先验线性化代入(17)并优化得到的二次成本，可以得到标准的迭代卡尔曼滤波器，计算如下（为了简化符号，设 H = [ H 1 κ T , ⋯ , H m κ T ] T , R = diag ⁡ ( R 1 , ⋯ , R m ) , P = ( J κ ) − 1 P ^ k ( J κ ) − T H = \left[H{1}^{\kappa^T}, \cdots, H_{m}^{\kappa^T}\right]^T, R = \operatorname{diag}\left(R_{1}, \cdots, R_{m}\right), P = \left(J^{\kappa}\right)^{-1} \widehat{P}{k} \left(J^{\kappa}\right)^{-T} H=[H1κT,⋯,HmκT]T,R=diag(R1,⋯,Rm),P=(Jκ)−1P k(Jκ)−T，和 z k κ = [ z 1 κ T , ⋯ , z m κ T ] T K = P H T ( H P H T + R ) − 1 x ^ k κ + 1 = x ^ k κ ⊞ ( − K z k κ − ( I − K H ) ( J κ ) − 1 ( x ^ k κ ⊟ x ^ k ) ) ( 18 ) z{k}^{\kappa} = \left[z_{1}^{\kappa^T}, \cdots, z_{m}^{\kappa^T}\right]^T \\K = P H^T \left(H P H^T + R\right)^{-1} \\ \widehat{x}{k}^{\kappa+1} = \widehat{x}{k}^{\kappa} \boxplus \left(-K z_{k}^{\kappa} - (I - K H) \left(J^{\kappa}\right)^{-1} \left(\widehat{x}{k}^{\kappa} \boxminus \widehat{x}{k}\right)\right) \quad (18) zkκ=[z1κT,⋯,zmκT]TK=PHT(HPHT+R)−1x kκ+1=x kκ⊞(−Kzkκ−(I−KH)(Jκ)−1(x kκ⊟x k))(18)

在常用的卡尔曼增益形式（18）中，一个问题是它需要求逆矩阵 H P H T + R H P H^T + R HPHT+R，这个矩阵的维度与测量值的数量相当。实际上，激光雷达特征点的数量通常非常多，求逆这样一个大矩阵是不切实际的。因此，现有的工作[21, 26]只使用了少量的测量值。在本文中，我们展示了如何避免这个限制。这种直觉来源于（17），其中代价函数是关于状态的，因此解决方案应该根据状态维度计算复杂度。实际上，如果直接求解（17），我们可以得到（18）中相同的解，但是以一种新的卡尔曼增益形式表示，如下所示：
K = ( H T R − 1 H + P − 1 ) − 1 H T R − 1 . (20) K = \left(H^T R^{-1} H + P^{-1}\right)^{-1} H^T R^{-1}. \quad \text{(20)} K=(HTR−1H+P−1)−1HTR−1.(20)

我们在附录B中证明了这两种卡尔曼增益形式确实是等价的，基于矩阵逆引理[27]。由于激光雷达测量是独立的，协方差矩阵 R 是（块）对角的，因此新公式只需要求逆两个状态维度的矩阵，而不是测量维度的矩阵。这种新的公式大大减少了计算量，因为状态维度通常远小于测量值的数量。

该公式大大节省了计算量，因为状态维度通常远小于激光雷达惯性里程计（LIO）中的测量值维度（例如，在一次扫描中可能有超过1,000个有效特征点，而状态维度仅为18）。

算法：
我们的状态估计总结在算法1中。

D. 地图更新

随着状态更新 x ‾ k \overline{x}{k} xk （因此 G T ‾ I k = ( G R ‾ I k , G p ‾ I k ) {}^{G}\overline{T}{I_{k}}=\left({}^{G}\overline{R}{I{k}},{}^{G}\overline{p}{I{k}}\right) GTIk=(GRIk,GpIk)，每个特征点 ( L k p f j ) \left({}^{L_{k}} p_{f_{j}}\right) (Lkpfj) 投影到机体帧 L k L_{k} Lk （见公式(10)），然后通过以下方式转换到全局帧：
G p ‾ f j = G T ‾ I k I T L L k p f j ; j = 1 , ⋯ , m . ( 21 ) {}^{G}\overline{p}{f_j} = {}^{G}\overline{T}{I_k}{}^{I} T_{L}{}^{L_k} p_{f_j}; \quad j=1,\cdots, m. \quad (21) Gpfj=GTIkITLLkpfj;j=1,⋯,m.(21)

这些特征点最终被添加到包含所有先前步骤中特征点的现有地图中。

E. 初始化

为了获得系统状态（例如，重力向量 G g {}^{G} g Gg 、偏置和噪声协方差）的良好初始估计，以加速状态估计器，需要进行初始化。在FAST-LIO中，初始化很简单：保持激光雷达静止几秒钟（本文中所有实验为2秒），然后收集到的数据用于初始化IMU偏置和重力向量。如果激光雷达支持非重复扫描（例如，Livox AVIA），保持静止还允许激光雷达捕获初始高分辨率地图，这对后续导航是有益的。

流程概述

状态传播：集成 IMU 测量值以预测下一状态。
反向传播：补偿 IMU 与 LiDAR 测量之间的时间差。
LiDAR 更新：计算 LiDAR 扫描特征点与地图点之间的残差，并使用卡尔曼滤波更新融合。

实验结果

结论
提出 FAST-LIO，一种计算效率高且稳健的 LiDAR-惯性里程计框架，采用紧耦合迭代卡尔曼滤波器。使用前向和后向传播来预测状态并补偿 LiDAR 扫描中的运动。此外证明并实现了一个等效公式，该公式可以大大降低卡尔曼增益计算的复杂度。FASTLIO 在无人机飞行实验中进行了测试，在具有大旋转速度的室内环境和室外环境中具有挑战性。在所有测试中，我们的方法都产生了精确、实时和可靠的导航结果。
附件
见文章末尾