【论文阅读】LIO-SAM

标题：LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping

作者：Tixiao Shan et al.

机构：MIT

来源：IROS 2020

🏷️摘要

我们提出了一个紧耦合的激光-惯性里程计LIO-SAM，通过优化包含LiDAR里程计因子，IMU预积分因子，GPS因子和回环因子来得到机器人的全局一致的位姿。加入IMU预积分可以给激光里程计提供初始猜测并帮助去除点云运动畸变。使用帧-局部地图匹配代替LOAM的帧-全局地图匹配，提高了帧图匹配的效率，使整个系统的实时性能得到了提升。

🏷️前人工作总结

LOAM是目前为止激光里程计（LO）领域最经典最广泛使用的方法。但是它存在一个问题，就是它直接存储全局体素地图而不是局部地图，从而很难执行回环检测以修正漂移，或者结合GPS等其他绝对测量值进行位姿修正。并且使用体素地图的优化效率会随时间降低。为了克服该问题，作者只独立地存储每个关键帧的特征，而不是在位姿估计完成后就将特征加入到全局地图中。

另一方面，IMU和LiDAR的联合位姿估计已经被广泛研究，大致分为两类。第一类是松耦合的方法，例如LOAM和LeGO-LOAM中使用IMU去除LiDAR点云的运动畸变，以及[8]-[12]使用EKF整合LiDAR和IMU的测量。第二类是紧耦合的方法，例如R-LINS[15]，使用误差状态卡尔曼滤波器迭代地修正机器人的位姿估计，再比如LIOM [16]联合优化LiDAR和IMU测量。但是LIOM一次性处理所有测量，因此不能实时运行。而本文也属于紧耦合的激光-惯性里程计方法，只是采用了因子图优化而不是滤波的方法。

🏷️主要工作及贡献

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

项目地址：https://github.com/TixiaoShan/LIO-SAM

🏷️算法流程

LIO-SAM整套算法基于因子图构建，主要包含四种因子。第一种是IMU预积分因子（橙色），由两个相邻关键帧之间的IMU测量积分得到。第二种是激光里程计因子（绿色），由每个关键帧和之前n个关键帧之间的帧图匹配结果得到。第三种是GPS因子（黄色），由每个关键帧的GPS测量得到。第四种是回环因子（黑色），由每个关键帧和候选回环关键帧的时序相邻的2m+1个关键帧之间的帧图匹配得到。此外，因子图优化通过iSAM2[18]完成。

🏷️四种因子

1.IMU预积分因子

详见论文“On-Manifold Preintegration for Real-Time Visual-Inertial Odometry”

2. 激光里程计因子

特征提取思路还是参考LOAM，提取Edge边缘特征和Planar平面特征。在i时刻激光雷达扫描提取的特征$\mathbb{F}_{i}=\left\{\mathrm{F}_{i}^{e}, \mathrm{~F}_{i}^{p}\right\}$，$\mathbb{F}$是在Body系下表示，使用每个激光雷达帧进行计算并向图中添加因子是很困难的，因此我们采用了在视觉 SLAM 领域广泛使用的关键帧的概念。当与之前状态$ \mathbf{x}_{i} $相比，机器人的姿态变化超过用户预先定义的阈值时，我们选择$\mathbb{F}_{i+1}$作为关键帧，并与新的机器人状态$ \mathbf{x}_{i+1} $相关联，两关键帧之间的激光雷达帧被丢弃，这种方式不仅可以在地图密度和内存消耗之间取得平衡，而且有助于保持相对稀疏的因子图，适合实时的非线性优化。在我们的工作中，添加新关键帧的位置和旋转变化阈值为1m和10°。

在以下步骤中描述了激光雷达里程计因子的生成：

(1) 体素地图的子关键帧

我们使用固定数量的滑动窗口方法来创建激光雷达扫描的点云图，我们没有优化两个连续激光帧之间的变换，而是提取了 n 个最近的关键帧，我们称之为子关键帧，用于估计。把$\left\{\mathbb{F}_{i-n}, \ldots, \mathbb{F}_{i}\right\}$转换到World系下，变换后的子关键帧被合并到一个体素地图$\mathbf{M}_{i}$中，$’\mathbf{F}_{i}^{e} 和’\mathbf{F}_{i}^{p}$表示转换到世界坐标系下的特征，$\mathbf{M}_{i}^{e}$和$\mathbf{M}_{i}^{p}$会降采样以消除落在同一体素单元中的重复特征，在本文中，n 选择为 25。$\mathbf{M}_{i}^{e}$和$\mathbf{M}_{i}^{p}$的降采样分辨率分别为 0.2m 和 0.4m。

