开源代码：GitHub - HaisenbergPeng/ROLL: A real-time, robust LiDAR-inertial localization system

ROLL: Long-Term Robust LiDAR-based Localization With Temporary Mapping in Changing Environments

ROLL：基于 LiDAR 的长期稳健定位，在不断变化的环境中提供临时测绘

　　摘要：长期场景变化给使用预建地图的定位系统带来了挑战。本文提出了一种基于激光雷达的系统，可以提供强大的定位来应对这些挑战。当对预构建地图的全局匹配不可靠时，我们的方法首先暂时激活建图过程。一旦再次获得可靠的匹配，临时地图将合并到预构建的地图上，以便以后的定位运行。我们进一步集成 LiDAR 惯性里程计 (LIO)，为全局匹配模块提供运动补偿 LiDAR 扫描和可靠的初始位姿猜测。为了生成用于导航目的的平滑实时轨迹，我们通过解决位姿图优化问题来融合里程计和全局匹配的位姿。我们在 NCLT 数据集（包括各种不断变化的室内和室外环境）上进行了大量实验来评估我们的定位系统，结果证明了一年多来稳健且准确的定位性能。这些实现在 GitHub1 上开源。

　　I. 简介

　　定位是许多具有自导航目的的机器人应用的一项基本功能。多种技术可以实现定位，但都有局限性。全球定位系统 (GPS) 可以在开放区域提供绝对定位，但会受到高层建筑附近或下方的多径效应的影响。如果没有这些问题，基于摄像头的定位系统便宜、准确且易于部署，但由于光照变化和尺度不确定性，它们的性能会下降。光探测和测距 (LiDAR) 传感器主动发射激光点来感知 3D 环境，因此本质上不受这些问题的影响。随着激光雷达技术的发展，激光雷达的价格现在已经可以被许多商业应用所接受。因此我们选择 LiDAR 作为我们的主要传感器。

　　基于 LiDAR 的定位系统通常需要专门运行来构建环境地图，并通过将当前 LiDAR 点与地图匹配来获得位姿估计。对于大规模户外场景，采用高度压缩格式的 LiDAR 点云图 [1]、[2]、[3] 来解决定位鲁棒性和存储问题。然而，这些地图并不适合部署在室内和室外的机器人以及智能轮椅、野外救援机器人等户外环境。此外，狭窄和开放场景需要不同级别的地图密度，以在内存消耗和定位精度之间取得平衡。 Posemap [5] 中的地图表示可以解决这些问题。它包括一组 LiDAR 关键帧。每个关键帧都在独特的位置进行观察，并与观察位姿相关联。然而，对这样的地图的匹配仍然存在以下问题：（1）在不断变化的环境中，从与预先构建的地图的全局匹配获得的位姿可能是有噪声的、不连续的甚至是错误的。 (2)全局匹配对初始值非常敏感，通常通过连续帧之间的匹配来提供。后者可能会受到快速俯仰运动（在激光雷达点密度稀疏的方向上）的严重影响。

　　为了解决上述问题，提出了一种具有临时建图功能的长期稳健的基于 LiDAR 的定位系统，名为“ROLL”。具体地，当匹配质量较低时，将暂时激活建图过程。在这些区域中，不可靠的全局匹配位姿将被放弃，以防止潜在的定位失败，并且关键帧将使用从 LiDAR 惯性里程计 (LIO) 导出的位姿进行注册。当进入具有可靠匹配的区域时，临时关键帧将被优化，然后将合并到全局地图中。临时建图允许系统在显着变化甚至部分未建图的区域提供强大的定位。除了用于临时建图之外，LIO 还可以提供运动补偿 LiDAR 扫描和用于全局匹配的准确位姿初始值。我们进一步将其与融合方案中的噪声全局匹配位姿结合起来[6]，以生成用于导航目的的平滑轨迹。然而，优化并不能保证达到全局最优，有时可能会逐渐超出范围。因此，我们实施一致性检查来避免此类问题。

　　所提出的系统在大规模、长期开放数据集的序列上进行评估[7]。这些序列涵盖了各种具有挑战性的场景，包括植被变化、建筑施工、长走廊中的门打开和关闭（如图 1 所示）。该系统一年多来表现稳定、准确的定位性能。

　　我们的主要贡献总结如下：

