FAST-LIO:A Fast Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter论文翻译

0 摘要

本文提出了一种计算高效且鲁棒的激光雷达惯性里程计框架。我们使用紧耦合的迭代扩展卡尔曼滤波（IEKF） 将激光雷达特征点与IMU数据融合，以便在可能会造成退化的快速运动、大量噪声或杂乱环境中进行稳健导航。为了在存在大量测量值的情况下降低计算量，我们提出了一个新的公式来计算卡尔曼增益。新公式的计算量取决于状态维数而不是测量维数。我们将所提出的方法进行了实现，并在各种室内外环境中进行了测试。在所有测试中，我们的方法都能够实时地产生可靠的导航结果：在四旋翼机载计算机上运行时，它在一次扫描中融合了1200多个有效特征点，并在25毫秒内完成一次IEKF步骤的所有迭代。我们的代码在Github上是开源的。开源地址为：https://github.com/hku-mars/FAST_LIO

I 引言

同时定位与建图（SLAM）是无人机（UAV）等移动机器人的基本前提。视觉（惯性）里程计（VO），如双目视觉里程计[1]和单目视觉里程计[2, 3]由于其重量轻且成本低，通常用于移动机器人。尽管提供了丰富的RGB信息，但视觉解决方案缺乏直接的深度测量，并且需要大量计算资源来重建三维环境以进行轨迹规划。此外，视觉方案对光照条件非常敏感。激光雷达（LiDAR）类传感器可以克服所有这些困难，但对于小型移动机器人来说成本太高（并且体积太大）。

固态激光雷达是近年来激光雷达发展的主要趋势，如基于MEMS扫描[4]和基于旋转棱镜[5]的激光雷达。这些激光雷达非常具有成本效益（成本接近全球快门相机），重量轻（可以由小型无人机携带），并且高性能（能够直接主动的进行远程的高精度三维测量）。这些特性使得此类激光雷达适用于无人机，特别是工业无人机，这些工业无人机需要获得精确的环境三维地图（如航空测绘）或者可能在具有严重照明变化的混乱环境中工作（如灾后搜索和检查）。

尽管固态激光雷达具有巨大的潜力，但它给SLAM带来了新的挑战：

1. 激光雷达测量中的特征点通常是几何结构（例如环境中的边缘和平面)。当无人机在没有强特征的杂乱环境中工作时，基于激光雷达的解决方案很容易退化。当激光雷达具有小视场时，这个问题更加明显。
2. 由于沿扫描方向的高分辨率，激光雷达扫描通常包含大量特征点（例如成千上万个）。虽然这些特征点并不足以可靠地确定在退化情况下的姿态，但将如此大量的特征点与IMU测量进行紧耦合需要巨大的计算资源，这是无人机机载计算机无法负担的。
3. 由于激光雷达采样点是由几个激光探头和接收器依次发射和接收激光雷达进行采样的，所以一次扫描中的激光点总是在不同的时间被采样，这样会导致运动畸变，从而显著降低扫描配准[6]。无人机螺旋桨和马达的不断旋转也给IMU测量带来了显著的噪声。

为了使激光雷达导航能够适用于小型移动机器人（如无人机），我们提出了FAST-LIO，这是一个计算高效并且鲁棒的激光雷达惯性里程计功能包。更具体地说，我们的贡献如下：

1. 为了应对可能会造成退化的快速运动、大量噪声或杂乱环境，我们采用紧耦合的迭代卡尔曼滤波IEKF将激光雷达特征点与IMU测量值融合。我们提出了一种正式的反向传播过程，以补偿运动畸变；
2. 为了降低大量激光雷达特征点所带来的计算负载，我们提出了一个新的公式来计算卡尔曼增益，并证明了它与传统的卡尔曼增益计算公式等价。新公式的计算复杂度取决于状态维数而不是测量维数。
3. 我们将我们的公式应用到一个快速而鲁棒的激光雷达惯性里程计软件包中。该系统能够在小型四旋翼机载计算机上运行。
4. 我们在各种室内和室外环境中进行实验，并通过实际的无人机（如图1）飞行测试来验证系统在存在快速运动或强烈振动噪声时的鲁棒性。

接下来的文章组织如下：第二节中，我们讨论了相关的研究工作。在第三节中，我们将对整个系统的pipeline进行概述并介绍每个关键组件的详细信息。第四节介绍了实验。最后在第五节进行总结。

II 相关工作

关于激光雷达SLAM的现有工作非常多。在此，我们将回顾最相关的工作：激光雷达里程计和建图、松耦合和紧耦合激光雷达惯性融合方法。

A 激光雷达里程计和建图

Besl等人[6]提出了一种用于扫描配准的迭代最近点方法（ICP），为激光雷达里程计奠定了基础。ICP方法在稠密三维扫描中表现良好。然而，对于激光雷达测量的稀疏点云，ICP方法要求的精确点匹配很少存在。为了解决这个问题，Segal等人[7]提出了一种基于点到平面距离的广义ICP方法。然后，Zhang等人[8]将这种ICP方法与点到边缘距离相结合，开发了激光雷达里程计和建图框架（LOAM）。此后，基于LOAM出现了很多种改进方法，比如LeGO-LOAM[9]和LOAM- livox[10]。虽然这些方法在结构环境和大视场的激光雷达中表现很好，但它们非常容易受到缺少特征的环境或小视场激光雷达[10]的影响。

B 松耦合激光雷达惯导里程计

