RadarSLAM：可用于全天候的大规模场景的毫米波雷达SLAM

文章：RadarSLAM: Radar based Large-Scale SLAM in All Weathers

作者：Ziyang Hong, Yvan Petillot and Sen Wang

编辑：点云PCL

欢迎各位加入免费知识星球，获取PDF论文，欢迎转发朋友圈。文章仅做学术分享，如有侵权联系删文。未经博主同意请勿擅自转载。

公众号致力于分享点云处理，SLAM，三维视觉，高精地图相关的文章与技术，欢迎各位加入我们，一起交流一起进步,有兴趣的可联系微信：920177957。本文来自点云PCL博主的分享，未经作者允许请勿转载，欢迎各位同学积极分享和交流。

摘要

在过去十年中，针对不同的传感器提出了许多同步定位和建图（SLAM）算法，然而，在极端天气条件下的稳健SLAM仍然是一个有待研究的问题，本文提出了一种基于毫米波雷达的RadarSLAM系统，用于大规模环境中的可靠定位和建图，它由姿态跟踪、局部建图、回环闭合检测和姿态图优化组成，并通过新的特征匹配和雷达图像上的概率点云进行增强。在一个公共雷达数据集和几个自采集的雷达序列上进行了广泛的实验，证明了在各种不利天气条件下，如黑夜、浓雾和大雪，具有先进的可靠性和定位精度。

图1：提出RadarSLAM在牛津雷达数据集上的建图结果（序列10-12-32-52），绿线显示估计轨迹，全长9.04 km。

主要贡献

在过去的几十年里，同步定位和建图（SLAM）已经通过多种传感器模式进行了广泛的研究，例如超声波、相机和激光雷达，然而，对于室外大型SLAM确保其稳健运行仍然非常困难，尤其是在恶劣天气条件下。近年来，能够在各种天气条件下工作的新型调频连续波（FMCW）雷达传感器越来越多地被用于自动潜水设备和移动机器人中。因此，一个有趣但尚未解决的问题是，这些雷达是否可以在极端天气条件下（如大雪）的大规模环境中用于稳健的SLAM。

本文提出了一种RadarSLAM系统，该系统利用雷达几何信息和图优化SLAM方法，在大型室外环境中进行鲁棒定位和建图。主要贡献包括：

• 生成雷达图像的概率点云大大减少了斑点噪声。
• 基于图优化的全雷达SLAM系统，即使在恶劣天气条件下也能运行。
• 在大规模环境中进行广泛的真实实验，首次证明在极端天气（如浓雾和大雪）下，雷达SLAM也是可靠的。

主要内容

毫米波雷达传感器

FMCW雷达其发射模块发送波形，接收机等待目标反射的回波，它不仅可以测量雷达与目标之间的距离，还可以通过分析发送和接收信号之间的频率差来预测目标的速度，然而，当前的FMCW雷达传感器存在多个噪声源，例如距离误差、角度误差以及假阳性和假阴性检测。例如，假阳性检测包括杂波、多径反射和接收模块饱和，因为传感器对表面反射率和反射器姿态高度敏感，多径反射会导致连续帧之间的不一致，从而引入额外的噪声和异常值。因此，毫米波雷达读数往往比相机和激光雷达数据噪声更大，这使得它们更难用于运动估计和SLAM。

图2:FMCW雷达的360的极坐标和笛卡尔表示，雷达图像的三种常见噪声是斑点噪声、接收机饱和和多径反射。

雷达几何信息:一个 360◦FMCW雷达以总共N个方位角连续扫描360度， 如图2所示，即方位角上的步长为2π/N，对于每个方位角，雷达都会发射一个波束，并将返回信号折叠为一个距离距离，而不考虑高程，因此，雷达图像可以提供距离的绝对度量信息，这与丢失深度的相机图像不同，原始极坐标扫描可以转换为笛卡尔空间，由灰度图像表示。因此，在极坐标图像上给定一个点（a，r），其中a和r分别表示方位角和距离，其笛卡尔坐标P可以通过

其中θ=2π•a/N是笛卡尔坐标中的测距角度，γ是图像像素空间和世界度量空间之间的比例因子。为了获得更好的分辨率，笛卡尔图像通常使用双线性插值进行插值。

毫米波雷达SLAM

给定一系列雷达扫描数据，RadarSLAM旨在使用图优化SLAM估计毫米波雷达位姿和全局一致性地图，为此，提出的RadarSLAM系统设计为具有四个主要子系统：姿态跟踪、局部建图、回环闭合检测和姿态图优化。系统概述如图3所示。

图3 系统概述

A、 位姿跟踪

为了跟踪当前雷达帧t在世界坐标系中的姿态Ct，相对变换Tt 需要计算当前帧t和具有姿势Ck的关键帧k之间的SE（2），SE（2）表示特殊的欧氏群，然后，假设已知关键帧姿势Ck，则可以通过以下公式计算Ct

