0. 简介

同时定位与地图构建（SLAM）技术广泛应用于地面机器人、无人机和自动驾驶汽车。本文介绍了一种高效、稳健和准确的激光雷达SLAM系统LTA-OM。该系统采用FAST-LIO2和稳定三角形描述符作为激光雷达-惯性测量单元（IMU）里程计和环路检测方法。LTA-OM实现了功能完备，包括环路检测和修正、误报环路闭合拒绝、长期关联地图和多会话定位与地图构建。本文的一个创新之处是实时长期关联（LTA）地图构建，它利用FAST-LIO2的直接扫描到地图的配准，并利用修正后的历史地图在全局上约束地图构建过程。LTA可以实现更加全局一致的地图构建和无漂移的里程计在重访地点。我们对LTA-OM和其他最先进的激光雷达系统进行了全面的基准测试，使用了18个数据序列。结果表明，LTA-OM在轨迹准确性、地图一致性和时间消耗方面稳定优于其他系统。LTA-OM的稳健性在一个具有相似结构的多层建筑的复杂场景中得到了验证。此外，设计了多会话模式，允许用户存储当前会话的结果，包括修正后的地图点、优化的里程计和描述符数据库，以供将来的会话使用。这种模式的好处是进一步提高准确性和一致的地图拼接，有助于终身地图构建。此外，LTA-OM具有对机器人控制和路径规划有价值的特性，包括高频率和实时的里程计、在重访地点无漂移的里程计和快速环路闭合收敛。此外，LTA-OM具有通用性，适用于多线旋转和固态激光雷达、移动机器人和手持平台。

1. 主要贡献

总结一下，本报告的主要贡献包括：

1. LTA-OM：一种高效、稳健和准确的SLAM系统。我们将FAST-LIO2、STD-LCD和循环优化集成到系统中。通过广泛的基准实验，包括城市、校园和非人工环境，以及相同或相反方向、小重叠重访和不同时间持续性（8∼112分钟）或行驶距离（1.1∼7.5公里），证明了LTA-OM的效率和准确性优于最先进的激光雷达系统。我们还将我们的系统与一个结构相似的场景进行了测试——一个具有不同层次的类似走廊的多层建筑，我们的系统仍然可以正常运行并构建出一个良好的地图。
2. 实时长期关联。我们的工作利用历史地图点在重访地点对LIO的在线定位和建图进行全局约束。当循环优化完成后，我们使用循环优化结果来校正FAST-LIO2的姿态状态，并动态从历史地图中重新加载地图点到实时地图中进行在线LIO。通过这种方式，LIO的地图构建变得全局一致，由于使用了校正后的历史地图，LIO的定位变得无漂移。此外，姿态校正和实时地图更新的机制经过精心设计，以实现实时性能并避免长时间的LIO延迟。
3. 多会话模式。我们的SLAM系统中的长期关联自然地扩展到多会话模式，用户可以将LTA-OM的地图存储在上一个会话中，并在当前会话中加载它进行重定位。具体而言，在这种模式下，系统首先使用LTA-OM的循环检测模块在预存储的地图中重新定位。然后，LTA-OM的LIO模块动态加载预存储的地图到当前会话中构建的实时地图，以约束在线定位和建图。结果，预存储的地图和实时地图自然地拼接在一起，LIO的姿态在预存储的地图上变得无漂移。当机器人进入预存储地图中未探索的区域时，LTA-OM会将新的点追加到拼接地图中，从而自动扩展地图而无需进一步合并。

2. 主要框架

我们系统的流程如图2所示。我们的系统由四个核心模块组成：LiDAR-IMU里程计（LIO）、回环检测、回环优化和回环校正。
流程工作流程简要说明如下。LIO模块是FAST-LIO2的一种变体，它包含了长期关联，提供了注册的扫描和估计的里程计给回环检测模块。回环检测模块STD-LCD提取关键点并检测回环闭合以供回环优化后端使用。然后，回环优化利用提供的里程计、关键点和回环闭合信息构建和维护一个位姿图，然后在检测到回环闭合时对位姿图进行优化。一旦成功完成回环优化，回环校正就利用优化后的里程计来校正位姿状态并替换LIO模块的地图（即ikd-Tree）

图2：LTA-OM的主要系统组件。带有“*”标记的组件仅在存在预存地图时需要（即所谓的多会话模式）

3. 雷达IMU里程计

我们采用FAST-LIO2作为我们的LIO模块。FAST-LIO2基于紧耦合迭代卡尔曼滤波器。它处理了LiDAR测量中存在的运动畸变问题，并为LiDAR和IMU帧提供在线外部估计。此外，它采用增量kd树（ikd-Tree）实现快速的直接（即无需特征提取）扫描到地图的注册和状态更新。

