作者丨chaochaoSEU
编辑丨3D视觉工坊
文献阅读）多传感器融合之LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping
文章地址：https://arxiv.org/abs/2007.00258
代码地址：https://github.com/TixiaoShan/LIO-SAM
代码安装与运行可参考：https://zhuanlan.zhihu.com/p/164517204
LIO-SAM实际上是LeGO-LOAM的扩展版本,添加了IMU预积分因子和GPS因子,去除了帧帧匹配部分.
摘要：我们提出了一个名为LIO-SAM的激光-惯性紧耦合的框架，该框架通过平滑与建图可实现高精度、实时的移动机器人轨迹估计和地图构建。LIO-SAM在因子图上添加了激光-惯性里程计，从而可以将来自不同来源的大量相对和绝对测量值（包括回路闭合）作为因子纳入该系统中。预积分可校准点云，并为激光雷达里程计优化提供初值。而得到的激光雷达里程计又用于估计IMU的偏差。为了确保实时的高性能，我们边缘化旧的激光雷达帧以进行姿势优化，而不是将激光雷达帧与全局地图进行匹配。采用局部匹配代替全局匹配可以显着提高系统的实时性能，关键帧的选择性引入以及滑动窗口方法，可以将新的关键帧输入到固定窗口的先验“子关键帧”，从而大大提高了系统的实时性能。在从不同规模和环境的三个平台收集的数据集上，对该方法进行了评估。
1 引言
状态估计、定位和建图是成功的智能移动机器人的基本先决条件，除其他许多功能外，还需要反馈控制、避障和规划。利用基于视觉和激光雷达的传感，人们致力于实现高性能的实时同时定位和建图(SLAM)，可以支持移动机器人的6 DOF状态估计。基于视觉的方法通常使用单目或立体摄像机，并在连续图像中对特征进行三角定位，以确定摄像机的运动。虽然基于视觉的方法特别适合于位置识别，但当它们单独用于支持自治导航系统时，它们对初始化、光照和导航系统时不可靠。另一方面，基于激光雷达的方法在很大程度上不受光照变化的影响。VelodyneVLS-128和OusterOS1-128的多线激光雷达更适合直接捕捉三维空间中环境的细节。因此，本文重点研究了基于激光雷达的状态估计和建图方法。在过去的二十年中，提出了许多基于激光雷达的状态估计和建图方法，他们中，[1]提出的用于低漂移和实时状态估计和建图的激光雷达里程计和建图(LOAM)方法是应用最广泛的方法之一。LOAM使用激光雷达和IMU实现了最先进的性能，并已被列为最高的激光雷达方法，发布在Kitti网站[2]。尽管它取得了成功，但LOAM仍然存在一些局限性，通过将其数据保存在全局体素地图中，通常很难执行环路闭合检测和纳入其他绝对测量（如GPS）以进行姿态校正。当这个体素地图在特征丰富的环境中变得稠密时，它的在线优化过程变得不那么有效。LOAM在大规模测试中也存在漂移，因为它的核心方法还是基于扫描匹配。
本文中，我们提出了一个名为LIO-SAM的激光-惯性紧耦合的框架，该框架通过平滑与建图可实现高精度、实时的移动机器人轨迹估计和地图构建。假设一个非线性运动模型对点云去畸变，使用原始IMU测量数据的激光雷达扫描数据的估计传感器的运动。除了点云去畸变外，估计的运动还作为激光雷达里程计优化的初始值。得到的激光里程计还可用于再因子图中估计IMU的偏差。通过引入全局因子图进行机器人轨迹估计，可以有效地利用融合lidar和IMU测量量，在机器人姿态之间进行位置识别，并引入绝对测量，如GPS定位和罗盘航向。这些来自不同来源的因子集合用于图的联合优化。此外，我们边缘化旧的激光雷达扫描帧以优化姿势，而不是将扫描帧与全局地图（如loam）匹配。采用局部匹配代替全局匹配可以显着提高系统的实时性能，关键帧的选择性引入以及滑动窗口方法，可以将新的关键帧输入到固定窗口的先验“子关键帧”。
我们的主要贡献可概括如下：1）一种建立在因子图之上的紧密耦合的激光雷达惯性里程计框架，适用于多传感器融合和全局优化。2）一种高效的、基于局部滑动窗口的扫描匹配方法，通过将有选择地选择的新关键帧注册到固定大小的先验子关键帧集来实现实时性能。
激光雷达测径通常是通过使用ICP[3]和GICP[4]等扫描匹配方法来寻找两个连续帧之间的相对变换来进行的。基于特征的匹配方法由于其计算效率成为一种流行的选择。例如，[5]提出了一种基于平面的实时激光雷达里程计配准方法。假设在结构化环境中，它从点云中提取平面，并通过求解最小二乘问题来匹配它们。[6]提出了一种基于collar line-based的方法用于里程估计。然而，由于现代三维激光雷达的旋转机制和传感器运动，扫描的点云往往是倾斜的。仅仅使用激光雷达进行姿态估计并不理想，因为使用斜点云或特征进行配准最终会导致大漂移。
因此，激光雷达通常与其他传感器(如IMU和GPS)一起用于状态估计和建图。这种设计方案根据传感器融合方式可以分为两类：松耦合和紧耦合。松耦合：引入LOAM和IMU对激光雷达扫描帧进行去畸变，并给出扫描匹配方法的运动先验值。然而，IMU并不参与算法的优化过程。因此，LOAM可归类为松耦合方法，lego-loam也一样。一种更流行的松耦合融合方法是使用扩展卡尔曼滤波器（EKF）。例如，在机器人状态估计的优化阶段，使用EKF整合来自LiDAR、IMU和GPS的测量值。
紧耦合：紧耦合系统通常提供更高的精度，目前是正在进行的研究的主要焦点。在[15]对IMU进行预积分，并用于点云去畸变。文献[16]提出了一种以机器人为中心的激光-惯性状态估计方法（R-LINS）。R-LINS使用误差状态卡尔曼滤波器递归修正机器人的状态估计。由于缺少其他可用的传感器来进行状态估计，它在长时间航行中会产生漂移。[17]中介绍了一种激光-惯性紧耦合的定位与建图框架（LIOM，LIO-mapping的缩写），其联合优化激光雷达和IMU的测量量，与LOAM相比获得类似或更好的准确性。
我们首先定义了我们在整个论文中使用的框架和符号。我们将世界框架表示为W，机器人身体框架表示为B，为了方便起见，我们还假设IMU框架与机器人身体框架重合。机器人状态x可以写成：
这个状态估计问题可以表示为最大后验(MAP)问题。我们使用一个因子图来建模。在高斯噪声模型的假设下，我们的MAP问题等价于求解非线性最小二乘问题[18]。请注意，在不失去通用性的情况下，所提出的系统还可以包含来自其他传感器的测量（例如高度计的高程）。关于因子图的构造，我们引入了四种类型的因子以及一种可变类型。表示机器人在特定时间的状态的变量可记为图的节点。这四种因子是：(A)IMU预积分因子，(B)激光里程计因子，(C)GPS因子，(D)环路闭合因子。当机器人姿态变化超过用户定义的阈值时，将在图中添加一个新的机器人状态节点x。因子图在插入新节点时使用贝叶斯树(iSAM2)[19]的增量平滑和建图进行优化。
B. IMU预积分因子
除了提高效率外，应用IMU预积分也为因子图提供了一种约束类型（IMU预积分因子）。IMU偏差与激光雷达里程计因子一起优化。
C. 激光里程计因子
特征提取（LOAM的方式）：
通过评估局部区域上的点的曲率来提取边缘和平面特征。曲率较大的点被划分为边缘特征。同样，曲率小的被分类为平面特征。我们将第i时刻激光雷达扫描帧提取的边缘和平面特征分别表示为Fie和Fip。我将在同一时刻提取的所有特征组成一个激光雷达帧Fi，其中Fi={Fie,Fip}。
关键帧选择：
使用一种简单而有效的启发式方法：与之前的状态xi相比，当机器人姿态变化超过用户定义的阈值时，我们选择激光雷达帧Fi+1作为关键帧。在因子图中，新保存的关键帧Fi+1与一个新的机器人状态节点xi+1关联。两个关键帧之间的激光雷达帧被丢弃。通过这种方式添加关键帧，既能平衡建图的密度和内存消耗，又能保持相对稀疏的因子图，适用于实时非线性优化。在我们的工作中，增加一个新的关键帧的位置和旋转变化阈值被选择为1米和10°。
激光雷达里程计因子的生成步骤如下：
1)体素地图的子关键帧（Sub-keyframes）:我们实现了一个滑动窗口的方法来创建一个包含固定数量的最近激光雷达扫描的点云地图。我们没有优化两个连续的激光雷达扫描之间的转换，而是提取n个最近的关键帧，我们称之为子关键帧，用于估计。然后子关键帧集合{Fi−n,…,Fi}使用与之关联的变换矩阵Ti−n,...,Ti转换到世界坐标系W。转换后的子关键帧被合并到一个体素地图Mi中。于我们在前面的特征提取步骤中提取了两种类型的特征，因此Mi由两个亚体素地图组成，分别是边缘特征体素地图和平面特征体素地图Mip。激光雷达帧和体素地图之间的关系如下:
2) edge和plane进行匹配
3）利用点-线与点-面之间的距离关系，建立位姿求解方程（参考LOAM）
D. GPS因子
当我们接收到GPS测量值时，我们首先使用[21]中提出的方法将其转换到局部笛卡尔坐标系中。在因子图中添加一个新节点之后，我们将一个新的GPS因子与这个节点关联起来。如果GPS信号与激光雷达帧之间没有硬件同步，则根据激光雷达帧的时间戳对GPS测量值进行线性插值。我们注意到，在GPS接收可用时，不断增加GPS因子是不必要的，因为激光雷达惯性里程计的漂移增长非常缓慢。在实际应用中，我们只在估计的GPS位置协方差大于接收到的GPS位置协方差时添加一个GPS因子。
E. 闭环因子
和lego-loam一样使用的基于欧式距离的闭环检测。闭环系数对于校正机器人高度的漂移特别有用，因为GPS的高程测量不准确。
实验结果
本文仅做学术分享，如有侵权，请联系删文。
