0. 简介

传统的地图生成方法一般是依靠Lidar和IMU结合的，但是问题在于，目前Lidar和IMU的紧耦合主要集中在前端里程计，基本没有涉及到后端全局优化以及建图的融合。为此文章《Globally Consistent and Tightly Coupled 3D LiDAR Inertial Mapping》提出了一种改进的地图生成方法，并设计一个在所有阶段(前端里程计+后端全局优化和建图)，IMU和LIDAR都紧耦合的SLAM系统。这是作者实验室的官网，后续应该其后续工作应该会在官网中查到。

1. 文章贡献

本文的LiDAR-IMU紧耦合方法贯穿所有状态估计阶段。是目前为止第一个在全局轨迹规划上同时利用了LiDAR和IMU的约束的工作。与此前的LiDAR-IMU SLAM框架相比有以下几点不同：

1. 不同于此前的LiDAR SLAM从点云中提取平面和边缘特征来进行匹配，本文使用体素化的GICP匹配代价因子（voxelized GICP matching cost factor），可以在GPU上并行计算；
2. 本方法中的紧耦合里程计模块使用固定滞后平滑；
3. 激光雷达和IMU的紧密耦合，这里的紧耦合不仅体现在前端，而且在后端的因子图优化中也大量体现了紧耦合。

2. 详细内容

系统框架如下图，主要包括4个模块，预处理模块，前端里程计模块，局部建图模块，全局建图模块，所有模块都是LIDAR-IMU紧耦合的。里程计估计(即前端)模块鲁棒地估计传感器运动，并提供最新传感器状态的初始估计。通过后续的局部映射模块对估计的传感器状态进行细化，并将多个局部帧合并为一个子地图。然后，全局映射模块优化子地图位姿，使全局配准误差最小化，同时保持地图的一致性。我们通过多线程并行运行这些模块。

将在估计模块中被估计的传感器状态 $x_t$ 定义为

$x_t = [T_t, v_t, b_t]^T$

其中 $T_t = [R_t| T_t]∈SE(3)$ 为传感器位姿， $v_t∈\mathbb{R}^3$ 为速度， $b_t = [b^a_t, b^ω_t]∈\mathbb{R}^6$ 为IMU加速度和角速度偏差。从激光雷达点云 $P_t$ 和IMU测量值(线性加速度 $a_t$ 和角速度 $ω_t$ )估计传感器状态的时间序列。请注意，我们将LiDAR点云转换为IMU坐标框架，并且为了效率和简单性，将它们视为统一的传感器坐标框架。

这部分的具体操作即为进行了一个离群点的剔除，只不过这里离群的定义是依赖于时间戳，由于激光雷达的扫描方法，落入同一个体素格子的点的时间戳应该是大差不差的，用这个时间来做剔除，从而减小噪声点对体素化的负面影响。

2.1 LIDAR匹配残差因子

匹配代价因子约束2个传感器位姿( $\mathbf{T}_i$ 和 $\mathbf{T}_j$ )，使点云( $\mathcal{P}_i$ 和 $\mathcal{P}_j$ )之间的匹配代价最小。选择voxelized GICP (VGICP)代价[20]作为匹配代价，它是适用于GPU计算的广义ICP[21]的变体。

VGICP将每个输入点 $p_k∈\mathcal{P}_i$ 建模为高斯分布 $p_k = (μ_k, C_k)$ ，根据当前点 $p_k$ 的邻近点计算协方差矩阵 $C_k$ ，而上一帧点云 $\mathcal{P}_j$ ，假设已经经过建图模块融合了的话，则 $\mathcal{P}_j$ 就已经被体素化，每个体素也是通过一个高斯分布来表示的，这个高斯分布是通过该体素内的所有点的高斯分布计算出来的，当前点云的 $p_k$ 点通过位姿变换： $\mathbf{T}^{-1}_i\mathbf{T}_j$ 上一帧点云中的某个体素也符合正态分布： $p'_k = (μ'_k, C'_k)$ 构成一组数据关联。如果我们把这组数据关联对应的残差记为 $d_k=μ'_k-\mathbf{T}^{-1}_i\mathbf{T}_jμ_k$

