FAST-LIO论文阅读

论文：《FAST-LIO: Fast LiDAR-Inertial Odometry with Robust Initialization》

I. 介绍

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

固态激光雷达最近成为激光雷达发展的主要趋势，例如基于微机电系统（MEMS）扫描[4]和旋转棱镜[5]的激光雷达。这些激光雷达非常具有成本效益（成本范围类似于全局快门相机），重量轻（可以由小型无人机携带），且性能高（能够主动和直接地进行远距离和高精度的3D测量）。这些特性使得这种激光雷达对于无人机，特别是工业无人机来说是可行的，这些无人机需要获取环境的精确3D地图（例如，航空测绘）或可能在杂乱环境中操作，光照变化剧烈（例如，灾后搜索和检查）。

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

1. 激光雷达测量中的特征点通常是环境中的几何结构（例如，边缘和平面）。当无人机在没有明显特征的杂乱环境中操作时，基于激光雷达的解决方案容易退化。当激光雷达的视场（FoV）较小时，这个问题更加明显。
2. 由于扫描方向上的高分辨率，激光雷达扫描通常包含许多特征点（例如，几千个）。虽然这些特征点在退化情况下不足以可靠地确定姿态，但将如此大量的特征点与IMU测量紧密融合需要巨大的计算资源，这是无人机机载计算机无法承受的。
3. 由于激光雷达使用少量激光/接收器对顺序采样点，扫描中的激光点总是在不同时间采样，导致运动失真，这将显著降低扫描配准的效果[6]。无人机螺旋桨和电机的持续旋转也会给IMU测量带来显著噪声。

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

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

剩余的论文组织如下：在第二部分，我们讨论相关的研究工作。在第三部分，我们概述了完整的系统流程并详细介绍了每个关键组件。实验在第四部分中展示，结论在第五部分中给出。

II. 相关工作

现有的关于激光雷达 SLAM的工作非常广泛。这里我们将评论限制在最相关的工作上：仅激光雷达的里程计和建图、松耦合和紧耦合的激光雷达-惯性融合方法。

A. 激光雷达里程计和建图

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

B. 松耦合激光雷达-惯性里程计

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

C. 紧耦合激光雷达-惯性里程计

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

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

III. 方法论

A. 框架概述

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

B. 系统描述

1) ⊞/⊟ 操作符

设\(M\)为考虑中的\(n\)维流形（例如，\(M = SO(3)\)）。由于流形在局部上同胚于\(\mathbb{R}^n\)，我们可以通过两个封装操作符\(\boxplus\)和\(\boxminus\)从\(M\)上的局部邻域到其切空间\(\mathbb{R}^n\)建立一个双射映射操作符\(\boxplus/\boxminus\)[23]：

\[ \boxplus:\mathcal{M}\times\mathbb{R}^n\to\mathcal{M};\qquad \boxminus:\mathcal{M}\times\mathcal{M}\to\mathbb{R}^n \\ \]

\[ \begin{gather} &\mathcal{M} = SO(3): &\mathbf{R} \boxplus \mathbf{r} = \mathbf{R}\text{Exp}(\mathbf{r}); &\quad \mathbf{R}_1 \boxminus \mathbf{R}_2 = \log(\mathbf{R}_2^T\mathbf{R}_1) \\ &\mathcal{M} = \mathbb{R}^n: &\mathbf{a} \boxplus \mathbf{b} = \mathbf{a} + \mathbf{b}; &\quad \mathbf{a} \boxminus \mathbf{b} = \mathbf{a} - \mathbf{b} \end{gather} \]

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