　　• 我们提出了一个包含临时建图的强大的全局匹配模块，可以防止在场景发生重大变化或地图覆盖不足的区域中出现定位失败。一旦匹配再次可靠，就可以将临时地图合并到全局地图上。

　　• 我们将融合方案扩展到LIO 和噪声全局匹配的轨迹。通过对导出的里程计漂移进行一致性检查，我们成功地防止了优化结果超出范围。

　　• 我们在大规模、长期数据集上评估定位系统。结果表明，该系统可以在各种变化的环境中生存，并表现出实时、稳健和准确的定位性能。

　　• 为了造福社区，我们将实现开源。

图 1. NCLT 数据集中的场景变化。我们利用同一区域不同序列的激光雷达数据构建了密集的局部地图进行比较。

（a）和（b）来自停车场。我们可以看到树木（P1）正在改变外观，汽车（P2）停在不同的地方。 （c）和（d）来自一条长走廊。一些门（P3）在（d）中打开。

　　二.相关工作

　　在长期工作过程中，不断变化的环境和预先构建的地图之间的不一致给定位系统带来了挑战。在本节中，我们将回顾几种应对挑战的策略（无论传感方式如何），并讨论它们在混合环境中 LiDAR 定位的潜在适用性。

　　在没有太多地形变化的大规模城市场景中，研究人员使用选择性压缩的环境表示作为地图来应对场景变化。早期的方法将注册的 LiDAR 扫描压缩为反射率图并达到厘米级精度 [8]、[9]。然而，在恶劣天气下，当雨或雪改变道路反射率时，精度可能会受到严重影响。进一步的研究在先验图中添加结构信息以增加鲁棒性，例如高斯混合图[2]，结合反射率和高度信息[10]，[1]，或提取杆状地标[11]，[3]等以上所有地图均不能应用于室内和室外场景的混合环境。

　　丘吉尔和纽曼 [12] 积累了同一地点的每种不同的视觉体验，并根据这些体验进行定位。事实证明，它可以应对一定程度的环境变化。莱恩加尔等人。 [13]设计了一种内存策略来优先加载过去的经验。与之前的方法相比，它在长期定位方面表现出更好的性能，且计算资源更少。马德恩等人。 [14]进一步将基于经验的定位扩展到2D推扫式激光雷达。它在变化的城市场景中实现了一年多的亚米级定位精度。

　　一些研究人员没有积累经验，而是利用数学模型来预测图中基本状态的变化。蒂尔帕迪等人。 [15] 将 2D LiDAR SLAM 中的占用网格状态建模为马尔可夫过程，实现了优于其他现有方法的最先进性能；克拉伊尼克等人。 [16]将视觉SLAM中的规则特征点视为调和函数的组合；将建模扩展到非周期性环境变化，自回归移动平均模型被用于视觉惯性 SLAM [17] 和 2D LiDAR SLAM [18]。

　　然而，对于使用点云图的 LiDAR 定位应用来说，基于经验和基于预测的方法在计算上都非常昂贵。最近，坎波斯等人。提出了一种基于视觉的SLAM系统ORBSLAM3[19]。不是针对多种体验进行定位，而是仅使用一张活动地图进行全局匹配，并且通过循环闭合来执行地图合并。所提出的临时建图与他们的方法类似。但在我们的激光雷达系统中，我们选择不维护多个非活动地图，因为它既不必要又计算成本昂贵。

　　三．方法

　　本节详细描述了所提出的定位系统的架构。该系统基于以下假设：（1）虽然我们只使用 LiDAR 和 IMU 进行长期定位，但用于构建全局一致地图的传感器没有限制。这是有道理的，因为在实际应用中，通常需要配备装备精良的传感器套件的专用运行来绘制感兴趣的地点。 (2) 机器人在预建地图中的初始位姿已知。它可以从 GPS 或开源地点识别算法（例如 Scan Context [20]）获得。

　　A. 系统概述

图 2. 所提出的基于 LiDAR 的定位系统架构。