1）定位：FAST-LIO2维护一个状态向量，包含全局IMU姿态、全局IMU速度、IMU偏差、重力向量和LiDAR到IMU的转换状态。它使用IMU测量和IMU噪声模型来前向传播状态。随后，使用带有IMU测量的运动模型，将LiDAR点向后传播到准确位置。向后传播可以补偿点的运动畸变。然后，使用补偿后的点计算状态更新残差和卡尔曼增益。最后，FAST-LIO2迭代更新状态直到收敛。

2）长期关联的建图：FAST-LIO2引入ikd-Tree实现增量点插入和删除。当将新点插入传统的kd树时，会破坏树的原始平衡，需要耗时的树重建。ikd-Tree引入了基于盒子的删除和延迟标签，以提高点删除效率，并采用并行树重建策略，以避免建图模块受到重建延迟的影响。此外，定位模块中提到的残差计算依赖于ikd-Tree。未注册的点被用作参考点，在ikd-Tree上搜索五个相邻点。这些相邻点与一个平面拟合，并计算状态更新残差作为点到平面的距离。通过迭代最小化残差，可以得到新的扫描姿态，然后将新的扫描与ikd-Tree和全局历史地图进行注册。

我们的里程计与标准的FAST-LIO2的一个关键区别是在建图模块中采用了长期关联（LTA）。除了合并新的扫描外，我们提议从全局历史地图中加载点到ikd-Tree，以便全局历史地图在在线状态更新中起到约束作用。具体而言，我们动态加载当前机器人位置周围的地图点到ikd-Tree。因此，ikd-Tree，即实时地图，包含了来自新扫描和全局历史地图的点，然后将随后的LiDAR扫描与来自历史地图的长期约束进行注册。

我们加载包含在历史扫描中的点，其姿态在当前LIO姿态的一定范围内（即加载范围）。在从历史扫描中加载点时，我们使用由循环优化模块更新的扫描姿态来校正点的位置。然后，如果已经在先前的加载过程中考虑过，将标记历史扫描索引为“已加载”，以避免冗余操作。因此，动态加载定期以有限的频率进行：当LIO姿态接近最后一个加载范围的边界时，将进行下一次动态加载。此外，这种动态加载策略自然地导致一个有趣的特性：当机器人移动到先前探索过的区域时，LTA将自动生效，将实时地图与历史地图在循环校正后合并；当机器人移动到未探索的区域（没有周围的历史扫描）时，实时地图将自动从历史地图中增长，而不受LTA的影响。

4. 环路闭合检测

我们使用STD-LCD作为我们的环路闭合检测模块。STD-LCD能够高效地检测环路闭合，并提供可靠的环路闭合几何变换。此外，它还能处理小重叠回环的挑战情况。
STD-LCD的详细工作流程如图2所示。具体而言，子地图累积组件用于累积来自LIO的足够注册的扫描数据（默认情况下，对于多线旋转激光雷达，累积20个移动激光雷达扫描）。累积子地图的姿态被选择为最终扫描的扫描姿态（即，激光雷达坐标系相对于全局坐标系）。此外，子地图索引被用于标记存储在历史地图中的扫描数据，以供未来的地图点修正过程使用。其他组件的详细说明如下。
1)关键点提取和描述符构建：使用累积的子地图，STD-LCD对子地图进行体素化，并通过检查平面点比率来找到平面体素。平面体素被合并成更大的平面。非平面体素的点被投影到相邻平面上构建图像。通过图像，通过找到局部最大点-平面距离的像素来提取关键点。此外，进行空间非最大值抑制以增强提取的可重复性。每三个提取的关键点可以构建一个三角形。基于三角形，STD-LCD将描述符设计为一个六维数组，包括三个升序三角形边长和三个投影法向量点积值。投影法向量被定义为点云表面上每个关键点的法向量。
2)环路闭合检索和验证：环路闭合检索和验证包括三个部分：粗略环路检测、精细环路检测和几何验证。粗略环路检测通过在描述符数据库的哈希表中搜索匹配的历史描述符，快速提出N个可能的候选项。精细环路检测通过计算成对三角形之间的所有变换矩阵，并对变换进行聚类，找到支持最多的变换。精细候选项是指最大变换支持者大于4（默认情况下）的候选项。最后，计算精细候选项的点-平面距离，并选择具有最小距离的候选项作为最佳环路闭合结果。只有当最佳候选项的整体点-平面距离小于阈值时，才接受最佳候选项作为环路闭合结果。点-平面距离的计算速度很快，因为精细环路检测提供了一个良好的初始猜测，并且只使用源子地图的关键点（<50）进行点-平面距离的计算。
注1：在环路检测中累积数十个激光雷达扫描数据不可避免地会对后续的环路优化和修正过程引入延迟。然而，由于具有良好的局部精度和较小的延迟，我们的在线LIO里程计也满足了路径规划或机器人控制的需求，这些需求通常需要局部信息。此外，在一次环路修正之后，LIO里程计在重访地点也会具有良好的全局精度（见第5节）。