(2) Scan-matching

首先把$\left\{\mathbf{F}_{i+1}^{e}, \mathbf{F}_{i+1}^{p}\right\}$从Body系变换到World系得到$\left\{‘\mathbf{F}_{i+1}^{e}, ‘\mathbf{F}_{i+1}^{p}\right\}$，这个变换的初始猜测$\tilde{\mathbf{T}}_{i+1}$由IMU预积分提供。对于$’\mathbf{F}_{i+1}^{e}和 ‘\mathbf{F}_{i+1}^{p}$中的每个特征，我们在$\mathbf{M}_{i}^{e}$和$\mathbf{M}_{i}^{p}$中找到它的对应。为简洁起见，此处省略了查找这些对应关系的详细过程，但在 [1]中进行了详细描述。【“Low-drift and Real-time Lidar Odometry and Mapping”】

(3) 相对变换

构建点线误差，点面误差，通过高斯牛顿法最小化误差来求得最优

最后我们可以得到$ \mathbf{x}_{i} $和$ \mathbf{x}_{i+1} $之间的相对变换$ \Delta \mathbf{T}_{i, i+1}
$，这是连接这两个位姿的激光雷达里程计因子$\Delta \mathbf{T}_{i, i+1}=\mathbf{T}_{i}^{\mathbf{\top}} \mathbf{T}_{i+1}$

3. GPS因子

虽然我们可以通过仅利用 IMU 预积分和激光雷达里程计因子来获得可靠的状态估计和映射，但系统在长时间导航任务中仍然会出现漂移。为了解决这个问题，我们可以引入能提供绝对测量的传感器，如高度计、指南针和 GPS。我们这里主要讨论 GPS，因为它广泛用于现实世界的导航系统。

当接收到GPS测量后，我们首先使用[21]的方法变换它们到笛卡尔坐标系中。当一个新的位姿节点被插入到因子图后，我们关联GPS因子到该位姿节点中去。如果 GPS 信号没有与激光雷达帧硬件同步，我们会根据激光雷达帧的时间戳在 GPS 测量值之间进行线性插值。

由于在GPS信号一直存在的时候，持续添加GPS因子没有意义。因为激光雷达惯性里程计的漂移很缓慢。所以在实际操作过程中，我们只在估计的位置协方差大于接收到的 GPS 位置协方差时才添加 GPS 因子。

4. 回环因子

由于使用了因子图，与 LOAM 和 LIOM 不同，回环检测也可以无缝地结合到系统中。我们描述并实现了一种简单但有效的基于欧氏距离的回环检测方法。我们还注意到，我们提出的框架与其他闭环检测方法效果相近，例如Scan Context [22] 和 [23]，它们生成点云描述符并将其用于位置识别。

当一个新状态$ \mathbf{x}_{i+1} $添加进因子图时，我们首先搜素图并寻找欧式空间中最接近$ \mathbf{x}_{i+1} $的先验状态。举个例子，$ \mathbf{x}_{3} $是一个返回的候选，我们就尝试把$ \mathbb{F}_{i+1}
$和$\left\{\mathbb{F}_{3-m}, \ldots, \mathbb{F}_{3}, \ldots, \mathbb{F}_{3+m}\right\}$进行scan-match匹配。在进行scan-match前，这些关键帧都会统一转换到世界坐标系下。我们获得相对变换$ \Delta \mathbf{T}_{3, i+1}
$并把它作为回环因子添加到因子图中。在本文中，我们选择m=12，回环搜索距离限制为距离$ \mathbf{x}_{i+1} $15m内。

在实践中，当 GPS 是唯一可用的绝对传感器时，我们发现添加回环因子对于纠正机器人高度的漂移特别有用。这是因为 GPS 的海拔测量非常不准确，在我们的测试中，在没有回环的情况下，高度误差接近 100m。