利用笛卡尔雷达图像关键点的几何特征计算Ttk，使用特征提取算法，例如SURF，分别从当前帧t和关键帧k提取两组关键点特征，然后，使用特征描述子匹配这两组关键点，与基于视觉的方法不同，雷达图像的特征匹配可以通过充分利用直接提供测距信息的雷达几何信息来进行。因此，采用了两种机制来减少雷达图像中不正确特征匹配的数量。首先，引入先验运行（如最大速度）来限制雷达局部坐标系上查询关键点对应的最大搜索半径。这既减少了不正确匹配的数量，也减少了穷举特征匹配所需的计算时间。第二个是成对一致性约束，考虑到成对的内联关键点对应应该遵循类似的运动趋势，它可以进一步剔除异常值，因此，对于当前帧t和关键帧k之间的任意两对关键点匹配，它们应该满足以下成对约束：

其中，|•|是绝对运算，|k•k|2是欧氏距离，Pi_t和Pi_k是局部坐标系中关键点对i的笛卡尔坐标，Pj_t和Pj_k是关键点对j的坐标，δ_c是小距离阈值。然后使用一致性矩阵G表示满足这种成对一致性的所有匹配，如果一对匹配满足此约束，则G中相应的条目设置为1。一旦获得最大内嵌集，其关键点匹配将用于通过奇异值分解（SVD）计算相对变换Tt_k。根据等式2，可以获得当前姿势Ct作为等式4中优化的初始测量值。图4给出了特征提取和匹配的示例。

图4：使用离群抑制进行特征提取和匹配，黄色和蓝色圆圈是检测到的关键点。初始匹配位于查询关键点的特定半径内，绿色图形是每个节点相互连接的最大类，红色连接表示不属于最大类并被剔除的错误匹配。

为了进一步约束局部漂移，通过最小化当前帧t和关键帧k之间成功匹配的关键点对的重投影误差，获得最终Ct：

在处理当前帧时，可以将其转换为关键帧，以获得更好的跟踪鲁棒性和准确性，遵循visual SLAM中用于关键帧生成的类似标准，即考虑关键点匹配的最小数量、当前帧和关键帧之间的平移和旋转的条件。

B、 局部建图

局部建图的目标是通过联合优化姿态和估计的局部地图来优化姿态估计和局部地图一致性，它与位姿跟踪线程并行运行，创建新关键帧后，其关键点将成为图优化点。然后，检索这些关键帧可以观察到的附近关键帧和图优化点，以执行局部捆集调整，即通过最小化加权平方和代价函数来优化关键帧的位置和凸优化点的位置：

其中X是关键帧姿势和图优化点位置的状态，ˆzi− zi（X）是第i个地图点的预测和观测之间的残差，Wi是对称正定加权矩阵。该优化通过使用Levenberg-Marquardt方法进行求解，为了限制所需的计算，如果上一个关键帧创建的图优化点不能被两个以上的关键帧观察到，则会对其进行剔除。

C、 回环检测

鲁棒的闭环检测对于减少SLAM系统的漂移至关重要，尽管事实证明，词袋模型对于基于视觉的SLAM算法是有效的，但它不适用于基于雷达的环路闭合检测，主要原因有三：第一，与光学图像相比，雷达图像在像素上的特征不太明显，这意味着相似的特征描述子可以在雷达图像中广泛重复；第二，雷达中的多径反射问题会给特征描述带来歧义；第三，雷达传感器的小旋转可能会产生巨大的场景变化，对描述子的直方图分布具有很大的变化。因此，我们采用了一种捕获场景结构并利用雷达点云反射密度的空间特征的技术。雷达图像首先转换为点云，一种直观而简单的方法是通过从每个方位角读数中找到局部最大值来检测峰值，如图5所示

图5：雷达扫描值的峰值检测。（a） ：原始笛卡尔图像。（b） ：使用局部最大值算法检测到峰值（黄色）。注意，大量峰值是散斑噪声。（c） ：使用提出的点云提取算法检测到的峰值，该算法保留了环境结构并抑制了多径反射和斑点噪声的检测。

由于散斑噪声，峰值可以在整个雷达图像中随机分布，即使对于没有真实物体的区域也是如此，因此，提出了一种使用概率模型的简单而有效的点云生成算法，假设每个方位扫描的峰值功率s服从正态分布，如下所示

其中，µ和σ是一次方位扫描数据中峰值功率的平均值和标准偏差，通过选择超出一个标准偏差且大于其平均值的峰，可以将真检测与假阳性分离。该过程如算法1所示。

从雷达图像生成点云后，采用M2DP，一种为3D点云设计的旋转不变全局描述子，对其进行描述以进行环路闭合检测，M2DP计算平面上点云的密度特征，并使用这些特征的左右奇异向量作为描述子。

D、 位姿图优化

随着雷达的移动，位姿图逐渐建立，检测到回环后，使用ICP和RANSAC作为几何约束，计算当前帧和检测到的关键帧之间的相对变换，并将其作为循环闭合约束添加到姿势图中，如果ICP收敛，则对所有关键帧执行姿势图优化，使用g2o库进行姿势图优化，成功优化关键帧的姿势后，更新全局地图的所有图优化点，当整个序列完成时，将执行全局捆绑调整以优化地图。

实验

A、 定量评估

