（LI论文）LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping

摘要

我们提出了一个通过平滑和建图实现紧耦合激光雷达惯性里程测量的框架LIO-SAM，该框架可实现高精度、实时的移动机器人轨迹估计和地图构建。LIO-SAM在因子图上制定激光雷达惯性里程计，允许从不同来源将大量相对和绝对测量（包括回路闭合）作为因子纳入系统。来自惯性测量单元（IMU）预积分的估计运动消除了点云的畸变，并产生了激光雷达里程计优化的初始猜测值。获得的激光雷达里程计用于估计IMU零偏。为了确保高性能的实时性，我们将旧的激光雷达扫描边缘化以优化姿态，而不是将激光雷达扫描与全局地图匹配。局部尺度而非全局尺度的扫描匹配显著提高了系统的实时性能，关键帧的选择性引入以及将一个新关键帧添加到一组固定大小的先前“子关键帧”的高效滑动窗口方法同样提高实时性能。在不同规模和环境下从三个平台收集的数据集上对所提出的方法进行了广泛的评估。

引言

状态估计、定位和建图是成功的智能移动机器人的基本先决条件，需要反馈控制、避障和规划等许多功能。利用基于视觉和基于激光雷达的传感技术，人们致力于实现高性能的实时同步定位和建图（SLAM），以支持移动机器人的六自由度状态估计。基于视觉的方法通常使用单目或立体相机，并在连续图像上对特征进行三角化，以确定相机的运动。尽管基于视觉的方法特别适合于位置识别，但它们对初始化、照明和范围的敏感性使得它们单独用于支持自主导航系统时不可靠。另一方面，基于激光雷达的方法在很大程度上对光照变化保持不变。特别是最近，随着远程、高分辨率3D激光雷达的出现，例如Velodyne VLS-128和Outster OS1-128，激光雷达变得更适合直接捕捉3D空间中环境的细节。因此，本文主要研究基于激光雷达的状态估计和建图方法。

在过去的二十年中，人们提出了许多基于激光雷达的状态估计和映射方法。其中，在[1]中提出的用于低漂移和实时状态估计和建图的激光雷达里程计和映射（LOAM）方法是应用最广泛的方法之一。LOAM使用激光雷达和惯性测量单元（IMU），具有最先进的性能，自其在KITTI里程计基准站点发布以来，一直被列为基于激光的最佳方法[2]。尽管取得了成功，但LOAM仍存在一些局限性——通过将其数据保存在全局体素图中，通常很难执行回环检测并结合其他绝对测量（例如GPS）进行姿势校正。当此体素地图在特征丰富的环境中变得稠密时，其在线优化过程的效率会降低。LOAM在大规模测试中也会出现漂移，因为它的核心是一种基于扫描匹配的方法。

在本文中，我们提出了一个通过平滑和建图的紧耦合激光雷达惯性里程计框架，LIO-SAM，来解决上述问题。我们假设点云畸变的非线性运动模型，使用原始IMU测量估计激光雷达扫描期间的传感器运动。除了消除点云的畸变，估计的运动也是激光雷达里程计优化的初始位姿。获得的激光雷达里程计解随后用于估计因子图中惯性测量单元的零偏。通过为机器人轨迹估计引入全局因子图，我们可以使用激光雷达和惯性测量单元测量有效地执行传感器融合，将位置识别纳入机器人姿态，并引入绝对测量，如GPS位置和罗盘航向(如果可用)。来自各种来源的因子的集合用于图的联合优化。此外，我们将旧的激光雷达扫描边缘化，以进行姿态优化，而不是像LOAM那样将扫描与全局地图进行匹配。在局部尺度而不是全局尺度上进行扫描匹配，可以显著提高系统的实时性能，正如有选择地引入关键帧和将新的关键帧注册到一组固定大小的先前“子关键帧”中的滑动窗口方法，可以我们工作的主要贡献可以总结如下:

• 建立在因子图上的紧耦合激光雷达惯性里程计框架，适用于多传感器融合和全局优化。
• 一种高效的、基于局部滑动窗口的扫描匹配方法，通过有选择性的选择的新关键帧与固定大小的一组先前子关键帧进行配准，实现实时性能。
• 提出的框架经过了各种规模、车辆和环境的数据集上的广泛验证。

相关工作