IMU测量通常用于减轻激光雷达在缺少特征的环境中退化的问题。松耦合激光雷达惯性里程计（LIO）方法通常分别处理激光雷达和IMU的测量结果，然后融合它们的结果。例如，使用IMU辅助的LOAM[8]将通过IMU测量值积分得到的位姿作为激光雷达扫描配准的初始估计。Zhen等人[11]使用误差状态EKF融合了IMU测量和激光雷达测量的高斯粒子滤波输出。Balazadegan等人[12]增加了imu重力模型来估计六自由度自我运动，以辅助激光雷达扫描配准。Zuo等人[13]使用多状态约束卡尔曼滤波（MSCKF）将扫描配准结果与IMU和视觉测量结果融合。松耦合方法的一个常见步骤是通过对一个新的扫描进行配准来获得位姿测量，然后将位姿测量与IMU测量融合。将扫描配准与数据融合分离，减少了计算负载。然而，它忽略了系统的其他状态（例如速度）和新扫描的位姿之间的相关性。此外，在缺少特征的环境下，扫描配准会在某些方向退化，导致后面步骤中不可靠的融合。

C 紧耦合激光雷达惯导里程计

与松散耦合的方法不同，紧耦合的激光雷达惯性里程计方法通常将激光雷达的原始特征点（而不是扫描配准结果）与IMU数据融合。紧耦合LIO主要有两种方法：基于优化的方法和基于滤波的方法。Geneva等人[14]利用IMU预积分约束[15]和激光雷达特征点的平面约束[16]进行图优化。最近，Ye等人[17]提出了LIOM功能包，该包使用了类似的图优化方法，但基于边缘和平面特征。对于基于滤波器的方法，Bry[18]使用高斯粒子滤波器(GPF)来融合IMU和平面2D激光雷达的数据。该方法也被用于波士顿动力Atlas类人机器人。由于粒子滤波的计算复杂度随着特征点个数和系统维数的增加而快速增长，卡尔曼滤波及其变种通常更受欢迎，比如扩展卡尔曼滤波[19]、无迹卡尔曼滤波[20]、迭代卡尔曼滤波[21]等。

我们的方法属于紧耦合方法。我们采用类似于[21]的迭代扩展卡尔曼滤波器来减小线性化误差。卡尔曼滤波器（及其变体）的时间复杂度为 $\mathcal{O}\left(m^2\right)$，其中m为测量维数[22]，这在处理大量激光雷达测量时可能导致非常高的计算负载。朴素的下采样将减少测量的数量，但以信息丢失为代价。类似[9]，[21]通过提取和拟合的地面来减少测量的数量。然而，这并不适用于不总是在地上运行的航空应用。

III 方法论

A 框架概览

本文将使用表I中所示的符号。我们的工作流程概述如图2所示。激光雷达输入被送入特征提取模块，以获得平面和边缘特征。然后，将提取的特征和IMU测量值输入到我们的状态估计模块，以10Hz−50Hz的频率进行状态估计。然后，使用估计的姿势将特征点注册到全局帧，并将它们与从一开始构建的特征点地图进行合并。更新后的地图最终用于在下一步中注册更多的新点。

• $t_k$：第 $k$ 次 Lidar 扫描的结束时间
• ${\tau }_i$：一次 Lidar 扫描中的 IMU 第 $i$ 次采样时间
• ${\rho }_j$：一次 Lidar 扫描中的 第 $j$ 个特征点的采样时间
• $I_i,I_j,I_k$：分别表示在 ${\tau }_i,{\rho }_j$ 和 $t_k$ 时刻的 IMU 坐标系
• $L_j,L_k$：分别表示 ${\rho }_j$ 和 $t_k$ 时刻的 Lidar 坐标系
• $\mathbf{x},\hat{\mathbf{x}},\overline{\mathbf{x}}$：分别表示系统状态的真值、传播值和更新值
• $\tilde{\mathbf{x}}$：表示真值 $\mathbf{x}$ 和估计值 $\overline{\mathbf{x}}$ 之间的误差
• ${\hat{\mathbf{x}}}^{\kappa }$：表示迭代卡尔曼滤波第 $k$ 次更新的状态传播值
• ${\mathbf{x}}_i,{\mathbf{x}}_i,{\mathbf{x}}_k$：表示在 ${\tau }_i,{\rho }_j$ 和 $t_k$ 时刻的系统状态
• ${\check{\mathbf{x}}}_j$：表示在反向传播时，与 ${\mathbf{x}}_k$ 相关的估计值 ${\mathbf{x}}_j$

B 系统描述

1）$⊞/\boxminus$ 运算符：

记 $\mathcal{M}$ 为 $n$ 维流形空间，比如：$\mathcal{M}=SO(3)$。由于流行在局部上和 ${\mathbb{R}}^n$ 是同胚（homeomorphic）的，我们可以通过两个封闭运算符 $⊞$ 和 $\boxminus$ 来构建从 $\mathcal{M}$ 的一个局部邻域到它的正切空间 ${\mathbb{R}}^n$ 之间的双向映射[23]：

$$⊞:\mathcal{M}\times {\mathbb{R}}^n\to \mathcal{M};\boxminus :\mathcal{M}\times \mathcal{M}\to {\mathbb{R}}^n$$

$$\mathcal{M}=SO(3):\mathbf{R}⊞\mathbf{r}=\mathbf{R}\mathrm{Exp}(\mathbf{r});{\mathbf{R}}_1\boxminus {\mathbf{R}}_2=\mathrm{Log}\left({\mathbf{R}}_2^{\mathrm{T}}{\mathbf{R}}_1\right)$$

$$\mathcal{M}={\mathbb{R}}^n:\mathbf{a}⊞\mathbf{b}=\mathbf{a}+\mathbf{b};\mathbf{a}\boxminus \mathbf{b}=\mathbf{a}-\mathbf{b}$$