\[ \begin{bmatrix}\mathbf{R}\\ \mathbf{a}\end{bmatrix}\boxplus\begin{bmatrix}\mathbf{r}\\ \mathbf{b}\end{bmatrix}=\begin{bmatrix}\mathbf{R}\boxplus\mathbf{r}\\ \mathbf{a}+\mathbf{b}\end{bmatrix};\quad\begin{bmatrix}\mathbf{R}_{1}\\ \mathbf{a}\end{bmatrix}\boxminus\begin{bmatrix}\mathbf{R}_{2}\\ \mathbf{b}\end{bmatrix}=\begin{bmatrix}\mathbf{R}_{1}\boxminus\mathbf{R}_{2}\\ \mathbf{a}-\mathbf{b}\end{bmatrix} \]

从上面定义，我们可以确认

\[ (\mathbf{x}\boxplus \mathbf{u})\boxminus \mathbf{x} = \mathbf{u};\quad(\mathbf{x}\boxplus \mathbf{y})\boxminus \mathbf{u} = \mathbf{y}; \quad ∀\mathbf{x},\mathbf{y}\in\mathcal{M}, ∀\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{aligned}{}^{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}}=\mathbf{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{b}\mathbf{a}}}\end{aligned} \tag{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}_{\mathbf{\omega}}\)是IMU测量的白噪声，\(\mathbf{b}_{\mathbf{a}}\)和\(\mathbf{b}_{\mathbf{\omega}}\)是IMU的偏置，建模为具有高斯噪声的随机游走过程，\(\mathbf{n}_{\mathbf{b}\mathbf{a}}\)和\(\mathbf{n}_{\mathbf{b}\mathbf{\omega}}\)是偏置的噪声，符号\(\lfloor\mathbf{a}\rfloor_{\wedge}\)表示向量\(\mathbf{a} \in \mathbb{R}^3\)的反对称矩阵，用于映射叉乘操作。

3) 离散模型

基于上述定义的⊞操作，我们可以在IMU采样周期\(\Delta t\)内使用零阶保持器对连续模型(1)进行离散化。得到的离散模型为：

\[ \mathbf{x}_{i+1}=\mathbf{x}_i \boxplus (\Delta t \mathbf{f}(\mathbf{x}_i, \mathbf{u}_i, \mathbf{w}_i)) \tag{2} \]

其中\(i\)为IMU测量值的索引，函数\(\mathbf{f}\)，状态\(\mathbf{x}\)，控制输入\(\mathbf{u}\)和过程噪声\(\mathbf{w}\)定义如下：

\[ \mathcal{M}=SO(3)\times\mathbb{R}^{15}, \dim(\mathcal{M})=18 \]

\[ \begin{gathered} \mathbf{x}\doteq\begin{bmatrix}{}^{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{bmatrix}^{T}\in\mathcal{M} \\ \mathbf{u}\doteq\begin{bmatrix}\mathbf{\omega}_m^T&\mathbf{a}_m^T\end{bmatrix}^T, \mathbf{w}\doteq\begin{bmatrix}\mathbf{n}_\omega^T&\mathbf{n}_\mathrm{a}^T&\mathbf{n}_{\mathbf{b}\mathbf{\omega}}^T&\mathbf{n}_{\mathbf{b}\mathbf{a}}^T\end{bmatrix}^T \\ \mathbf{f}\left(\mathbf{x}_{i},\mathbf{u}_{i},\mathbf{w}_{i}\right)=\begin{bmatrix}\mathbf{\omega}_{m_{i}}-\mathbf{b}_{\mathbf{\omega}_{i}}-\mathbf{n}_{\mathbf{\omega}_{i}}\\G_{\mathbf{v}_{I_{i}}}\\\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{\omega}_{i}}\\\mathbf{n}_{\mathbf{b}\mathbf{a}_{i}}\\\mathbf{0}_{3\times1}\end{bmatrix} \end{gathered} \tag{3} \]

4) 激光雷达测量预处理

