Exploration with Global Consistency Using Real-Time Re-integration and Active Loop Closure¶
ICRA 2022
作者信息：Yichen Zhang, 港科大 HDJI 团队，自主避障
Abstract¶
为了解决定位漂移问题，这篇文章之前的探索策略都是假设 localization 是 drift-free 的，这个假设在现实中往往不成立。
本文提出了一种实时重积分的帧剪枝建图算法以及一个考虑历史 viewpoints 的探索规划算法，并结合二者加上主动闭环检测构建了一个 framework
该方法主要解决两个问题：
- 长时间 VIO 导致的定位漂移，地图可能严重扭曲
- 被动闭环检测难以发现闭环，地图质量不高
Introduction 总体介绍¶
自主探索是在未知环境下建图的过程，它是航空摄影、地理测绘、灾害管理以及搜救等各种应用的基本问题，所以逐渐成为热门研究方向。
现有方法问题：
- 现有方法大多假设定位是无漂移的，但现实中 VIO 会随着时间积累漂移，导致地图严重扭曲，影响后续的探索和导航。
- 现有方法大多依赖被动闭环检测，难以发现闭环，导致地图质量不高。
本文贡献：
- 提出基于 TSDF 的建图框架，该框架具有实时重积分功能和基于集合覆盖公式化选择标准的帧剪枝机制，生成一个全局一致的地图。
- 对所有帧进行实时重积分对无人机设备算力要求过高，应用了一种基于集合覆盖公式的关键帧选择方法来剪枝冗余的帧。
- 利用空间划分来增量式地解决集合覆盖问题，从而提升了实时性能。 - 在探索规划中采用了一种主动闭环检测策略，以获得更高的回环检测机会，这进一步减少了定位漂移并提升了地图质量。
Background¶
Autonomous Exploration¶
-
基于边界点 (Frontier-based) 的方法
-
概念：边界点 (Frontier) 是指已知区域和未知区域的交界处。这种策略就像你在游戏中“开图”一样，机器人会优先移动到这些边界，因为那里最有可能发现新的未知空间。
-
演进：最早的方法是去最近的边界点，后来为了适应高速无人机，发展为去转向最平缓（速度变化最小）的边界点。
-
基于采样 (Sampling-based) 的方法
-
概念：机器人在周围随机生成大量可能的“下一步视点”（采样点），然后评估去哪个视点能获得最多的新信息（例如，能看到最大面积的未知区域），这个过程也叫计算“下一最佳视角 (NBV)”。
-
演进：通过生长 RRTs（快速探索随机树） 这种高效的路径规划算法来选择最有价值的探索方向。
-
混合方法 (Hybrid Approaches)
-
研究者们发现，两种方法各有优劣，于是尝试将它们结合。例如，在全局尺度上用边界点规划一个大致方向（去哪里开图），在局部尺度上用采样法精细地规划具体路径（怎么走过去最好）。
Globally Consistent Dense Mapping¶
核心问题：地图变形了，怎么“掰”回来？
当机器人发现自己回到了原点（回环闭合），并计算出累积的误差后，就需要修正整个地图。下面是文中讨论的三种“修正”技术：
- 弹性变形法 (Deformable Mesh / Surfels)
- 概念：把整个三维地图看作一个有弹性的物体（像一个橡胶模型）。当发现轨迹有误差时，就通过数学方法对这个“橡胶模型”进行拉伸、扭曲，直到它恢复正确的形状。面元 (Surfels) 是构成这种模型的小表面片。
- 这种方法只关心“哪里有东西”，但无法区分“哪里没东西 (自由空间)”和“还没去看过的地方 (未知空间)”。对于需要知道“下一步该去哪里探索”的机器人来说，这个缺点是致命的。
- 拼图法 (Collection of Submaps)
- 概念：建图时不是画一张大地图，而是画很多张小地图（子图），像拼图一样。当发现误差时，不需要修改每张小地图内部的细节，只需要调整这些“拼图块”之间的相对位置即可。
- 复杂度高：机器人规划路径时，如果路线跨越了多个“拼图块”，就需要分别向它们查询信息，增加了复杂性。
- 内部畸变无法消除：如果单个“拼图块”本身因为漂移画得不准，这种方法是无法修正其内部错误的。
- 重新绘制法 (Re-integration)
- 概念：这是最彻底、最精确的方法。它会把机器人一路走来所有的原始传感器数据（比如深度图像）都保存下来。当发现轨迹有误差后，它会用修正后的新轨迹，利用所有原始数据生成一张全新的、完全正确的地图。
- 能彻底消除所有畸变，生成全局一致的精确地图。最终得到的是一张完整的地图，运动规划器查询起来很简单（恒定的查询时间）。
- 但计算量极大，通常难以在资源有限的机载计算机上实时运行。
This Work¶
使用这种 Re-integration 方法来确保地图的全局一致性，同时通过帧剪枝和增量式 TSDF 更新来提升实时性能。
Truncated Signed Distance Field(TSDF)¶
- Signed Distance Field (SDF) 是一种隐式的空间表示方式。
它为空间中的每一个点 \( \mathbf{x} = (x, y, z) \) 定义一个符号距离值：
- TSDF: 在实际计算中，我们不需要（也存不下）全空间的距离，因此引入截断半径 \( \mu \)：
- 若距离太远（>|μ|），直接截断为 ±1；
- 只精确保存表面附近的一层体素（±μ 范围内）。
TSDF 地图更新¶
TSDF 通常存储在一个 3D 体素网格 中：
每个体素存储两项数据：
- 距离值 \( f \)：表面到该体素中心的截断符号距离；
- 权重 \( w \)：融合置信度（用于融合多个深度帧）。
当新的深度图到来时，会通过相机模型投影更新 TSDF：
[
f' = \frac{w \cdot f + w{new} \cdot f{w + w}{new}}
]
[
w' = w + w
]
也就是说，每帧深度都会对 TSDF 网格做一次加权平均更新，最终形成平滑稳定的三维表面。
Method¶
整体流程
-
感知与定位: 机器人启动，通过深度相机观察环境，并通过状态估计模块得出自己的初始位置。
-
建图与记录: 机器人向前移动，不断将新的观测数据更新到 TSDF 地图中，并将有代表性的 viewpoints 存为关键帧。
-
loop closure: 在移动过程中，闭环检测模块一直在工作。如果机器人碰巧回到了一个老地方并认了出来，系统会立刻触发一次大规模修正：更新历史轨迹，并重积分整个地图，使其恢复一致性。
-
exploration planning:
- 找到地图上的 frontiers
- 找到历史 viewpoints 中适合闭环的点
- 决策：是去探索新的 frontier，还是回去做闭环校准 - 循环: 机器人沿着规划好的路径移动，然后回到第 1 步，不断循环这个“感知-建图-决策”的过程，直到整个环境被探索完毕。