其中，$\mathrm{Exp}(\mathbf{r})=\mathbf{I}+\frac{\mathbf{r}}{\Vert \mathbf{r}\Vert }\sin (\Vert \mathbf{r}\Vert )+\frac{{\mathbf{r}}^2}{\Vert \mathbf{r}{\Vert }^2}(1-\cos (\Vert \mathbf{r}\Vert ))$ 是指数映射[23]，$\mathrm{Log}(\cdot )$ 是其逆映射。对于复合流形：$\mathcal{M}=SO(3)\times {\mathbb{R}}^n$，我们有：

$$\left[\begin{array}{c}\mathbf{R}\\ \mathbf{a}\end{array}\right]⊞\left[\begin{array}{l}\mathbf{r}\\ \mathbf{b}\end{array}\right]=\left[\begin{array}{c}\mathbf{R}⊞\mathbf{r}\\ \mathbf{a}+\mathbf{b}\end{array}\right];\left[\begin{array}{c}{\mathbf{R}}_1\\ \mathbf{a}\end{array}\right]\boxminus \left[\begin{array}{c}{\mathbf{R}}_2\\ \mathbf{b}\end{array}\right]=\left[\begin{array}{c}{\mathbf{R}}_1\boxminus {\mathbf{R}}_2\\ \mathbf{a}-\mathbf{b}\end{array}\right]$$

由上面的定义，我们很容易证明：

$$(\mathbf{x}⊞\mathbf{u})\boxminus \mathbf{x}=\mathbf{u};\mathbf{x}⊞(\mathbf{y}\boxminus \mathbf{x})=\mathbf{y};\forall \mathbf{x},\mathbf{y}\in \mathcal{M},\forall \mathbf{u}\in {\mathbb{R}}^n$$

2）连续模型：

假设IMU稳固的连接在了激光雷达上面，并且外参已知 ${}^I{\mathbf{T}}_L=\left({}^I{\mathbf{R}}_L,{}^I{\mathbf{p}}_L\right)$。将 IMU 坐标系（记为 $I$ ）作为本体参考坐标系，从而可以推出动力学模型：

$$\begin{array}{rl}{}^G{\dot{\mathbf{p}}}_I& ={}^G{\mathbf{v}}_I,{}^G{\dot{\mathbf{v}}}_I={}^G{\mathbf{R}}_I\left({\mathbf{a}}_m-{\mathbf{b}}_{\mathbf{a}}-{\mathbf{n}}_{\mathbf{a}}\right)+{}^G\mathbf{g},{}^G\dot{\mathbf{g}}=0\\ {}^G{\dot{\mathbf{R}}}_I& ={}^G{\mathbf{R}}_I{\lfloor {\mathbf{\omega }}_m-{\mathbf{b}}_{\mathbf{\omega }}-{\mathbf{n}}_{\mathbf{\omega }}\rfloor }_{\wedge },{\dot{\mathbf{b}}}_{\mathbf{\omega }}={\mathbf{n}}_{\mathbf{b}\mathbf{\omega }},{\dot{\mathbf{b}}}_{\mathbf{a}}={\mathbf{n}}_{\mathbf{ba}}\end{array}\text{\hspace{1em}(1)}$$

其中 ${}^G{\mathbf{p}}_I,{}^G{\mathbf{R}}_I$ 分别表示 IMU 坐标系在世界坐标系下的位置和姿态（第一帧 IMU 坐标系表示世界坐标系，记为 $G$ ），${}^G\mathbf{g}$ 表示世界坐标系下未知的重力向量，${\mathbf{a}}_m$ 和 ${\mathbf{\omega }}_m$ 分别表示 IMU 加速度计和陀螺仪的测量值，${\mathbf{n}}_{\mathbf{a}}$ 和 ${\mathbf{n}}_{\omega }$ 表示IMU测量的白噪声，${\mathbf{b}}_{\mathbf{a}}$ 和 ${\mathbf{b}}_{\omega }$ 分别表示 IMU 加速度计和陀螺仪的偏置，分别建模为高斯白噪声 ${\mathbf{n}}_{{\mathrm{b}}_{\mathrm{a}}}$ 和 ${\mathbf{n}}_{{\mathrm{b}}_{\omega }}$ 的随机游走过程。叉乘运算符$\lfloor \mathbf{a}{\rfloor }_{\wedge }$ 表示为向量 $\mathbf{a}\in {\mathbb{R}}^{\overline{3}}$ 的反对称矩阵。

3）离散模型：

基于运算符 $⊞$ ，我们可以使用零阶保持器（zero-order holder）将 IMU 采样周期 $\Delta t$ 内的连续模型离散化。离散模型表示为：

$${\mathbf{x}}_{i+1}={\mathbf{x}}_i⊞\left(\Delta t\mathbf{f}\left({\mathbf{x}}_i,{\mathbf{u}}_i,{\mathbf{w}}_i\right)\right)\text{\hspace{1em}(2)}$$

其中，$i$ 是 IMU 测量的索引，系统状态 $\mathbf{x}$ 、输入 $\mathbf{u}$ 和噪声项 $\mathbf{w}$ 、函数 $\mathbf{f}$ 定义如下：