激光雷达测量是在其局部机体框架中的点坐标。由于原始激光雷达点以非常高的速率采样（例如，200kHz），通常不可能在接收到每个新点时立即处理。更实际的方法是将这些点累积一段时间并一次性处理。在FAST-LIO中，最小累积间隔设置为20毫秒，导致高达50Hz的全状态估计（即里程计输出）和地图更新，如图2(a)所示。这样累积的一组点称为扫描，处理它的时间记为\(t_k\)（见图2(b)）。从原始点中，我们提取具有高局部平滑度的平面点[8]和具有低局部平滑度的边缘点[10]。假设特征点的数量为\(m\)，每个点在时间\(\rho_j \in (t_{k-1}, t_k]\)采样，并记为\(L_j\mathbf{p}_{f_j}\)，其中\(L_j\)是时间\(\rho_j\)时的激光雷达局部框架。在一次激光雷达扫描期间，还有多个IMU测量，每个在时间\(\tau_i \in [t_{k-1}, t_k]\)采样，并具有相应的状态\(\mathbf{x}_i\)，如公式(2)所示。注意，最后一个激光雷达特征点是扫描的结束，即\(\rho_m = t_k\)，而IMU测量不一定与扫描的开始或结束对齐。

C. 状态估计

为了估计状态公式(2)中的状态，我们使用迭代扩展卡尔曼滤波器。此外，我们在状态估计的切空间中表征估计协方差，如[23, 24]所述。假设在\(t_{k-1}\)时刻的最后一次激光雷达扫描的最优状态估计为\(\bar{\mathbf{x}}_{k-1}\)，其协方差矩阵为\(\bar{\mathbf{P}}_{k-1}\)。那么\(\bar{\mathbf{P}}_{k-1}\)表示下面定义的随机误差状态向量的协方差：

\[ \widetilde{\mathbf{x}}_{k-1}\dot{=}\mathbf{x}_{k-1}\boxminus\bar{\mathbf{x}}_{k-1}=\begin{bmatrix}\delta\boldsymbol{\theta}^T&^G\widetilde{\mathbf{p}}_I^T&^G\widetilde{\mathbf{v}}_I^T&\widetilde{\mathbf{b}}_\omega^T&\widetilde{\mathbf{b}}_\mathbf{a}^T&^G\widetilde{\mathbf{g}}^T\end{bmatrix}^T \]

其中，\(\delta\boldsymbol{\theta} = \text{Log}({}^{G}\bar{\mathbf{R}}_{I}^T {}^{G}\mathbf{R}_{I})\)是姿态误差，其余部分是标准的加性误差（即，量\(x\)的估计\(\bar{x}\)的误差为\(\tilde{x} = x - \bar{x}\)）。直观地说，姿态误差\(\delta\boldsymbol{\theta}\)描述了真实姿态和估计姿态之间的（小）偏差。这种误差定义的主要优点是它允许我们通过\(3 \times 3\)协方差矩阵\(\mathbb{E}\{\delta\boldsymbol{\theta}\delta\boldsymbol{\theta}^T\}\)表示姿态不确定性。由于姿态具有3个自由度（DOF），这是一个最小表示。

1) 前向传播

前向传播在接收到IMU输入后执行（见图2）。更具体地说，通过将过程噪声\(\mathbf{w}_i\)设为零，按照公式(2)传播状态：

\[ \widehat{\mathbf{x}}_{i+1}=\widehat{\mathbf{x}}_i\boxplus\left(\Delta t\mathbf{f}(\widehat{\mathbf{x}}_i,\mathbf{u}_i,\mathbf{0})\right); \widehat{\mathbf{x}}_0=\bar{\mathbf{x}}_{k-1}. \tag{4} \]

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