　　我们的定位系统的架构如图2所示，它由四个模块组成：LiDAR惯性里程计（LIO）、全局匹配、临时建图和位姿融合。我们的系统对于任何 LIO 都是通用的，因为我们只需要来自里程计的自我运动估计和运动补偿 LiDAR 扫描。全局匹配模块（第 III-B 节）将提取的 LiDAR 特征点投影到全局地图上，并根据里程计得出初始猜测。然后，它对当前机器人位姿进行优化，以减少相关特征点之间的残差。在临时建图模块（第III-C节）中，计算匹配内点比率以检测场景变化显着或地图覆盖不足的区域，我们将在下面将其称为异常。检测到异常时将激活临时建图。为了生成平滑的轨迹，通过解决位姿融合模块中的图优化问题来融合里程计位姿和全局匹配位姿（第 III-D 节）。

　　B.全局匹配

　　全局匹配模块改编自LOAM [4]，在特征关联方面进行了一些增强。为了理解以下章节，需要进行简要介绍。

　　1）全局地图：全局地图包含一系列关键帧，这些关键帧由相应的优化观察位姿组织而成。对传入的运动补偿激光雷达帧进行下采样，并根据平滑度分为边缘特征和表面特征。每当机器人经过一定的平移或旋转距离时，这些特征就会组合在一起形成特征关键帧。当触发临时建图时，全局地图将会更新。

　　选择这样的特征关键帧而不是单个地图的原因如下：（1）特征关键帧是灵活的，因为可以针对闭环或地图扩展来优化和调整相关位姿。 （2）在全局匹配中，使用此类特征关键帧提取附近点云的效率更高。 (3)允许不同地图区域的密度不同。对于大规模站点的操作，需要进一步对全局地图进行下采样以减少内存消耗。然而，对于覆盖室内和室外场景的应用，用于下采样的体素滤波器尺寸应该在密闭环境和开放环境之间变化。这里提出了一种简单的算法来确定体素大小。如果特征关键帧中靠近 LiDAR 点（距离小于 dc，以米为单位）的比率超过某个阈值 (rc)，则我们定义空间受限。对于走廊等受限区域，体素滤波器尺寸相对较小，以增加覆盖强度。

　　2）特征提取和关联：将LiDAR坐标系表示为L，地图坐标系M和odom坐标系O，这里引入odom坐标系来描述里程计的固有漂移。在时间 k，LIO 的运动补偿 LiDAR 扫描被下采样并分为边缘特征点 Ek 和表面特征点 Sk。对于每个位姿输出 ̃ TO L,k 来自 LIO。我们在地图框中得到位姿猜测：

　　其中 TM O,k−1 是在时间 k − 1 时从奥多姆坐标系到地图坐标系的变换，这里我们假设一个恒速模型。请注意，使用直接变换的位姿猜测也用于实时位姿输出，以提供针对第 III-D 节中的融合位姿的基线。比较见表四

　　利用位姿猜测̃ TM L,k，对特征关键帧的观察位姿进行一定距离内的kdtree搜索，提取附近的云进行匹配。相应的特征关键帧一起形成为一个局部边缘图Pk和局部表面图Qk。这些局部建图存储在两个 kdtree 中，以便在优化过程中进一步快速搜索。对于每个边缘点 pk,i ∈ Ek 用 ̃ TM L,k 投影到图框上后，通过 kdtree 搜索找到局部边缘图 Pk 中两个最接近的点 pk,i+1、pk,i+2。残差是点到线的距离：

 　　对于投影后的每个表面点 qk,i ∈ Sk，通过 kdtree 搜索找到表面图 Qk 中最近的三个点 qk,i+1、qk,i+2、qk,i+3。残差是点到面的距离：

 　　3) 残差计算和变换更新：我们有一个关于 LiDAR 位姿 ̃ TM L,k、特征点 mk,i 和局部地图的残差非线性函数：

 　　将所有特征点的残差叠加，得到列向量f(̃TM L,k)。由于其高度非线性，优化位姿 ̄ TM L,k 是通过迭代降低残差向量 f( ̃ TM L,k) 的范数得到的。更多详细信息请参阅原始论文。  然后使用 TM L,k 更新 odom 和地图框 TM O,k 之间的变换，以进行下一次全局匹配。

　　C. 临时建图

　　在长期定位中，全局匹配的优化位姿 ̄ TM L,k 并不总是适合变换更新，因为地图异常可能导致附近的特征关键帧对于匹配不可靠。