$$\begin{array}{rl}& \mathcal{M}=SO(3)\times {\mathbb{R}}^{15},\dim (\mathcal{M})=18\\ & \mathbf{x}\doteq {\left[\begin{array}{cccccc}{}^G{\mathbf{R}}_I^T& {}^G{\mathbf{p}}_I^T& {}^G{\mathbf{v}}_I^T& {\mathbf{b}}_{\mathbf{\omega }}^T& {\mathbf{b}}_{\mathbf{a}}^T& {}^G{\mathbf{g}}^T\end{array}\right]}^T\in \mathcal{M}\\ & \mathbf{u}\doteq {\left[\begin{array}{ll}{\mathbf{\omega }}_m^T& {\mathbf{a}}_m^T\end{array}\right]}^T,\mathbf{w}\doteq {\left[\begin{array}{llll}{\mathbf{n}}_{\mathbf{\omega }}^T& {\mathbf{n}}_{\mathbf{a}}^T& {\mathbf{n}}_{\mathbf{b}\mathbf{\omega }}^T& {\mathbf{n}}_{\mathbf{ba}}^T\end{array}\right]}^T\\ & \mathbf{f}\left({\mathbf{x}}_i,{\mathbf{u}}_i,{\mathbf{w}}_i\right)=\left[\begin{array}{c}{\mathbf{\omega }}_{m_i}-{\mathbf{b}}_{{\mathbf{\omega }}_i}-{\mathbf{n}}_{{\mathbf{\omega }}_i}\\ {}^G{\mathbf{v}}_{I_i}\\ {}^G{\mathbf{R}}_{I_i}\left({\mathbf{a}}_{m_i}-{\mathbf{b}}_{{\mathbf{a}}_i}-{\mathbf{n}}_{{\mathbf{a}}_i}\right)+{}^G{\mathbf{g}}_i\\ {\mathbf{n}}_{\mathbf{b}{\mathbf{w}}_i}\\ {\mathbf{n}}_{{\mathbf{ba}}_i}\\ 0_{3\times 1}\end{array}\right]\end{array}\text{\hspace{1em}(3)}$$

4）激光雷达点云预处理：

激光雷达点云测量包括其局部坐标系下的点云坐标。由于原始Lidar点的采样频率很高，所以不太可能对采样到的每个点都进行实时处理。在 FAST-LIO 中，设定了最小为20ms的数据采集间隔，即在一定时间内积累数据点，之后再一次性处理，这样使得状态估计（里程计输出）与地图更新最高可达 50Hz。这样一组累积的点被称为一次扫描，这些点的处理时间记为 $t_k$（如图2(b)）。

假设特征点的数量为 $m$ ，每个特征点的采样时间记为 ${\rho }_j$，${\rho }_j\in \left(t_{k-1},t_k\right]$ 。特征点记为 ${}^{L_j}{\mathbf{p}}_{f_j}$，其中 $L_j$ 表示 ${\rho }_j$ 时刻的Lidar局部坐标系。在一次Lidar扫描中，会有多个IMU测量，每个IMU测量的采样时间记为 ${\tau }_i\in \left[t_{k-1},t_k\right]$ ，每个IMU测量对应公式（2）中的一个系统状态 ${\mathbf{x}}_i$。

需要注意的是，最后一个Lidar特征点在一次Lidar扫描的最后时刻采样，即 ${\rho }_m=t_k$；但是IMU测量的时间不一定与一次Lidar扫描的开始或结束时间对齐。

C 状态估计

我们使用迭代扩展卡尔曼滤波IEKF进行状态方程（2）中的状态估计。此外我们在[23, 24]中表征了状态估计的切线空间中的估计协方差。

假设上一次激光雷达扫描在 $t_{k-1}$ 时刻优化后的状态估计是 ${\overline{\mathbf{x}}}_{k-1}$，协方差矩阵为 ${\overline{\mathbf{P}}}_{k-1}$。然后 ${\overline{\mathbf{P}}}_{k-1}$ 表示下面定义的随机误差状态向量的协方差：

$${\tilde{\mathbf{x}}}_{k-1}\doteq {\mathbf{x}}_{k-1}\boxminus {\overline{\mathbf{x}}}_{k-1}={\left[\begin{array}{llllll}\delta {\mathbf{\theta }}^T& {}^G{\tilde{\mathbf{p}}}_I^T& G{\tilde{\mathbf{v}}}_I^T& {\tilde{\mathbf{b}}}_{\mathbf{\omega }}^T& {\tilde{\mathbf{b}}}_{\mathbf{a}}^T& {}^G{\tilde{\mathbf{g}}}^T\end{array}\right]}^T$$

其中 $\delta \mathbf{\theta }=\log \left({}^G{\overline{\mathbf{R}}}_I^{TG}{\mathbf{R}}_I\right)$ 是姿态误差，剩下的都是标准加法误差（比如，状态 $\mathbf{x}$ 的估计值 $\overline{\mathbf{x}}$ 的误差为 $\tilde{\mathbf{x}}=\mathbf{x}-\overline{\mathbf{x}}$）。直观上，姿态误差 $\delta \mathbf{\theta }$ 描述了真实姿态和估计姿态之间的（微小）偏离。此处误差定义的主要优点是，它允许我们通过 $3\times 3$ 协方差矩阵 E { δ θ δ θ T } \mathbb{E}\{\delta \boldsymbol{\theta} \delta \boldsymbol{\theta}^{T}\} E{δθδθT} 来表示姿态的不确定性。由于姿态具有3个自由度，因此这是一个最小的表示。

1）前向传播：

一旦接收到IMU输入，就进行前向传播（如图2(b)）。更具体的，状态传播按照公式（2）进行，并且将过程噪声 ${\mathbf{w}}_i$ 设为0：

$${\hat{\mathbf{x}}}_{i+1}={\hat{\mathbf{x}}}_i⊞\left(\Delta t\mathbf{f}\left({\hat{\mathbf{x}}}_i,{\mathbf{u}}_i,0\right)\right);{\hat{\mathbf{x}}}_0={\overline{\mathbf{x}}}_{k-1}\text{\hspace{1em}(4)}$$

其中，$\Delta t={\tau }_{i+1}-{\tau }_i$。为了传播协方差，我们使用以下误差状态的动力学模型：

