精华内容
下载资源
问答
  • LIO-SAM探秘第一章之论文解析
    千次阅读
    2020-11-04 11:25:35

    LIO-SAM:经由平滑和建图实现的紧耦合激光雷达惯性里程计

    注意:通读文章后发现整篇文章没啥实质性的东西,慎读。。。
    此链接是文章的主要内容的摘要,强烈推荐
    https://blog.csdn.net/unlimitedai/article/details/107378759#t2

    代码的链接为:https://github.com/TixiaoShan/LIO-SAM
    论文的链接为:https://github.com/TixiaoShan/LIO-SAM/blob/master/config/doc/paper.pdf

    摘要

    我们提出了一种通过平滑和建图的LIO-SAM紧耦合激光雷达惯性里程测量的框架,该框架可实现高精度,实时的移动机器人轨迹估计和地图构建。 LIO-SAM在因子图的顶部制定了激光雷达惯性里程表,从而可以将来自不同来源的大量相对和绝对测量值(包括回路闭合)作为因子合并到系统中。来自惯性测量单元(IMU)预积分的估计运动纠正点云畸变,并为激光雷达里程计优化提供了初始猜测。获得的激光雷达里程计解决方案用于估计IMU的偏差。为了确保实时高性能,我们将旧的激光雷达扫描边缘化以进行姿势优化,而不是将激光雷达扫描与全局地图匹配。选择性地引入关键帧以及以有效的滑动窗口方法将新的关键帧注册到固定大小的先验“子关键帧”集合中,在局部范围而不是全局范围内进行扫描匹配可显着提高系统的实时性能 。在从不同规模和环境的三个平台收集的数据集上,对该方法进行了广泛的评估。

    1. INTRODUCTION

    状态估计,定位和建图是成功的智能移动机器人的基本先决条件,是反馈控制,避障和计划以及许多其他功能所必需的。使用基于视觉和基于激光雷达的传感技术,已经做出了巨大的努力,以实现高性能的实时同时定位和制图(SLAM),该技术可以支持移动机器人的六个自由度状态估计。基于视觉的方法通常使用单目或立体相机,并在连续图像中对特征进行三角测量以确定相机的运动。尽管基于视觉的方法特别适合于位置识别,但是当它们单独用于支持自主导航系统时,它们对初始化,光照和范围的敏感性使其不可靠。另一方面,基于激光雷达的方法在很大程度上不受光照的影响。尤其是随着长距离、高分辨率的3D激光雷达(例如Velodyne VLS-128和Ouster OS1-128)的推出,激光雷达变得更适合直接捕获3D空间中环境的精细细节。因此,本文重点研究基于激光雷达的状态估计和建图方法。

    在过去的二十年中,已经提出了许多基于激光雷达的状态估计和建图方法。 其中,在文献[1]中提出的用于低漂移和实时状态估计和制图的激光雷达测距和制图(LOAM)方法是使用最广泛的方法。 使用激光雷达和惯性测量单元(IMU)的LOAM达到了最先进的性能,并且自从其在KITTI里程表基准站点发布以来就被评为基于激光雷达的顶级方法[2]。 尽管取得了成功,但LOAM仍然存在一些局限性-通过将其数据保存在全局体素图中,通常很难执行闭环检测,以及融入其他绝对测量值(例如GPS)以进行姿势校正。 当体素图在特征丰富的环境中变得稠密时,其在线优化过程将变得效率较低。 LOAM在大规模测试中也存在偏差,因为它的核心是基于扫描匹配的方法。

    在本文中,我们提出了一个通过平滑和建图的LIO-SAM紧密耦合激光雷达惯性里程计的框架,以解决上述问题。我们假设了点云畸变的非线性运动模型,使用原始IMU测量来估计激光雷达扫描期间的传感器运动。除了消除点云畸变外,估计的运动还可以作为激光雷达里程计优化的初始猜测。然后,将获得的激光雷达里程计解决方案用于估计因子图中IMU的偏差。通过引入用于机器人轨迹估计的全局因子图,我们可以使用激光雷达和IMU测量有效地执行传感器融合,在机器人姿态中合并位置识别,并在可用时引入绝对测量,例如GPS定位和指南针航向。来自各种来源的这些因素集合用于图形的联合优化。此外,我们将旧的激光雷达扫描 边缘化以进行姿势优化,而不是将扫描匹配到像LOAM这样的全局地图。选择性地引入关键帧以及以有效的滑动窗口方法将新的关键帧注册到固定大小的先验 “子关键帧”集合中,在局部范围而不是全局范围内进行扫描匹配可显着提高系统的实时性能。我们工作的主要贡献可以概括如下:

    • 在因子图上建立了紧密耦合的激光雷达惯性里程表框架,适用于多传感器融合和全局优化。
    • 一种有效的,基于本地滑动窗口的扫描匹配方法,通过选择性将选择的新关键帧注册到固定大小的先前子关键帧集来实现实时性能。
    • 所提出的框架已通过各种规模,车辆和环境的测试得到了广泛验证。

    2. RELATED WORK

    激光雷达测距法通常是通过使用扫描匹配方法(例如ICP [3]和GICP [4])找到两个连续帧之间的相对变换来执行的。由于不是匹配整个点云,基于特征的匹配方法因其计算效率已成为一种流行的替代方法。例如,在[5]中,提出了一种基于平面的配准方法用于实时激光雷达里程计。假设在结构化环境中进行操作,它将从点云中提取平面,并通过解决最小二乘问题来匹配它们。在文献[6]中提出了一种基于领线的方法,用于里程计估计。在这种方法中,线段是从原始点云中随机生成的,以后用于配准。但是,由于现代3D激光雷达的旋转机制和传感器运动,扫描的点云经常会偏斜。仅使用激光雷达进行姿势估计并不理想,因为使用偏斜点云或特征进行配准最终会导致较大的漂移。

    因此,激光雷达通常与其他传感器(例如IMU和GPS)结合使用,以进行状态估计和映射。 这种利用传感器融合的设计方案通常可以分为两类:松耦合融合和紧密耦合融合。 在LOAM [1]中,引入了IMU以使激光雷达扫描偏斜并在进行扫描匹配之前先进行运动。 但是,IMU不参与算法的优化过程。 因此,LOAM可以归为松散耦合方法。 在[7]中提出了一种针对地面车辆制图任务的轻型且地面优化的激光雷达测距和制图(LeGO-LOAM)方法。 IMU测量值的融合与LOAM相同。 松耦合融合的一种更流行的方法是使用扩展卡尔曼滤波器(EKF)。 例如,[9]-[13]在用于机器人状态估计的优化阶段,使用EKF集成了来自激光雷达,IMU和可选GPS的测量值。

    紧耦合系统通常可提供更高的精度,并且目前是正在进行的研究的重点[14]。 在[15]中,利用预积分的IMU测量来消除点云的畸变。 文献[16]提出了一个以机器人为中心的激光雷达惯性状态估计器R-LINS。 R-LINS使用错误状态卡尔曼滤波器以紧密耦合的方式递归校正机器人的状态估计。 由于缺少用于状态估计的其他可用传感器,因此在长时间导航期间会发生漂移。 文献[17]中介绍了紧密耦合的激光雷达惯性里程表和测绘框架LIOM。 LIOM是LIO-Mapping的缩写,与LOAM相比,可共同优化激光雷达和IMU的测量,并获得相似或更好的精度。 由于LIOM被设计为处理所有传感器测量,因此无法实现实时性能 - 在我们的测试中,它的运行速度约为(我们的)0.6倍。

    3. LIDAR INERTIAL ODOMETRY VIA SMOOTHING AND MAPPING

    A System Overview

    我们首先定义在整篇论文中使用的坐标系和符号。 我们将世界坐标系表示为W,将机器人主体坐标系表示为B。为方便起见,我们还假定IMU坐标系与机器人主体坐标系重合。 机器人状态x可以写成:

    在这里插入图片描述
    拟议系统的概述如图1所示。该系统从3D激光雷达,IMU和GPS(可选)接收传感器数据。 我们试图使用这些传感器的观测值来估计机器人的状态及其轨迹。 该状态估计问题可以表述为最大后验(MAP)问题。 我们使用一个因子图对该问题进行建模,因为与贝叶斯网络相比,它更适合执行推理。 在高斯噪声模型的假设下,针对我们问题的MAP推论等同于解决非线性最小二乘问题[18]。 请注意,在不失一般性的前提下,建议的系统还可以合并其他传感器的测量值,例如高度计的高度或指南针的航向。
    在这里插入图片描述

    我们介绍了四种类型的因子以及一种用于因子图构造的变量类型。 此变量代表机器人在特定时间的状态,该变量归因于图表的节点。 四种类型的因子是:(a)IMU预积分因子,(b)激光雷达里程计因子,(c)GPS因子和(d)回路闭合因子。 当机器人姿势的变化超过用户定义的阈值时,会将新的机器人状态节点x添加到图中。 当使用增量平滑和与贝叶斯树(iSAM2)映射时,在插入新节点时优化因子图[19]。 以下各节介绍了生成这些因素的过程。

    B IMU Preintegration Factor

    使用公式定义来自IMU的角速度和加速度的测量值。 2和3:
    在这里插入图片描述现在,我们可以使用来自IMU的测量值来推断机器人的运动。 机器人在时间t +Δt处的速度,位置和旋转可以计算如下:在这里插入图片描述由于篇幅所限,我们请读者参考[20]中的描述,以详细了解方程式[7-9]。除了它的效率外,应用IMU预积分自然也为我们提供了一种针对因子图的约束类型-IMU预积分因子。 IMU偏差与图中的激光雷达里程计因子一起被共同优化。

    C. Lidar Odometry Factor

    当新的激光雷达扫描到达时,我们首先执行特征提取。 通过评估局部区域上点的粗糙度来提取边缘和平面特征。 粗糙度值大的点被分类为边缘特征。 类似地,按低粗糙度值对平面特征进行分类。 我们将在时间i从激光雷达扫描中提取的边缘和平面特征分别表示为Fie和Fip。 时间i提取的所有特征组成一个激光雷达框架Fi,其中Fi= {Fie,Fip}。 注意,激光雷达框架FB表示。如果使用 range image ,则在[1]或[7]中可以找到特征提取过程的更详细描述。

    使用每个激光雷达框架进行计算并向图中添加因子在计算上是棘手的,因此我们采用了关键帧选择的概念,该概念已在视觉SLAM领域中广泛使用。 使用一种简单但有效的启发式方法,当机器人姿势的变化与先前状态xi相比超出用户定义的阈值时,我们选择激光雷达框架Fi+1作为关键帧。 新保存的关键帧Fi+1与因子图中的新机器人状态节点xi+1关联。 两个关键帧之间的激光雷达帧将被丢弃。 以这种方式添加关键帧,不仅可以在稠密地图和内存消耗之间实现平衡,而且还有助于维持相对稀疏的因子图,这适合于实时非线性优化。 在我们的工作中,用于添加新关键帧的位置和旋转变化阈值选择为1米和10度。

    让我们假设我们希望将新的状态节点xi+1添加到因子图中。 与该状态关联的激光雷达关键帧是Fi+1。 以下步骤描述了激光雷达测距因子的生成:

    1)体素贴图的子关键帧:我们实现了滑动窗口方法来创建包含固定数量的最近的激光雷达扫描的点云图。 我们没有优化两个连续的激光雷达扫描之间的转换,而是提取了n个最新的关键帧(称为子关键帧)进行估算。 然后将子关键帧集合{Fi-n,…,Fi} 使用相关联的变换{Ti-n,…,Ti} 变换为帧W。 变换后的子关键帧将合并到一个体素贴图Mi中。 由于我们在上一个特征提取步骤中提取了两种类型的特征,因此Mi由两个子体素贴图组成,分别表示为Mie(边缘特征体素贴图)和Mip(平面特征体素贴图)。 激光雷达框架和体素贴图相互关联,如下所示:
    在这里插入图片描述2)扫描匹配:我们通过扫描匹配将新获得的激光雷达帧Fi+1(也为{Fi+1e,Fi+1p})与Mi匹配。 为此,可以使用各种扫描匹配方法,例如[3]和[4]。 由于在各种挑战性环境中的计算效率和鲁棒性,这里我们选择使用[1]中提出的方法。

    我们首先将{Fi+1e,Fi+1p}从B转换为W,然后获得{'Fi+1e,'Fi+1p}。 通过使用来自IMU的预测机器人运动T̃i+1获得初始转换。 对于’Fi+1e或’Fi+1p中的每个特征,我们然后在Mie或Mip中找到其边缘或平面对应关系。 为了简洁起见,此处省略了查找这些对应关系的详细过程,但在[1]中进行了详细描述。

    3)相对变换:可以使用以下方程式计算特征与其边缘或平面 对应关系之间的距离:
    在这里插入图片描述我们注意到,获得∆Ti,i+1的另一种方法是将子关键帧转换为xi的帧。 换句话说,我们将Fi+1与xi帧中表示的体素贴图匹配。 这样,可以直接获得真实的相对变换∆Ti,i+1。 由于变换后的特征’Fie和’Fip可以重复使用多次,因此我们选择使用本教程中介绍的方法(III-C.1)以提高计算效率。

    D. GPS Factor

    尽管我们仅通过使用IMU预积分和激光雷达测距因子就可以获得可靠的状态估计和映射,但是在长时间的导航任务中,系统仍会出现漂移。 为了解决这个问题,我们可以引入提供绝对测量以消除漂移的传感器。 这样的传感器包括高度计,指南针和GPS。 出于说明目的,我们讨论了GPS,因为它已在现实世界的导航系统中广泛使用。

    当我们收到GPS测量值时,我们首先使用[21]中提出的方法将它们转换为局部笛卡尔坐标系。 在将新节点添加到因子图中后,我们然后将新的GPS因子与此节点关联。 如果GPS信号未与激光雷达框架进行硬件同步,我们将基于激光雷达框架的时间戳在GPS测量值之间进行线性插值。

    我们注意到,由于GPS激光雷达惯性里程表的漂移增长得非常缓慢,因此在GPS接收可用时不必不断添加GPS因子实际上,我们仅在估计的位置协方差大于接收到的GPS位置协方差时添加GPS因子。

    E. Loop Closure Factor

    由于使用了因子图,因此与LOAM和LIOM相比,闭环也可以无缝地集成到建议的系统中。 为了说明的目的,我们描述并实现了一种朴素但有效的基于欧几里德距离的闭环检测方法。 我们还注意到,我们提出的框架与其他用于闭环检测的方法兼容,例如[22]和[23],它们会生成点云描述子并将其用于位置识别。

    当将新状态xi+1添加到因子图中时,我们首先搜索该图并找到 在欧几里得空间中接近xi+1的先验状态。 如图1所示,例如,x3是返回的候选之一。 然后,我们尝试使用扫描匹配将Fi+1与子关键帧{F3-m,…,F3,…,F3+m}进行匹配。 请注意,在扫描匹配之前,首先将Fi+1和过去的子关键帧转换为W。 我们获得相对变换ΔT3,i+1 并将其作为闭环因子添加到图中。 在整个本文中,我们选择索引m为12,并且从新状态xi+1到闭环的搜索距离设置为15m。

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

    4. EXPERIMENTS

    现在,我们描述了一系列实验,以定性和定量地分析我们提出的框架。

    本文使用的传感器套件包括Velodyne VLP-16激光雷达,MicroStrain 3DM-GX5-25 IMU和Reach M GPS。

    为了验证,我们收集了5个不同规模,平台和环境的不同数据集。 这些数据集分别称为旋转,步行,校园,公园和阿姆斯特丹。 传感器安装平台如图2所示。前三个数据集是使用MIT校园内的定制手持设备收集的。 该公园数据集是使用无人地面交通工具(Clearpath Jackal)在植被覆盖的公园中收集的。 通过将传感器安装在船上并在阿姆斯特丹的运河中巡游,收集了最后一个数据集,即阿姆斯特丹。 这些数据集的详细信息显示在表I中。
    在这里插入图片描述

    我们将建议的LIO-SAM框架与LOAM和LIOM进行比较。 在所有实验中,LOAM和LIO-SAM都必须实时运行。 另一方面,LIOM被赋予无限时间来处理每个传感器测量。 所有方法均以C ++实现,并在配备了Intel i7-10710U CPU的笔记本电脑上使用Ubuntu Linux中的机器人操作系统(ROS)[24]执行。 我们注意到,只有CPU用于计算,而没有启用并行计算。 我们在LIO-SAM上的实现可在Github 1上免费获得。 可以在下面的链接2中找到所执行实验的补充详细信息,包括所有测试的完整可视化。

    A. Rotation Dataset

    在此测试中,当仅将IMU预积分和激光雷达测距因子添加到因子图中时,我们专注于评估框架的鲁棒性。旋转数据集是由拿着传感器套件并在静止不动的情况下执行一系列剧烈旋转操作的用户收集的。该测试中遇到的最大转速为133.7度/秒。图3(a)中显示了填充有结构的测试环境。从LOAM和LIO-SAM获得的图显示在图1和2中。分别参见图3(b)和(c)。因为LIOM使用与[25]中相同的初始化管道,所以它继承了视觉惯性SLAM的相同初始化敏感性,并且无法使用此数据集正确初始化。由于无法产生有意义的结果,因此未显示LIOM的图。如图所示,与LOAM相比,LIO-SAM的地图保留了更精细的环境结构细节。这是因为即使机器人快速旋转,LIO-SAM仍可以将每个激光雷达框架精确地记录在SO(3)中。

    B.行走数据集

    该测试旨在评估当系统在SE(3)中经历剧烈的平移和旋转时我们方法的性能。 该数据集遇到的最大平移和旋转速度分别为1.8 m / s和213.9 o / s。 在数据收集过程中,用户持有图2(a)所示的传感器套件,并迅速走过MIT校园(图4(a))。 在该测试中,图4(b)所示的LOAM图在遇到剧烈旋转时会在多个位置发散。 在此测试中,LIOM优于LOAM。 但是,图4(c)所示的地图在各个位置仍然略有不同,并且由许多模糊结构组成。 因为LIOM被设计为处理所有传感器测量,所以它仅以0.56倍的实时速度运行,而其他方法则是实时运行。 最后,LIO-SAM的性能优于这两种方法,并且生成的地图与可用的Google Earth图像一致。

    C. Campus Dataset

    此测试旨在显示引入GPS和环路闭合因子的好处。为此,我们有意禁止在图中插入GPS和环路闭合因子。当同时禁用GPS和环路闭合因子时,我们的方法称为LIO-odom,它仅利用IMU预积分和激光雷达测距因子。当使用GPS因子时,我们的方法称为LIO-GPS,它使用IMU预积分,激光雷达里程计和GPS因子进行图形构建。 LIO-SAM在可用时会使用所有因素。

    为了收集此数据集,用户使用手持设备在MIT校园中漫步并返回相同的位置。由于地图区域中的建筑物和树木众多,因此GPS接收很少可用,而且大部分时间都不准确。在滤除不一致的GPS测量值之后,可用GPS的区域在图5(a)中显示为绿色部分。这些区域对应于未被建筑物或树木包围的少数区域。

    LOAM,LIO-odom,LIO-GPS和LIO-SAM的估计轨迹如图5(a)所示。 由于LIOM无法正确初始化并无法产生有意义的结果,因此未显示结果。 如图所示,与所有其他方法相比,LOAM的轨迹明显漂移。 如果不校正GPS数据,则LIO-odom的轨迹开始明显地漂移到地图的右下角。 借助GPS数据,LIO-GPS可以在可用时校正漂移。 但是,GPS数据在数据集的后半部分不可用。 结果,当机器人由于漂移而返回到起始位置时,LIO-GPS无法关闭环路。 另一方面,LIO-SAM可以通过在图形中添加环路闭合因子来消除漂移。 LIO-SAM的地图与Google Earth图像高度吻合,如图5(b)所示。 表II显示了机器人返回起点时所有方法的相对平移误差。

    D.公园数据集

    在此测试中,我们将传感器安装在UGV上,并沿着森林远足径驾驶车辆。驾驶40分钟后,机器人将返回其初始位置。 UGV在三个路面上行驶:沥青,被草覆盖的地面和被尘土覆盖的小路。由于没有悬架,因此在非沥青路面上行驶时,机器人会出现低振幅但会产生高频振动的情况。

    为了模拟具有挑战性的地图绘制场景,我们仅在机器人处于宽阔的区域时才使用GPS测量,这在图6(a)中用绿色部分表示。这种映射方案代表了一项任务,在该任务中,机器人必须映射多个GPS拒绝的区域并定期返回具有GPS可用性的区域以校正漂移。

    与以前的测试结果相似,LOAM,LIOM和LIO-dom会遭受明显的漂移,因为没有绝对的校正数据。此外,LIOM仅实时运行0.67倍,而其他方法则实时运行。

    尽管LIO-GPS和LIO-SAM的轨迹在水平面上重合,但它们的相对平移误差却有所不同(表II)。由于没有可靠的绝对海拔高度测量值,因此LIO-GPS会出现高度漂移,并且在返回机器人的初始位置时无法闭合环路。 LIO-SAM没有这种问题,因为它利用环路闭合因子来消除漂移。

    E.阿姆斯特丹数据集

    最后,我们将传感器套件安装在船上,并沿着阿姆斯特丹的运河航行了3个小时。尽管在此测试中传感器的移动相对平稳,但由于以下几个原因,绘制运河图仍具有挑战性。运河上的许多桥梁构成了简陋的场景,因为当小船在它们下面时,几乎没有有用的功能,类似于穿过一条漫长而毫无特色的走廊。平面特征的数量也明显减少,因为不存在地面。当阳光直射在传感器视场中时,我们会从激光雷达观察到许多错误的检测结果,在数据收集过程中,这种情况大约占20%的时间。由于桥梁和城市建筑的上方,我们也只能获得间歇性的GPS接收。

    由于这些挑战,LOAM,LIOM和LIO-dom都无法在此测试中产生有意义的结果。与Park数据集中遇到的问题类似,由于海拔高度的变化,LIO-GPS在返回机器人的初始位置时无法闭合环路,这进一步激发了我们在LIO-SAM中使用环路闭合因子的动机。

    F. Benchmarking Results

    由于完整的GPS覆盖范围仅在P ark数据集中可用,因此我们向GPS测量历史显示均方根误差(RMSE)结果,这被视为地面真实情况。此RMSE错误未考虑沿z轴的错误。如表III所示,就GPS地面真相而言,LIO-GPS和LIO-SAM实现了相似的RMSE误差。请注意,我们可以通过完全访问所有GPS测量值来将这两种方法的误差至少降低一个数量级。

    但是,在许多地图设置中,始终无法完全访问GPS。我们的目的是设计一个可以在各种挑战性环境中运行的强大系统。

    表IV中显示了在所有五个数据集中配准一个激光雷达帧的三种竞争方法的平均运行时间。在所有测试中,LOAM和LIO-SAM被迫实时运行。换句话说,如果在激光雷达转速为10Hz的情况下运行时间超过100毫秒,则会丢弃某些激光雷达帧。 LIOM有无限的时间来处理每个激光雷达框架。如图所示,LIO-SAM使用的运行时间比其他两种方法少得多,这使其更适合部署在低功耗嵌入式系统上。

    我们还通过比实时提供数据更快的速度对LIO-SAM进行压力测试。 与数据回放速度为1倍实时时的结果相比,当LIO-SAM实现类似性能而没有失败时,记录了最大数据回放速度,并显示在表IV的最后一栏中。 如图所示,LIO-SAM能够比实时更快地处理数据,速度高达13倍。

    我们注意到,LIO-SAM的运行时间受特征图密度的影响更大,而受因子图中节点和因子数量的影响较小。例如,Park数据集是在特征丰富的环境中收集的,植被导致大量的特征,而Amsterdam数据集产生了稀疏的特征图。虽然Park检验的因子图由4,573个节点和9,365个因子组成,但是Amsterdam检验中的图具有23,304个节点和49,617个因子。尽管如此,与Park测试中的运行时相比,LIO-SAM在Amsterdam测试中使用的时间更少。

    5. CONCLUSIONS AND DISCUSSION

    我们已经提出了LIO-SAM,它是通过平滑和映射用于紧密耦合激光雷达惯性里程测量的框架,用于在复杂环境中执行实时状态估计和映射。通过在要素图上制定激光雷达惯性里程表,LIO-SAM特别适用于多传感器融合。附加的传感器测量值可以轻松地作为新因素纳入框架。

    提供绝对测量值的传感器(例如GPS,指南针或高度计)可用于消除长时间积累的激光雷达惯性里程表漂移,或者在功能较差的环境中。位置识别也可以轻松集成到系统中。为了提高系统的实时性能,我们提出了一种滑动窗口方法,该方法将旧的激光雷达框架边缘化以进行扫描匹配。关键帧被选择性地添加到因子图中,并且在生成激光雷达测距法和闭环因子时,新的关键帧仅被注册到一组固定大小的子关键帧。这种在本地而不是全球范围内的扫描匹配促进了LIO-SAM框架的实时性能。所提出的方法在各种环境下的三个平台上收集的数据集上进行了全面评估。结果表明,与LOAM和LIOM相比,LIO-SAM可以达到相似或更好的精度。未来的工作涉及在无人驾驶飞机上测试所提议的系统。

    更多相关内容
  • 我们提出一个框架用于通过SAM紧耦合雷达惯性里程计LIO-SAM,可实现高精度、实时的移动机器人轨迹估计和地图构建。LIO-SAM是基于因子图构建雷达惯性里程计,可以将大量的相对测量值、绝对测量值、回环等多种不同数据...


    本文参考:https://blog.csdn.net/weixin_37835423/article/details/111184679?spm=1001.2014.3001.5506

    作者简介:作者Tixiao Shan在2018年发表过LeGO-LOAM,当时他还在史蒂文斯理工学院读博士,19年毕业之后去了MIT做助理研究员。这篇文章LIO-SAM实际上是LeGO-LOAM的扩展版本,添加了IMU预积分因子和GPS因子,去除了帧帧匹配部分,然后更详细地描述了LeGO-LOAM帧图匹配部分的设计动机和细节。

    论文原文论文原文链接

    开源代码开源代码链接

    演示视频

    ICRA2021 | lio-sam作者开源新工作:激光雷达的鲁棒位置识别

    摘要

    我们提出一个框架用于通过SAM紧耦合雷达惯性里程计LIO-SAM,可实现高精度、实时的移动机器人轨迹估计和地图构建。LIO-SAM是基于因子图构建雷达惯性里程计,可以将大量的相对测量值、绝对测量值、回环等多种不同数据作为因子融入雷达惯性里程计系统中。通过imu预积分获得的运动估计可以用于点云的偏斜矫正和用于雷达里程计优化的初始值获得的雷达里程计结果又可以反过来估计imu的偏置。为了确保在实时性上有更高的性能,我们在进行位姿优化时边缘化掉了一些老的雷达数据,而不是将雷达点云与整个地图进行匹配。Scan-matching 在局部范围内进行而不是在全局范围内进行,可以有效的提高系统的实时性,原因是因为选择性引入关键帧和有效的滑动窗口策略(只将新的关键帧与固定尺寸的多个先前关键帧匹配)。提出的方法已经在拥有多种场景多种范围的三个测试平台采集的数据集上进行了广泛的评估测试。

    一、引言

    (这一段主要对视觉与激光SLAM方法进行了简单对比)

    对于一个可以反馈控制、避障、轨迹规划和其它应用的智能移动机器人,状态估计、定位和建图是基本的要求。使用基于视觉和基于雷达的传感技术,付出巨大的努力支持移动机器人进行6自由度状态估计的高实时性的SLAM技术中。基于视觉的方法一般是使用一个单目相机或立体相机,通过连续采集的照片三角化特征来确定相机的运动。虽然基于视觉的方法非常适合于位置识别,但对初始化、光照、范围等条件特别敏感,以致于单独使用它们去支持一个自动导航系统是不可靠的。另一方面,基于雷达的方法对于光照的改变是没有影响的。特别是最近长距离高分辨率的3D激光雷达(像Velodyne VLS-128和Ouster OS1-128)已经出现了,使得雷达变得更加适合于直接在3D空间获取精细的环境信息。因此,这篇论文集中在基于雷达的状态估计和建图方法。

    这一段主要介绍了LOAM方法存在的一些限制

    许多的基于激光雷达的状态估计和建图方法在最近20年被提出来.其中LOAM(Lidar Odometry And Mapping)方法对于小漂移实时状态估计和建图是最被广泛使用的。LOAM使用一个雷达和一个imu实现了最好的性能,自从它发布在KITTI odometry benchmark site上,就一直排在最上面。虽然LOAM算法很成功,但依然存在一些限制。首先是它保存地图在一个全局体素地图中,其次它没法进行回环检测,也没法融入其它的像GPS这样的绝对测量数据进行位姿矫正。而且LOAM也存在在大场景测试中偏移问题,原因是因为它核心还是一个基于scan-matching的方法。

    这一段介绍本文提出的LIO-SAM方法,及主要贡献

    在这篇论文中,我们提出了一个通过SAM紧耦合雷达惯性里程计框架LIO-SAM,用于解决上述的问题。我们假设了一个非线性运动模型用于点云的偏斜矫正,使用原始IMU测量值估计激光雷达扫描期间的传感器运动。该运动估计除了用于点云的偏斜矫正而且还可以作为雷达里程计优化过程中的初始位姿估计。获得的雷达里程计结果反过来被用于估计因子图中imu的偏置。通过引入因子图用于机器人的轨迹估计,我们可以进行有效的基于雷达和imu作为测量值的传感器融合,比方说引入位置识别,引入GPS位置信息和指南针的朝向信息等绝对测量值。这些来自不同数据源的多种因子集合将会在因子图中共同优化。此外,我们在进行位姿优化时,边缘化掉老的雷达帧,而不是像LOAM那样将雷达帧同整个全局点云进行匹配Scan-matching是在局部范围而不是全局范围内执行,明显的提升了系统的实时性,因为采取了关键帧的选择引入和有效的滑动窗口方法(只将新的关键帧与固定尺寸的多个先前关键帧匹配)。我们工作的主要贡献可以被总结如下:

    • 一个基于因子图的紧耦合的雷达惯性里程计框架,适合多传感器融合和全局优化;
    • 一个有效的基于局部滑窗的scan-matching方法,其通过匹配选择的关键帧和估计数量的先前关键帧策略,确保了系统的实时性;
    • 提出的框架已在多范围、多设备和多种环境下进行了广泛的测试验证。

    二、相关工作

    雷达里程计典型实现是通过使用scan-matching方法找到两个相邻帧之间的相对转换关系,这样的方法有ICP、GICP等。除了匹配完整点云,基于特征匹配的方法由于其计算的有效性已经变得流行起来。例如,在[5]中提出了一个基于平面特征匹配的雷达里程计。假设运行场景在结构化的环境中,其从点云中提取平面特征,并使用最小二乘法对特征进行匹配。[6]是一个基于线特征的雷达里程计,其是从原始点云里面随机分割线特征,然后再进行匹配。然而由于现代3D激光雷达使用旋转机械件和设备的移动,导致点云经常会有偏斜。因此,仅仅使用激光雷达进行位姿估计是不理想的,原因是因为使用偏斜的点云或特征进行匹配必将会造成大的漂移。

    因此,雷达通常是与其它传感器(例如IMU、GPS)融合使用来进行状态估计和建图。这样一种利用传感器融合思路的方法可以分为两类:松耦合融合和紧耦合融合。例如LOAM[1]算法,其使用IMU进行点云的偏斜矫正和为扫描匹配提供运动先验,然而,IMU并没有引入到算法的优化处理过程中,因此LOAM可以被归类为松耦合方法。另一个轻量的、基于地面优化的雷达里程计和建图算法LeGO-LOAM[7]被用于地面车辆的建图任务,该算法融合IMU测量的方法同LOAM是一致的。一种更加流行的松耦合方法是使用EKF,例如,[9]-[13]在优化阶段使用EKF集成了来自激光雷达、IMU和选择性GPS的测量数据,用于机器人状态估计

    紧耦合系统通常能够提供更高的精度,并且是当前的主要研究方向。在[15]方法中,预积分的IMU测量值被用于对点云进行偏斜矫正。[16]提出了一种以机器人为中心的雷达惯性里程计LINS,其用一种紧耦合的方式使用ESKF算法对机器人的状态进行矫正,由于缺乏其它可用于状态估计的传感器,该算法在经过长时间导航后会有漂移问题。[17]提出了一个紧耦合的雷达惯性里程计框架LIOM(雷达惯性里程计建图),其共同优化雷达和IMU的测试值,实现了比LOAM更好的精度。因为LIOM的设计是处理所有的传感器测量,因此,它不能实时运行——在我们的测试中,它以大约0.6×的实时速度运行。

    三、基于SAM的雷达惯性里程计

    A.系统概述

    在这里插入图片描述
    我们首先定义了我们在整个论文中使用的坐标系和符号。我们定义W为世界坐标系,B为机器人坐标系。为了方便,我们也假设IMU坐标系和机器人本体坐标系是一致的。机器人的状态x可按下面定义:
    在这里插入图片描述
    在这里插入图片描述
    我们提出的系统概述如图1所示。系统接收来自3D雷达、IMU和可选择的GPS等传感器的数据。我们争取使用这些传感器数据估计出机器人的状态和轨迹。这个状态估计问题可以被构造为一个最大化后验问题(MAP)。我们使用一个因子图去刻画这个问题,因为它是最适合进行贝叶斯网络的推断。由于高斯模型假设,我们的MAP推断问题与求解非线性最小二乘问题是等价的。注意,我们的系统可以不失一般性地引入其它传感器的测量数据,例如高度计的海拔数据、指南针的朝向数据。

    对于因子图的构造,我们对一个变量引入四种因子。该表示在指定时刻机器人状态的变量对应因子图中的节点。四种类型的因子分别是:(a) IMU预积分因子,(b) 雷达里程计因子,©GPS因子和(d)回环因子。当机器人的位姿改变超过人为设定的阈值,则添加新的机器人状态节点x到因子图中。一旦插入了新的节点,因子图就会使用iSAM2进行优化。下面部分将介绍产生这些因子的过程。

    B. IMU预积分因子

    来自于IMU的角速度和加速度测量值定义如等式(2)和(3):
    在这里插入图片描述
    在这里插入图片描述
    我们现在可以使用IMU测量值进行机器人运动的推断。机器人在时刻 t + Δt 的速度、位置和姿态计算方法如下:

    在这里插入图片描述

    式中Rt为从坐标系B到坐标系W的旋转,并且我们假设基准坐标系B的角速度和加速度在上述积分过程中是不变的。
    在这里插入图片描述
    由于篇幅的限制,读者们可以阅读[20]获得更多的关于公式(7)~(9)的推导。除了有效性外,应用IMU预积分也自然的给我们一种类型的因子图约束-IMU预积分因子。IMU的偏置是同雷达里程计因子一起在因子图中共同优化。

    C. 雷达里程计因子

    这段介绍了基于边和面的特征提取

    当接收到一个新的雷达扫描数据,我们首先通过评估一个局部区域中点的凹凸程度来提取边和面特征点,当点的凹凸程度较大,则将这些点分类为边特征点。类似的,当凹凸程度较小时,这些点将被分类为面特征点。我们将从在时刻i获取的雷达点云中提取的边特征和面特征的集合表示为F ie,F ip。在时刻i ii提取的所有特征点构成了雷达帧F i = [ F i e , F i p ]注意雷达帧F是表达在B坐标系下.关于特征点提取的详细描述可查看[1][7]。

    这一段介绍了本文添加关键帧的方法

    如果将每一个雷达帧进行计算并添加因子到因子图中进行优化是很难计算的,因此我们使用关键帧策略,该策略已在视觉SLAM领域广泛使用。当机器人的位姿相对于先前状态x i 的改变超过用户设定的阈值,则选择对应的雷达帧Fi+1作为新的关键帧,新的关键帧与因子图中的一个新的机器人状态节点x i+1相关联的,相邻两个关键帧之间的雷达帧被丢弃。这样添加关键帧的方法不仅实现了地图密度和内存消耗的平衡,而且帮助维护了一个相对稀疏的适用于实时非线性优化的因子图。在我们的工作中,用于添加关键帧的位置和旋转改变阈值参数分别设置为1米和10度。

    下面介绍雷达里程计因子的产生步骤

    假设我们在因子图中添加一个新的状态节点xi+1其相关的雷达关键帧为Fi+1则产生一个雷达里程计因子的步骤如下:

    1)体素地图的子关键帧: 我们使用滑窗的方法产生一个点云地图,该地图包含固定数量的最近的雷达帧。而不是直接通过两个相邻的雷达帧来获得相对转换关系,我们提取了n个最近的关键帧,我们称之为子关键帧,用于估计相对转换关系。子关键帧的集合{Fi−n , . . . , Fi }然后使用和他们相关的转换关系{Ti−n , . . . , Ti }将他们转换到世界坐标系下。转换过的子关键帧融合到一起构成一个体素地图Mi(体素地图是在世界坐标系下的)。因为我们在先前特征提取步骤提取了两种类型的特征,因此 Mi 是由两个子体素地图组成,分别是边特征体素地图Mie和面特征体素地图Mip,雷达帧和体素地图是按下面彼此相关的:
    在这里插入图片描述

    使用降采样去删除重复特征点

    在这里插入图片描述
    2)扫描匹配

    论文[1]:J. Zhang and S. Singh, “Low-drift and Real-time Lidar Odometry and
    Mapping
    ,” Autonomous Robots, vol. 41(2): 401-416, 2017.

    在这里插入图片描述

    ​3) 相对转换:一个特征和它相关的边或者平面之间的距离可按下式计算:

    在这里插入图片描述
    在这里插入图片描述

    D. GPS因子

    虽然我们可以只利用IMU预积分因子和雷达里程计因子获得可靠的状态估计和建图,但系统还是会在长时间导航任务中遇到漂移问题。为了解决这个问题,我们可以引入能够提供绝对测量的传感器来消除漂移。这样的传感器包括高度计、指南针和GPS。在这为了说明目的,我们讨论GPS,因为它在真实世界的导航系统中被广泛的使用。

    当我们接收到GPS测量值时,我们首先使用[21]中提出的方法将这些值转换到局部笛卡尔坐标系。当添加一个新的节点到因子图中时,我们然后将一个新的GPS因子关联到这个节点。如果GPS信号不是同雷达帧时间同步的,我们基于雷达帧的时间戳线性插值出GPS测量值

    我们注意到当GPS数据可利用时,我们没有必要不断地添加GPS因子,因为雷达惯性里程计的漂移增长缓慢。在实际应用中,我们只当估计的位置协方差是比接收到的GPS位置协方差大的时候,我们才添加GPS因子

    E. 回环因子

    由于因子图的利用,回环因子可以无缝引入我们提出的系统中,不像LOAM和LIOM。为了说明的目的,我们介绍了和实现了一个简单有效的基于欧拉距离的回环检测方法。我们也注意到我们提出的框架是兼容其它回环检测方法的,例如[22]和[23],其产生点云描述子并使用这些描述子进行位置识别。

    这一段引出回环因子

    在这里插入图片描述

    添加回环因子的作用

    在实际应用中,当GPS是可得到的唯一的绝对传感器,我们发现添加回环因子在矫正机器人的海拔漂移问题非常有效。这是因为GPS的高程测量非常不准确,在没有回环的情况下,我们的测试中出现的高度误差接近100m。

    四. 实验

    我们现在描述了一系列的实验,以定性和定量地分析我们提出的框架。本文中使用的传感器套件包括一个Velodyne VLP-16激光雷达、一个 MicroStrain 3DM-GX5-25 IMU和一个Reach M GPS。为了进行验证,我们在不同的规模、平台和环境中收集了5个不同的数据集。这些数据集分别被称为旋转、步行、校园、公园和阿姆斯特丹。传感器安装平台如图2所示。前三个数据集是使用麻省理工学院校园里定制的手持设备收集的。公园的数据集是在一个被植被覆盖的公园里收集的,使用的是无人驾驶的地面车辆(UGV)——the Clearpath Jackal。最后一个数据集,阿姆斯特丹,是通过将传感器安装在一艘船上并在阿姆斯特丹的运河中巡航而收集的。这些数据集的细节如表一所示。
    在这里插入图片描述
    在这里插入图片描述
    我们将所提出的LIO-SAM框架与LOAM和LIOM进行了比较。在所有的实验中,LOAM和LIO-SAM都被迫实时运行。另一方面,LIOM有无限的时间来处理每一个传感器的测量。所有的方法都是在C++中实现的,并在配备Intel i7-10710UCPU的笔记本电脑上执行,使用Ubuntu Linux中的机器人操作系统(ROS)。我们注意到,只有CPU用于计算,而没有启用并行计算。我们的LIO-SAM实现可以在Github上免费获得。所执行的实验的补充细节,包括所有测试的完整可视化,可以在下面的链接中找到。

    A. 旋转数据集

    在这个测试中,我们的重点是评估当只有IMU预集成和激光雷达测速因子被添加到因子图中时,我们的框架的鲁棒性。旋转数据集由手持传感器套件并在静止不动时执行一系列积极的旋转操作的用户收集。本试验中遇到的最大转速为133.7◦/s。用结构填充的测试环境如图3(a)所示。从LOAM和LIO-SAM中获得的地图分别如图3(b)和©所示。因为LIOM使用了来自[25]的相同的初始化途径,所以它继承了与视觉-惯性SLAM相同的初始化敏感性,并且不能使用这个数据集正确地初始化。由于它未能产生有意义的结果,LIOM的地图没有显示出来。如图所示,与LOAM相比,LIO-SAM地图保留了更精细的环境结构细节。这是因为LIO-SAM能够在SO(3)中精确地注册每个激光雷达帧,即使机器人经历了快速旋转。
    在这里插入图片描述

    B.步行数据集

    该测试旨在评估我们的方法在SE(3)中进行正向平移和旋转时的性能。在这个数据集中遇到的最大平移速度和旋转速度分别为1.8m/s和213.9◦/s。在数据收集过程中,用户持有如图2(a)所示的传感器套件。快速穿过麻省理工学院校园。(图4(a))。在本测试中,LOAM地图,如图4(b)所示。当遇到主动旋转时,会在多个位置发散。LIOM在此测试中优于LOAM。然而,其地图,如图4©所示。在不同的位置仍然略有不同,并由许多模糊的结构组成。因为LIOM被设计为处理所有的传感器测量值,所以它只在0.56×下实时运行,而其他方法则是实时运行的。最后,LIO-SAM优于这两种方法,并产生了一个与可用的谷歌地球图像一致的地图。

    在这里插入图片描述

    C. 校园数据集

    在这里插入图片描述
    本测试旨在展示引入GPS和回路闭合因子的好处。为了做到这一点,我们故意禁用在图中插入GPS和环关闭因子。当GPS和环路闭合因子都被禁用时,我们的方法被称为LIO-odom,它仅利用IMU预积分和激光雷达测速因子。当使用GPS因子时,我们的方法被称为LIO-GPS,它使用IMU预积分、激光雷达测距和GPS因子进行图的构建。LIO-SAM在可用时使用所有因素。

    为了收集这个数据集,用户使用手持设备在麻省理工学院校园周围走动,并返回到相同的位置。由于地图区域有大量的建筑和树木,GPS接收很少可用,而且大多不准确。在过滤掉不一致的GPS测量值后,GPS可用的区域在图5(a)中以绿色线段展示。这些区域对应于少数没有被建筑物或树木包围的区域。

    LOAM、LIO-odom、LIOGPS和LIO-SAM的估计轨迹如图5(a)所示。由于LIOM未能正确初始化并产生有意义的结果,因此没有显示LIOM的结果。如图所示,与其他所有方法相比,LOAM的轨迹显著漂移。如果没有对GPS数据的校正,lio-odom的轨迹开始在地图的右下角出现明显的漂移。在GPS数据的帮助下,LIO-GPS可以在存在时纠正漂移。然而,在数据集的后面部分中没有GPS数据。因此,当由于漂移导致机器人返回到起始位置时,LIO-GPS无法关闭循环。另一方面,LIO-SAM可以通过向图中添加环路闭合因子来消除漂移。LIO-SAM的地图与谷歌地图对齐一致,如图5(b)所示。当机器人返回起点时,所有方法的相对平移误差如表二所示。
    在这里插入图片描述

    D. 公园数据集

    在这个测试中,我们将传感器安装在UGV上,并驾驶车辆沿着森林徒步旅行路线行驶。机器人在驾驶40分钟后返回到初始位置。UGV在三个路面上驱动:沥青、草地覆盖的地面和泥土覆盖的小径。由于缺乏悬挂系统,该机器人在非沥青路面上行驶时,会经历低振幅但高频的振动。

    为了模拟一个具有挑战性的映射场景,我们只在机器人处于开阔区域时使用GPS测量,如图6(a)中的绿色段表示。这种映射场景代表了一项任务,其中机器人必须映射多个GPS拒绝的区域,并定期返回具有GPS可用性的区域,以纠正漂移。

    在这里插入图片描述

    与之前的测试结果相似,LOAM、LIOM和LIO-odom存在显著的漂移,因为没有绝对的校正数据。此外,LIOM只在0.67×下实时运行,而其他方法则实时运行。虽然LIO-GPS和LIO-SAM的运动轨迹在水平平面上一致,它们的相对翻译误差是不同的(表二)。由于没有可靠的绝对高程测量值,LIO-GPS存在高度漂移问题,在返回机器人的初始位置时无法关闭回路。LIO-SAM没有这样的问题,因为它利用环路闭合因子来消除漂移。

    E. 阿姆斯特丹数据集

    最后,我们把传感器套件安装在船上,沿着阿姆斯特丹的运河航行了3个小时。虽然在这个测试中,传感器的运动相对平滑,但由于几个原因,绘制管道仍然具有挑战性。运河上的许多桥梁都构成了退化的场景,因为当船在运河下面时,几乎没有什么有用的特征,类似于穿过一条长长的、没有特色的走廊。平面特征的数量也明显减少,因为地面不存在。当阳光直射在传感器视场中时,我们从激光雷达中观察到许多错误的检测,这在数据收集过程中发生了大约20%的时间。由于头顶上存在桥梁和城市建筑,我们也只能获得间歇性的GPS接收。

    由于这些挑战,LOAM、LIOM和LIO-odom都未能在这个测试中产生有意义的结果。与在Park数据集中遇到的问题类似,由于高度的漂移,LIO-GPS在返回到机器人的初始位置时无法关闭环路,这进一步激励了我们在LIO-SAM中使用环路闭合因子。

    F. 基准测试结果

    在这里插入图片描述
    由于完整的GPS覆盖仅在Park数据集中可用,我们将GPS测量历史的均方根误差结果视为地面真值。这个RMSE误差没有考虑到沿z轴的误差。正如图表3所示,LIO-GPS和LIO-SAM对GPS地面真值实现类似的RMSE误差。请注意,我们可以通过完全访问这两种方法的所有GPS测量,使它们完全减少至少一个数量级。然而,在许多映射设置中并不总是可用。我们的目的是设计一个可以在各种具有挑战性的环境中运行的健壮的系统。

    在所有五个数据集上注册一个激光雷达帧的平均运行时如表四所示。在所有的测试过程中,LOAM和LIO-SAM被迫实时运行。换句话说,当激光雷达的旋转速率为10Hz时,如果运行时需要超过100ms,那么一些激光雷达帧就会被丢弃。LIOM有无限的时间来处理每个激光雷达帧。如图中所示,LIO-SAM使用了明显不少于其他两种方法的运行时间,这使得它更适合部署在低功耗嵌入式系统上。
    在这里插入图片描述
    我们还对LIO-SAM进行压力测试,比实时快。最大数据回放速度被记录和展示在表四的最后一列中当LIO-SAM也实现了类似的性能与当数据回放速度为1×实时性相比。如图所示,LIO-SAM能够比实时处理数据快到13×。

    我们注意到,LIO-SAM的运行时受特征图密度的影响更显著,受因子图中节点数量和因子的影响较小。例如,公园数据集是在一个特征丰富的环境中收集的,其中植被产生大量的特征,而阿姆斯特丹数据集产生一个更稀疏的特征图。Park测试的因子图由4573个节点和9365个因子组成,而阿姆斯特丹测试的图有23,304个节点和49,617个因子。尽管如此,与公园测试中的运行时相比,LIO-SAM在阿姆斯特丹测试中使用的时间更少。

    在这里插入图片描述

    五. 总结

    我们提出了一个基于SAM的紧耦合雷达惯性里程计框架LIO-SAM,为了实现在复杂环境中实时的状态估计和建图。通过在因子图上构造雷达惯性里程计,LIO-SAM是特别适合于多传感器的融合。除了传感器的测量可以轻松的作为新的因子添加到框架中。提供绝对测量值的传感器(高度计、指南针、GPS)可以被使用去消除长期运行引入的漂移,特别是在特征缺少的环境中。位置识别也可以轻松的融入系统。为了提高系统的实时性,我们提出了一种滑窗方法,我们在扫描匹配过程中,边缘化掉老的雷达帧。关键帧是被选择性的添加到因子图中,并且当雷达里程计和回环因子产生时,新的关键帧只会同固定数量的子关键帧进行匹配。这种扫描匹配在局部范围而不是全局范围的策略,使得LIO-SAM框架能够实时运行。我们提出的方法是在三个平台的数据集上全面的进行评估过,结果显示当同LOAM和LIOM比较时,LIO-SAM可以实现更高的精度。

    展开全文
  • LIO-SAM: 论文翻译

    2021-12-24 15:21:03
    目录LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and MappiAbstractI. IntroductionII. Related WorkIII. Lidar Inertial Odometry Via Smoothing and MappingA. System OverviewB. IMU ...

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

    代码:https://github.com/TixiaoShan/LIO-SAM
    视频:https://www.youtube.com/watch?v=A0H8CoORZJU

    Abstract

    We propose a framework for tightly-coupled lidar inertial odometry via smoothing and mapping, LIO-SAM, that achieves highly accurate, real-time mobile robot trajectory estimation and map-building. LIO-SAM formulates lidar-inertial odometry atop a factor graph, allowing a multitude of relative and absolute measurements, including loop closures, to be incorporated from different sources as factors into the system. The estimated motion from inertial measurement unit (IMU) pre-integration de-skews point clouds and produces an initial guess for lidar odometry optimization. The obtained lidar odometry solution is used to estimate the bias of the IMU. To ensure high performance in real-time, we marginalize old lidar scans for pose optimization, rather than matching lidar scans to a global map. Scan-matching at a local scale instead of a global scale significantly improves the real-time performance of the system, as does the selective introduction of keyframes, and an efficient sliding window approach that registers a new keyframe to a fixed-size set of prior “sub-keyframes.” The proposed method is extensively evaluated on datasets gathered from three platforms over various scales and environments.
    我们提出了一个通过smoothing and mapping(gtsam优化库)实现紧耦合激光雷达惯性里程计的框架LIO-SAM,该framework可实现高精度、实时的移动机器人轨迹估计和地图构建。LIO-SAM在因子图( factor graph)上实现了激光雷达惯性里程计,允许从different sources将大量相对和绝对测量(包括回环)作为因子整合到系统中。可以通过IMU预积分的运动估计消除点云的畸变(de-skew),并生成激光雷达里程计优化的 initial guess。获得的激光雷达里程计的解被用于估计IMU的偏差(bias)。为了确保高精度的实时性,我们将旧的激光雷达scans边缘化以优化姿态,而不是将激光雷达scans与全局地图匹配。在局部尺度而不是全局尺度上进行scan匹配可以显著提高系统的实时性能,引入关键帧以及滑动窗口的方法可以将新关键帧对齐到固定大小的prior“子关键帧”集合(位于滑窗中的关键帧)中。我们在三个平台收集的在不同规模和环境生成的数据集上对所提出的方法进行了广泛的评估。

    I. Introduction

    State estimation, localization and mapping are fundamental prerequisites for a successful intelligent mobile robot, required for feedback control, obstacle avoidance, and planning, among many other capabilities. Using vision-based and lidar-based sensing, great efforts have been devoted to achieving high-performance real-time simultaneous localization and mapping (SLAM) that can support a mobile robot’s six degree-of-freedom state estimation. Vision-based methods typically use a monocular or stereo camera and triangulate features across successive images to determine the camera motion. Although vision-based methods are especially suitable for place recognition, their sensitivity to initialization, illumination, and range make them unreliable when they alone are used to support an autonomous navigation system. On the other hand, lidar-based methods are largely invariant to illumination change. Especially with the recent availability of long-range, high-resolution 3D lidar, such as the Velodyne VLS-128 and Ouster OS1-128, lidar becomes more suitable to directly capture the fine details of an environment in 3D space. Therefore, this paper focuses on lidar-based state estimation and mapping methods.

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

    Many lidar-based state estimation and mapping methods have been proposed in the last two decades. Among them, the lidar odometry and mapping (LOAM) method proposed in [1] for low-drift and real-time state estimation and mapping is among the most widely used. LOAM, which uses a lidar and an inertial measurement unit (IMU), achieves state-ofthe-art performance and has been ranked as the top lidarbased method since its release on the KITTI odometry benchmark site [2]. Despite its success, LOAM presents some limitations - by saving its data in a global voxel map, it is often difficult to perform loop closure detection and incorporate other absolute measurements, e.g., GPS, for pose correction. Its online optimization process becomes less efficient when this voxel map becomes dense in a feature-rich environment. LOAM also suffers from drift in large-scale tests, as it is a scan-matching based method at its core
    在过去的二十年中,人们提出了许多基于激光雷达的状态估计和建图方法。其中,在[1]中提出的用于低漂移和实时状态估计和映射的激光雷达里程计和建图(LOAM)方法是应用最广泛的方法之一。LOAM使用激光雷达和惯性测量单元(IMU),具有最先进的性能,自其在KITTI里程计基准站点发布以来,一直被列为基于激光的最佳方法[2]。尽管取得了成功,但LOAM仍存在一些局限性—通过将其数据保存在全局体素图(voxel map)中,通常很难执行回环检测并结合其他绝对测量(例如GPS)进行位姿校正。当voxel map在特征丰富的环境中变得稠密时,其在线优化的效率会降低。在大型场景的测试中,LOAM也会受到漂移的影响,因为它的核心是一种基于扫描匹配的方法。

    In this paper, we propose a framework for tightly-coupled lidar inertial odometry via smoothing and mapping, LIO-SAM, to address the aforementioned problems. We assume a nonlinear motion model for point cloud de-skew, estimating the sensor motion during a lidar scan using raw IMU measurements. In addition to de-skewing point clouds, the estimated motion serves as an initial guess for lidar odometry optimization. The obtained lidar odometry solution is then used to estimate the bias of the IMU in the factor graph. By introducing a global factor graph for robot trajectory estimation, we can efficiently perform sensor fusion using lidar and IMU measurements, incorporate place recognition among robot poses, and introduce absolute measurements, such as GPS positioning and compass heading, when they are available. This collection of factors from various sources is used for joint optimization of the graph. Additionally, we marginalize old lidar scans for pose optimization, rather than matching scans to a global map like LOAM. Scan-matching at a local scale instead of a global scale significantly improves the real-time performance of the system, as does the selective introduction of keyframes, and an efficient sliding window approach that registers a new keyframe to a fixedsize set of prior “sub-keyframes.” The main contributions of our work can be summarized as follows:
    在本文中,我们提出了一个通过smoothing and mapping的紧耦合激光雷达惯性里程计框架LIO-SAM,以解决上述问题。我们假定一个点云去畸变(de-skew)的非线性运动模型,使用原始IMU测量值估计激光雷达扫描期间的传感器运动。除了对点云去畸变外,估计的运动还可作为激光雷达里程计优化的initial guess。然后使用获得的激光雷达里程计的解来估计因子图中IMU的偏差。通过引入用于机器人轨迹估计的全局因子图,我们可以使用激光雷达和IMU测量有效地执行传感器融合,在机器人位姿之间加入位置识别,并在可用时引入绝对测量,如GPS定位和指南针朝向。This collection of factors from various sources用于因子图的联合优化。此外,为了优化姿态,我们将旧的激光雷达scans边缘化,而不是将scans与global map匹配(不同于LOAM的方法)。在局部尺度而不是全局尺度上进行扫描匹配,可以显著提高系统的实时性能,选择性引入关键帧以及高效的滑动窗口方法(将新关键帧对齐到先前“子关键帧”的固定大小set)也是如此。我们工作的主要贡献可总结如下:

    • A tightly-coupled lidar inertial odometry framework built atop a factor graph, that is suitable for multi-sensor
    fusion and global optimization.
    • An efficient, local sliding window-based scan-matching approach that enables real-time performance by registering selectively chosen new keyframes to a fixed-size set of prior sub-keyframes.
    • The proposed framework is extensively validated with tests across various scales, vehicles, and environments.
    •一个紧耦合的激光雷达惯性里程计框架,构建在因子图之上,适用于多传感器融合和全局优化。
    •一种高效、基于局部滑动窗口的扫描匹配方法,通过选择性地将新关键帧对齐到固定大小的prior子关键帧集中,实现实时性能。
    •提出的框架在各种规模、车辆和环境的测试中得到广泛验证。

    II. Related Work

    III. Lidar Inertial Odometry Via Smoothing and Mapping

    A. System Overview

    We first define frames and notation that we use throughout the paper. We denote the world frame as W and the robot body frame as B. We also assume the IMU frame coincides with the robot body frame for convenience. The robot state x can be written as:
    我们首先定义了在本文中使用的框架和符号。我们将世界坐标系表示为W,机器人body坐标系表示为B。为了方便起见,我们还假设IMU坐标系与body坐标系重合。机器人状态x(15维9+3+1+2)可以写为:
    在这里插入图片描述
    where R ∈ S O ( 3 ) \mathbf{R} \in S O(3) RSO(3) is the rotation matrix, p ∈ R 3 \mathbf{p} \in \mathbb{R}^{3} pR3 is the position vector, v \mathbf{v} v is the speed, and b \mathbf{b} b is the IMU bias. The transformation T ∈ S E ( 3 ) \mathbf{T} \in S E(3) TSE(3) from B \mathbf{B} B to W \mathbf{W} W is represented as T = [ R ∣ p ] \mathbf{T}=[\mathbf{R} \mid \mathbf{p}] T=[Rp].
    其中, R ∈ S O ( 3 ) \mathbf{R} \in S O(3) RSO(3) 是旋转矩阵, p ∈ R 3 \mathbf{p} \in \mathbb{R}^{3} pR3是位置向量, v \mathbf{v} v是速度, b \mathbf{b} b是IMU偏差。从 B \mathbf{B} B W \mathbf{W} W的变换 T ∈ S E ( 3 ) \mathbf{T} \in S E(3) TSE(3)被表示为 T = [ R ∣ p ] \mathbf{T}=[\mathbf{R} \mid \mathbf{p}] T=[Rp]

    An overview of the proposed system is shown in Figure 1 . The system receives sensor data from a 3D lidar, an IMU and optionally a GPS. We seek to estimate the state of the robot and its trajectory using the observations of these sensors. This state estimation problem can be formulated as a maximum a posteriori (MAP) problem. We use a factor graph to model this problem, as it is better suited to perform inference when compared with Bayes nets. With the assumption of a Gaussian noise model, the MAP inference for our problem is equivalent to solving a nonlinear least-squares problem [18]. Note that without loss of generality, the proposed system can also incorporate measurements from other sensors, such as elevation from an altimeter or heading from a compass.
    系统的overview如图1所示。该系统从3D激光雷达、IMU和可选的GPS中接收传感器数据。我们试图利用这些传感器的观测来估计机器人的状态及其轨迹。这种状态估计问题可以表述为最大后验概率(MAP)问题。我们使用因子图来模拟这个问题,因为与贝叶斯网络相比,因子图更适合进行推理(inference)。在高斯噪声模型的假设下,我们的最大后验问题等价于解决非线性最小二乘问题[18]。请注意,在不丢失通用性的情况下,提出的系统还可以包含来自其他传感器的测量,例如来自高度计的高度信息或来自指南针的方向信息。
    在这里插入图片描述Fig. 1: The system structure of LIO-SAM. The system receives input from a 3D lidar, an IMU and optionally a GPS. Four types of factors are introduced to construct the factor graph.: (a) IMU preintegration factor, (b) lidar odometry factor, © GPS factor, and (d) loop closure factor. The generation of these factors is discussed in Section III.
    图1:LIO-SAM的系统结构。该系统接收来自3D激光雷达、IMU和(可选)GPS的输入。引入四种类型的因子来构建因子图:(a) IMU预积分系数,(b)激光雷达里程计系数,(c)GPS系数和(d)回环系数。Section III讨论了这些因素的产生。

    We introduce four types of factors along with one variable type for factor graph construction. This variable, representing the robot’s state at a specific time, is attributed to the nodes of the graph. The four types of factors are: (a) IMU preintegration factors, (b) lidar odometry factors, © GPS factors, and (d) loop closure factors. A new robot state node x is added to the graph when the change in robot pose exceeds a user-defined threshold. The factor graph is optimized upon the insertion of a new node using incremental smoothing and mapping with the Bayes tree (iSAM2) [19]. The process for generating these factors is described in the following sections.
    我们介绍了四种类型的因子以及一种用于因子图构造的变量类型。此变量表示机器人在特定时间的状态,主要来自于因子图的节点。这四类因素是:(a)IMU预积分系数,(b)激光雷达里程计系数,(c)GPS系数,以及(d)回环系数。当机器人姿势的变化超过用户定义的阈值时,新的机器人状态节点x将添加到因子图中。在插入新节点时,使用增量平滑(incremental smoothing )和贝叶斯树(iSAM2)映射优化因子图[19]。生成这些因素的过程将在以下章节中描述。

    B. IMU Preintegration Factor

    The measurements of angular velocity and acceleration from an IMU are defined using Eqs. 2 and 3:
    IMU的角速度和加速度测量值使用等式2,3定义:
    在这里插入图片描述
    where ω t ^ \hat{ω_t} ωt^ and a t ^ \hat{a_t} at^ are the raw IMU measurements in B B B at time t t t. ω t ^ \hat{ω_t} ωt^ and a t ^ \hat{a_t} at^ are affected by a slowly varying bias b t b_t bt and white noise n t n_t nt. R t B W R^{BW}_t RtBW is the rotation matrix from W W W to B B B. g g g is the constant gravity vector in W W W
    其中 ω t ^ \hat{ω_t} ωt^ a t ^ \hat{a_t} at^ t t t时刻在 B B Body坐标系下原始的IMU测量值。 ω t ^ \hat{ω_t} ωt^ a t ^ \hat{a_t} at^受到缓慢变化的偏差 b t b_t bt和白噪声 n t n_t nt的影响。 R t B W R^{BW}_t RtBW是从 W W W B B B的旋转矩阵。 g g g是在World坐标系下的恒定重力矢量。

    We can now use the measurements from the IMU to infer the motion of the robot. The velocity, position and rotation of the robot at time t + ∆ t t + ∆t t+t can be computed as follows:
    我们现在可以使用IMU的测量值来推断机器人的运动。机器人在时间 t + ∆ t t + ∆t t+t的速度、位置和旋转的计算方法如下:
    在这里插入图片描述

    对比VINS,LIO-SAM这里也是在世界坐标系下(vins中认为加速度与重力加速度同向,所以用+,LIO-SAM假定为反向,所以是-, 其实正负无所谓,依据个人定义而定)VINS中采用了积分的形式,只是表达方式不同,与LIO-SAM的公式含义是相同的。最多一个是中值积分,另一个是欧拉积分
    p b k + 1 w = p b k w + v b k w Δ t k + ∬ t ∈ [ t k , t k + 1 ] ( R t w ( a ^ t − b a t − n a ) − g w ) d t 2 \mathbf{p}_{b_{k+1}}^{w}=\mathbf{p}_{b_{k}}^{w}+\mathbf{v}_{b_{k}}^{w} \Delta t_{k}+\iint_{t \in\left[t_{k}, t_{k+1}\right]}\left(\mathbf{R}_{t}^{w}\left(\hat{\mathbf{a}}_{t}-\mathbf{b}_{a_{t}}-\mathbf{n}_{a}\right)-\mathbf{g}^{w}\right) d t^{2} pbk+1w=pbkw+vbkwΔtk+t[tk,tk+1](Rtw(a^tbatna)gw)dt2
    v b k + 1 w = v b k w + ∫ t ∈ [ t k , t k + 1 ] ( R t w ( a ^ t − b a t − n a ) − g w ) d t \mathbf{v}_{b_{k+1}}^{w}=\mathbf{v}_{b_{k}}^{w}+\int_{t \in\left[t_{k}, t_{k+1}\right]}\left(\mathbf{R}_{t}^{w}\left(\hat{\mathbf{a}}_{t}-\mathbf{b}_{a_{t}}-\mathbf{n}_{a}\right)-\mathbf{g}^{w}\right) d t vbk+1w=vbkw+t[tk,tk+1](Rtw(a^tbatna)gw)dt
    q b k + 1 w = q b k w ⊗ ∫ t ∈ [ t k , t k + 1 ] 1 2 Ω ( ω ^ t − b w t − n w ) q t b k d t \mathbf{q}_{b_{k+1}}^{w}=\mathbf{q}_{b_{k}}^{w} \otimes \int_{t \in\left[t_{k}, t_{k+1}\right]} \frac{1}{2} \boldsymbol{\Omega}\left(\hat{\boldsymbol{\omega}}_{t}-\mathbf{b}_{w_{t}}-\mathbf{n}_{w}\right) \mathbf{q}_{t}^{b_{k}} d t qbk+1w=qbkwt[tk,tk+1]21Ω(ω^tbwtnw)qtbkdt
    其中, Ω ( ω ) = [ ω 0 ] R = [ − ⌊ ω ⌋ x ω − ω T 0 ] , ⌊ ω ⌋ × = [ 0 − ω z ω y ω z 0 − ω x − ω y ω x 0 ] \boldsymbol{\Omega}(\boldsymbol{\omega})=\left[\begin{array}{c}\boldsymbol{\omega} \\ 0\end{array}\right]_{R}=\left[\begin{array}{cc}-\lfloor\boldsymbol{\omega}\rfloor_{x} & \boldsymbol{\omega} \\ -\boldsymbol{\omega}^{T} & 0\end{array}\right], \quad\lfloor\boldsymbol{\omega}\rfloor_{\times}=\left[\begin{array}{ccc}0 & -\omega_{z} & \omega_{y} \\ \omega_{z} & 0 & -\omega_{x} \\ -\omega_{y} & \omega_{x} & 0\end{array}\right] Ω(ω)=[ω0]R=[ωxωTω0],ω×=0ωzωyωz0ωxωyωx0为四元数右乘矩阵的虚部

    where R t = R t W B = R t B W T R_t = R^{WB}_ t = {R^{BW}_t}^T Rt=RtWB=RtBWT. Here we assume that the angular velocity and the acceleration of B B B remain constant during the above integration.
    其中 R t = R t W B = R t B W T R_t = R^{WB}_ t = {R^{BW}_t}^T Rt=RtWB=RtBWT。这里我们假设 B B B的角速度和加速度在上述积分过程中保持不变。

    We then apply the IMU preintegration method proposed in [20] to obtain the relative body motion between two timesteps. The preintegrated measurements ∆ v i j , ∆ p i j ∆v_{ij}, ∆p_{ij} vij,pij, and ∆ R i j ∆R_{ij} Rij between time i i i and j j jcan be computed using:
    然后,我们应用[20]中提出的IMU预积分方法来获得两个时间步之间的相对body运动。时间 i i i j j j之间的预积分测量 ∆ v i j , ∆ p i j ∆v_{ij},∆p_{ij} vijpij,以及 ∆ R i j ∆R_{ij} Rij可以使用以下公式计算:
    在这里插入图片描述

    对比vins:

    Due to space limitations, we refer the reader to the description from [20] for the detailed derivation of Eqs. 7 -9. Besides its efficiency, applying IMU preintegration also naturally gives us one type of constraint for the factor graph - IMU preintegration factors. The IMU bias is jointly optimized alongside the lidar odometry factors in the graph.
    由于篇幅限制,我们请读者参考[20]中的描述,以了解等式7 -9的详细推导。除了效率之外,使用IMU预积分也自然地为因子图提供了一种类型的约束——IMU预积分因子。IMU偏差与因子图中的激光雷达里程计因子一起进行了联合优化。

    C. Lidar Odometry Factor

    When a new lidar scan arrives, we first perform feature extraction. Edge and planar features are extracted by evaluating the roughness of points over a local region. Points with a large roughness value are classified as edge features. Similarly, a planar feature is categorized by a small roughness value. We denote the extracted edge and planar features from a lidar scan at time i i i as F i e \mathrm{F}_{i}^{e} Fie and F i p \mathrm{F}_{i}^{p} Fip respectively. All the features extracted at time i i i compose a lidar frame F i \mathbb{F}_{i} Fi, where F i = { F i e ,   F i p } \mathbb{F}_{i}=\left\{\mathrm{F}_{i}^{e}, \mathrm{~F}_{i}^{p}\right\} Fi={Fie, Fip}. Note that a lidar frame F \mathbb{F} F is represented in B. A more detailed description of the feature extraction process can be found in [1], or [7] if a range image is used.
    当新的激光雷达scan到达时,我们首先执行特征提取。通过评估局部区域上点的曲率(roughness)来提取边缘和平面特征。曲率较大的点被分类为边缘特征。类似地,平面特征通过较小的曲率进行分类。我们将从i时刻的激光雷达scan中提取的边缘和平面特征分别表示为 F i e \mathrm{F}_{i}^{e} Fie F i p \mathrm{F}_{i}^{p} Fip。在i时刻提取的所有特征组成一个激光雷达帧 F i \mathbb{F}_{i} Fi​, 其中 F i = { F i e ,   F i p } \mathbb{F}_{i}=\left\{\mathrm{F}_{i}^{e}, \mathrm{~F}_{i}^{p}\right\} Fi={Fie, Fip}。注意,激光雷达帧在B中表示为 F \mathbb{F} F。更详细的特征提取过程描述可在[1]中找到,如果使用范围图像(range image),则在[7]中找到。

    Using every lidar frame for computing and adding factors to the graph is computationally intractable, so we adopt the concept of keyframe selection, which is widely used in the visual SLAM field. Using a simple but effective heuristic approach, we select a lidar frame F i + 1 \mathbb{F}_{i+1} Fi+1 as a keyframe when the change in robot pose exceeds a user-defined threshold when compared with the previous state x i \mathbf{x}_{i} xi. The newly saved keyframe, F i + 1 \mathbb{F}_{i+1} Fi+1, is associated with a new robot state node, x i + 1 \mathbf{x}_{i+1} xi+1, in the factor graph. The lidar frames between two keyframes are discarded. Adding keyframes in this way not only achieves a balance between map density and memory consumption but also helps maintain a relatively sparse factor graph, which is suitable for real-time nonlinear optimization. In our work, the position and rotation change thresholds for adding a new keyframe are chosen to be 1   m 1 \mathrm{~m} 1 m and 1 0 ∘ 10^{\circ} 10.
    使用每个激光雷达帧进行计算并向图形中添加因子在计算量上非常有难度,因此我们采用了关键帧选择的策略,这在视觉SLAM领域中有着广泛的应用。我们使用了一种简单但有效的启发式方法,当机器人姿势的变化与先前状态 x i \mathbf{x}_{i} xi相比,超过用户定义的阈值时,我们选择一个激光雷达帧 F i + 1 \mathbb{F}_{i+1} Fi+1 作为关键帧。新保存的关键帧 F i + 1 \mathbb{F}_{i+1} Fi+1与因子图中的新机器人状态节点 x i + 1 \mathbf{x}_{i+1} xi+1相关联。两个关键帧之间的激光雷达帧将被丢弃。以这种方式添加关键帧不仅实现了map密度和内存消耗之间的平衡,而且有助于保持相对稀疏的因子图,这适合于实时非线性优化。在我们的工作中,用于添加新关键帧的位置和旋转变化阈值被选择为 1   m 1\mathrm{~m} 1 m 1 0 ∘ 10^{\circ} 10

    Let us assume we wish to add a new state node x i + 1 \mathbf{x}_{i+1} xi+1 to the factor graph. The lidar keyframe that is associated with this state is F i + 1 \mathbb{F}_{i+1} Fi+1. The generation of a lidar odometry factor is described in the following steps:
    假设我们希望向因子图添加一个新的状态节点 x i + 1 \mathbf{x}_{i+1} xi+1。与此状态关联的激光雷达关键帧为 F i + 1 \mathbb{F}_{i+1} Fi+1。以下步骤描述了激光雷达里程计系数的生成过程:

    (1) Sub-keyframes for voxel map: We implement a sliding window approach to create a point cloud map containing a fixed number of recent lidar scans. Instead of optimizing the transformation between two consecutive lidar scans, we extract the n n n most recent keyframes, which we call the sub-keyframes, for estimation. The set of sub-keyframes { F i − n , … , F i } \left\{\mathbb{F}_{i-n}, \ldots, \mathbb{F}_{i}\right\} {Fin,,Fi} is then transformed into frame W \mathbf{W} W using the transformations { T i − n , … , T i } \left\{\mathbf{T}_{i-n}, \ldots, \mathbf{T}_{i}\right\} {Tin,,Ti} associated with them. The transformed sub-keyframes are merged together into a voxel map M i \mathbf{M}_{i} Mi. Since we extract two types of features in the previous feature extraction step, M i \mathbf{M}_{i} Mi is composed of two subvoxel maps that are denoted M i e \mathbf{M}_{i}^{e} Mie, the edge feature voxel map, and M i p \mathbf{M}_{i}^{p} Mip, the planar feature voxel map. The lidar frames and voxel maps are related to each other as follows:
    (1) 子关键帧的voxel map(局部地图):我们实现了一种滑动窗口方法来创建包含固定数量的最近(recent)激光雷达扫描的点云map。我们提取 n n n个最近的关键帧(我们称之为子关键帧)进行估计,而不是优化两次连续激光雷达扫描之间的变换。然后子关键帧集 { F i − n , … , F i } \left\{\mathbb{F}_{i-n}, \ldots, \mathbb{F}_{i}\right\} {Fin,,Fi} 使用与之相关联的变换 { T i − n , … , T i } \left\{\mathbf{T}_{i-n}, \ldots, \mathbf{T}_{i}\right\} {Tin,,Ti} 转换到 W \mathbf{W} W坐标系。变换后的子关键帧被合并到一个voxel map M i \mathbf{M}_{i} Mi中。由于我们在前面的特征提取步骤中提取了两种类型的特征, M i \mathbf{M}_{i} Mi由两个子voxel map组成,分别表示为 M i e \mathbf{M}_{i}^{e} Mie,即边缘特征体素图和 M i p \mathbf{M}_{i}^{p} Mip,即平面特征体素图。激光雷达帧和voxel maps相互关联如下:
    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 \mathrm{F}_{i-1}^{e} \cup \ldots \cup^{\prime} \mathrm{F}_{i-n}^{e} \\ &\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=FieFi1eFineMip=FipFi1pFinp

    每个地图是各个关键帧的总和

    ′ F i e { }^{\prime} \mathrm{F}_{i}^{e} Fie and ′ F i p { }^{\prime} \mathrm{F}_{i}^{p} Fip are the transformed edge and planar features in W . M i e \mathbf{W} . \mathbf{M}_{i}^{e} W.Mie and M i p \mathbf{M}_{i}^{p} Mip are then downsampled to eliminate the duplicated features that fall in the same voxel cell. In this paper, n n n is chosen to be 25 . The downsample resolutions for M i e \mathbf{M}_{i}^{e} Mie and M i p \mathbf{M}_{i}^{p} Mip are 0.2   m 0.2 \mathrm{~m} 0.2 m and 0.4   m 0.4 \mathrm{~m} 0.4 m, respectively.
    ′ F i e { }^{\prime} \mathrm{F}_{i}^{e} Fie ′ F i p { }^{\prime} \mathrm{F}_{i}^{p} Fip W \mathbf{W} W中变换后的边和平面特征。然后对 M i e \mathbf{M}_{i}^{e} Mie M i p \mathbf{M}_{i}^{p} Mip 进行下采样,以消除落在同一体素单元中的重复特征。在本文中, n n n被选择为25(临近关键帧的选取个数)。 M i e \mathbf{M}_{i}^{e} Mie M i p \mathbf{M}_{i}^{p} Mip的下采样分辨率分别为 0.2   m 0.2\mathrm{~m} 0.2 m 0.4   m 0.4\mathrm{~m} 0.4 m

    (2) Scan-matching: We match a newly obtained lidar frame F i + 1 \mathbb{F}_{i+1} Fi+1, which is also { F i + 1 e ,   F i + 1 p } \left\{\mathrm{F}_{i+1}^{e}, \mathrm{~F}_{i+1}^{p}\right\} {Fi+1e, Fi+1p}, to M i \mathbf{M}_{i} Mi via scan matching. Various scan-matching methods, such as [3] and [4], can be utilized for this purpose. Here we opt to use the method proposed in [1] due to its computational efficiency and robustness in various challenging environments.
    (2) 扫描匹配:我们通过扫描匹配将新获得的激光雷达帧 F i + 1 \mathbb{F}_{i+1} Fi+1,也就是 { F i + 1 e ,   F i + 1 p } \left\{\mathrm{F}_{i+1}^{e}, \mathrm{~F}_{i+1}^{p}\right\} {Fi+1e, Fi+1p}匹配到 M i \mathbf{M}_{i} Mi。为此,可以使用各种扫描匹配方法,例如[3]和[4]。在这里,我们选择使用[1]中提出的方法(kdtree搜索最近邻),因为它在各种具有挑战性的环境中具有计算效率和鲁棒性。

    We first transform { F i + 1 e ,   F i + 1 p } \left\{\mathrm{F}_{i+1}^{e}, \mathrm{~F}_{i+1}^{p}\right\} {Fi+1e, Fi+1p} from B \mathbf{B} B to W \mathbf{W} W and obtain { ′ F i + 1 e , ′ F i + 1 p } \left\{{ }^{\prime} \mathrm{F}_{i+1}^{e},^{\prime} \mathrm{F}_{i+1}^{p}\right\} {Fi+1e,Fi+1p}. This initial transformation is obtained by using the predicted robot motion, T ~ i + 1 \tilde{\mathbf{T}}_{i+1} T~i+1, from the IMU. For each feature in ′ F i + 1 e { }^{\prime}F^e_{i+1} Fi+1e or ′ F i + 1 p { }^{\prime}F^p_{i+1} Fi+1p , we then find its edge or planar correspondence in M i e M^e_i Mie or M i p M^p_i Mip . For the sake of brevity, the detailed procedures for finding these correspondences are omitted here, but are described thoroughly in [1].
    我们首先将 { F i + 1 e ,   F i + 1 p } \left\{\mathrm{F}_{i+1}^{e}, \mathrm{~F}_{i+1}^{p}\right\} {Fi+1e, Fi+1p} B \mathbf{B} B转换为 W \mathbf{W} W,获得 { ′ F i + 1 e , ′ F i + 1 p } \left\{{ }^{\prime} \mathrm{F}_{i+1}^{e},^{\prime} \mathrm{F}_{i+1}^{p}\right\} {Fi+1e,Fi+1p}。该初始转换是通过使用来自IMU的预测机器人运动 T ~ i + 1 \tilde{\mathbf{T}}_{i+1} T~i+1,获得的。对于 ′ F i + 1 e { }^{\prime}F^e_{i+1} Fi+1e ′ F i + 1 p { }^{\prime}F^p_{i+1} Fi+1p中的每个特征,我们可以在 M i e M^e_i Mie M i p M^p_i Mip中找到其边或平面的对应关系。为简洁起见,此处省略了查找这些对应关系的详细程序,但在[1]中进行了详细描述。

    (3) Relative transformation: The distance between a feature and its edge or planar patch correspondence can be computed using the following equations:
    (3) 相对变换:特征与其边缘或planar patch对应关系之间的距离可以使用以下公式计算:
    在这里插入图片描述

    (10)点到线的距离
    (11)点到面的距离

    where k , u , v k, u, v k,u,v, and w w w are the feature indices in their corresponding sets. For an edge feature p i + 1 , k e \mathbf{p}_{i+1, k}^{e} pi+1,ke in ′ F i + 1 e , p i , u e { }^{\prime} \mathrm{F}_{i+1}^{e}, \mathbf{p}_{i, u}^{e} Fi+1e,pi,ue and p i , v e \mathbf{p}_{i, v}^{e} pi,ve are the points that form the corresponding edge line in M i e . \mathbf{M}_{i}^{e} . Mie. For a planar feature p i + 1 , k p \mathbf{p}_{i+1, k}^{p} pi+1,kp in ′ F i + 1 p , p i , u p , p i , v p { }^{\prime} \mathrm{F}_{i+1}^{p}, \mathbf{p}_{i, u}^{p}, \mathbf{p}_{i, v}^{p} Fi+1p,pi,up,pi,vp, and p i , w p \mathbf{p}_{i, w}^{p} pi,wp form the corresponding planar patch in M i p \mathbf{M}_{i}^{p} Mip. The GaussNewton method is then used to solve for the optimal transformation by minimizing:
    其中, k , u , v k,u,v kuv w w w是其相应集合中的特征索引。对于在 ′ F i + 1 e { }^{\prime} \mathrm{F}_{i+1}^{e} Fi+1e的边特征 p i + 1 , k e \mathbf{p}_{i+1, k}^{e} pi+1,ke而言, p i , u e \mathbf{p}_{i, u}^{e} pi,ue p i , v e \mathbf{p}_{i, v}^{e} pi,ve M i e \mathbf{M}_{i}^{e} Mie中构成相应边线的两个点。对于在 ′ F i + 1 p , p i , u p , p i , v p { }^{\prime} \mathrm{F}_{i+1}^{p}, \mathbf{p}_{i, u}^{p}, \mathbf{p}_{i, v}^{p} Fi+1p,pi,up,pi,vp的平面特征 p i + 1 , k p \mathbf{p}_{i+1, k}^{p} pi+1,kp而言, p i , u p , p i , v p \mathbf{p}_{i, u}^{p}, \mathbf{p}_{i, v}^{p} pi,up,pi,vp,和 p i , w p \mathbf{p}_{i, w}^{p} pi,wp M i p \mathbf{M}_{i}^{p} Mip中构成相应面点的三个点。然后,使用GaussNewton方法通过最小化以下各项来求解最佳变换:
    min ⁡ T i + 1 { ∑ p i + 1 , k e ∈ F i + 1 e d e k + ∑ p i + 1 , k p ∈ F i + 1 p d p k } . \min _{\mathbf{T}_{i+1}}\left\{\sum_{\mathbf{p}_{i+1, k}^{e} \in{F}_{i+1}^{e}} \mathbf{d}_{e_{k}}+\sum_{\mathbf{p}_{i+1, k}^{p} \in{F}_{i+1}^{p}} \mathbf{d}_{p_{k}}\right\} . Ti+1minpi+1,keFi+1edek+pi+1,kpFi+1pdpk.
    At last, we can obtain the relative transformation Δ T i , i + 1 \Delta \mathbf{T}_{i, i+1} ΔTi,i+1 between x i \mathbf{x}_{i} xi and x i + 1 \mathbf{x}_{i+1} xi+1, which is the lidar odometry factor linking these two poses:
    最后,我们可以得到介于 x i \mathbf{x}_{i} xi x i + 1 \mathbf{x}_{i+1} xi+1之间的相对变换 Δ T i , i + 1 \Delta\mathbf{T}_{i,i+1} ΔTii+1,它是连接这两个姿势的激光雷达里程计因子:

    在这里插入图片描述

    对第i帧和第i+1帧的激光位姿进行约束

    We note that an alternative approach to obtain Δ T i , i + 1 \Delta \mathbf{T}_{i, i+1} ΔTi,i+1 is to transform sub-keyframes into the frame of x i . \mathbf{x}_{i} . xi. In other words, we match F i + 1 \mathbb{F}_{i+1} Fi+1 to the voxel map that is represented in the frame of x i \mathbf{x}_{i} xi. In this way, the real relative transformation Δ T i , i + 1 \Delta \mathbf{T}_{i, i+1} ΔTi,i+1 can be directly obtained. Because the transformed features ′ F i e { }^{\prime} \mathrm{F}_{i}^{e} Fie and ′ F i p { }^{\prime} \mathrm{F}_{i}^{p} Fip can be reused multiple times, we instead opt to use the approach described in Sec. III-C.1 for its computational efficiency.
    我们注意到,获得 Δ T i , i + 1 \Delta\mathbf{T}_{i,i+1} ΔTii+1的另一种方法是将子关键帧转换为 x i \mathbf{x}_{i} xi帧。换句话说,我们将 F i + 1 \mathbb{F}_{i+1} Fi+1与在 x i \mathbf{x}_{i} xi帧中表示的 voxel map相匹配。这样,可以直接获得真实的相对变换 Δ T i , i + 1 \Delta\mathbf{T}_ {i,i+1} ΔTii+1。由于转换后的特征 ′ F i e {}^{\prime}\mathrm{F}_{i}^{e} Fie ′ F i p {}^{\prime}\mathrm{F}_{i}^{p} Fip可以多次重用,因此我们选择使用Sec. III-C.1中描述的方法来提高计算效率。

    为什么要投影到世界坐标系而不是投影到第 x i \mathbf{x}_{i} xi帧?比如我们想从第10帧获得第12帧,如果是投影到相对帧的形式,我们需要把第10帧投到第11帧再投到12帧来获得第十二帧,而在投到世界坐标系下的情况下,每次都只需要进行一次转换即可获得,属于一劳永逸

    D. GPS Factor

    Though we can obtain reliable state estimation and mapping by utilizing only IMU preintegration and lidar odometry factors, the system still suffers from drift during longduration navigation tasks. To solve this problem, we can introduce sensors that offer absolute measurements for eliminating drift. Such sensors include an altimeter, compass, and GPS. For the purposes of illustration here, we discuss GPS, as it is widely used in real-world navigation systems.
    虽然我们可以仅利用IMU预积分和激光雷达里程计因子获得可靠的状态估计和建图,但系统在长时间导航任务中仍会出现漂移。为了解决这个问题,我们可以引入提供绝对测量以消除漂移的传感器。这些传感器包括高度表、指南针和GPS。为了便于说明,我们将讨论GPS,因为它广泛应用于现实世界的导航系统中。

    When we receive GPS measurements, we first transform them to the local Cartesian coordinate frame using the method proposed in [21]. Upon the addition of a new node to the factor graph, we then associate a new GPS factor with this node. If the GPS signal is not hardware-synchronized with the lidar frame, we interpolate among GPS measurements linearly based on the timestamp of the lidar frame.
    当我们接收到GPS测量值时,我们首先使用[21]中提出的方法将其转换为局部笛卡尔坐标系(因为GPS得到的数据是经纬度,不能直接拿来应用)。在因子图中添加新节点后,我们将新的GPS因子与该节点关联。如果GPS信号与激光雷达帧不同步,我们将根据激光雷达帧的时间戳在GPS测量值之间进行线性插值。

    We note that adding GPS factors constantly when GPS reception is available is not necessary because the drift of lidar inertial odometry grows very slowly. In practice, we only add a GPS factor when the estimated position covariance is larger than the received GPS position covariance.
    我们注意到,当GPS接收可用时,不需要不断添加GPS因子,因为激光雷达惯性里程计的漂移增长非常缓慢。实际上,我们仅在估计的位置协方差大于接收到的GPS位置协方差时添加GPS因子。

    E. Loop Closure Factor

    Thanks to the utilization of a factor graph, loop closures can also be seamlessly incorporated into the proposed system, as opposed to LOAM and LIOM. For the purposes of illustration, we describe and implement a naive but effective Euclidean distance-based loop closure detection approach. We also note that our proposed framework is compatible with other methods for loop closure detection, for example, [22] and [23], which generate a point cloud descriptor and use it for place recognition.
    由于使用了因子图,回环检测也可以很好地整合到提出的系统中,而这是LOAM和LIOM不能实现的。为了便于说明,我们描述并实现了一种简单但有效的基于欧几里德距离的回环检测方法(两帧距离较近而时间较远,就可尝试回环)。我们还注意到,我们提出的框架与回环检测的其他方法兼容,例如,[22]和[23],它们生成点云描述符并将其用于位置识别。

    When a new state x i + 1 \mathbf{x}_{i+1} xi+1 is added to the factor graph, we first search the graph and find the prior states that are close to x i + 1 \mathbf{x}_{i+1} xi+1 in Euclidean space. As is shown in Fig. 1, for example, x 3 x_{3} x3 is one of the returned candidates. We then try to match F i + 1 \mathbb{F}_{i+1} Fi+1 to the sub-keyframes { F 3 − m , … , F 3 , … , F 3 + m } \left\{\mathbb{F}_{3-m}, \ldots, \mathbb{F}_{3}, \ldots, \mathbb{F}_{3+m}\right\} {F3m,,F3,,F3+m} using scan-matching. Note that F i + 1 \mathbb{F}_{i+1} Fi+1 and the past sub-keyframes are first transformed into W \mathbf{W} W before scan-matching. We obtain the relative transformation Δ T 3 , i + 1 \Delta \mathbf{T}_{3, i+1} ΔT3,i+1 and add it as a loop closure factor to the graph. Throughout this paper, we choose the index m m m to be 12 , and the search distance for loop closures is set to be 15   m 15 \mathrm{~m} 15 m from a new state x i + 1 \mathbf{x}_{i+1} xi+1.
    当一个新状态 x i + 1 \mathbf{x}_{i+1} xi+1被添加到因子图中时,我们首先搜索该图,并在欧氏空间中找到接近 x i + 1 \mathbf{x}_{i+1} xi+1的先前状态。如图1所示,例如, x 3 x_{3} x3是返回的候选者之一。我们尝试使用scan匹配将 F i + 1 \mathbb{F}_{i+1} Fi+1与子关键帧 { F 3 − m , … , F 3 , … , F 3 + m } \left\{\mathbb{F}_{3-m}, \ldots, \mathbb{F}_{3}, \ldots, \mathbb{F}_{3+m}\right\} {F3m,,F3,,F3+m} 匹配(使用ICP)。请注意 F i + 1 \mathbb{F}_{i+1} Fi+1和过去的子关键帧在scan匹配之前会首先转换到 W \mathbf{W} W。我们获得相对变换 Δ T 3 , i + 1 \Delta \mathbf{T}_{3, i+1} ΔT3,i+1,并将其作为回环因子添加到图中。在本文中,我们将索引 m m m设置为12( 往左边找12,往右边找12,一共25个关键帧构成的子地图进行配准),回环检测的搜索距离设置为从新状态 x i + 1 \mathbf{x}_{i+1} xi+1开始的 15   m 15\mathrm{~m} 15 m

    帧与帧配准的精度一定小于帧与地图配准的精度,详细见LOAM论文

    在这里插入图片描述
    In practice, we find adding loop closure factors is especially useful for correcting the drift in a robot’s altitude, when GPS is the only absolute sensor available. This is because the elevation measurement from GPS is very inaccurate - giving rise to altitude errors approaching 100   m 100 \mathrm{~m} 100 m in our tests, in the absence of loop closures.
    在实践中,我们发现,当GPS是唯一可用的绝对传感器时,添加回环因子对于校正机器人高度漂移特别有用。这是因为GPS的高程测量非常不准确—在没有回路闭合的情况下,在我们的测试中,高度误差接近 100   m 100\mathrm{~m} 100 m

    IV. Experiment

    V. Conclusion and Discussion

    We have proposed LIO-SAM, a framework for tightlycoupled lidar inertial odometry via smoothing and mapping, for performing real-time state estimation and mapping in complex environments. By formulating lidar-inertial odometry atop a factor graph, LIO-SAM is especially suitable for multi-sensor fusion. Additional sensor measurements can easily be incorporated into the framework as new factors. Sensors that provide absolute measurements, such as a GPS, compass, or altimeter, can be used to eliminate the drift of lidar inertial odometry that accumulates over long durations, or in feature-poor environments. Place recognition can also be easily incorporated into the system. To improve the real-time performance of the system, we propose a sliding window approach that marginalizes old lidar frames for scan matching. Keyframes are selectively added to the factor graph, and new keyframes are registered only to a fixedsize set of sub-keyframes when both lidar odometry and loop closure factors are generated. This scan-matching at a local scale rather than a global scale facilitates the realtime performance of the LIO-SAM framework. The proposed method is thoroughly evaluated on datasets gathered on three platforms across a variety of environments. The results show that LIO-SAM can achieve similar or better accuracy when compared with LOAM and LIOM. Future work involves testing the proposed system on unmanned aerial vehicles.
    我们提出了LIO-SAM,一个通过smoothing和mapping实现紧耦合激光雷达惯性里程测量的框架,用于在复杂环境中执行实时状态估计和建图。LIO-SAM通过在因子图上建立激光雷达惯性里程计,特别适用于多传感器融合。额外的传感器测量可以很容易地作为新的因素纳入到框架中。提供绝对测量值的传感器,如GPS、指南针或高度计,可用于消除激光雷达惯性里程计在长时间内或在特征较少的环境中的累积漂移。位置识别也可以很容易地集成到系统中。为了提高系统的实时性,我们提出了一种滑动窗口方法来边缘化旧的激光雷达帧进行scan匹配。有选择地将关键帧添加到因子图中,并且在生成激光雷达里程计和环路闭合因子时,新关键帧仅对齐(registered)到固定大小的子关键帧集。这种局部范围而非全局范围的scan匹配有助于LIO-SAM框架的实时性能。我们在三个平台收集的数据集上对所提出的方法进行了全面的评估,这些数据集跨越各种环境。结果表明,与LOAM和LIOM相比,LIO-SAM可以达到相似或更好的精度。未来的工作包括在无人机上测试拟提出的系统。

    在这里插入图片描述
    点云的去畸变补偿在代码中只应用于旋转部分

    展开全文
  • LIO-SAM: Tightly-coupled Lidar Inertial Odometry viaSmoothing and Mapping 摘要 我们提出了一种通过平滑和建图实现紧耦合的激光雷达里程计的框架——LIO-SAM,它实现了高精度、实时的移动机器人轨迹估计和地图...

    LIO-SAM: Tightly-coupled Lidar Inertial Odometry viaSmoothing and Mapping

    摘要

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

    引言

    状态估计、定位和建图是成功的智能移动机器人的基本先决条件,是反馈控制、避障和规划等许多其他能力所必需的。使用基于视觉和基于激光雷达的传感,已经付出了巨大的努力来实现高性能的实时同步定位和建图(SLAM),其可以支持移动机器人的六自由度状态估计。基于视觉的方法通常使用单目或双目相机,并在连续图像上三角化特征来确定相机的运动。虽然基于视觉的方法特别适合于位置识别,但是当它们单独用于支持自主导航系统时,它们对初始化、光照和距离的敏感性使得它们不可靠。另一方面,基于激光雷达的方法对光照变化具有强不变性。特别是随着大测量范围、高分辨率三维激光雷达的出现,如Velodyne VLS-128和 Ouster OS1-128,激光雷达更适合直接捕捉三维空间中的环境细节。因此,本文重点研究了基于激光雷达的状态估计和建图方法。

    在过去的二十年中,已经提出了许多基于激光雷达的状态估计和建图方法。其中,[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.系统概述

    我们首先定义我们在整个论文中使用的坐标系和符号。我们将世界坐标系表示为 W W W,将机器人机体坐标系表示为 B B B。为了方便起见,我们还假设惯性测量单元坐标系与机器人机体坐标系是一致的。机器人状态 x x x可以写成:
    x = [ R T , p T , v T , b T ] T (1) x = [R^T,p^T,v^T,b^T]^T \tag{1} x=[RT,pT,vT,bT]T(1)
    其中 R ∈ S O ( 3 ) R \in SO(3) RSO(3)为旋转矩阵, p ∈ R 3 p \in \mathbb{R}^{3} pR3是位置向量, v v v是速度, b b b是惯性测量单元的零偏。从 B B B W W W的变换矩阵 T ∈ S E ( 3 ) T \in SE(3) TSE(3)被表示为 T = [ R ∣ p ] T = [R|p] T=[Rp]

    图1展示了本文中提出的系统的概述。
    在这里插入图片描述

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

    我们为因子图的构造引入了四种带有单变量类型的因子。这个代表机器人在特定时间的状态的变量被归为因子图的节点。这四种类型的因子是:(a)惯性测量单元预积分因子,(b)激光雷达里程计因子,© GPS因子,以及(d)闭环因子。当机器人姿态的变化超过用户定义的阈值时,一个新的机器人状态节点被添加到因子图中。因子图在插入新节点时使用增量平滑和建图的贝叶斯树(iSAM2)进行优化[19]。产生这些因子的过程将在以下章节中描述。

    B.IMU预积分因子

    来自惯性测量单元的角速度和加速度的测量值用方程2和3定义:
    ω ^ t = ω t + b t ω + n t ω (2) \hat{\omega}_t = \omega_t + b_t^{\omega} + n_t^{\omega} \tag{2} ω^t=ωt+btω+ntω(2)
    a ^ t = R t B W ( a t − g ) + b t a + n t a (3) \hat{a}_t = R_t^{BW}(a_t - g)+b_t^a + n_t^a \tag{3} a^t=RtBW(atg)+bta+nta(3)

    其中 ω ^ t \hat{\omega}_t ω^t a ^ t \hat{a}_t a^t是指在 B B B坐标系下时刻 t t t时原始的惯性测量单元测量值。 ω ^ t \hat{\omega}_t ω^t a ^ t \hat{a}_t a^t受到缓慢变化的零偏 b t b_t bt和白噪声 n t n_t nt的影响。 R t B W R_t^{BW} RtBW是从 W W W B B B的旋转矩阵, g g g W W W坐标系下的恒定重力矢量。

    我们现在可以使用来自惯性测量单元的测量来推测机器人的运动。机器人在时间 t + Δ t t + \Delta t t+Δt时的速度、位置和旋转可以计算如下:
    v t + Δ t = v t + g Δ t + R t ( a ^ t − b t a − n t a ) Δ t (4) v_{t + \Delta t} = v_t + g \Delta t + R_t(\hat{a}_t - b_t^a - n_t^a) \Delta t \tag{4} vt+Δt=vt+gΔt+Rt(a^tbtanta)Δt(4)
    p t + Δ t = p t + v t Δ t + 1 2 g Δ t 2 + 1 2 R t ( a ^ t − b t a − n t a ) Δ t 2 . (5) p_{t+\Delta t} = p_t + v_t \Delta t + \frac{1}{2}g \Delta t^2 + \frac{1}{2} R_t(\hat{a}_t - b_t^a - n_t^a) \Delta t^2. \tag{5} pt+Δt=pt+vtΔt+21gΔt2+21Rt(a^tbtanta)Δt2.(5)
    R t + Δ t = R t e x p ( ( ω ^ t − b t ω − n t ω ) Δ t 2 ) . (6) R_{t+\Delta t} = R_t exp((\hat{\omega}_t - b_t^{\omega} - n_t^{\omega} )\Delta t^2). \tag{6} Rt+Δt=Rtexp((ω^tbtωntω)Δt2).(6)

    其中 R t = R t W B = R t B W T R_t = R_t^{WB} = {R_t^{BW}}^{T} Rt=RtWB=RtBWT 。这里我们假设在上述积分过程中, B B B坐标系下的角速度和加速度保持不变。

    然后,我们应用文献[20]中提出的惯性测量单元预积分方法来获得两个时间步长之间的相对机体运动。两个时刻 i i i j j j之间的预积分测量值 Δ v i j , Δ p i j \Delta v_{ij},\Delta p _{ij} ΔvijΔpij Δ R i j \Delta R_{ij} ΔRij可使用以下公式计算:
    Δ v i j = R i T ( v j − v i − g Δ t i j ) (7) \Delta v_{ij} = R_i^T(v_j - v_i - g \Delta t_{ij}) \tag{7} Δvij=RiT(vjvigΔtij)(7)
    Δ p i j = R i T ( p j − p i − v i Δ t i j − 1 2 g Δ t i j 2 ) . (8) \Delta p_{ij} = R_i^T(p_j - p_i - v_i \Delta t_{ij} - \frac{1}{2} g \Delta t_{ij}^2). \tag{8} Δpij=RiT(pjpiviΔtij21gΔtij2).(8)
    Δ R i j = R i T R j (9) \Delta R_{ij} = R_i^TR_j \tag{9} ΔRij=RiTRj(9)

    由于篇幅有限,我们请读者参考[20]的描述,以获得方程7-9的详细推导。除了效率之外,应用惯性测量单元预积分也自然地给我们提供了一种对因子图的约束——惯性测量单元预积分因子。在因子图中,惯性测量单元零偏与激光雷达里程计因子一起进行了联合优化。

    C.雷达里程计因子

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

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

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

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

    我们实现了一种滑动窗口方法来创建包含固定数量的最近激光雷达扫描的点云图。我们没有优化两次连续激光雷达扫描之间的变换,而是提取 n n n个最近的关键帧,我们称之为子关键帧,来用于估计。该组子关键帧 { F i − n , . . . , F i } \{\mathbb{F}_{i-n},...,\mathbb{F_i}\} {Fin,...,Fi}然后被使用与他们相对应的变换 T i − n , . . . , T i {T_{i-n},...,T_{i}} Tin,...,Ti转换到世界坐标系 W W W。转换后的子关键帧被合并到一个体素地图 M i M_i Mi中。由于我们在先前的特征提取步骤中提取了两种类型的特征,所以 M i M_i Mi由两个子体素地图组成,它们是边缘特征体素地图 M i e M_i^e Mie和平面特征体素地图 M i p M_i^p Mip。激光雷达帧和对应的体素地图如下所示:
    M i = { M i e , M i p } M_i = \{M_i^e,M_i^p\} Mi={Mie,Mip}
    这里
    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 M_i^e = 'F_i^e \cup ' F_{i-1}^e \cup ... \cup'F_{i-n}^e \\ M_i^p = 'F_i^p \cup ' F_{i-1}^p \cup ... \cup'F_{i-n}^p \\ Mie=FieFi1e...FineMip=FipFi1p...Finp
    ′ F i e 'F_i^e Fie ′ F i p 'F_i^p Fip是变换后的边缘和平面特征。然后,对 M i e M_i^e Mie M i p M_i^p Mip进行下采样,以消除落在同一个体素单元中的重复特征。在本文中, n n n被选择为25。下采样分辨率分别为 0.2 m 0.2m 0.2m 0.4 m 0.4m 0.4m

    2)扫描匹配

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

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

    3)相对变换

    特征与其对应的边缘或平面特征之间的距离可以用以下公式计算:
    d e k = ∣ ( p i + 1 , k e − p i , u e ) × ( p i + 1 , k e − p i , v e ) ∣ ∣ p i , u e − p i , v e ∣ (10) d_{e_k} = \frac{| (p_{i+1,k}^e - p_{i,u}^e) \times (p_{i+1,k}^e - p_{i,v}^e) |}{ | p_{i,u}^{e} - p_{i,v}^e |} \tag{10} dek=pi,uepi,ve(pi+1,kepi,ue)×(pi+1,kepi,ve)(10)

    d p k = ∣ ( p i + 1 , k p − p i , u p ) ( p i , u p − p i , v p ) × ( p i , u p − p i , w p ) ∣ ∣ ( p i , u p − p i , v p ) × ( p i , u p − p i , w p ) ∣ . (11) d_{p_k} = \frac{| (p_{i+1,k}^p - p_{i,u}^p) (p_{i,u}^p - p_{i,v}^{p}) \times (p_{i,u}^{p} - p_{i,w}^p) | }{| (p_{i,u}^p - p_{i,v}^{p}) \times (p_{i,u}^{p} - p_{i,w}^p) |}. \tag{11} dpk=(pi,uppi,vp)×(pi,uppi,wp)(pi+1,kppi,up)(pi,uppi,vp)×(pi,uppi,wp).(11)

    其中 k k k u u u v v v和是它们对应集合中的特征索引。对于在 ′ F i + 1 e 'F_{i+1}^e Fi+1e中的边特征 p i + 1 , k e p_{i+1,k}^e pi+1,ke p i , u e p_{i,u}^e pi,ue p i , v e p_{i,v}^e pi,ve是在 M i e M_i^e Mie中改变形成相应边缘线的点。对于 ′ F i + 1 p 'F_{i+1}^p Fi+1p中的平面特征 p i + 1 , k p p_{i+1,k}^p pi+1,kp p i , u p p_{i,u}^p pi,up, p i , v p p_{i,v}^p pi,vp p i , w p p_{i,w}^p pi,wp是在 M i p M_i^p Mip中形成相应的平面的点。然后使用高斯牛顿法通过最小化以下代价函数来求解最优变换:
    min ⁡ T i + 1 { ∑ p i + 1 , k e ∈ ′ F i + 1 e d e k + ∑ p i + 1 , k p ∈ ′ F i + 1 p d p k } \min_{T_{i+1}}{\{ \sum_{p_{i+1,k}^e \in 'F_{i+1}^e}{d_{e_k}} + \sum_{p_{i+1,k}^p \in 'F_{i+1}^p}{d_{p_k}} \}} Ti+1min{pi+1,keFi+1edek+pi+1,kpFi+1pdpk}
    最后,我们可以得到 x i x_i xi x i + 1 x_{i+1} xi+1之间的相对变换 Δ T i , i + 1 = T i T T i + 1 \Delta T_{i,i+1} = T_i^T T_{i+1} ΔTi,i+1=TiTTi+1,这是连接这两个位姿的激光雷达里程计因子:

    我们注意到获得 Δ T i , i + 1 \Delta T_{i,i+1} ΔTi,i+1的另一种方法是将子关键帧转换到 x i x_i xi的坐标系下。换句话说,我们匹配 F i + 1 \mathbb{F}_{i+1} Fi+1与在到 x i x_i xi坐标系下表示的体素地图。这样就可以直接得到相对变换 Δ T i , i + 1 \Delta T_{i,i+1} ΔTi,i+1。因为变换后的特征 ′ F i p 'F_i^p Fip ′ F i p 'F_i^p Fip可以被多次重用,所以为了效率我们选择使用1)中描述的方法。

    D.GPS因子

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

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

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

    E.闭环因子

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

    当一个新的状态 x i + 1 x_{i+1} xi+1被添加到因子图中时,我们首先搜索该图,并在欧氏空间中找到与状态 x i + 1 x_{i+1} xi+1接近的先前状态。如图1所示,例如, x 3 x_3 x3是返回的候选之一。

    然后,我们尝试将 F i + 1 \mathbb{F}_{i+1} Fi+1与子关键帧 { F 3 − m , . . . , F 3 + m } \{\mathbb{F}_{3-m},...,\mathbb{F}_{3+m}\} {F3m,...,F3+m}进行扫描匹配。请注意,在扫描匹配之前, F i + 1 \mathbb{F}_{i+1} Fi+1和过去的子关键帧首先被转换到世界坐标系 W W W下。我们得到相对变换 Δ T 3 , i + 1 \Delta T_{3,i+1} ΔT3,i+1,并把它作为一个闭环因子加到因子图中。在本文中,我们选择索引 m m m为12,并且从新的状态 x i + 1 x_{i+1} xi+1开始,闭环的搜索距离设置为 15 m 15m 15m

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

    实验

    我们现在描述一系列实验来定性和定量分析我们提出的框架。本文使用的传感器套件包括一台Velodyne VLP-16激光雷达、一台MicroStrain 3DM-GX5-25惯性测量单元和一台Reach M GPS。为了验证,我们收集了5个不同规模、平台和环境的不同数据集。这些数据集分别被称为旋转、步行、校园、公园和水坝。传感器安装平台如图2所示。
    在这里插入图片描述
    前三个数据集是使用定制的手持设备在麻省理工学院校园内收集的。公园数据集是在一个被植被覆盖的公园中收集的,使用的是无人驾驶地面车辆(UGV)——Clearpath Jackal。最后一个数据集,Amsterdam,是通过在船上安装传感器并在阿姆斯特丹的运河中巡航收集的。这些数据集的详情见表一
    在这里插入图片描述
    我们将提出的LIO-SAM框架与LOAM和LIOM进行了比较。在所有的实验中,LOAM和LIO-SAM被迫实时运行。另一方面,LIOM有无限的时间来处理每个传感器测量。所有这些方法都是用C++实现的,并在一台配备英特尔i7-10710U CPU的笔记本电脑上使用Ubuntu Linux中的机器人操作系统(ROS) [24]执行。我们注意到,只有CPU用于计算,而没有启用并行计算。我们在Github上开源了LIO-SAM。进行的实验的补充细节,包括所有测试的完整可视化,可以在下面的链接中找到

    在这里插入图片描述

    A.旋转数据集

    在本次测试中,我们重点评估了只有惯性测量单元预积分因子和激光雷达里程计因子被添加到因子图时我们框架的鲁棒性。用户手持传感器套件并在站立时执行一系列激进的旋转动作来收集旋转数据集。在这个测试中遇到的最大转速为 133.7 ° / s 133.7 \degree /s 133.7°/s。试验环境如图3(a)所示。
    在这里插入图片描述

    图3(b)和©分别显示了从LOAM和LIO-SAM获得的结果图。
    在这里插入图片描述

    因为LIOM使用与[25]相同的初始化流程,所以它继承了与视觉惯性SLAM相同的初始化灵敏度,并且无法使用该数据集正确初始化。由于未能产生有意义的结果,因此没有显示LIOM的地图。如图所示,与LOAM地图相比,LIO-SAM地图保留了更精细的环境结构细节。这是因为LIO-SAM能够在 S O ( 3 ) SO(3) SO(3)中精确配准每一个激光雷达帧,即使当机器人经历快速旋转时。

    B.步行数据集

    该测试旨在评估我们的方法在系统经历强烈的平移和旋转时的性能。遇到的最大平移和旋转速度分别是 1.8 m / s 1.8m/s 1.8m/s 213.9 ° / s 213.9 \degree /s 213.9°/s。在数据收集过程中,用户手持图2(a)所示的传感器套件,快速穿过麻省理工学院校园(图4(a))。
    在这里插入图片描述
    在这里插入图片描述

    在这个测试中,如图4(b)所示,当遇到强烈的旋转时,LOAM的地图在多个位置发散。
    在这里插入图片描述

    在这次测试中,LIOM的表现优于LOAM。然而,它的地图,如图4©所示,仍然在不同的位置轻微发散,由无数模糊的结构组成。
    在这里插入图片描述

    因为LIOM被设计为处理所有传感器测量,所以它只以0.56倍的实时速率运行,而其他方法是实时运行的。最后,LIO-SAM优于这两种方法,并产生了一个与谷歌地球图像一致的地图。
    在这里插入图片描述

    C.校园数据集

    该测试旨在展示引入GPS和闭环因子的好处。为了做到这一点,我们特意禁止在因子图中插入GPS因子和闭环因子。当GPS因子和闭环因子都被禁用时,我们的方法被称为LIO-里程计,它只利用惯性测量单元预积分和激光雷达里程计因子。当使用GPS因子时,我们的方法被称为LIO-GPS,它使用惯性测量单元预积分、激光雷达里程计和GPS因子来构建因子图。LIO-SAM使用所有可用的因子。

    为了收集这个数据集,用户使用手持设备在MIT校园周围走动,然后返回到相同的位置。因为在建图区域有无数的建筑和树木,GPS接收很少可用,而且大部分时间都不准确。在过滤掉不一致的GPS测量后,GPS可用的区域在图5(a)中显示为绿色部分。这些地区相当于少数没有被建筑物或树木包围的区域。
    在这里插入图片描述
    LOAM、LIO-odom、LIO-GPS和LIO-SAM的估计轨迹如上图所示。由于无法正确初始化和产生有意义的结果,所以没有显示LIOM的结果。如图所示,与所有其他方法相比,LOAM的轨迹漂移非常明显。没有GPS数据的校正,LIO-odom的轨迹在地图右下角开始明显漂移。在GPS数据的帮助下,当GPS可用时LIO-GPS可以矫正漂移。然而,GPS数据在数据集的后半部分不可用。结果,当机器人由于漂移而返回起始位置时,LIO-GPS无法完成闭环。另一方面,LIO-SAM可以通过在因子图中增加闭环因子来消除漂移。LIO-SAM的地图与谷歌地球的图像非常吻合,如图5(b)所示。
    在这里插入图片描述
    当机器人返回起点时,所有方法的相对平移误差如表二所示。
    在这里插入图片描述

    D.公园数据集

    在这次测试中,我们将传感器安装在UGV上,并驾驶车辆沿着森林覆盖的徒步旅行路线行驶。行驶40分钟后,机器人回到初始位置。UGV在三个路面上行驶:柏油路、被草覆盖的地面和被泥土覆盖的小路。由于没有悬架,机器人在非沥青路面上行驶时会受到低振幅但高频率的振动。

    为了模拟一个具有挑战性的建图场景,我们仅在机器人处于开阔区域时使用GPS测量,这在图6(a)中用绿色线段表示。
    在这里插入图片描述

    这种映建图场景代表了一种任务,在这种任务中,机器人必须建图多个拒绝GPS的区域,并周期性地返回GPS可用的区域,以校正漂移。

    与之前测试的结果类似,由于没有绝对校正数据,LOAM、LIOM和LIOM-odom都存在明显的漂移。此外,LIOM仅以0.67倍的实时速率运行,而其他方法可以实时运行。尽管LIO-GPS和LIO-SAM的轨迹在水平面上重合,但它们的相对平移误差是不同的(表二)。
    在这里插入图片描述
    由于没有可靠的绝对高度测量数据,LIO-GPS在返回机器人初始位置时,会受到系统漂移的影响,无法完成闭环。LIO-SAM没有这样的问题,因为它利用闭环因子来消除漂移。

    E.阿姆斯特丹数据集

    最后,我们将传感器组件安装在船上,在阿姆斯特丹的运河上巡航了3个小时。尽管在这次测试中,传感器的移动相对平稳,但由于几个原因,绘制运河地图仍然是一项挑战。运河上的许多桥梁构成了退化的场景,因为当船在它们下面时,很少有有用的特征,类似于穿过一条长而无特征的走廊。平面特征的数量也显著减少,因为地面不存在。当太阳光直射在传感器视场中时,我们从激光雷达中获得许多错误观测,这种情况在数据收集过程中20%的时间发生。由于头顶上有桥梁和城市建筑,我们也只能间歇性的获得GPS信号。

    由于这些挑战,LOAM、LIOM和LIOM-odom未能在该测试中产生有意义的结果。类似于在公园数据集中遇到的问题,由于高度漂移,LIO-GPS在返回机器人的初始位置时无法完成闭环,这进一步推动了我们在LIO-SAM中使用闭环。

    F.基准数据集

    由于完整的GPS覆盖仅在公园数据集中可用,我们显示了GPS测量历史的均方根误差(RMSE)结果,它被视为真值。这个RMSE误差没有考虑沿 z z z轴的误差。如表三所示,相对于GPS真值,LIO-GPS和LIO-SAM获得了类似的RMSE误差。
    在这里插入图片描述

    请注意,我们可以通过让这两种方法完全访问所有的GPS测量,将它们的误差进一步减小至少一个数量级。然而,在许多建图场景中,并不总是可以使用完整的GPS访问。我们的目标是设计一个能够在各种挑战性环境中运行的鲁棒系统。

    表四显示了三种竞争方法在所有五个数据集上配准一个激光雷达帧的平均运行时间。
    在这里插入图片描述

    在所有测试中,LOAM和LIO-SAM被迫实时运行。换句话说,当激光雷达旋转速率为 10 H z 10Hz 10Hz时,如果运行时间超过 100 m s 100ms 100ms,则会丢弃一些激光雷达帧。LIOM有无限的时间来处理每个激光雷达帧。如图所示,与其他两种方法相比,LIO-SAM使用更少的运行时间,这使得它更适合在低功耗的嵌入式系统中部署。

    我们还通过以比实时更快的速度向LIO-SAM供数据,对其进行压力测试。与数据回放速度为1倍实时时的结果相比,当LIO-SAM在没有故障的情况下获得类似的性能时,最大数据回放速度被记录并显示在表IV的最后一列。如图所示,LIO-SAM处理数据的速度比实时快13倍。

    我们注意到,LIO-SAM算法的运行时间受特征图密度的影响更大,而不受因子图中节点数和因子的影响。例如,公园数据集是在要素丰富的环境中收集的,在这种环境中,植被会产生大量的特征,而阿姆斯特丹数据集会产生更稀疏的特征地图。虽然公园数据集的因子图由4573个节点和9365个因子组成,而阿姆斯特丹数据集的因子图有23304个节点和49617个因子。尽管如此,相比起公园数据集,LIO-SAM在阿姆斯特丹数据集中运行使用的时间更少。

    结论和讨论

    我们已经提出了LIO-SAM,一个通过平滑和建图进行紧耦合激光雷达惯性里程计的框架,用于在复杂环境中执行实时状态估计和建图。通过在因子图上建立激光雷达-惯性里程计,LIO-SAM特别适用于多传感器融合。额外的传感器测量可以作为新的因子纳入框架。额外的提供绝对测量的传感器,如GPS、指南针或高度计,可用于消除长期积累的雷达惯性里程计漂移,或在特征缺乏的环境中。位置识别也可以很容易地结合到系统中。为了提高系统的实时性能,我们提出了一种滑动窗口方法,将旧的激光雷达帧边缘化以进行扫描匹配。关键帧被选择性地添加到因子图中,并且当生成激光雷达里程计和闭环因子时,新的关键帧仅被配准到固定大小的一组子关键帧中。这种局部尺度而非全局尺度的扫描匹配有助于LIO-SAM框架的实时性能。所提出的方法在不同环境下的三个平台上收集的数据集上进行了全面评估。结果表明,与LOAM和LIOM相比,LIO-SAM可以达到相似或更好的精度。未来的工作包括在无人驾驶飞行器上测试提出的系统。

    展开全文
  • LIO-SAM论文翻译

    千次阅读 2020-12-18 16:53:08
    LIO-SAM: 通过SAM紧耦合雷达惯性里程计 摘要 我们提出一个框架用于通过SAM紧耦合雷达惯性里程计LIO-SAM,可实现高精度、实时的移动机器人轨迹估计和地图构建.LIO-SAM是基于因子图构建雷达惯性里程计,可以将大量的...
  • 作者丨chaochaoSEU@知乎来源丨https://zhuanlan.zhihu.com/p/364651745编辑丨3D视觉工坊文献阅读)多传感器融合之LIO-SAM: Tightl...
  • 点云PCL免费知识星球,点云论文速读。标题:LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapp...
  • 一:Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono),基于视觉-激光-惯导里程计的SLAM框架,环境搭建和跑通过程_goldqiu的博客-CSDN博客 二.激光SLAM框架学习之A-LOAM框架---介绍及其演示_goldqiu的博客-CSDN博客 ...
  • loam 介绍 固态雷达和机械雷达的区别 筛掉不好的点 位姿的迭代估计 外点和动态点的过滤 回环检测 参考链接 loam: lego-loam: cartographer 3D: lio-sam: lvi-sam: livox-loam: PPT: 从2D到3D——Cartographer ...
  • #论文阅读 LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping
  • 文章目录摘要 摘要 LIO-SAM将lidar-惯性里程计置于一个因子图之上,允许从不同来源将多种相对和绝对测量,包括环闭检测,作为因子合并到系统中。
  • 一:Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono),基于视觉-激光-惯导里程计的SLAM框架,环境搭建和跑通过程_goldqiu的博客-CSDN博客 二.激光SLAM框架学习之A-LOAM框架---介绍及其演示_goldqiu的博客-CSDN博客 ...
  •   LIO-SAM是一个使用因子图融合激光,IMU和GPS信息的SLAM算法。论文依次介绍了IMU,激光关键帧,GPS和回环节点。 IMU约束   使用IMU预积分作为里程计的初值,并作为因子图的节点。 激光里程计   在位移和旋转...
  • LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping 本文主要是对LIO-SAM论文的一些学习总结,如有不对之处,希望大家提出宝贵的意见,共同进步~
  • LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping 关键词:紧耦合、惯性测量单元、平滑 摘要 本文提出了一种通过平滑和映射实现紧密耦合激光雷达惯性里程计的框架——LIO-SAM,该框架...
  • LIO-SAM跑KITTI数据集和自己数据集代码修改参考文献 参考文献 1、ubuntu18运行编译LIO-SAM并用官网和自己的数据建图(修改汇总) 2、LIO-SAM运行自己数据包遇到的问题解决–SLAM不学无数术小问题 3、使用开源激光...
  • 作者简介:LVI-SAM是Tixiao Shan的最新力作,Tixiao Shan是Lego-loam(基于激光雷达里程计的SLAM框架)和Lio-sam(基于惯性-雷达紧耦合的SLAM框架)的作者,LVI-SAM是Tixiao Shan最新开源的基于视觉-激光-惯导
  • LVI-SAM: Tightly-coupled Lidar-Visual-Inertial Odometryvia Smoothing and Mapping 摘要 我们提出了一个通过平滑和映射实现的紧耦合激光雷达视觉惯性里程计的框架LVI-SAM,该框架实现了实时状态估计和地图构建,...
  • 继续在家读文献~ 希望下周能继续打工,最近还是更多的关注激光里程计相关的方向。 论文地址:Removal-First Tightly-coupled Lidar Inertial Odometry in ...论文提出了一种动态 SLAM 框架 RF-LIO,它建立在 L
  • Inertial Navigation System Algorithms 一个测试和评估的框架,可以用来检测算法的一致性和鲁棒性 17.CodeVIO: Visual-Inertial Odometry with Learned Optimizable Dense Depth 苏黎世联邦理工 提出了一个深度估计...
  • 使用evo工具评测SLAM

    2020-02-14 16:00:00
    evo是一款用于视觉里程计和slam问题的轨迹评估工具。核心功能是能够绘制相机的轨迹,或评估估计轨迹与真值的误差。支持多种数据集的轨迹格式(TUM、KITTI、EuRoC MAV、ROS的bag),同时支持这些数据格式之间进行相互...
  • 新闻(2020年11月):与LIO-SAM集成 还发布了名为LIO-SAM的扫描上下文集成。 新闻(2020年10月):雷达扫描上下文 上传了用于雷达位置识别的评估代码(又名“雷达扫描上下文”)。 请参阅fast_evaluator_radar目录。...
  • 使用imu_utils标定imu的内参,用于跑slam。包含ceres库安装过程
  • FAST-LIO2基于一个高效的紧密耦合迭代卡尔曼滤波器,FAST-LIO2有两个关键的新特性,允许快速、鲁棒和精确的激光雷达导航(和建图)。第一个方法是将原始点直接配准到地图上(随后更新地图,即建图),而不提取特征。...
  • 直接计算相机位姿的真实值与SLAM系统的估计值之间的差,程序首先根据位姿的时间戳将真实值和估计值进行对齐, 然后计算每对位姿之间的差值, 并最终以图表的形式输出, 该标准非常适合于评估视觉 SLAM 系统的性能。...
  • 彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM) 9.从零搭建一套结构光3D重建系统[理论+源码+实践] 10.单目深度估计方法:算法梳理与代码实现 11.自动驾驶中的深度学习模型部署...
  • 但是当我们去掉GNSS测量时,LIO_SAM估计的状态会出现跳变,由此产生的误差比LOAM_IMU更大。这是因为LIO_SAM在估计时使用的特征比LOAM_IMU少。此外,我们发现在室外环境中,从LiDAR中提取的特征相对稀疏,所以与室内...
  • RF-LIO: 面向高动态场景的紧耦合LiDAR惯导融合里程计单位:西安交通大学针对问题:实际场景中动态因素的引入造成基于静态假设的LIO严重位姿漂移提出方法:提出基于自适应的多分辨率Ra...
  • 特征点配准最主流的做法是提取特征点,然后优化特征点的 point-to-line/surface 距离,如 LOAM/LeGO-LOAM/Lio-SAM,以及基于深度学习提取特征的 CAE-LO 等,但深度学习的方法受限于特征必须要事先学习。 ...
  • 点击上方“3D视觉工坊”,选择“星标”干货第一...现有的基于Heatmap的两阶段方法并不是最优的,因为它们不是端到端训练的,且训练依赖于替代L1损失,不等价于最大化评估度量,即目标关键点相似度(OKS)。YOLO-Pose...

空空如也

空空如也

1 2 3 4 5 ... 9
收藏数 168
精华内容 67
关键字:

评估lio-sam