4DRadarSLAM: 基于位姿图优化的大规模环境下的4D成像雷达SLAM系统

文章：4DRadarSLAM: A 4D Imaging Radar SLAM System for Large-scale Environments based on Pose Graph Optimization

作者：Jun Zhang, Huayang Zhuge , Zhenyu Wu, Guohao Peng, Mingxing Wen, Yiyao Liu and Danwei Wang

编辑：点云PCL

代码：https://github.com/zhuge2333/4DRadarSLAM

欢迎各位加入知识星球，获取PDF论文，欢迎转发朋友圈。文章仅做学术分享，如有侵权联系删文。

公众号致力于点云处理，SLAM，三维视觉，高精地图等领域相关内容的干货分享，欢迎各位加入，有兴趣的可联系dianyunpcl@163.com。侵权或转载联系微信cloudpoint9527。

摘要

基于激光雷达的SLAM在恶劣天气条件下（如雨、雪、烟、雾）容易出现故障，而毫米波雷达则不受影响。然而，目前的研究主要集中在2D（x，y）或3D（x，y， doppler ）雷达和3D激光雷达上，很少有关于4D雷达（x，y，z， doppler ）的研究。作为市场上的新产品，4D雷达输出带有高程信息的3D点云，而不是2D点云；与3D激光雷达相比，4D雷达的点云噪声大且稀疏，使其更具挑战性，难以提取几何特征（边缘和平面）。在本文中，我们提出了一个完整的4D雷达SLAM系统，包括三个模块：

  • 前端模块执行扫描帧到扫描帧的匹配，基于GICP计算基于每个点的概率分布的里程计；

  • 回环检测利用多个基于规则的回环预过滤步骤，然后是一个强度扫描上下文步骤来识别回环候选项，再通过里程计检查来拒绝假回环；

  • 后端使用前端里程计、回环检测和可选GPS数据构建位姿图。通过g2o实现最佳位姿。

81ecb9439ff98c4ae5c72243c659e021.png

图1：提出了4DRadarSLAM，旨在利用4D成像雷达实现大规模SLAM。（a）测试了两种平台上的提出系统：手推车（低速：约1m/s），汽车（中速：25-30km/h）。展示了两个选定数据集的制图结果：（b）在一个花园中。（c）沿着校园主要道路。

主要贡献

这篇论文提出了4DRadarSLAM是一个包括三个部分的完整系统：前端、回环检测和后端。在前端执行扫描帧到扫描帧的匹配来计算里程计。由于从4D雷达点云中提取边缘和平面很困难，直接在原始点云上使用广义ICP（GICP）。通过考虑每个点的概率分布，我们提出的APDGICP（自适应概率分布-GICP）提高了性能。在回环检测中执行回环预处理过滤，以识别可能的回环候选项，然后利用强度扫描上下文来找到回环闭合。我们还进行里程计检查，以确保几何一致性。在后端，我们使用g2o构建并解决了一个位姿图，并输出了优化后的位姿。在图1和图7中，展示了测试平台和制图结果。主要贡献包括：

  • 为4D成像雷达提出了一个完整的SLAM系统。我们开源了代码，以促进相关研究。

  • 在前端（APDGICP）中，我们考虑了点测量的概率分布。

  • 在回环检测中，我们引入了强度扫描上下文来查找回环候选项，结合回环预过滤和里程计检查，我们可以获得良好的回环闭合。

  • 在后端位姿图中考虑了里程计、回环闭合和GPS。

  • 我们进行了广泛的实验，使用了两种类型的平台和五个数据集，展示了其准确性、稳健性和实时性能。

内容概述

系统概述

4DRadarSLAM系统的概述如图2所示，由三个模块组成：前端、环路检测和后端。在前端，4D雷达点云用作输入，用于估算里程计和生成关键帧。环路检测模块评估每个新的关键帧，以确定它是否可以形成回环闭合。在后端使用g2o构建并优化位姿图，生成优化的姿势作为输出。

e53623c5aa84800ee3120a3005281d6b.png