然后，基于GICP分布到分布的距离定义 $\mathcal{P}_i$ 和 $\mathcal{P}_j$ 之间的匹配代价 $e^M$ ,点云利用体素来实现一个离散化，对落入体素内的点的分布情况进行处理，可以得到内部点的高斯分布，那么按照一般SLAM求位姿的思路，我们的流程大概率是列一个残差的式子，然后寻找一个位姿变化 $T$ 来最小化这个残差，在GICP里面作者使用的是下面这样一个残差的表示方法。

其中 $p'_k =(µ'_k, C'_k)$ 是通过查找 $\mathcal{P}_j$ 的体素图得到的 $p_k$ 对应体素的均值和协方差， $d_k =µ'_k−T_{ij}µ_k$ 是 $µ_k$ 和 $µ'_k$ 之间的残差。

从公式2的导数中，我们得到一个Hessian因子来约束 $\mathbf{T}_i$ 和 $\mathbf{T}_j$ 之间的相对姿态。值得强调的是，在每次优化迭代中，都在当前线性化点重新评估和线性化 $e^M$ ， $e^M$ 就是GICP的成本因子，我们可以理解为是总的残差里面的一部分，就像VINS一样。从而得到比传统SE3相对位姿约束[4]更精确的约束。

2.2 IMU的预积分因子

利用IMU预积分技术[22]有效地将IMU约束融入因子图中。给定IMU测量值( $a_t$ 和 $ω_t$ )，在给定IMU测量的情况下，传感器的未来状态可以估计为：

IMU预积分因子整合了 $i$ 和 $j$ 两个时间步间的系统演化，得到了相对的身体运动约束(详细推导参见[22]):

IMU预积分因子使系统能够在几何特征不足且 LiDAR 因子可能不足的环境中保持因子图的良好约束。

3. SLAM pipeline

3.1 数据预处理

这部分中论文作者提到了一个偏移校正的方法，采用时间戳来进行校正。原文的意思个人感觉是给点和体素格子都赋一个时间戳，如果点的时间戳和体素格子的时间戳的偏差超过了一定的范围，就将点移动到另一个体素格子里面去，从而避免在体素进行整合的时候出现明显的偏差。但是论文并没有对这个体素格子的时间戳做说明，个人感觉应该就是进行了一个离群点的剔除，只不过这里离群的定义是依赖于时间戳，由于激光雷达的扫描方法，落入同一个体素格子的点的时间戳应该是大差不差的，用这个时间来做剔除，从而减小噪声点对体素化的负面影响。

3.2 里程计估计

里程计估计模块通过融合LiDAR和IMU测量值来补偿传感器的快速运动，并鲁棒地估计传感器状态。首先，基于IMU动力学的运动预测将点云数据转换到IMU帧，校正传感器运动引起的点云失真;然后，我们使用预先计算的邻近点来计算每个点的协方差。在点的偏移矫正完成之后，按照下图构建因子图。

为了限制计算开销并保证系统的实时性，我们使用固定滞后的平滑方法并边缘化旧帧。受直接稀疏里程计[7]的启发，提出一种关键帧机制，用于完成高效的低漂移的轨迹估计。关键帧是一组经过选择的帧，它们在空间上均匀分布，同时与最新帧有足够的重叠。在最新帧和每个关键帧之间建立一个匹配代价因子，有效地减少估计漂移。如果关键帧已经从固定滞后平滑器中边缘化，则认为关键帧姿态是固定的，并创建一个一元匹配成本因子，约束相对于固定关键帧的最新传感器姿态。

为了表征两帧点云的重叠程度，本文定义了重叠率这一概念。点云 $\mathcal{P}_i$ 和 $\mathcal{P}_j$ 的重叠率指的是 $\mathcal{P}_i$ 中能落在 $\mathcal{P}_j$ 的体素中的点所占的比例。于是，当有新的一帧点云数据来到时，通过检查该点云与最后一个关键帧点云的重叠率，可以判断是否把新一帧点云数据设为新的关键帧：假如这个重叠率低于某个阈值（例如90%），则将该帧点云设置为新的关键帧。此外，还必须移除窗口中冗余的关键帧。按以下两种情况操作：

