SLAM经典文献之：LIO-SAM（激光-IMU紧耦合图优化）-技术圈

作者丨chaochaoSEU@知乎

来源丨https://zhuanlan.zhihu.com/p/364651745

编辑丨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的高程测量不准确。

实验结果

本文仅做学术分享，如有侵权，请联系删文。

下载1

在「3D视觉工坊」公众号后台回复：3D视觉，即可下载 3D视觉相关资料干货，涉及相机标定、三维重建、立体视觉、SLAM、深度学习、点云后处理、多视图几何等方向。

下载2

在「3D视觉工坊」公众号后台回复：3D视觉github资源汇总，即可下载包括结构光、标定源码、缺陷检测源码、深度估计与深度补全源码、点云处理相关源码、立体匹配源码、单目、双目3D检测、基于点云的3D检测、6D姿态估计源码汇总等。

下载3

在「3D视觉工坊」公众号后台回复：相机标定，即可下载独家相机标定学习课件与视频网址；后台回复：立体匹配，即可下载独家立体匹配学习课件与视频网址。

重磅！3DCVer-学术论文写作投稿交流群已成立
扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。
▲长按加微信群或投稿
▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、orb-slam3等视频课程）、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答五个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近2000星球成员为创造更好的AI世界共同进步，知识星球入口：

SLAM经典文献之：LIO-SAM（激光-IMU紧耦合图优化）

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款
圈里有高质量教程资料、可答疑解惑、助你高效解决问题
觉得有用，麻烦给个赞和在看~

相关文章推荐

SLAM经典文献之：LIO-SAM（激光-IMU紧耦合图优化）

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款 圈里有高质量教程资料、可答疑解惑、助你高效解决问题觉得有用，麻烦给个赞和在看~

添加附言

相关文章推荐

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款
圈里有高质量教程资料、可答疑解惑、助你高效解决问题
觉得有用，麻烦给个赞和在看~