5. 循环优化

本小节包括循环优化的三个关键部分：图构建与优化、误报拒绝和关键点因子边缘化。

5.1 图构建与优化

图3：图形概述。

图形表示优化问题中不同约束之间的关系。我们使用的图形（见图3）是传统位姿图的变体版本。传统位姿图由位姿节点、里程计因子和闭环因子组成。位姿图优化的数学表达式如下所示。

在这里，^GX表示估计的子地图姿态的集合，相对于全局坐标系，索引为k = [1, …, M]，M为子地图姿态的数量，\tilde{T}_{ij}和T_{ij}分别表示第i和j个子地图姿态之间的测量和估计的变换，O和L分别表示里程计约束和闭环约束的索引对集合。
我们使用从闭环检测模块提取的关键点，并为相邻的姿态节点添加关键点因子。此外，我们使用闭环关键点因子代替闭环因子。

我们的位姿图优化的数学表达如下所示

在这里，^L\tilde{P}_i和^L\tilde{P}_j分别表示i和j子地图的相关关键点对，^LP表示相对于局部子地图坐标系的关键点，K_N和K_L分别表示相邻和闭环关键点约束的索引对集合。通过在全局坐标系中使用kd树搜索和半径阈值来关联邻近关键点对，而STD-LCD则关联闭环关键点对。一旦闭环关键点约束插入到图中，将执行位姿图优化来进行闭环优化。

5.2 假阳性拒绝

假阳性闭环对位姿图优化产生了严重影响，因此我们引入了一个假阳性拒绝（False Positive Rejection，FPR）模块来排除它们。我们的FPR方法基于一个事实，即优化后的关键点因子应该输出合理的残差大小。换句话说，当假阳性闭环约束引入到闭环优化中时，结果地图中相同表面或边缘的明显分离将出现，导致关键点对之间的距离变大（即不合理的因子残差）。我们提出的FPR方法的详细伪代码如算法1所示。我们首先计算并阈值化闭环子地图对的重叠比例。尽管STD-LCD可以处理小重叠情况，但在我们的系统中，我们只信任重叠比例大于0.5的闭环（第1至3行）。在每次闭环优化之前，我们备份图和其变量值（第8行）。然后，我们尝试使用闭环关键点因子和一致性检查（即检查关键点因子的结果残差是否小于一个阈值）进行位姿图优化（第14至21行）。当图变得不一致时，我们拒绝该闭环，并将图恢复到备份状态（第9至13行）。用于控制关键点因子残差的阈值可以被视为一个点对点距离阈值。它是一个一维变量，因此很容易调整。然而，当处理第一个闭环或者当前位姿与闭环位姿之间的距离较大（例如，大于地图上x、y和z的最大范围的10%）的闭环时，我们需要更加小心，因为这个闭环将严重影响后续闭环的接受。为了更好地处理这些情况，我们维护一个临时存储未添加到图中的关键点因子的容器L_p（第4行）。在这些情况下，我们建议一次检查两个连续的闭环，因为只要它们的内部一致性和图内一致性都良好，它们很少会是假阳性（第5至7行）。
我们的FPR模块不执行PGO，而是简单地将图恢复到先前的状态，避免像RRR（Latif等，2013）中那样给系统引入额外的延迟。与鲁棒估计器相比，它对结构相似的场景更加鲁棒，并且具有快速的闭环收敛能力。

5.3 关键点因素边缘化

引入关键点因子到循环优化中会逐渐减慢优化的速度，因为优化时间与因子数量成正比，需要对关键点因子进行边缘化，以防止因子数量无限增加。此外，还采用了一种增量优化框架ISAM2（Kaess等，2012）来提高优化效率。

关键点因子边缘化的思想可以通过图4中的一个简单示例来说明。以这个图为例，当机器人从第一个姿态移动到第五个姿态时，FPR模块检测到并接受了一个闭环。在优化之后，闭环图将被用于重新计算相邻姿态之间的相对变换。重新计算的相对姿态被插入到图中作为新的里程计因子，以替换之前的因子，包括关键点因子和旧的里程计因子。然后，对于循环{1, 2, 3, 4, 5}进行边缘化。类似地，当机器人从第五个姿态移动到第六个姿态时，另一个闭环被接受，随后对于循环{1, 2, 5, 6}进行边缘化。

6. 循环校正