$$\begin{array}{rl}{\tilde{\mathbf{x}}}_{i+1}& ={\mathbf{x}}_{i+1}\boxminus {\hat{\mathbf{x}}}_{i+1}\\ & =\left({\mathbf{x}}_i⊞\Delta t\mathbf{f}\left({\mathbf{x}}_i,{\mathbf{u}}_i,{\mathbf{w}}_i\right)\right)\boxminus \left({\hat{\mathbf{x}}}_i⊞\Delta t\mathbf{f}\left({\hat{\mathbf{x}}}_i,{\mathbf{u}}_i,0\right)\right)\\ & \simeq {\mathbf{F}}_{\tilde{\mathbf{x}}}{\tilde{\mathbf{x}}}_i+{\mathbf{F}}_{\mathbf{w}}{\mathbf{w}}_i.\end{array}\text{\hspace{1em}(5)}$$

上式的推导参考附录A。推导结果在公式（7）中，其中，类似于 [25]， ${\hat{\mathbf{\omega }}}_i={\mathbf{\omega }}_{m_i}-{\mathbf{b}}_{{\mathbf{\omega }}_i}$，${\hat{\mathbf{a}}}_i={\mathbf{a}}_{m_i}-{\hat{\mathbf{b}}}_{{\mathbf{a}}_i}$ 以及 $\mathbf{A}(\mathbf{u}{)}^{-1}$ 定义如下：

$$\begin{array}{rl}\mathbf{A}(\mathbf{u}{)}^{-1}& =\mathbf{I}-\frac{1}{2}\lfloor \mathbf{u}{\rfloor }_{\wedge }+(1-\mathbf{\alpha }(\Vert \mathbf{u}\Vert ))\frac{{\mathbf{u}}^2}{\Vert \mathbf{u}{\Vert }^2}\\ \alpha (\mathrm{m})& =\frac{\mathrm{m}}{2}\cot \left(\frac{\mathrm{m}}{2}\right)=\frac{\mathrm{m}}{2}\frac{\cos (\mathrm{m}/2)}{\sin (\mathrm{m}/2)}\end{array}\text{\hspace{1em}(6)}$$

$$\begin{gathered} {\mathbf{F}}_{\tilde{\mathbf{x}}}=\left[\begin{array}{cccccc}\mathrm{Exp}\left(-{\hat{\mathbf{\omega }}}_i\Delta t\right)& 0& 0& -\mathbf{A}{\left({\hat{\mathbf{\omega }}}_i\Delta t\right)}^T\Delta t& 0& 0\\ 0& \mathbf{I}& \mathbf{I}\Delta t& 0& 0& 0\\ -{}^G{\hat{\mathbf{R}}}_{I_i}{\lfloor {\hat{\mathbf{a}}}_i\rfloor }_{\wedge }\Delta t& 0& \mathbf{I}& 0& -{}^G{\hat{\mathbf{R}}}_{I_i}\Delta t& \mathbf{I}\Delta t\\ 0& 0& 0& \mathbf{I}& 0& 0\\ 0& 0& 0& 0& \mathbf{I}& 0\\ 0& 0& 0& 0& 0& \mathbf{I}\end{array}\right], \\ {\mathbf{F}}_{\mathbf{w}}=\left[\begin{array}{cccc}-\mathbf{A}{\left({\hat{\mathbf{\omega }}}_i\Delta t\right)}^T\Delta t& 0& 0& 0\\ 0& 0& 0& 0\\ 0& -{}^G{\hat{\mathbf{R}}}_{I_i}\Delta t& 0& 0\\ 0& 0& \mathbf{I}\Delta t& 0\\ 0& 0& 0& \mathbf{I}\Delta t\\ 0& 0& 0& 0\end{array}\right]\text{\hspace{1em}(7)} \end{gathered}$$

记白噪声 $\mathbf{w}$ 的协方差为 $\mathbf{Q}$ ，然后传播的协方差 ${\hat{\mathbf{P}}}_i$ 可以通过下面式子进行迭代计算：

$${\hat{\mathbf{P}}}_{i+1}={\mathbf{F}}_{\tilde{\mathbf{x}}}{\hat{\mathbf{P}}}_i{\mathbf{F}}_{\tilde{\mathbf{x}}}^T+{\mathbf{F}}_{\mathbf{w}}\mathbf{Q}{\mathbf{F}}_{\mathbf{w}}^T;{\hat{\mathbf{P}}}_0={\overline{\mathbf{P}}}_{k-1}\text{\hspace{1em}(8)}$$

前向传播一直持续到一个新扫描帧的结束时间 $t_k$ ，此时所传播的状态和协方差分别表示为 ${\hat{\mathbf{x}}}_k,{\hat{\mathbf{P}}}_k$。然后，${\hat{\mathbf{P}}}_k$ 表示系统状态的真值 ${\mathbf{x}}_k$ 和传播值 ${\hat{\mathbf{x}}}_k$ 之间的误差 ${\tilde{\mathbf{x}}}_k$ （ ${\tilde{\mathbf{x}}}_k={\mathbf{x}}_k\boxminus {\hat{\mathbf{x}}}_k$）的协方差。

2）反向传播和运动补偿：

当点的积累时间间隔到达 $t_k$ ，应将新扫描的特征点与传播的状态 ${\hat{\mathbf{x}}}_k$ 和协方差 ${\hat{\mathbf{P}}}_k$ 融合，以形成最优的状态更新。但是，尽管新扫描是在时间 $t_k$ 时，但特征点在其各自的采样时间 ${\rho }_j\le t_k$ 时被测量（请参阅第III-B.4和图2(b)），导致在本体参考坐标系中的不匹配。

为了补偿时间 ${\rho }_j$ 和时间 $t_k$ 之间的相对运动（即运动畸变），我们对公式（2）进行反向传播 ${\check{\mathbf{x}}}_{j-1}={\check{\mathbf{x}}}_j⊞\left(-\Delta t\mathbf{f}\left({\check{\mathbf{x}}}_j,{\mathbf{u}}_j,0\right)\right)$，我们以 ${\hat{\mathbf{x}}}_k$ 为零时刻的位姿和静止状态（比如速度和偏置）出发进行反向传播。反向传播以特征点的频率进行，通常比IMU频率高得多。对于在两个IMU测量之间采样的所有特征点，我们将左侧IMU测量用于反向传播中的输入。此外，注意到公式（3）中 $\mathbf{f}\left({\mathbf{x}}_j,{\mathbf{u}}_j,0\right)$ 的最后三个块元素（分别对应于陀螺仪偏置，加速度计偏置和外参）为零，可以将反向传播简化为：