\[ \begin{aligned} \widetilde{\mathbf{x}}_{i+1}& \begin{aligned}&=\mathbf{x}_{i+1} \boxminus \widehat{\mathbf{x}}_{i+1}\end{aligned} \\ &\boldsymbol{=}(\mathbf{x}_i\boxplus\Delta t\mathbf{f}\left(\mathbf{x}_i,\mathbf{u}_i,\mathbf{w}_i\right))\boxminus(\widehat{\mathbf{x}}_i\boxplus\Delta t\mathbf{f}\left(\widehat{\mathbf{x}}_i,\mathbf{u}_i,\mathbf{0}\right)) \\ &\overset{(23)}{\operatorname*{\simeq}}\mathbf{F}_{\widetilde{\mathbf{x}}}\widetilde{\mathbf{x}}_i+\mathbf{F}_{\mathbf{w}}\mathbf{w}_i. \end{aligned} \tag{5} \]

矩阵\(\mathbf{F}_{\widetilde{\mathbf{x}}}\)和\(\mathbf{F}_{\mathbf{w}}\)在公式(5)中按照附录A计算。结果如公式(7)所示，其中\(\hat{\boldsymbol{\omega}}_i = \boldsymbol{\omega}_{m_i} - \hat{\mathbf{b}}_{\boldsymbol{\omega}_i}\)，\(\hat{\mathbf{a}}_i = \mathbf{a}_{m_i} - \hat{\mathbf{b}}_{\mathbf{a}_i}\)，\(\mathbf{A}(\mathbf{u})^{-1}\)的定义与[25]中的定义相同，如下所示：

\[ \begin{aligned}\mathbf{A}\left(\mathbf{u}\right)^{-1}&=\mathbf{I}-\frac{1}{2}\lfloor\mathbf{u}\rfloor_{\wedge}+\left(1-\boldsymbol{\alpha}\left(\lVert\mathbf{u}\rVert\right)\right)\frac{\lfloor\mathbf{u}\rfloor_{\wedge}^{2}}{\lVert\mathbf{u}\rVert^{2}}\\\alpha\left(\mathrm{m}\right)&=\frac{\mathrm{m}}{2}\mathrm{cot}\left(\frac{\mathrm{m}}{2}\right)=\frac{\mathrm{m}}{2}\frac{\cos(\mathrm{m}/2)}{\sin(\mathrm{m}/2)}\end{aligned} \tag{6} \]

\[ \begin{gather} \mathrm{F}_{\tilde{\mathbf{x}}}=\begin{bmatrix}\mathrm{Exp}\left(-\widehat{\boldsymbol{\omega}}_{i}\Delta t\right)&\mathbf{0}&\mathbf{0}&-\mathbf{A}(\widehat{\boldsymbol{\omega}}_{i}\Delta t)^{T}\Delta t&\mathbf{0}&\mathbf{0}\\\mathbf{0}&\mathbf{I}&\mathbf{I}\Delta t&\mathbf{0}&\mathbf{0}&\mathbf{0}\\-^{G}\widehat{\mathbf{R}}_{{I_{i}}}\left\lfloor\widehat{\mathbf{a}}_{i}\right\rfloor_{\wedge}\Delta t&\mathbf{0}&\mathbf{I}&\mathbf{0}&-^{G}\widehat{\mathbf{R}}_{{I_{i}}}\Delta t&\mathbf{I}\Delta t\\\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{I}&\mathbf{0}&\mathbf{0}\\\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{I}\end{bmatrix},\\ \quad\mathbf{F}_{\mathbf{w}}=\begin{bmatrix}-\mathbf{A}\left(\widehat{\boldsymbol{\omega}}_{i}\Delta t\right)^{T}\Delta t&\mathbf{0}&\mathbf{0}&\mathbf{0}\\\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}\\\mathbf{0}&-^{G}\widehat{\mathbf{R}}_{{I_{i}}}\Delta t&\mathbf{0}&\mathbf{0}\\\mathbf{0}&\mathbf{0}&\mathbf{I}\Delta t&\mathbf{0}\\\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{I}\Delta t\\\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}\end{bmatrix} \end{gather}\tag{7} \]

将白噪声\(\mathbf{w}\)的协方差记为\(\mathbf{Q}\)，则传播后的协方差\(\hat{\mathbf{P}}_i\)可以按照以下公式迭代计算：

