💻 computer science

FD-SLAM: Fast Dense Radar-Inertial SLAM with Frequency-Domain Loop Closure and Pose Graph Optimization

本文提出了 FD-SLAM，一种快速密集雷达惯性 SLAM 系统，该系统通过频域回环检测和位姿图优化来提高轨迹精度，在视觉退化的环境中展示了在仅 CPU 硬件上的竞争性能和实时效率。

原作者： Nader J. Abu-Alrub, Nathir A. Rawashdeh

发布于 2026-06-16

📖 1 分钟阅读☕ 轻松阅读

原作者： Nader J. Abu-Alrub, Nathir A. Rawashdeh

原始论文采用 CC BY 4.0 许可（http://creativecommons.org/licenses/by/4.0/）。 ✨ 这是对下方论文的AI生成解释。它不是由作者撰写或认可的。如需技术准确性，请参阅原始论文。阅读完整免责声明

想象一下，你正驾车行驶在浓雾、暴雪或漆黑的隧道中。你的眼睛（摄像头）看不见任何东西，你的激光扫描仪（LiDAR）可能会被飘落的雪花或尘埃干扰。但你的车拥有一种特殊的“雷达之眼”，它能穿透这一切混乱。问题在于，雷达有点“嘈杂”且模糊，就像看着波动的池塘中的倒影一样。在长途驾驶过程中，这种模糊感会不断累积，导致汽车开始认为自己处于一个与实际位置不同的地方。

这篇论文介绍了一种名为 FD-SLAM 的新方法，旨在帮助自动驾驶汽车即使在恶劣天气下也能精确地确定自己的位置。

以下是其工作原理的分解，分为几个简单的步骤：

1. “雷达之眼”与“内耳”

把汽车的雷达想象成一个拍摄世界图像的相机，但它捕捉的不是颜色，而是热量和反射。这些图像看起来有点像旧电视上的静电噪声。

问题： 如果你只是逐一观察这些静态图像，你可能会迷失方向，因为静电噪声让你很难判断自己是移动了一点点还是移动了很多。
解决方案： 该系统将雷达与 IMU（惯性测量单元）相结合。你可以把 IMU 想象成汽车的“内耳”（就像你的平衡系统）。它能瞬间感知每一次细微的转弯和颠簸。
结果： 系统利用“内耳”在两帧雷达图像之间预测汽车的位置，并利用“雷达之眼”来修正这个预测。这创建了一个非常准确的、关于“此时此刻”车辆位置的局部地图。

2. “频率指纹”（回环检测）

驾驶中最难的部分是意识到：“等等，我以前来过这里！”在现实世界中，这被称为回环检测（Loop Closure）。

挑战： 由于雷达图像非常嘈杂且模糊，很难将 10 分钟前的图像与现在的图像进行匹配。这就像是在人群中试图认出一个戴着面具且灯光闪烁的朋友。
神奇妙招： FD-SLAM 并没有尝试直接匹配那些混乱的图像，而是将雷达图像转化为一个紧凑的“频率指纹”。
- 想象一下，将一张城市照片转化为一个音乐和弦。即使照片很模糊，那个“和弦”（频率模式）依然保持不变。
- 系统为每一次雷达扫描创建一个简短、简单的代码。当汽车行驶时，它会不断检查：“这个新代码听起来像我之前听过的哪个代码？”
- 如果找到了匹配项，它就知道：“啊哈！我以前来过这个地方！”

3. “侦探的清单”

仅仅因为两个指纹看起来相似，并不意味着它们是同一个人（你可能有个双胞胎）。为了避免错误，系统不会仅仅信任第一个匹配项。它会运行一个多阶段侦探检查：

时间检查： “我们是不是刚刚经过这个地方 5 秒钟前？”如果是，请忽略（那只是向前移动，而不是回环）。
相位检查： “雷达图像中的波形是否完美对齐？”
形状检查： “建筑物的形状是否吻合？”
逻辑检查： “距离和角度在物理学上是否合理？”

只有当候选目标通过了所有这些测试时，系统才会判定：“是的，这是一个真实的回环！”

4. “全局地图修正”

一旦系统确认它曾经访问过某个地点，它就会在地图上画一条线，连接“当时”与“现在”。

效果： 想象你在走路时以为自己走的是直线，但实际上你稍微向左偏移了。每当你意识到自己曾经来过这里时，系统就会把你整个路径拉回到一条直线上。它修正了你在过去一小时内犯下的所有微小错误，使最终生成的地图完美精确。

为什么它很特别？

大多数其他系统都试图在处理之前，将模糊的雷达图像转化为清晰的点或点云。而这篇论文说：“不，让我们保持图像原样！”

他们将雷达扫描视为稠密图像（类似于照片），而不是一组点列表。
他们利用数学（频域）在不需要先清理噪声的情况下寻找匹配项。
它运行速度极快，可以在标准计算机芯片（CPU）上运行，而不需要昂贵的图形显卡。

核心结论

作者在各种条件下使用真实驾驶数据（MulRan 数据集）对该系统进行了测试。他们发现：

它比仅使用雷达和“内耳”的方法要准确得多。
它的表现与目前现有的最佳雷达系统持平甚至更好。
它足以实现实时运行，这意味着汽车可以利用它在雾、雨或雪天安全地进行自动驾驶。

简而言之，FD-SLAM 赋予了自动驾驶汽车一种超能力：通过将嘈杂的雷达静电转化为可靠的、具有自我修正能力的地图，从而在最恶劣的天气下也能自信地导航。

FD-SLAM 技术摘要：基于频域回环检测与位姿图优化的快速稠密雷达-惯性 SLAM