$$\begin{array}{rl}& {}^{I_k}{\check{\mathbf{p}}}_{I_{j-1}}={}^{I_k}{\check{\mathbf{p}}}_{I_j}-{}^{I_k}{\check{\mathbf{v}}}_{I_j}\Delta t,\text{ s.f. }{}^{I_k}{\check{\mathbf{p}}}_{I_m}=0\text{; }\\ & {}^{I_k}{\check{\mathbf{v}}}_{I_{j-1}}={}^{I_k}{\check{\mathbf{v}}}_{I_j}-{}^{I_k}{\check{\mathbf{R}}}_{I_j}\left({\mathbf{a}}_{m_{i-1}}-{\hat{\mathbf{b}}}_{{\mathbf{a}}_k}\right)\Delta t-{}^{I_k}{\hat{\mathbf{g}}}_k\Delta t\text{, }\\ & \text{ s.f. }{}^{I_k}{\check{\mathbf{v}}}_{I_m}={}^G{\hat{\mathbf{R}}}_{I_k}^T{}^G{\hat{\mathbf{v}}}_{I_k},{}^{I_k}{\hat{\mathbf{g}}}_k={}^G{\hat{\mathbf{R}}}_{I_k}^T{}^G{\hat{\mathbf{g}}}_k\text{; }\\ & {}^{I_k}{\check{\mathbf{R}}}_{I_{j-1}}={}^{I_k}{\check{\mathbf{R}}}_{I_j}\mathrm{Exp}\left(\left({\hat{\mathbf{b}}}_{{\mathbf{\omega }}_k}-{\mathbf{\omega }}_{m_{i-1}}\right)\Delta t\right)\text{, s.f. }{}^{I_k}{\mathbf{R}}_{I_m}=\mathbf{I}\text{. }\end{array}\text{\hspace{1em}(9)}$$

其中，${\rho }_{j-1}\in \left[{\tau }_{i-1},{\tau }_i\right),\Delta t={\rho }_j-{\rho }_{j-1}$，$s.f$ 表示从 … 开始。

反向传播将在时间 ${\rho }_j$ 和一次扫描的结束时间 $t_k$ 之间产生相对位姿 ${}^{I_k}{\check{\mathbf{T}}}_{I_j}=\left({}^{I_k}{\check{\mathbf{R}}}_{I_j},{}^{I_k}{\check{\mathbf{p}}}_{I_j}\right)$。该相对位姿使我们能够将局部测量 ${}^{L_j}{\mathbf{p}}_{f_j}$ 投影到扫描结束时刻的测量 ${}^{L_k}{\mathbf{p}}_{f_j}$ 上（见图2），即：

$${}^{L_k}{\mathbf{p}}_{f_j}={}^I{\mathbf{T}}_L^{-1I_k}{\check{\mathbf{T}}}_{I_j}{}^I{\mathbf{T}}_L{}^{L_j}{\mathbf{p}}_{f_j}\text{\hspace{1em}(10)}$$

其中，${}^I{\mathbf{T}}_L$ 是激光雷达和IMU之间的已知的外参。然后，投影点 ${}^{L_k}{\mathbf{p}}_{f_j}$ 用于在下面的部分中构建残差。

3）残差计算：

通过公式（10）中的运动补偿，我们可以在同一时刻 $t_k$ 查看一次扫描中的所有特征点 $\left\{{}^{L_k}{\mathbf{p}}_{f_j}\right\}$ ，并使用它来构造残差。假设迭代卡尔曼滤波器的当前迭代为 $\kappa$，相应的状态估计为 ${\hat{\mathbf{x}}}_k^{\kappa }$。当 $\kappa =0$ ，${\hat{\mathbf{x}}}_k^{\kappa }={\hat{\mathbf{x}}}_k$ 时，此时状态为公式（4）中传播完成的预测状态。然后，特征点 $\left\{{}^{L_k}{\mathbf{p}}_{f_j}\right\}$ 可以变换到世界坐标系下，如下所示：

$${}^G{\hat{\mathbf{p}}}_{f_j}^{\kappa }={}^G{\hat{\mathbf{T}}}_{I_k}^{\kappa }{}^I{\mathbf{T}}_L{}^{L_k}{\mathbf{p}}_{f_j};j=1,\cdots ,m\text{\hspace{1em}(11)}$$

对于每一个激光雷达特征点，找到地图中该点附近的特征点所定义的最近的平面或边缘，并假设所找到的平面或者是边缘为该点真正所属的位置。也就是说，残差定义为特征点在世界坐标系中被估计的坐标 ${}^G{\hat{\mathbf{p}}}_{f_j}^{\kappa }$ 与地图中最近的平面（或边缘）之间的距离。记 ${\mathbf{u}}_j$ 为相应平面（或边缘）的法向量（或边缘朝向），其中 ${}^G{\mathbf{q}}_j$ 为相应平面（或边缘）伤的一个点，则残差 ${\mathbf{z}}_j^{\kappa }$ 可以通过以下公式计算：

$${\mathbf{z}}_j^{\kappa }={\mathbf{G}}_j\left({}^G{\hat{\mathbf{p}}}_{f_j}^{\kappa }-{}^G{\mathbf{q}}_j\right)\text{\hspace{1em}(12)}$$