定量评估是为了了解提出RadarSLAM系统的位姿估计精度，遵循流行的KITTI里程计评估标准，即以100米的增量计算长度100至800米的平均平移和旋转误差。还比较了使用不同传感器的最新里程计和SLAM算法，牛津雷达机器人雷达数据集用于定量评估，因为它是一个开放的大型雷达数据集，易于基准测试。它包括32个雷达数据序列，这些数据是在牛津穿越同一条路线时用地面实况姿态采集的。雷达数据由Navtech CTS350-X（调频连续波扫描雷达）捕获。它配置为在400个方位角以4.32cm的分辨率返回3768个功率读数，并以4Hz的频率运行（最大范围为163m）。由于页面限制，表I中给出了8个序列的定位结果，图6中给出了5个序列。

图6:Oxford Radar RobotCar数据集6个序列的估计的轨迹和真值。

1）与最新雷达的比较：两种最先进的基于雷达的姿态估计算法使用360◦选择FMCW雷达进行比较。一种是基于雷达几何形状和从雷达图像中提取的点云，另一种是一种基于深度学习的方法。由于它们都没有开源实现，并且它们的论文中报告的结果只包含平均误差。可以看出，提出的雷达里程法和SLAM方法都优于[6]中的方法。我们的雷达SLAM系统在平移方面达到2.21%的误差，在旋转方面达到0.007度/米，这比我们的纯里程计方法的结果要精确得多。

2） 与基于相机和LiDAR的方法的比较：为了全面评估RadarSLAM的鲁棒性和准确性，对基于视觉和LiDAR的算法进行了比较。ORB-SLAM 双目方法和SuMa由于严重的动态环境，无法完成整个序列或漂移过快。因此，表I中的结果一直报告到它们失去跟踪的程度。注意，视觉和激光雷达方法提供了6个自由度姿势，这些姿势投影到XY平面上进行评估，可以看出，提议的RadarSLAM可以在增强鲁棒性的情况下实现相当或更好的定位精度。

B、 定性评估

为了验证所提议的雷达系统在不利天气条件下对其他传感器模式的鲁棒性，在恶劣天气条件下（包括雾/雨、雪和夜间）自行采集了5个雷达序列1。我们的数据采集车配备了GPS/IMU导航系统、Velodyne HDL-32、ZED立体摄像头和NavTech雷达扫描仪。我们的NavTech雷达最大射程为100米，距离分辨率为0.1736像素/米，与牛津雷达数据集中的雷达略有不同，序列长度如表二所示。

请注意，“雪”、“乡村”和“夜晚”序列具有回环闭合，而雾中的其他两个序列没有循环。图10显示了雾/雨、夜和雪序列的一些示例相机图像，显示了基于视觉的方法的重大挑战。雪序列特别具有挑战性，厚厚的雪覆盖了相机、激光雷达和雷达传感器的大部分视角。

图10：在雾/雨（顶部）、夜（中部）和雪（底部）中收集的图像。在这种情况下，图像质量会显著降低，这对基于视觉的算法来说非常具有挑战性。请注意底部的雪序列，相机被大雪完全遮挡。

如图9所示，在极端天气下收集的序列的估计轨迹如图7所示，绘制在谷歌地图上，两个“雾/雨”序列的姿势估计值随着时间推移而漂移，因为没有循环，而“雪”、“乡村”和“夜晚”的结果接近真值。

图7：谷歌地图上极端天气序列的估计的轨迹

图9：大雪场景序列。上图：雪中拍摄的雷达图像。注意，由于雷达上覆盖着厚厚的积雪，雷达前半部分数据丢失。左下：数据收集期间的大雪照片。右下：相机、激光雷达和雷达上覆盖了大量积雪。

相反，基于激光雷达的方法会受到雾和雪的影响。由于相机在雾天/雨天会被水滴挡住，基于视觉的方法也会失败。在夜间序列中，相机图像具有严重的运动模糊，见图10。雪景序列是最具挑战性的序列，因为大雪会导致所有传感器，尤其是相机被遮挡。在所有三种传感器模式中，只有雷达系统能够在所有天气条件下可靠运行和定位。

图8：谷歌卫星图像上的估计轨迹和地图点。蓝色：轨迹，黑色：地图点。在视图中放大后，会在红色框中高亮显示。

我们的方法生成的地图点和轨迹也覆盖在图8中的谷歌卫星图像上，这表明我们的方法能够构建一致的地图。

C、 计算时间

RadarSLAM系统是在没有GPU的C++中实现的，整个系统实现大约 6Hz，配备Intel i7 2.60GHz CPU和16 GB RAM的笔记本电脑，图11显示了37分钟序列（约9000张图像）的运行时间，展示了其实时性能。

图11显示了37分钟序列（约9000张图像）的运行时间

总结

在这项工作中，为FMCW雷达设计了一个完整的SLAM系统，该系统能够在大规模环境中在线可靠运行，并构建全局一致的地图，提出了一种有效的点云生成概率模型，用于环路闭合检测，在不利天气条件下，对公开提供的牛津雷达数据集和自行收集的数据集进行了广泛的实验，未来的工作将研究惯性测量单元与雷达的融合，以获得更好的精度。