\[ \widehat{\mathbf{P}}_{i+1}=\mathbf{F}_{\widetilde{\mathbf{x}}}\widehat{\mathbf{P}}_i\mathbf{F}_{\widetilde{\mathbf{x}}}^T+\mathbf{F}_{\mathbf{w}}\mathbf{Q}\mathbf{F}_{\mathbf{w}}^T; \widehat{\mathbf{P}}_0=\bar{\mathbf{P}}_{k-1}. \tag{8} \]

传播过程持续到达新扫描的结束时间\(t_k\)，此时传播后的状态和协方差分别记为\(\hat{\mathbf{x}}_k\)和\(\hat{\mathbf{P}}_k\)。然后，\(\hat{\mathbf{P}}_k\)表示真实状态\(\mathbf{x}_k\)和状态传播\(\hat{\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 \leq t_k\)测量的（见第III-B.4节和图2(b)），这导致在机体参考框架中的不匹配。

为了补偿时间\(\rho_j\)和时间\(t_k\)之间的相对运动（即运动失真），我们从零姿态和\(\hat{\mathbf{x}}_k\)的其余状态（例如速度和偏置）开始，按照公式(2)进行反向传播：\(\check{\mathbf{x}}_{j-1} = \check{\mathbf{x}}_j \boxplus (-\Delta t \mathbf{f}(\check{\mathbf{x}}_j, \mathbf{u}_j, 0))\)，反向传播以特征点的频率进行，这通常比IMU的速率高得多。对于两个IMU测量之间采样的所有特征点，我们使用左侧的IMU测量作为反向传播的输入。此外，注意到\(\mathbf{f}(\mathbf{x}_j, \mathbf{u}_j, 0)\)的最后三个块元素（对应于陀螺仪偏置、加速度计偏置和外参）为零，反向传播可以简化为：

\[ \begin{aligned} ^{I_k}\check{\mathbf{p}}_{I_{j-1}}&={}^{I_k}\check{\mathbf{p}}_{I_j}-{}^{I_k}\check{\mathbf{v}}_{I_j}\Delta t,\quad s.f.{}^{I_k}\check{\mathbf{p}}_{I_m}=\mathbf{0};\\^{I_k}\check{\mathbf{v}}_{I_{j-1}}&={}^{I_k}\check{\mathbf{v}}_{I_j}-{}^{I_k}\check{\mathbf{R}}_{I_j}(\mathbf{a}_{m_{i-1}}-\widehat{\mathbf{b}}_{\mathbf{a}_k})\Delta t-{}^{I_k}\widehat{\mathbf{g}}_k\Delta t,\\s.f.{}^{I_k}\check{\mathbf{v}}_{I_m}&={}^G\widehat{\mathbf{R}}_{I_k}^T{}^G\widehat{\mathbf{v}}_{I_k},{}^{I_k}\widehat{\mathbf{g}}_k={}^G\widehat{\mathbf{R}}_{I_k}^T{}^G\widehat{\mathbf{g}}_k;\\^{I_k}\check{\mathbf{R}}_{I_{j-1}}&={}^{I_k}\check{\mathbf{R}}_{I_j}\operatorname{Exp}((\widehat{\mathbf{b}}_{\boldsymbol{\omega}_k}-\boldsymbol{\omega}_{m_{i-1}})\Delta t), s.f. {}^{I_k}\mathbf{R}_{I_m}=\mathbf{I}. \end{aligned} \]

其中 \(\rho_{j-1} \in [\tau_{i-1},\tau_i), \Delta t = \rho_j - \rho_{j-1}\) 和 \(s.f.\) 意思为“starting from”。

反向传播将产生时间\(\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^{-1} {}^{I_k}\check{\boldsymbol{T}}_{I_j}{}^I\mathbf{T}_L{}^{L_j}\mathbf{p}_{f_j}. \]