在本小节中，我们首先使用一个序列流程图（见图5）展示循环校正模块的工作原理，然后详细介绍了管道中的四个关键步骤，包括动态加载、地图点校正、ikd-Tree重建和扫描重新注册（图2）。
值得注意的是，我们使用一个名为“ikd-Tree重建线程”的独立线程来执行额外的任务。这种异步操作可以防止原始的FAST-LIO2线程出现显著的停顿。更多的实现细节将在下面的小节中讨论。

6.1 点校正和ikd-Tree重建

对所有历史点进行朴素的ikd-Tree重建是耗时的，而动态加载历史点可以缓解这个问题。在从循环优化节点（见图5）接收到优化后的里程计数据后，ikd-Tree重建线程加载当前位置周围的历史点。这种动态加载是基于扫描位置的kd-Tree进行的。优化后的里程计中的所有扫描位置都用于创建一个位置kd-Tree，以便在动态加载中搜索相邻的扫描。加载的点会根据优化后的里程计进行校正，并用于重建一个新的ikd-Tree。

注2：用于点校正的存储子地图点是FAST-LIO2的树上下采样结果，因此点的数量本质上要比原始点的数量小得多。此外，当重访发生重复时，下采样比例会增加，因此存储的子地图点的数量不会无限增加。详细的时间消耗将在第5.2节中讨论。

6.2 姿态校正和ikd-Tree替换

在循环检测、循环优化、点校正和ikd-Tree重建过程中，LIO的姿态已经从X_k移动到X_{k+m}。循环优化的第k个姿态X^′_k相对于最新的LIO姿态X_{k+m}已经过时（见图5）。我们不能直接将LIO姿态重置为X^′_k。相反，我们需要使用从X_k到X^′_k的变换（表示为T_{k^′,k}）将X_{k+m}转换为X^′_{k+m}。然而，由于优化结果不完美，T_{k^′,k}并不总是足够精确地适配重建的ikd-Tree和X^′_{k+m}。因此，我们需要进行ICP来将使用X^′_{k+m}投影的扫描重新注册到重建的ikd-Tree中（图2中的“扫描重新注册”），在此期间，FAST-LIO2必须停顿，ICP平均只需约8毫秒（见表7）。
之后，FAST-LIO2线程将其姿态校正为X^∗_{k+m}，并在短时间内（<1毫秒）用重建的ikd-Tree替换它的ikd-Tree，以使LTA投入使用。总之，最优LIO姿态的计算如下：

T_{icp}表示ICP的结果。让姿态校正变换T_{corr}为：

6.3 维护策略

随着越来越多的循环约束优化里程计的准确性逐渐提高，我们以一定的频率执行循环校正，以便长期关联能够利用最新的循环优化结果。该频率由空间间隔（默认为50米）控制。在上一次循环校正执行中标记为“已加载”的所有扫描将在当前循环校正执行中被重置为“未加载”。

6.4 里程计因子重新计算

姿态校正会对LIO模块引入突然的姿态状态变化，从而影响里程计因子的计算准确性。我们需要重新计算受影响的里程计因子。假设姿态校正发生在连续子地图姿态X_i和X_j之间（其中i > j），则X_j必须重新计算为：

然后，重新计算的里程计因子测量值为：

6.5 多会话模式

当LTA-OM处理一个数据序列时，我们可以将得到的里程计、地图和描述符数据库作为未来定位、建图和环路检测的先验信息进行存储。从一个序列加载先验信息以处理另一个序列被称为多会话模式。这种模式已被证明可以提高当前会话的准确性（见图10）。此外，该模式使得实时地图与预存地图保持一致（称为地图拼接，见表8）。多会话模式对于机器人导航非常有用，因为它避免了在机器人在指定区域执行重复任务时重新构建地图。

多会话模式的实现在图2中简要介绍。单会话模式和多会话模式之间有四个主要区别，包括：1. 预存地图被加载到FAST-LIO节点用于LTA模块，但不需要使用预存地图点进行校正；2. 预存描述符数据库被加载到环路检测节点用于先前地图上的环路闭合检索，并且我们将其优先级设置为高于对历史地图的检索；3. 预存子地图姿态被加载到环路优化节点用于先验因子计算；4. 重新定位被实现以将LIO姿态校正到预存地图上。

备注3：考虑到环路检测结果并不总是正确的，我们必须谨慎地拒绝错误的正例环路闭合，以避免系统崩溃。由于我们的FPR模块，我们可以主动地拒绝它们，这是传统鲁棒估计器无法实现的。当检测到两个连续的针对先验描述符数据库的环路闭合时，我们可以计算在线子地图姿态的相应先验因子，并尝试使用先验因子优化图。如果它们彼此一致，我们接受它们并完成后续的重新定位。