图2：提出的4DRadarSLAM系统概述。它由三个模块组成：(a) 前端：用于计算里程计。(b) 回环检测：用于检测回环闭合。(c) 后端：姿势图构建和优化。

前端

预处理：为了确保SLAM系统的鲁棒性，首先应该过滤掉动态物体。雷达的多普勒速度信息可用于识别这些物体。在本研究使用[33]中提出的线性最小二乘方法来估计毫米波雷达的自身速度。利用估计的多普勒速度和自身速度，我们能够确定物体的真实速度。

扫描匹配：在这一步中输入是上一个关键帧和一个新帧，目标是找到变换矩阵，由于4D雷达的点云含有噪声，不容易提取几何特征（如边和平面），GICP相对于ICP和NDT来说能够输出可接受的结果。因此提出了一种名为自适应概率分布-GICP（APDGICP）的新算法，它考虑了GICP中每个点的空间概率分布。根据毫米波雷达的手册，点的测距不确定性由σr = 0.00215r给出，其中r和σr分别是测得的距离和其不确定性。方位和俯仰角的精度分别为0.5°和1.0°，这导致了球坐标中方位和俯仰方向的不确定性，近似为σa ≈ sin(0.5°)r和σe ≈ sin(1.0°)r。所得的概率分布如图3所示，类似一个椭球（橙色），其中一个轴指向原点，三个半轴的长度分别为σr（距离）、σa（方位）和σe（俯仰）。

125038d4eea866603adebb227d99efa2.png

图3：一个点的概率分布。

关键帧选择：第一帧被指定为固定的关键帧，而后续的关键帧则根据以下两个条件之一来确定：i) 当前帧和上一个关键帧之间的平移超过阈值δt；ii) 当前帧和上一个关键帧之间的旋转超过阈值δr。阈值参数根据经验设置如下：δt = 0.5米或2米，δr = 15°。第k和第k+1个关键帧之间的扫描匹配结果被添加到姿势图中，作为SE(3)的二元边。边的协方差是基于两个关键帧点云的拟合分数来计算的。

回环检测 

在这一步中，每个关键帧都与数据库中的关键帧进行比较，以确定是否形成一个回环闭合。

回环预处理过滤：为了避免在环路检测中搜索整个数据库，这里进行了回环预过滤步骤，根据四个规则来识别潜在的回环： 

  • i) 遵守距离限制，这意味着新回环的查询帧不应离上一个回环的查询帧太近，回环的帧之间也不应离得太近； 

  • ii) 确保回环的帧在某一半径范围内（在空间上靠近）。我们会根据帧之间的行驶距离自适应调整搜索半径，一旦找到一个回环，如果候选回环靠近，搜索半径将相应减小； 

  • iii) 强制设置2米的高度差阈值，基于气压计提供的高度信息； 

  • iv) 确保回环的帧具有相似的偏航角，对于我们的雷达，20°的阈值适用于避免误匹配。

扫描上下文：在预过滤回环帧之后，只有潜在的候选帧会被送到强度扫描上下文模块，我们选择强度扫描上下文而不是扫描上下文，因为由于传感器的限制和多次反射回波，雷达高度信息具有噪声。使用最大高度作为上下文将不准确。相反，我们使用反射信号的最大强度/功率来构建扫描上下文矩阵，因为它更稳定且对雷达更有价值。扫描上下文最初是针对具有360°方位FOV的激光雷达提出的。我们将其适应了具有较窄的110°方位FOV的4D雷达。为此，我们将点云分成40个环和20个扇区，导致单位环间隙为2米，单位扇区角为5.5°。

里程计几何一致性：在执行扫描上下文以找到最可能的回回环后，必须考虑几何一致性。仅使用扫描上下文可能引入几何不一致性，这将对后端姿势图优化造成灾难性影响。为避免此问题，采用了LAMP中的里程计检查步骤，图4说明了基本概念。

317143c6af76e9089c95b53599a4eb17.png

图4: 姿势图和回环检测。

后端 