　　1) 地图异常检测：对于优化的位姿 ̄ TM L,k ，我们定义一个称为匹配内点比率的标准来检测这些异常：

 　　其中 Γ(p, q) 是获取 p 中小于 q 的元素比例的函数。这里dt是一个经验值，我们选择dt = 1.0，这足以满足实验中的所有会话。 μk 被设计为一种相当宽容的方式来检查当前传感器观测值与历史地图之间的不一致，以便仅在发生严重不一致时才激活临时建图

　　2）关键帧剔除：当μk低于某个阈值μE时，系统进入临时建图模式。在此模式下，TM L,k 不再用于变换更新，并且使用常数 TM O,k 从 LIO 变换而来的机器人位姿用于剔除特征关键帧。剔除策略与构建全球地图时使用的策略相同。这些关键帧与历史关键帧不同，只是暂时存在，因此被称为“临时关键帧”。请注意，当未检测到地图异常时，临时关键帧会以滑动窗口方式保留一定数量的帧。如图 3 所示，在位姿 xM 处开始临时建图之前，已经存储了由具有全局匹配位姿的最近观察结果组成的 M − 1 个临时关键帧。它是为以后的位姿优化而设计的，因为当面对显着的地图异常时，xM 附近的全局匹配位姿被认为是不可靠的。同时，正在计算 μk 以用于具有更高阈值 μM 的潜在地图合并。一旦 μk 高于 μM，全局匹配位姿 ̄ TM L,k 被认为再次可靠，可以更新 TM O,k 。

图3.临时关键帧观察位姿的图优化示意图。请注意，这里的优化与传统的闭环优化不同，因为我们认为历史关键帧的观察位姿不需要校正

 　　3) 位姿优化：为了合并临时关键帧，对观察位姿 χt = {x0, x1, ..., xN} 执行位姿图优化，其中 xi ∈ SE(3)。如图3所示。“里程计因子”表示两个位姿之间的关联，优化的残差可以表示为：

 　　其中 ̃ xi j 表示位姿 xi 和 x j 之间的里程计观测值，对数运算是以向量形式从李群建图到李代数 se(3)。 “全局匹配因子”是一元因子，将残差定义为：

 　　其中 ̃ xi 表示来自可靠全局匹配的位姿约束。通过求解非线性最小二乘问题来优化残差：

　　其中马哈拉诺比斯范数 ‖r‖Ω = rT Ω−1r。协方差矩阵ΩO i 和ΩG i 根据经验设置为常数对角矩阵。然后，通过替换附近的历史特征关键帧，将优化后的临时关键帧合并到全局地图中。

　　D. 位姿融合

　　位姿融合的关键是获得优化的里程漂移（表示为 TM O ），因为我们输出位姿估计与 LIO 一样快。每个里程计位姿的实时位姿输出 ̃ TO L,k ：

　　1）滑动窗口优化：我们扩展了融合方案[6]以将连续里程计位姿与噪声全局匹配位姿融合。本质上，对于每个可靠的全局匹配位姿到达（不存在地图异常），表示 n 个累积里程计位姿 χO = { ̃ TO L,k−n+1, ̃ TO L,k−n+2, ..., ̃ TO L,k} 和 m 个全局匹配构成 χM = { ̃ TM L,k−n+1, ̃ TM L,k−n+2, ..., ̃ TM L,k}。最大似然估计问题是根据地图框架 χ f = {Tk−n+1, Tk−n+2, ..., Tk} 中的机器人位姿制定的，以导出 TM O，注意这里我们省略了框架的表达式为了简单起见，在符号中。请注意，这里我们遵循[6]中使用的约定，将位姿表示为 T = {p, q}，其中 p 和 q 是位姿 T 的相应位置和方向四元数。假设机器人位姿的所有测量都是独立的并且具有高斯分布分布 p(zk|χ f ) ∼ N ( ̃ zk, Ωk)，问题可推导为：

 　　对于里程计位姿测量，残差：

 　　其中符号定义了获取两个位姿之间差异的操作。对于平移部分，是逐元素减法，而对于四元数 qi = {qi,s, ˇ qi}，是虚部 ˇ qi 的逐元素减法。对于全局匹配位姿测量，残差：

 　　为了减少计算负担，我们设置 n < nt，以便当队列大小超过限制时，最新的里程计位姿将弹出队列中最旧的位姿。上述最小二乘问题可以使用开源非线性求解器 Ceres [21] 来求解。让我们表示优化后的机器人位姿 χ* f = {T* k−n+1, T* k−n+2, ..., T* k}，对于每个 T* i ，对应的里程计漂移：

 　　2）一致性检查：在原始算法中，使用最近的里程漂移TM*O,k来更新TMO。由于场景变化引入的全局匹配噪声，解决方案可能会陷入局部最优，并且继续使用导出的里程计漂移进行初始值更新最终会导致发散。我们对里程计漂移进行一致性检查，直觉是漂移在 nt​​ 帧内应该很小。漂移变化定义为：

 　　其中 ΔTM O = {ΔpM O , ΔqM O }。如果距离变化 ∥ ∥ΔpM O ∥ ∥ 超过某个阈值，则认为优化陷入局部最优。为了重置该值，我们使用最近的里程计位姿和全局匹配位姿来获得里程计漂移，其可以表示为：

 　　请注意，融合是作为独立线程实现的，因此更新的里程计漂移不会影响全局匹配的位姿猜测。