激光雷达里程计通常通过使用扫描匹配方法(如ICP [3]和GICP[4])找到两个连续帧之间的相对变换。基于特征的匹配方法由于不需要匹配完整的点云可以获得极高的计算效率已经成为了一种流行的扫描匹配方法。例如，在[5]中，提出了一种用于实时激光雷达里程计的基于平面的配准方法。假设在结构化环境中进行操作，它从点云提取平面，并通过求解最小二乘问题来匹配它们。文献[6]提出了一种基于collar line的里程计估算方法。在这种方法中，线段从原始点云随机生成，并在之后用于配准。然而，由于现代三维激光雷达的旋转机制和传感器的运动，扫描的点云经常是带畸变的。单独使用激光雷达进行位置估计的效果并不理想，因为使用带畸变点云或特征进行配准最终会导致较大的漂移。

因此，激光雷达通常与其他传感器结合使用，如惯性测量单元和GPS，用于状态估计和建图。这种用于传感器融合的设计方案通常可以分为两类:松耦合融合和紧耦合融合。在LOAM[1]中，引入了惯性测量单元来消除激光雷达扫描的畸变，并给出扫描匹配的运动先验。然而，惯性测量单元没有参与算法的优化过程。因此LOAM可以被归类为松耦合的方法。[7]中提出了一种轻量级、地面优化的激光雷达里程计和建图方法，用于地面车辆建图任务[8]。它以和LOAM相同的方法融合了惯性测量单元的测量结果。松耦合融合的一种更流行的方法是使用扩展卡尔曼滤波器(EKF)。例如，[9]-[13]在机器人状态估计的优化阶段，使用EKF集成激光雷达、惯性测量单元和可选的GPS的测量结果。

紧耦合的系统通常能提高精度，是目前正在进行的研究的方向[14]。在[15]中，预先积分的惯性测量单元的测量值被用于点云畸变去除。文献[16]介绍了一种机器人中心的激光雷达-惯性状态估计器，R-LINS。R-LINS使用误差状态卡尔曼滤波器以紧耦合的方式迭代地校正机器人的状态估计。由于缺乏其他可用于状态估计的传感器，它在长时间导航中发生漂移。文献[17]中介绍了一种紧耦合的雷达惯性里程计和建图框架LIOM。LIOM是LIO-mapping的缩写，它联合优化了激光雷达和IMU的测量结果，并在与LOAM进行比较时获得了相似或更好的精度。由于LIOM是为处理所有传感器测量而设计的，因此无法实现实时性能——在我们的测试中，它的运行速度大约为实时性能的0.6倍。

通过平滑和建图实现的雷达惯性里程计

A.系统概述

我们首先定义我们在整个论文中使用的坐标系和符号。我们将世界坐标系表示为$\mathbf{W}$，将机器人机体坐标系表示为$\mathbf{B}$。为了方便起见，我们还假设惯性测量单元坐标系与机器人机体坐标系是一致的。机器人状态$\mathbf{x}$可以写成:
$$\mathbf{x}={\left[{\mathbf{R}}^{\mathbf{T}},{\mathbf{p}}^{\mathbf{T}},{\mathbf{v}}^{\mathbf{T}},{\mathbf{b}}^{\mathbf{T}}\right]}^{\mathbf{T}}\text{\hspace{1em}(1)}$$
其中$\mathbf{R}\in SO(3)$为旋转矩阵，$\mathbf{p}\in {\mathbb{R}}^3$位置向量，$\mathbf{v}$是速度，$\mathbf{b}$是惯性测量单元的零偏。从$\mathbf{B}$到$\mathbf{W}$的变换矩阵$\mathbf{T}\in SE(3)$被表示为$\mathbf{T}=[\mathbf{R}\mid \mathbf{p}]$。
所提出系统的概述如图 1 所示。

图1:LIO-SAM的系统结构。该系统接收来自3D激光雷达、IMU和可选GPS的输入。引入四种类型的因子来构建因子图：（a）IMU预积分因子，（b）激光雷达里程计因子，（c）GPS因子和（d）回环因子。第三节讨论了这些因子的产生。

该系统从3D激光雷达、惯性测量单元和可选的GPS系统接收传感器数据。我们试图利用这些传感器的观测来估计机器人的状态及其轨迹。这个状态估计问题可以表述为最大后验概率(MAP)问题。我们使用因子图来建模这个问题，因为与贝叶斯网络相比，它更适合执行推理。在高斯噪声模型的假设下，我们问题的MAP推断相当于求解一个非线性最小二乘问题[18]。请注意，不失一般性，本文提出的系统也可以结合其他传感器的测量值，如高度计的高度值或罗盘的航向。
我们为因子图的构造引入了四种因子。这个代表机器人在特定时间的状态的变量被归为因子图的节点。这四种类型的因子是:

1. 惯性测量单元预积分因子
2. 激光雷达里程计因子
3. GPS因子
4. 闭环因子。

当机器人姿态的变化超过用户定义的阈值时，一个新的机器人状态节点被添加到因子图中。因子图在插入新节点时使用增量平滑和建图的贝叶斯树(iSAM2)进行优化[19]。产生这些因子的过程将在以下章节中描述。

B.IMU预整合因子

IMU的角速度和加速度测量值使用等式2和3定义：
$${\hat{\mathbf{\omega }}}_t={\mathbf{\omega }}_t+{\mathbf{b}}_t^{\mathbf{\omega }}+{\mathbf{n}}_t^{\mathbf{\omega }}\text{\hspace{1em}(2)}$$
$${\hat{\mathbf{a}}}_t={\mathbf{R}}_t^{\mathrm{B}\mathrm{W}}\left({\mathbf{a}}_t-\mathbf{g}\right)+{\mathbf{b}}_t^{\mathrm{a}}+{\mathbf{n}}_t^{\mathrm{a}}\text{\hspace{1em}(3)}$$
式中，${\hat{\mathbf{\omega }}}_t$和${\hat{\mathbf{a}}}_t$是时间$t$时$\mathbf{B}$中的原始IMU测量值。${\hat{\mathbf{\omega }}}_t$和${\hat{\mathbf{a}}}_t$受到缓慢变化的零偏${\mathbf{b}}_t$和白噪声${\mathbf{n}}_t$的影响。${\mathbf{R}}_t^{\mathrm{B}\mathrm{W}}$是从$\mathbf{W}$到$\mathbf{B}$的旋转矩阵。$\mathbf{g}$是$\mathbf{W}$中的恒定重力矢量。

我们现在可以使用IMU的测量值来推断机器人的运动。时间$t+∆t$时机器人的速度、位置和旋转的计算公式如下：
$$\begin{array}{rl}{\mathbf{v}}_{t+\Delta t}& ={\mathbf{v}}_t+\mathbf{g}\Delta t+{\mathbf{R}}_t\left({\hat{\mathbf{a}}}_t-{\mathbf{b}}_t^{\mathrm{a}}-{\mathbf{n}}_t^{\mathrm{a}}\right)\Delta t\\ {\mathbf{p}}_{t+\Delta t}& ={\mathbf{p}}_t+{\mathbf{v}}_t\Delta t+\frac{1}{2}\mathbf{g}\Delta t^2+\frac{1}{2}{\mathbf{R}}_t\left({\hat{\mathbf{a}}}_t-{\mathbf{b}}_t^{\mathrm{a}}-{\mathbf{n}}_t^{\mathrm{a}}\right)\Delta t^2\\ {\mathbf{R}}_{t+\Delta t}& ={\mathbf{R}}_t\exp \left(\left({\hat{\mathbf{\omega }}}_t-{\mathbf{b}}_t^{\omega }-{\mathbf{n}}_t^{\omega }\right)\Delta t\right)\end{array}\text{\hspace{1em}(4-6)}$$
式中，${\mathbf{R}}_t={\mathbf{R}}_t^{\mathrm{W}\mathrm{B}}={\mathbf{R}}_t^{\mathrm{B}\mathrm{W}}{}^{\top }$。这里我们假设在上述积分过程中，$\mathbf{B}$的角速度和加速度保持不变。

