LTA-OM（一）论文

目录1.摘要2.相关工作3.系统总体流程3.1LiDAR-IMU 里程计3.2回环检测3.3回环优化图优化FPR方法因子规模管理3.4回环矫正地图点校正与 ikd-Tree 重建位姿校正与 ikd-Tree 替换维护策略里程计因子重新计算3.5Multisession模式LTA-OM（Long-Term Association LiDAR-Inertial Odometry and Mapping）是一个高效、鲁棒且精确的 SLAM 系统。它集成了 LIO（FAST-LIO2）、回环检测（STD）、回环优化以及长期关联（LTA）建图模块。该系统还支持多会话模式。1.摘要本文的一项创新是实时 LTA 建图：它利用 FAST-LIO2 的直接扫描到地图配准（scan-to-map registration），并采用经过校正的历史地图为 LIO 建图过程提供直接的全局约束。LTA 建图的显著优势在于，在重复访问场所能够实现无漂移的里程计。此外，系统设计了多次会话模式，允许用户保存当前会话的结果，包括校正后的地图点、优化后的里程计以及描述子数据库，以便未来会话使用。该模式的优势在于进一步提高建图精度，实现一致性地图拼接，有利于长期建图应用。LIO 可以保证局部地图的一致性，但全局一致性需要通过回环检测和 位姿图优化（ PGO） 实现。具体来说，回环检测可以识别重复访问的位置，并利用回环约束来修正长期里程计漂移，从而生成全局一致的地图。LTA 建图的优势在于：显著降低了重复访问场所的里程计漂移。提高了地图的一致性。当机器人移动到新区域时，系统可以无缝扩展地图，将新点加入在线地图。2.相关工作里程计：FAST-LIO2回环检测：STD误检回环剔除（False‐positive rejection，FPR）回环检测算法无法保证 100% 精度，因此需要FPR 模块剔除误检回环。本文采用图一致性检查的思想，主动剔除误检回环：若检测到的回环约束导致图不一致，则剔除该约束并恢复优化前状态，无需重新估计参数，从而保证系统鲁棒性。与传统鲁棒估计相比，本文方法对结构相似场景（如多层建筑）更稳健，并能快速收敛回环优化。3.系统总体流程LIO 模块：使用 FAST-LIO2 并结合 LTA 方法，为回环检测模块提供已配准的扫描和估计的里程计。回环检测模块（STD）：提取关键点并检测回环，为回环优化模块提供候选回环信息。回环优化模块：利用 LIO 提供的里程计、关键点和回环信息构建位姿图，并在检测到回环后进行优化。回环校正模块：将优化后的里程计用于校正 LIO 模块的位姿状态，并替换其 ikd-Tree地图，从而实现 LTA 的功能。系统流程总结：LIO 提供在线里程计与扫描，回环检测识别回环候选并验证，回环优化构建和优化位姿图，回环校正将优化结果反馈到 LIO，实现历史地图与在线地图的长期关联。3.1LiDAR-IMU 里程计LIO采用了FAST-LIO2：基于紧密耦合的迭代扩展卡尔曼滤波；可以在线估计LiDAR-IMU外参；利用 ikdtree 实现 scan-to-map 的配准；相较于标准 FAST-LIO2 的关键区别是在建图中加入 LTA。除了将新扫描点融合到在线地图中，系统还从全局历史地图动态加载地图点到 ikd-Tree，使历史地图对在线状态更新提供全局约束；动态加载的点位于当前机器人位姿一定范围内（加载范围），加载时使用回环优化更新后的扫描位姿校正点位置，并标记已加载的历史扫描索引，避免重复操作。加载按一定频率周期执行：当 LIO 位姿接近上次加载范围边界时触发下一次动态加载。当机器人进入之前探索过的区域，动态加载会自动将在线地图与历史地图融合，实现 LTA 功能；离开这些区域时，动态加载失效，LTA 自动停用。3.2回环检测回环检测模块使用了STD（之前的文章介绍过），该模块能够高效检测回环，并提供可靠的回环几何变换，同时可处理小重叠的重复访问场景。其工作流程为：1.子地图积累模块会累积LIO 模块已配准的帧；累积子地图的位姿取最后一帧的位姿（LiDAR 坐标系相对于全局坐标系 T_wl）；子地图索引用于标记历史地图中帧的点，以便后续进行地图点校正。2.关键点提取与描述子构建将累积的子地图进行体素化，识别平面体素并合并为更大平面；将非平面体素的点投影到邻近平面，生成图像，用于关键点提取；在图像中选取局部最大点到平面的距离作为关键点，并进行空间非极大值抑制以增强提取重复性；每三个关键点形成一个三角形，基于三角形构建6D 描述子：包括三条升序三角边长度和三条投影法向量点积；投影法向量定义为非平面点投影到邻近平面后的法向量。3.回环检索与验证回环检索分三个阶段：粗略回环、精细回环、几何验证；粗略回环：通过哈希表在历史描述子中快速检索 N 个候选回环。精细回环：计算候选三角形对的所有变换矩阵，并通过RANSAC找出支持者最多的变换，选出最佳候选；几何验证：计算候选平面之间的重叠度，选择重叠度最大的候选为回环结果，仅当重叠度超过阈值时才接受该回环；由于精细回环提供初步变换猜测，平面重叠度计算快速，平面数量远少于原始点数量。3.3回环优化回环优化模块主要包含三个关键部分：图优化（Graph Optimization）、误检回环剔除（FPR, False-Positive Rejection） 和 因子规模管理（Factor Size Management）图优化常规位姿图由以下组成：位姿节点、里程计约束、回环约束。本文采用回环检测模块提取的关键点对，替代传统的回环因子。位姿图优化公式更新为关键点约束形式，如公式（2）。邻近关键点对通过全局坐标系下的 kd-Tree 搜索关联，回环关键点对由 STD关联。插入回环关键点约束后，即可执行 PGO 进行回环优化。FPR方法误检回环会严重影响 PGO，因此引入 FPR 模块主动剔除；原理：优化后的关键点残差应在合理范围内。若误检回环引入，地图中同一表面或边会明显分离，产生大的关键点对距离（残差异常）。算法流程：计算回环子地图对的重叠度（overlap ratio），小于阈值的回环直接剔除每次回环优化前，备份图和变量状态使用回环关键点因子进行 PGO，并进行图一致性检查（检查关键点残差是否小于阈值）若图不一致，剔除该回环并恢复至备份状态特别处理初次回环或当前位姿与回环位姿距离较大的情况（例如超过地图 x、y、z 最大范围的 10%）：临时保存未加入图的关键点因子，检查连续两个回环的内部一致性与图一致性，确认无误后再加入优化。因子规模管理随着关键点因子数量增加，回环优化时间会随之增长，因此需管理因子规模，防止无界增长。当机器人进入重复访问区域并接受回环后：使用 FPR 优化结果重新计算回环循环中连续位姿之间的相对变换；将重新计算的相对位姿插入图中，作为新的里程计因子，替代循环内的关键点因子和旧里程计因子；此策略保证回环优化高效，同时维护地图与位姿图的准确性。3.4回环矫正回环校正模块负责将回环优化结果应用到LIO 模块，更新位姿和地图，以实现长期关联（LTA）的功能。其核心包括动态加载历史地图点、地图点校正、ikd-Tree 重建以及单帧重新配准。为了避免阻塞 LIO 模块，系统在 LIO 中单独开启了ikd-Tree 重建线程（ikd-Tree rebuild thread），异步执行额外任务，从而保证 FAST-LIO2 主线程流畅运行。地图点校正与 ikd-Tree 重建对所有历史点进行传统 ikd-Tree 重建耗时较长，动态加载历史点可缓解这一问题；接收到回环优化节点传来的优化里程计后，ikd-Tree 重建线程会加载当前位置附近的历史点；动态加载基于历史扫描位姿构建的 kd-Tree 进行邻域搜索，加载的点使用优化后的里程计进行位置校正，然后用于重建新的 ikd-Tree。位姿校正与 ikd-Tree 替换在回环检测、回环优化、点云校正以及 ikd-Tree 重建的过程中，LIO 的位姿从X_k移动到了X_k+m。此时，经过回环优化的第 k 帧位姿X_k相对于最新的 LIO 位姿X_{k+m}已经过时（图 5）。我们不能直接将 LIO 的位姿重置为X_k​。相反，我们需要将X_{k+m}​通过从X_k到X_k的变换T_{k,k}转换为X_{k+m}​。然而，由于优化结果并不完美，T_{k,k}并不总是足够精确来将X_{k+m}​ 转换为适合重建 ikd-Tree 的X_{k+m}。因此，需要使用 ICP 将投影后的扫描点重新配准到重建后的 ikd-Tree 中（图 2），在这个过程中 FAST-LIO2 线程会暂停 ICP 操作，仅需大约 8ms。完成后，FAST-LIO2 线程将其位姿校正为X*_{k+m}​，并用重建后的 ikd-Tree 替换原来的 ikd-Tree，在极短的时间内（1ms）使 LTA 可用。总而言之，最优 LIO 位姿计算如下：其中 T_{ICP}表示 ICP 的结果。设位姿校正变换为：则：维护策略随着回环约束增加，优化后的里程计精度逐渐提高。系统按固定空间间隔（默认 200 m）执行回环校正，使 LTA 能及时使用最新优化结果。上次回环校正中标记为已加载（loaded）的帧，在本次校正前重置为未加载（not loaded），以便重新动态加载。里程计因子重新计算3.5Multisession模式当 LTA-OM 处理一条数据序列时，可以将生成的里程计、地图和描述子数据库保存下来，作为未来会话的先验信息；将这些先验信息加载到新的数据序列中进行处理，即形成多会话（Multisession）模式；实验表明，多次会话模式能够提升当前会话的定位精度（见论文实验部分）。此模式可实现在线地图与预存地图的一致拼接，避免机器人在特定区域执行重复任务时重复构建地图，非常适合长期导航与建图应用。Single-session模式和Multisession模式之间有四个主要区别：先验地图加载：先验地图会加载到 FAST-LIO 节点中的 LTA 模块，但不需要对先验地图点进行校正。描述子数据库加载：将先验描述子数据库加载到回环检测节点，用于在先验地图上检索回环；设置优先级高于历史地图上的回环检索。子地图位姿加载：将先验子地图位姿加载到回环优化节点，用于先验因子计算。重定位：在先验地图上校正 LIO 位姿，实现无漂移定位。