其中，如果 ${\mathbf{G}}_j={\mathbf{u}}_j^T$ 则表示平面特征，${\mathbf{G}}_j={\lfloor {\mathbf{u}}_j\rfloor }_{\wedge }$ 则表示边缘特征。${\mathbf{u}}_j$ 的计算和地图中附近点的查找（这些点用于定义相应的平面或边缘）是通过在当前最新的地图中构建点的Kd-Tree来实现的[10]。此外，我们仅考虑范数低于一定阈值（例如0.5m）的残差。超过此阈值的残差对应的点被认为是离群点或者是新观察到的点。

4）迭代状态更新：

为了将由公式（12）中计算出来的残差 ${\mathbf{z}}_j^{\kappa }$ 与从IMU数据中传播得到的状态预测 ${\hat{\mathbf{x}}}_k$ 和协方差 ${\hat{\mathbf{P}}}_k$ 进行融合，我们需要对将残差 ${\mathbf{z}}_j^{\kappa }$ 与状态的真值 ${\mathbf{x}}_k$ 联系来的测量模型进行线性化，以及对测量噪声进行线性化。测量噪声来源于激光雷达测量点 ${}^{L_j}{\mathbf{p}}_{f_j}$ 时的探测和光束方向噪声 ${}^{L_j}{\mathbf{n}}_{f_j}$。由点的测量值 ${}^{L_j}{\mathbf{p}}_{f_j}$ 减去噪声可以得到点的位置的真值：

$${}^{L_j}{\mathbf{p}}_{f_j}^{\mathrm{gt}}={}^{L_j}{\mathbf{p}}_{f_j}-{}^{L_j}{\mathbf{n}}_{f_j}\text{\hspace{1em}(13)}$$

真值点通过公式（10）投影到坐标系 $L_k$ 之后，就可以通过状态真值 ${\mathbf{x}}_k$（比如位姿）投影到世界坐标系下。投影之后的真值点应完全位于地图中的平面（或边缘）上。也就是说，将公式（13）代入到公式（10）中，然后再代入公式（11）中，然后再进一步进入（12）中，应该为零。也就是：

$$0={\mathbf{h}}_j\left({\mathbf{x}}_k,{}^{L_j}{\mathbf{n}}_{f_j}\right)={\mathbf{G}}_j\left({}^G{\mathbf{T}}_{I_k}{}^{I_k}{\check{\mathbf{T}}}_{I_j}{}^I{\mathbf{T}}_L\left({}^{L_j}{\mathbf{p}}_{f_j}-{}^{L_j}{\mathbf{n}}_{f_j}\right)-{}^G{\mathbf{q}}_j\right)$$

通过在 ${\hat{\mathbf{x}}}_k^{\kappa }$ 处上述方程式进行一阶近似有：

$$\begin{array}{rl}0& ={\mathbf{h}}_j\left({\mathbf{x}}_k,{}^{L_j}{\mathbf{n}}_{f_j}\right)\simeq {\mathbf{h}}_j\left({\hat{\mathbf{x}}}_k^{\kappa },0\right)+{\mathbf{H}}_j^{\kappa }{\tilde{\mathbf{x}}}_k^{\kappa }+{\mathbf{v}}_j\\ & ={\mathbf{z}}_j^{\kappa }+{\mathbf{H}}_j^{\kappa }{\tilde{\mathbf{x}}}_k^{\kappa }+{\mathbf{v}}_j\end{array}\text{\hspace{1em}(14)}$$

其中 ${\tilde{\mathbf{x}}}_k^{\kappa }={\mathbf{x}}_k\boxminus {\hat{\mathbf{x}}}_k^{\kappa }$ （或者 ${\mathbf{x}}_k={\hat{\mathbf{x}}}_k^{\kappa }⊞{\tilde{\mathbf{x}}}_k^{\kappa }$），${\mathbf{H}}_j^{\kappa }$ 是 ${\mathbf{h}}_j\left({\hat{\mathbf{x}}}_k^{\kappa }⊞{\tilde{\mathbf{x}}}_k^{\kappa },{}^{L_j}{\mathbf{n}}_{f_j}\right)$ 关于 ${\tilde{\mathbf{x}}}_k^{\kappa }$ 在值为0时的雅可比矩阵，${\mathbf{v}}_j\in \mathcal{N}\left(0,{\mathbf{R}}_j\right)$ 来自于原始测量噪声 ${}^{L_j}{\mathbf{n}}_{f_j}$ 。

注意到在第 III-C.1节中通过前向传播获得的 ${\mathbf{x}}_k$ 的先验分布用于下式：

$${\mathbf{x}}_k\boxminus {\hat{\mathbf{x}}}_k=\left({\hat{\mathbf{x}}}_k^{\kappa }⊞{\tilde{\mathbf{x}}}_k^{\kappa }\right)\boxminus {\hat{\mathbf{x}}}_k={\hat{\mathbf{x}}}_k^{\kappa }\boxminus {\hat{\mathbf{x}}}_k+{\mathbf{J}}^{\kappa }{\tilde{\mathbf{x}}}_k^{\kappa }\text{\hspace{1em}(15)}$$

其中 ${\mathbf{J}}^{\kappa }$ 是 $\left({\hat{\mathbf{x}}}_k^{\kappa }⊞{\tilde{\mathbf{x}}}_k^{\kappa }\right)\boxminus {\hat{\mathbf{x}}}_k$ 关于 ${\tilde{\mathbf{x}}}_k^{\kappa }$ 在值为0的偏导数：

$${\mathbf{J}}^{\kappa }=\left[\begin{array}{cc}\mathbf{A}{\left({}^G{\hat{\mathbf{R}}}_{I_k}^{\kappa }{\boxminus }^G{\hat{\mathbf{R}}}_{I_k}\right)}^{-T}& 0_{3\times 15}\\ 0_{15\times 3}& {\mathbf{I}}_{15\times 15}\end{array}\right]\text{\hspace{1em}(16)}$$

