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