然后，我们应用文献[20]中提出的惯性测量单元预积分方法来获得两个时间步长之间的相对机体运动。预积分测量$\Delta {\mathbf{v}}_{ij},\Delta {\mathbf{p}}_{ij}$，和时间$i$和$j$之间的$\Delta {\mathbf{R}}_{ij}$ 可使用以下公式计算:
$$\begin{array}{rl}\Delta {\mathbf{v}}_{ij}& ={\mathbf{R}}_i^{\top }\left({\mathbf{v}}_j-{\mathbf{v}}_i-\mathbf{g}\Delta t_{ij}\right)\\ \Delta {\mathbf{p}}_{ij}& ={\mathbf{R}}_i^{\top }\left({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\Delta t_{ij}-\frac{1}{2}\mathbf{g}\Delta t_{ij}^2\right)\\ \Delta {\mathbf{R}}_{ij}& ={\mathbf{R}}_i^{\top }{\mathbf{R}}_j\end{array}\text{\hspace{1em}(7-9)}$$
由于篇幅限制，我们请读者参考[20]中的描述，以了解等式7-9的详细推导。除了效率之外，应用IMU预积分也自然地为因子图提供了一种类型的约束——IMU预积分因子。IMU偏差与图中的激光雷达里程计系数一起进行了联合优化。

C.激光雷达里程计因子

当新的激光雷达扫描到达时，我们首先执行特征提取。通过评估局部区域上的点的粗糙度来提取边缘和平面特征。粗糙度值较大的点被归类为边缘特征。类似地，平面特征式对应于粗糙度值较小的点。我们以${\mathrm{F}}_i^e$和${\mathrm{F}}_i^p$ 来表示分别在时刻$i$时从激光雷达扫描中提取的边缘和平面特征。在时刻$i$时提取的所有特征组成一个雷达帧 F i = { F i e ,   F i p } \mathbb{F}_{i}=\left\{\mathrm{F}_{i}^{e}, \mathrm{~F}_{i}^{p}\right\} Fi​={Fie​, Fip​}。请注意，激光雷达帧$\mathbb{F}$被表示在$\mathbf{B}$系中。如果使用距离图像，特征提取过程的更详细描述可以在[1]或[7]中找到。

使用每一个激光雷达帧来计算和给因子图添加因子在计算上是困难的，所以我们采用了关键帧的概念，这在视觉SLAM领域中被广泛使用。使用简单但有效的启发式方法，当机器人位姿相对于先前状态${\mathbf{x}}_i$的变化超过用户定义的阈值时，我们选择雷达帧${\mathbb{F}}_{i+1}$作为关键帧。新保存的关键帧${\mathbb{F}}_{i+1}$与因子图中的机器人新的状态节点${\mathbf{x}}_{i+1}$相关联。两个关键帧之间的其他激光雷达帧将被丢弃。以这种方式添加关键帧不仅实现了地图密度和内存消耗之间的平衡，而且有助于保持相对稀疏的因子图，这适用于实时的非线性优化。在我们的工作中，添加新关键帧的位置和旋转变化阈值被选择为$1m$和$10°$。

假设我们希望在因子图中添加一个新的状态节点${\mathbf{x}}_{i+1}$。与此状态相关联的激光雷达关键帧为${\mathbb{F}}_{i+1}$。以下步骤描述了激光雷达里程计因子的生成：

1. 用于体素地图的子关键帧:

   我们实现了一种滑动窗口方法来创建包含固定数量的最近激光雷达扫描的点云图。我们没有优化两次连续激光雷达扫描之间的变换，而是提取$n$个最近的关键帧，我们称之为子关键帧，来用于估计。该组子关键帧 { F i − n , … , F i } \left\{\mathbb{F}_{i-n}, \ldots, \mathbb{F}_{i}\right\} {Fi−n​,…,Fi​}与他们相对应的变换 { T i − n , … , T i } \left\{\mathbf{T}_{i-n}, \ldots, \mathbf{T}_{i}\right\} {Ti−n​,…,Ti​}转换到世界坐标系$\mathbf{W}$。转换后的子关键帧被合并到一个体素地图${\mathbf{M}}_i$中。由于我们在先前的特征提取步骤中提取了两种类型的特征，所以${\mathbf{M}}_i$由两个子体素地图组成，边缘特征体素地图${\mathbf{M}}_i^e$和平面特征体素地图${\mathbf{M}}_i^p$。激光雷达帧和对应的体素地图如下所示:
   M i = { M i e , M i p }  where  : M i e = ′ F i e ∪ ′ F i − 1 e ∪ … ∪ ′ F i − n e M i p = ′ F i p ∪ ′ F i − 1 p ∪ … ∪ F i − n p \begin{aligned} &\mathbf{M}_{i}=\left\{\mathbf{M}_{i}^{e}, \mathbf{M}_{i}^{p}\right\} \\ &\text { where }: \mathbf{M}_{i}^{e}={ }^{\prime} \mathrm{F}_{i}^{e} \cup^{\prime} \mathrm{F}_{i-1}^{e} \cup \ldots \cup^{\prime} \mathrm{F}_{i-n}^{e} \\ &\quad \mathbf{M}_{i}^{p}={ }^{\prime} \mathrm{F}_{i}^{p} \cup^{\prime} \mathrm{F}_{i-1}^{p} \cup \ldots \cup \mathrm{F}_{i-n}^{p} \end{aligned} ​Mi​={Mie​,Mip​} where :Mie​=′Fie​∪′Fi−1e​∪…∪′Fi−ne​Mip​=′Fip​∪′Fi−1p​∪…∪Fi−np​​
   ${}^{\prime }{\mathrm{F}}_i^e$和${}^{\prime }{\mathrm{F}}_i^p$是$\mathbf{W}$中的变换后的边缘和平面特征。然后对${\mathbf{M}}_i^e$和${\mathbf{M}}_i^p$进行下采样，以消除落在同一体素单元中的重复特征。本文选择$n$为25。${\mathbf{M}}_i^e$和${\mathbf{M}}_i^p$的下采样分辨率分别为$0.2m$和$0.4m$。

2. 扫描匹配：

   我们通过扫描匹配${\mathbf{M}}_i$来得到一个新获得的雷达帧${\mathbb{F}}_{i+1}$，也就是$\left\{{\mathrm{F}}_{i+1}^e,{\mathrm{F}}_{i+1}^p\right\}$得位姿。各种扫描匹配方法，如[3]和[4]，可用于此目的。在这里，我们选择使用[1]中提出的方法，因为它在各种挑战性环境中的计算效率和鲁棒性。

   我们首先将$\left\{{\mathrm{F}}_{i+1}^e,{\mathrm{F}}_{i+1}^p\right\}$从$\mathbf{B}$ 坐标系转换到$\mathbf{W}$坐标系，得到$\left\{{}^{\prime }{\mathrm{F}}_{i+1}^e,{}^e{\mathrm{F}}_{i+1}^p\right\}$。这个初始变换是通过使用来自IMU预测的机器人运动${\tilde{\mathbf{T}}}_{i+1}$获得的。为${}^{\prime }{F}_{i+1}^e$或${}^{\prime }{F}_{i+1}^p$中的每个特征可以在${\mathbf{M}}_i^e$和${\mathbf{M}}_i^p$中找到其边缘或平面对应关系。为了简洁起见，这里省略了了寻找这些对应关系的详细步骤，但在[1]中有详细描述。

3. 相对变换：特征与其边缘或平面面片对应关系之间的距离可以使用以下公式计算：
   $${\mathbf{d}}_{e_k}=\frac{\left|\left({\mathbf{p}}_{i+1,k}^e-{\mathbf{p}}_{i,u}^e\right)\times \left({\mathbf{p}}_{i+1,k}^e-{\mathbf{p}}_{i,v}^e\right)\right|}{\left|{\mathbf{p}}_{i,u}^e-{\mathbf{p}}_{i,v}^e\right|}\text{\hspace{1em}(10)}$$

   $${\mathrm{d}}_{p_k}=\frac{\left|\begin{array}{c}\left({\mathbf{p}}_{i+1,k}^p-{\mathbf{p}}_{i,u}^p\right)\\ \left({\mathbf{p}}_{i,u}^p-{\mathbf{p}}_{i,v}^p\right)\times \left({\mathbf{p}}_{i,u}^p-{\mathbf{p}}_{i,w}^p\right)\end{array}\right|}{\left|\left({\mathbf{p}}_{i,u}^p-{\mathbf{p}}_{i,v}^p\right)\times \left({\mathbf{p}}_{i,u}^p-{\mathbf{p}}_{i,w}^p\right)\right|}\text{\hspace{1em}(11)}$$

   其中$k,u,v$和$w$是它们对应集合中的特征索引，对于${ }^{\prime} \mathrm{F}{i+1}^{e} $的边特征$\mathbf{p}{i+1, k}^{e}$ ，${\mathbf{p}}_{i,u}^e$和${\mathbf{p}}_{i,v}^e$是构成${\mathbf{M}}_i^e$中对应边的点。对于${ }^{\prime} \mathrm{F}{i+1}^{p} $的平面特征$\mathbf{p}{i+1, k}^{p}$，$\mathbf{p}{i, u}^{p}, \mathbf{p}{i, v}^{p}$和$\mathbf{p}_{i, w}^{{p}$是在$\mathbf{M}_{i}}{p}$中形成相应的平面的点。然后使用高斯牛顿法通过最小化以下代价函数来求解最优变换:
   $$\underset{{\mathbf{T}}_{i+1}}{\min }\left\{\sum _{{\mathbf{p}}_{i+1,k}^e{\in }^{\prime }{\mathbf{F}}_{i+1}^e}{\mathbf{d}}_{e_k}+\sum _{{\mathbf{p}}_{i+1,k}^p{\in }^{\prime }{\mathbf{F}}_{i+1}^p}{\mathbf{d}}_{p_k}\right\}$$
   最后，我们可以得到 ${\mathbf{x}}_i$ 和 ${\mathbf{x}}_{i+1}$之间的相对变换$\Delta {\mathbf{T}}_{i,i+1}$ ，这就是这两种姿态的激光雷达里程因子：
   $$\Delta {\mathbf{T}}_{i,i+1}={\mathbf{T}}_i^{\top }{\mathbf{T}}_{i+1}\text{\hspace{1em}(12)}$$
   注意到，另一种获得$\Delta {\mathbf{T}}_{i,i+1}$ 的方法是将子关键帧转换到${\mathbf{x}}_i$的坐标系下。换句话说，我们匹配${\mathbb{F}}_{i+1}$和在${\mathbf{x}}_i$的坐标系下的体素地图就可以直接得到相对变换$\Delta {\mathbf{T}}_{i,i+1}$。因为变换后的特征${}^{\prime }{\mathrm{F}}_i^e$和${}^{\prime }{\mathrm{F}}_i^p$可以被多次重用，所以为了效率我们选择第III-C.1节中描述的方法。

D.GPS因子

尽管仅利用惯性测量单元预积分和激光雷达里程计因子就能获得可靠的状态估计和地图绘制，但在长时间导航任务中，系统仍存在漂移问题。为了解决这个问题，我们可以引入为消除漂移提供绝对测量的传感器。这种传感器包括高度计、指南针和GPS。出于说明的目的，我们在这里讨论GPS，因为它广泛用于现实世界的导航系统。

当我们接收到GPS测量值时，我们首先使用[21]中提出的方法将它们转换为局部笛卡尔坐标系。在因子图中增加一个新节点后，我们将一个新的GPS因子与这个节点相关联。如果GPS信号与激光雷达帧没有硬件同步，我们根据激光雷达帧的时间戳对GPS测量进行插值。

我们注意到，当GPS可用时，没有必要不断增加GPS因子，因为激光雷达惯性里程计的漂移增长非常缓慢。实际上，我们只在估计的位置协方差比接收的GPS位置协方差大时才增加一个GPS因子。

E.回环因子

由于使用了因子图，闭环也可以无缝地集成到本文提出的的系统中，这与LOAM和LIOM不同。出于说明的目的，我们描述并实现了一种简单但有效的基于距离的闭环检测方法。我们还注意到，我们提出的框架与用于闭环检测的其他方法兼容，例如[22]和[23]，它们生成点云描述子并将其用于位置识别。

当一个新的状态${\mathbf{x}}_{i+1}$被添加到因子图中时，我们首先搜索该图，并在欧氏空间中找到接近${\mathbf{x}}_{i+1}$的先验状态。如图1所示，例如，${\mathbf{x}}_3$是返回的候选者之一。然后，我们尝试将${\mathbb{F}}_{i+1}$与子关键帧 { F 3 − m , … , F 3 , … , F 3 + m } \left\{\mathbb{F}_{3-m}, \ldots, \mathbb{F}_{3}, \ldots, \mathbb{F}_{3+m}\right\} {F3−m​,…,F3​,…,F3+m​}进行扫描匹配。请注意，在扫描匹配之前，${\mathbb{F}}_{i+1}$与子关键帧 { F 3 − m , … , F 3 , … , F 3 + m } \left\{\mathbb{F}_{3-m}, \ldots, \mathbb{F}_{3}, \ldots, \mathbb{F}_{3+m}\right\} {F3−m​,…,F3​,…,F3+m​}首先转换为$\mathbf{W}$坐标系下。我们得到了相对变换$\Delta {\mathbf{T}}_{3,i+1}$，并将其作为回环因子添加到图中。在本文中，我们选择索引$m$为12，并且从新的状态${\mathbf{x}}_{i+1}$开始，闭环的搜索距离设置为$15m$。

在实践中，我们发现当GPS是唯一可用的绝对传感器时，增加闭环因子对于校正机器人高度的漂移特别有用。这是因为GPS的相关测量非常不准确——在我们的测试中，缺乏闭环的情况下，高度误差接近100米。

总结：

1. 改进了LOAM没法后端优化的缺点
2. 添加了4种因子做优化
3. 滑窗局部地图实现帧到局部地图匹配
4. 紧耦合，imu预积分作为lidar里程计的初值，lidar里程计用于估计imu的bias