其中 $\mathbf{A}(\cdot {)}^{-1}$ 在公式（6）中定义。对于第一次迭代（即扩展卡尔曼滤波器的情况），${\hat{\mathbf{x}}}_k^{\kappa }={\hat{\mathbf{x}}}_k$，并且 ${\mathbf{J}}^{\kappa }=\mathbf{I}$ 。

将公式（15）中的先验与公式（14）中的后验分布相结合，可以得到最大后验估计（MAP）：

$$\underset{{\tilde{\mathbf{x}}}_k^{\kappa }}{\min }\left({\Vert {\mathbf{x}}_k\boxminus {\hat{\mathbf{x}}}_k\Vert }_{{\hat{\mathbf{P}}}_k^{-1}}^2+\sum _{j=1}^m{\Vert {\mathbf{z}}_j^{\kappa }+{\mathbf{H}}_j^{\kappa }{\tilde{\mathbf{x}}}_k^{\kappa }\Vert }_{{\mathbf{R}}_j^{-1}}^2\right)\text{\hspace{1em}(17)}$$

其中，$\Vert \mathbf{x}{\Vert }_{\mathbf{M}}^2={\mathbf{x}}^T\mathbf{Mx}$。

将公式（15）中先验的线性化代入到公式（17）并优化得到的二次代价，可以得到标准的迭代卡尔曼滤波[21]，可以通过下式计算（为了简化表示，令 $\mathbf{H}={\left[{\mathbf{H}}_1^{{\kappa }^T},\cdots ,{\mathbf{H}}_m^{{\kappa }^T}\right]}^T$，$\mathbf{R}=\mathrm{diag}\left({\mathbf{R}}_1,\cdots {\mathbf{R}}_m\right)$，$\mathbf{P}={\left({\mathbf{J}}^{\kappa }\right)}^{-1}{\hat{\mathbf{P}}}_k{\left({\mathbf{J}}^{\kappa }\right)}^{-T}$，${\mathbf{z}}_k^{\kappa }={\left[{\mathbf{z}}_1^{{\kappa }^T},\cdots ,{\mathbf{z}}_m^{{\kappa }^T}\right]}^T$）：

$$\begin{array}{rl}\mathbf{K}& =\mathbf{P}{\mathbf{H}}^T{\left(\mathbf{HP}{\mathbf{H}}^T+\mathbf{R}\right)}^{-1}\\ {\hat{\mathbf{x}}}_k^{\kappa +1}& ={\hat{\mathbf{x}}}_k^{\kappa }⊞\left(-\mathbf{K}{\mathbf{z}}_k^{\kappa }-(\mathbf{I}-\mathbf{KH}){\left({\mathbf{J}}^{\kappa }\right)}^{-1}\left({\hat{\mathbf{x}}}_k^{\kappa }\boxminus {\hat{\mathbf{x}}}_k\right)\right)\end{array}\text{\hspace{1em}(18)}$$

然后使用更新后的估计值 ${\hat{\mathbf{x}}}_k^{\kappa +1}$ 计算 III-C.3 中的残差，重复该过程直至收敛（$\Vert {\hat{\mathbf{x}}}_k^{\kappa +1}\boxminus {\hat{\mathbf{x}}}_k^{\kappa }\Vert <ϵ$）。收敛后，最优状态估计和协方差为：

$${\overline{\mathbf{x}}}_k={\hat{\mathbf{x}}}_k^{\kappa +1},{\overline{\mathbf{P}}}_k=(\mathbf{I}-\mathbf{KH})\mathbf{P}\text{\hspace{1em}(19)}$$

公式（18）中常用的卡尔曼增益形式的一个问题是，它需要对矩阵 $\mathbf{HP}{\mathbf{H}}^T+\mathbf{R}$ 求逆，但是该矩阵的维度是测量数的维度。在实际应用中，激光雷达特征点的数量非常庞大，对维度如此大的矩阵求逆很难实现。因此，现有的工作[21,26]只使用少量的测量。在本文中，我们表明，这种限制是可以避免的。直觉来源于公式（17） ，其中代价函数建立在状态之上，因此解决方案应该根据状态维数的复杂度进行计算。事实上，如果直接对公示（17）进行求解，我们可以得到公式（18）中的相同解，但是可以使用一种新的形式的卡尔曼增益：

$$\mathbf{K}={\left({\mathbf{H}}^T{\mathbf{R}}^{-1}\mathbf{H}+{\mathbf{P}}^{-1}\right)}^{-1}{\mathbf{H}}^T{\mathbf{R}}^{-1}\text{\hspace{1em}(20)}$$

我们在附录B中证明，基于矩阵逆引理（matrix inverse lemma）[27]的两种形式的卡尔曼增益等效。由于激光雷达测量值是独立的，因此协方差矩阵为（块）对角，因此新公式仅需要在状态维度而不是测量维度的情况下对两个矩阵求逆。新公式大大节约了计算量，因为状态维度通常比LIO中的测量数要低得多（例如，在10 Hz的扫描频率中，一次扫描包含超过1000个有效特征点，而状态维度仅为18）。

5）算法：

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

D 地图更新

使用状态更新 ${\overline{\mathbf{x}}}_k$ 将每一个特征点 $\left({}^{L_k}{\mathbf{p}}_{f_j}\right)$ 先投影到体坐标系中（如公式（10）），然后再投影到世界坐标系中：

$${}^G{\overline{\mathbf{p}}}_{f_j}={}^G{\overline{\mathbf{T}}}_{I_k}{}^I{\mathbf{T}}_L{}^{L_k}{\mathbf{p}}_{f_j};j=1,\cdots ,m\text{\hspace{1em}(21)}$$

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

E 初始化

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