1. 有些旧的关键帧与最新的关键帧之间的重叠率小于某个阈值（例如5%），此时将这些关键帧移除。
2. 当窗口中的关键帧数量超过设定值，计算重叠率相关分数，移除分数最低的关键帧。该分数的计算公式为： 其中 $o(i,j)$ 就是第 $i$ 帧和第 $j$ 帧之间的重叠率。

里程计估计器中会将当前帧与关键帧建立GICP匹配代价因子：与已被边缘化的关键帧建立的是一元的代价因子（因为这个关键帧已经被边缘化也就不再继续更新其位姿估计），与固定滞后平滑窗口中的关键帧建立的是二元的代价因子。

此外，当前帧也会与最近几帧建立GICP匹配代价因子以应对快速运动，当前帧和上一帧之间建立IMU预积分因子。

3.3 局部建图

一旦从里程计边缘化一个帧，它就被馈送到局部映射模块，作为传感器状态的初始估计被送到局部建图模块。局部映射模块将多个局部帧合并为一个子映射，以减少全局映射模块中的优化变量数量。

首先，对被边缘化状态重新进行歪斜和协方差估计，期望改进里程计估计开始时的初始预测。然后，我们评估该帧与子映射中最新帧之间的重叠率，如果重叠率小于阈值(例如90%)，则将该帧插入子映射因子图。

如图3所示，我们为子图中的每个帧组合创建一个匹配代价因子(即all-to - all registration)。我们还在连续帧之间添加IMU预积分因子，并根据边缘状态为每帧的速度和偏差添加一个先验因子，以更好地稳定子图优化。

每两帧相邻帧之间有IMU预积分因子约束，每两帧之间有GICP匹配代价因子约束，另外每帧还添加了零偏/速度先验因子约束，先验值来自里程计估计模块边缘化状态的零偏/速度。当局部地图中的帧数达到一定值（例如15）或者局部地图的第一帧和最后一帧的重叠率小于某个阈值（例如5%）时，使用LM法对局部地图因子图进行优化，根据优化结果将局部地图融合。本部分的操作目的就是将几帧整合为了一个子图，然后让子图作为一个建图的基本单位

3.4 全局建图

全局建图模块对位姿进行校正以获得全局一致的建图结果。因为论文使用的这种因子图的连接方法，会产生一个很密集的因子图，而且存在隐式闭环的可能性，而且可能会丢失信息，因此论文在子图的两边，加了两帧，叫做端点，它们保存子图中第一帧和最后一帧相对于子图位姿的状态的传感器状态。

给定子地图xi中传感器状态 $[x_0，···，x_{N^{sub}}]$ 的估计，我们将子地图原点 $t_i$ 定义为传感器姿态在中心 $T_{N^{sub}/2}$ 处的姿态。则传感器状态xt相对于子地图原点为:

所以，子图的两个端点关于子图的变化可以用上述公式描述，相邻时间间隔短的帧之间的约束直接通过IMU因子约束，从而可以强约束子图位姿，同时避免局部建图模块估计的速度和偏差信息的丢失。

所以总的来说，这篇论文的中心就在于这个因子图的组织形式，采用的连接方法以及子图、端点的设计，都是服务于因子图优化。局部子图将IMU因子，LIDAR因子进行合并输出到全局图，全局图考虑计算成本等因素，重新构建一种因子图，即端点+因子图，端点和因子图之间的优化可以承受时间消耗较大的优化，因子图内部直接采用相邻的IMU约束进行快速更新。

4. 参考链接

https://mp.weixin.qq.com/s?__biz=MzI5MTM1MTQwMw

https://blog.csdn.net/weixin_43849505/article/details/126630214

https://xuwuzhou.top/%E8%AE%BA%E6%96%87%E9%98%85%E8%AF%BB62/