问题陈述
自动驾驶地面车辆需要鲁棒的同步定位与地图构建（SLAM）能力，特别是在 GPS 受限或视觉退化（如雾、尘、黑暗）的环境中，这些环境下相机和 LiDAR 往往会失效。虽然扫描式调频连续波（FMCW）雷达提供了适用于此类条件的稠密、360 度、长程测量，但它们也带来了显著挑战：测量数据噪声大、包含多路径反射和斑点伪影、存在运动畸变，且采样率低于其他传感器。现有的雷达 SLAM 系统通常将稠密的雷达扫描转换为稀疏的点集或特征表示，这可能会丢弃雷达数据中固有的、具有价值的“类图像”结构信息。此外，许多系统依赖于从 LiDAR 或相机流水线中改编而来的描述符或验证机制，而不是利用雷达扫描原生的频域结构。本文旨在探讨是否可以保留并利用扫描式雷达的稠密、类图像结构，贯穿于从局部运动估计到回环检测及全局优化的整个 SL SLAM 流水线中。

方法论
所提出的系统 FD-SLAM 是一个完整的基于图的 SLAM 流水线，它扩展了作者之前的快速稠密雷达-惯性里程计（FD-RIO）前端。该系统围绕稠密的、雷达原生的表示进行设计，避免了将扫描降维为稀疏地标。其架构由三个主要部分组成：

雷达-惯性里程计前端：
- 该模块提供连续位姿之间的局部运动估计。
- 它使用卡尔曼滤波器将基于相关性的稠密雷达里程计与高频 IMU 测量进行融合。
- 雷达部分在傅里叶域中通过极坐标下的幅值谱估计相对旋转，随后在空间域进行平移估计。这避免了显式的特征提取。
- IMU 用于约束雷达扫描之间的漂移，而雷达则提供外感知修正。
- 这些估计值在位姿图中形成序列里程计约束。
回环检测与验证：
- 描述符生成： 每个雷达扫描被转换为一个源自其频域表示的紧凑描述符。该过程涉及计算二维傅里叶幅值谱，将其转换为极坐标，应用对数压缩，并聚合列均值和标准差剖面。这产生了一个 182 维的向量，该向量对于旋转和平移具有相对不变性。
- 候选检索： 在选定的关键帧处触发回环搜索。通过计算当前描述符与数据库中先前描述符之间的余弦相似度来检索前 $K$ 个候选对象。
- 多阶段验证： 为防止误报，候选对象需经过一个确定性的多阶段验证流水线：
  - 时间滤波： 剔除在时间上距离当前扫描过近的候选对象。
  - 相位相关筛选： 使用极频域中的归一化互功率谱来评估对齐得分。
  - 扫描对齐相似度： 评估对齐后的候选扫描对参考扫描强度模式的解释程度。
  - 几何一致性检查： 验证估计的相对变换（旋转、平移及残差）是否处于物理合理的限制范围内。
位姿图优化：
- 经过验证的回环被作为非序列约束，与序列里程计因子一起添加到 SE(2) 位姿图中。
- 一个先验因子锚定第一个位姿。
- 系统求解一个加权非线性最小二乘问题，以最小化测量约束与估计轨迹之间的误差，从而减少累积漂移并提高全局一致性。
- 系统采用异步运行方式：前端实时运行，而回环检测和图优化在后台执行。

核心贡献
本文声称有三个主要贡献：

一个完整的稠密雷达-惯性 SLAM 流水线： 它将基于相关性的雷达-惯性里程计扩展到了具备描述符回环检测和位姿图优化的功能，并全程保持了稠密表示。
一种紧凑的频域极坐标描述符与验证流水线： 该方法引入了一种特定的描述符和确定性的多阶段验证过程，保留了扫描式雷达的稠密结构，实现了高效的候选检索和稳健的验证，且无需依赖学习特征或稀疏点云。
全面的评估： 系统在公开数据集（MulRan）上使用 Intel i7-13700 CPU（无 GPU）进行了评估，并使用标准的 KITTI 指标进行了测试。

结果
系统在 MulRan 数据集的十个序列上进行了评估，使用的是 Intel i7-13700 CPU（无 GPU）。

精度： 在大多数序列上，FD-SLAM 在平移和旋转精度方面均优于 FD-RIO 里程计基准。例如，在 DCC03 序列中，平移误差从 2.30% 降低到了 1.17%。
对比： 该方法达到了与当前最先进的雷达 SLAM 方法（包括 TBV SLAM 和 RadarSLAM）相当的竞争性能。值得注意的是，FD-SLAM 在旋转精度方面表现出色，通常在各序列中达到最低或并列最低的旋转误差，表明其能有效约束航向漂移。
运行时间： 雷达-惯性前端的运行频率约为 14 Hz，显著高于传感器 4 Hz 的采样率，确保了实时性能。回环检测和图优化在后台运行，累计耗时仍低于总序列处理时间，支持异步执行。
定性分析： 视觉结果显示，优化后的轨迹在修正全局漂移的同时，保留了里程计前端的局部结构。

意义与主张
本文将 FD-SLAM 定位为雷达 SLAM 领域的一个“互补设计点”。其重要性在于证明了稠密频域雷达表示可以支持完整的 SLAM 流水线，而无需将数据转换为稀疏特征或依赖视觉特征描述符。通过保留扫描式雷达原生的类图像结构并利用其频域特性进行配准和回环检测，该系统为基于稀疏特征的方法提供了一个鲁棒的替代方案。作者声称，这种方法可以在保持适合实时自主导航的计算效率的同时，实现有效的回环候选检索、验证和全局轨迹修正。这项工作表明，稠密雷达原生流水线可以实现与依赖复杂预处理或学习模型的算法相媲美的精度。