将基于前端的里程计、回环闭合以及GPS信号（如果可用）构建一个姿势图。如图4所示，关键帧表示为节点，两个节点之间的边代表里程约束（二进制边）。当确定了回环闭合（红色虚线）时，将其添加为约束（二进制边）。如果有GPS信号可用，它也可以作为带有从GPS数据直接获得的协方差的一元边添加到姿势图中。最后，使用g2o库对姿势图进行优化，得到优化后的位姿。

实验

为了收集我们的数据集，我们使用了图1(a)所示的两个平台：一个手推车和一辆配备了我们的传感器组的汽车。这使我们能够在小型和大型环境中、有结构和无结构的环境中、以及低速和中速下收集数据。表I总结了5个数据集的信息，它们都位于新加坡国立大学的校园内。数据集的卫星图像在图1(b)(c)和图7中呈现出来。

c4caabd4c73bf8b032d8e0d7f65ce48c.png

使用了Oculii Eagle 4D雷达，其测距范围可达400米，方位角和俯仰角分辨率分别为0.5°和1°。

8e9ca03c07b74b1031c7e5f1e867332f.png

图6呈现了5个数据集的ATE结果，可以得出以下观察结果：

  • i) 对于所有配置，ATE随着行驶距离的增加而增加。

  • ii) 在前端，我们提出的APDGICP在小规模数据集中优于GICP，但对于大规模数据集GICP表现更好。

  • iii) 准确的回环闭合显著提高了准确性，而在后端中，GICP和APDGICP之间的差异变得较小。因此，正确的回环闭合和后端优化对性能贡献很大。

  • iv) 当GPS可用时，ATE很小，可以忽略不计，表明GPS显著提高了性能。

8b442327b4dbe6322e778bdd74ac6313.png

图6: 绝对轨迹误差（ATE）

为了直观展示，不同方法在5个数据集上的轨迹绘制在图5中。

2aa5d49aabd85f09dfaf44cae7c6e067.png

图5: 将我们估算的轨迹与地面真值轨迹进行比较，涵盖了5个数据集。

为了评估效率，我们记录了所有数据集上每个算法步骤的计算时间，并在表III中列出了中位值。

3176b9f6859426b17dbd2820ab2d3678.png

为了进行定性分析，使用三种选项可视化了5个数据集的点云地图：没有闭环检测（w/o loop）；带有闭环检测和后端优化（w/ loop）；以及带有GPS和后端优化（w/ gps），如图7所示。结果表明，当不使用闭环检测时，地图中存在明显的错误。然而，使用闭环检测或GPS后，这些错误得到显著减少。

1a0901c9d02d71672648601b6ce2c404.png

图7：5个数据集的卫星图像和地图可视化。显然，闭环和GPS约束都改善了地图质量（黄圈部分）。

总结

本论文介绍了一个完整的4D成像雷达SLAM系统，包括三个模块：前端、闭环检测和后端。在前端估计雷达自身速度以去除动态物体，并提出了APDGICP算法，它考虑了原始GICP中每个点的概率分布，用于扫描匹配。在闭环检测中引入了几种闭环筛选方法，使用强度扫描上下文来查找闭环候选项。还实施了一个里程计检查模块来确定最佳闭环。在后端基于前端里程计、检测到的闭环和GPS数据构建了一个位姿图。我们进行了广泛的实验，使用自行收集的数据集，覆盖了各种环境和速度，包括结构化和非结构化、小尺度和大尺度、低速和中速环境，实现了相对误差（RE）2.05％，0.0052deg/m，和绝对轨迹误差（ATE）2.35m，实现了在笔记本上的实时性能。

以上内容如有错误请留言评论，欢迎指正交流。如有侵权，请联系删除

a26a40a3a8e95690612b5bed2c6e6d02.png

扫描二维码

                   关注我们

让我们一起分享一起学习吧！期待有想法，乐于分享的小伙伴加入知识星球注入爱分享的新鲜活力。分享的主题包含但不限于三维视觉，点云，高精地图，自动驾驶，以及机器人等相关的领域。

分享与合作方式：微信“cloudpoint9527”（备注：姓名+学校/公司+研究方向） 联系邮箱：dianyunpcl@163.com。

为分享的伙伴们点赞吧！

abdc534b2c4f4a46d7ea5fc5ad6503d6.gif