　　四．实验

　　所有实验均在配备 Intel Xeon W-2102 [email protected] GHz 和 16 GB RAM 的台式机上进行。该算法在 NCLT 数据集的 10 个序列上进行了测试 [7]。 NCLT数据集是由UGV（无人车）采集的大规模、多模态、长期的解剖数据集地面车辆）在北密歇根大学北校区工作了一年多。覆盖室内外环境，场景变化多样，如下雪、树叶飘落、建筑施工、长廊开门关门等。例如，白天下雪“12-01-15 ”、“13-01-10”、“13-0223”（为简单起见，使用序列记录的日期“年月日”来指代序列）。此外，序列“12-05-11”和“12-11-17”覆盖了一些未探索的区域，这需要定位系统能够扩展预先构建的地图。数据集的地面实况是从所有会话的 RTK（实时运动学）GPS 和 LiDAR 扫描匹配的大型离线 SLAM 解决方案中获得的。

　　由于该算法主要是为定位而设计的，因此我们使用“2015 年 12 月 1 日”记录的 UGV 和 LiDAR 数据的地面真实位姿来构建初始全局地图和其他 9 个序列进行定位实验。对初始全局地图进行下采样后，整个校园的地图大小为 53.8 MB，大小约为 800×600 m2。完成最后一个会话后，其大小最终为 61.9 MB。图 4 显示了下采样的演示。我们可以看到实现了不同的关键帧密度。我们选择 FAST-LIO2 [22] 作为我们的 LiDAR 惯性里程计，因为它比其他现有方法表现出最先进的性能。

图 4. 定位系统中使用的地图演示。地图在开放区域和受限区域使用不同尺寸的滤镜进行处理，每个红点代表一个特征关键帧。右两图是黄色矩形区域的放大图，在图中的右上方可以观察到较密集的红点。

　　对于定位运行，UGV 的初始位姿由地面实况给出。初始化之后我们直接使用来自 Velodyne HDL-32E LiDAR 和 Microstrain 3DM-GX3-45 IMU 的原始数据。一些参数根据经验设置如下：dc = 5.0，rc = 0.3，μE = 0.3，μM = 0.5，pt = 0.5，nt = 100。

　　A.长期定位性能

　　如表一所示，我们用常用的绝对轨迹误差来评估定位精度[23]。具体来说，我们将定位位姿 Xi(Xi ∈ SE(3), i = 1, ..., n) 与地面真值 Gi(Gi ∈ SE(3), i = 1, ..., m) 进行比较，得到与时间相关的绝对平移误差 Fi，时间同步是通过 kdtree 在 50 ms 内搜索真实值的时间线。除了 RMSE 之外，为了呈现长期定位性能的整体情况，我们引入了平移距离误差 ei = ‖trans(Fi)‖ 的误差百分比。误差百分比定义为所有ei中小于某一阈值的ei的比例。在所有会话中，该算法展示了 0.239 米的 RMSE 和实时性能（9.91 Hz，大约是 LiDAR 数据的速率）。

　　每个会话仅激活临时建图几次。激活位置在误差轨迹中标记，如图 5 所示。地图异常的两个示例用黄色和红色矩形标记。从误差轨迹中，我们可以观察到红色矩形标记的区域相对于其他区域表现出更大的定位误差。这可以通过该地区的地图扩展来解释。黄色矩形标记的区域是图1中提到的停车场。该区域非常开放，可用于匹配的稳定特征很少，提示激活临时建图。

图 5. 测试的 9 个 NCLT 序列的定位轨迹，用平移距离误差着色。红点表示检测到地图异常的位置。两种类型的地图异常由黄色（重大场景变化）和红色（地图覆盖不足）矩形标记

　　比较：在表 II 中，我们将我们的算法与序列“12-05-11”上最先进的 LiDAR SLAM LOAM [4] 的变体进行了比较。具体来说，对于“LOAM(M)”，我们使用与 ROLL 相同的地图表示形式来保存和加载地图，但我们仅匹配没有点云注册的预构建地图。对于“LOAM(M)+TM”，我们将临时建图集成到“LOAM(M)”中，以便为场景更改启用地图更新。为了公平比较，这两种基线算法也使用地面实况地图，并且用于比较的定位位姿是通过 LiDAR 里程计和建图之间的直接转换生成的，类似于等式 1。因为并非所有算法都能在整个会话中生存，我们定义一个成功率（S.R.）来指代所有激光雷达帧中 ei 小于一米的帧的比例。由于临时建图，“LOAM(M)+TM”在鲁棒性上比“LOAM(M)”有显着提高。在 LIO 和融合算法的辅助下，ROLL 的性能优于两者。位置误差比较如图 6 所示。“LOAM(M)”由于进入部分未建图区域而在 440 秒左右失败； ROLL和“LOAM(M)+TM”都可以在整个会话中生存，但ROLL的误差明显小于“LOAM(M)+TM”。

图 6. 与基线算法的位置和方向误差比较

　　B. 地图更新性能

　　地图更新的示例如图 7 所示。如图 7(a) 所示，在运行序列“12-05-11”之前，全局地图的一部分尚未探索，但算法能够识别该异常并更新该区域的地图，如图 7(b) 中的白色点云所示。随后在序列“12-06-15”中，一个建筑工地也成功更新到全球地图上，如图7（c）所示。以上都证明了算法的鲁棒性。针对地图异常，包括地图覆盖不足和重大场景变化。

图 7. 地图更新演示。注意圆圈区域。 （a）“11年5月12日”之前部分未测绘区域的点云地图鸟瞰图； (b) 与(a)相同的视图，在“12-0511”天更新地图后，白色点云是合并的临时地图； (c) 与 (b) 相同的视图，在“12-06-15”天更新地图后，我们可以观察到施工围栏已更新到全局地图上。

　　C. 消融研究

　　为了评估所提出的方法对整个算法的贡献，我们进行了两组消融实验。第一组实验对四个序列进行，以证明临时建图和一致性检查的必要性，如表III所示。选择成功率作为评价标准。如果没有临时建图，当访问序列“12-02-02”中发生显着变化的区域或序列“12-05-11”中部分未建图的区域时，算法有时会失败。此外，消融实验也清楚地解释了一致性检查的必要性。原来的融合方法，如果不加修改就直接使用的话，连百分之十的会话都无法处理。对所有序列进行第二组实验，以比较位姿融合（方程 9）与直接变换（方程 1），如表 IV 所示。在最大误差略有牺牲的情况下，位姿融合总体上表现出更好的误差百分比。

　　五、结论

　　我们提出了一个长期稳健的基于 LiDAR 的定位系统。它采用临时建图模块，以避免在显着变化或部分未建图的区域中由于错误的全局匹配而导致潜在的定位失败。激活临时建图可以更新全局地图上的这些区域，以供以后的定位会话使用。此外，LiDAR 惯性里程计的结合提供了具有运动补偿 LiDAR 点和不受场景变化影响的初始位姿的全局匹配模块。改进了全局匹配与里程计的融合方案，优化后对里程计漂移进行了一致性检查，显着提高了融合算法对场景变化的鲁棒性。对 NCLT 数据集的评估证明，所提出的系统可以在一年多的变化的室内和室外环境中提供实时、准确和鲁棒的定位。在未来的工作中，我们计划针对不同环境何时进入或退出临时建图设计一种自适应方案，而不是设置恒定的阈值。