Paper Reading--RadarSLAM: Radar based Large-Scale SLAM in All Weathers

2021-10-16

科研

Word count: 3.7k | Reading time≈ 15 min

RadarSLAM: Radar based Large-Scale SLAM in All Weathers

主页

数据集可下载，无代码；

本文对标orb-slam，实验部分和orb-slam2做的对比。

0. 摘要

本文提出的RadarSLAM是一个全雷达图形SLAM系统，该系统包含位姿跟踪、局部建图、回环检测、位姿图优化，并使用一个新颖的特征匹配算法、以及从雷达图像中生成的概率点云来提升系统性能。

测试数据集包括公开的雷达数据集以及多个self-collected的雷达序列。实验验证了该系统在各种恶劣天气条件下（如黑夜、浓雾、大雪）都具有SOTA的可靠性和定位精度。

1. 介绍

Motivation：
- 再过去的十年间，基于各种传感器（声呐sonar、相机、LIDAR）的SLAM技术蓬勃发展。但是室外的大规模SLAM仍受限于恶劣的天气条件。
- 调频连续波（FMCM）雷达传感器可以在多种天气环境下工作（该传感器多使用与自动驾驶和自动机器人领域）
- 我们想把FMCM雷达传感器应用在SLAM上
已知：雷达的landmark extraction & motion estimation：
- 雷达路标路标提取和运动估计方法
  - [1,2,3]
  - [4]，radar scan表示为点云形式，使用ICP算法估计相对运动
- 雷达里程器(radar odometery)：Feature based geometric method[6,7,8]
  - [6]，雷达路标提取算法：估计雷达功率信号
  - [7]，图匹配算法：实现不需要运动先验的雷达关键点数据关联
- 雷达里程器：直接法[9]
  - [9]，使用Fourier Mellin Transform 和局部图优化来估计相对位姿
- 基于雷达的定位技术：[5,14,15,16,18,19]
已知：基于深度学习的雷达特征提取和里程计方法
- [12,13,20]
  - [13]，学习多个测量结果的coherence，来决定在读数中保留哪些信息；同时生成一个mask来过滤[12]中雷达读数的噪声
  - [20]，用于在Cartesian雷达图像中检测关键点的自监督框架，除了雷达里程计，该方法还可以用于后续的运动估计和回环检测。
本文：
- 提出了一种基于radar geometry 和graph SLAM的RadarSLAM，该系统可以应用于大规模户外场景下的鲁棒性定位和建图。
贡献：
- 提出了一种特征匹配和位姿跟踪算法（使用雷达几何和图表征）
- 从雷达图像中生成概率点云（降低了speckle noises）
- 基于图优化的全雷达SLAM系统（恶劣天气条件下可工作）
- 在大规模场景环境进行了大量的实验，首次证明了算法可以在极端天气环境中工作(如浓雾和大雪)。

2. 雷达传感器

FMCM雷达：不但可以测距，还可以测速（通过分析发射信号个接收信号的频率差来分析预测目标速度）[21]。

目前FMCM主要的问题是存在多种噪声源：范围误差、角误差、假阳性检测、假阴性检测等[22]

例如，假阳性检测包括：(产生原因主要因为FMCM传感器对surface reflection和reflector pose高度敏感)
- 杂波clutter
- 旁波sidelobes
- 多径反射multi-path reflections
  - 会使得连续帧不一致，引起额外的噪声和异常值(outliers)
- 接收器饱和receiver saturation

鉴于此，相比相机和LIDAR数据，雷达读数更嘈杂，这个点造成雷达传感器在运动估计和SLAM中使用困难。

2.1 Radar Geometry

雷达图像常见噪声：

斑点噪声speckle noise
接收机饱和receiver saturation
多路径反射multi-path reflection

360°FMCM雷达连续扫过Ns``方位角(azimuth angles)（步长=2pi/Ns）。对于每个方位角，雷达发射波束，然后将返回的信号压缩成一个距离，这个过程不考虑高度信息。

鉴于此，雷达图像可以提供absolute metric information of range distance（也就是相机图像缺失的深度信息）。

原始的polar scan可以转换到笛卡尔空间(Cartesian space)（灰度图像？）。

在polar图像中给定点(a,r)，笛卡尔坐标P为：

其中θ=2pi*a/Ns（ranging angle），γ是图像像素空间和世界度量空间的尺度因子

为了得到更好的分辨率，笛卡尔图像通常使用bi-linear interpolation来插值。
本文基于笛卡尔表示法。

3. 基于雷达的SLAM

RadarSLAM的目标是：给定一个radar scans序列，估计雷达位姿和全局一致的地图(使用graph SLAM)。

RadarSLAM系统包括了4个子系统：

位姿跟踪
局部建图
回环检测
位姿图优化

3.1 位姿跟踪

位姿跟踪：利用关键帧，对雷达位姿进行连续的online估计。

具体一点，为了在世界坐标系中追踪当前雷达帧t的位姿Ct，需要先算一个当前帧t到关键帧k（位姿为Ck）的转换T_tk。如果Ck已知，Ct就可计算：【用的是哪个关键帧k？是参考帧。】【参考帧怎么确定的？】

示例：特征提取+特征匹配

利用笛卡尔雷达图像中的关键点的geometry来计算T。

关键点特征提取
- SURF[23]算法，从当前帧t和关键帧k中分别提取两组关键点特征。
根据这两组关键点特征的描述子进行特征匹配。
- 与视觉不同，雷达图像的特征匹配可以利用radar geometry 来实现。（radar geometry可以直接提供metric ranging信息）
- 减少雷达图像特征匹配错误：
  - 机制1：引入motion prior（如最大速度）来限制在雷达局部坐标系中搜索关键点时的最大搜索半径。（减少错误匹配数量，降低特征匹配的计算开销）
  - 机制2：pairwise consistenct constraint—成对的inliner关键点对应需要有相似的运动趋势，基于这一原则，进一步排除异常值(outliers)
  当前帧t和关键帧k上的一对匹配的关键点需要满足以下pairwise constraint:
  
  其中，||·||2是欧氏距离，P_t_i和P_k_i是局部坐标系中关键点对i的笛卡尔坐标，δ_c是最小距离阈值。
- G矩阵：表示满足pariwise consistency约束的所有匹配。
  - 寻找匹配一致的最大inliner set等价于求出G表示的图的最大团，使用[26]可以有效地解决这个问题。
获得最大inliner set后，可以利用匹配的关键点来计算对应的变换矩阵T_tk
- 计算方法：SVD[24]
得到T后，可以用Eq.2来算当前位姿Ct。这里算出来的Ct是Eq.4优化的初始估计。

为了进一步限制local drifts，对Ct做进一步的优化（最小化当前帧t和关键帧k中成功匹配的点对的重投影误差），目标函数为：

其中M_tk是帧t和k的匹配点对集合，P_w_i是世界坐标下的对应地图点。

处理完当前帧t后（根据当前帧估计了Ct），当前帧可以转换成关键帧。

生成关键帧的方法：遵循ORB-SLAM中的关键帧生成方案[25]，该方案要考虑匹配关键点最小数量、当前帧和关键帧之间的平移和旋转。

3.2 局部建图

局部建图的目标：通过联合优化估计的位姿和局部地图来改善位姿估计和局部地图的一致性。

局部建图线程和位姿跟踪线程并行进行。

一旦生成了一个新的关键帧，该帧上的关键点就变成了地图点【？】。

然后，恢复附近的关键帧和这些关键帧可以观测到的地图点，执行局部BA[27]。即通过最小化损失函数（加权的Squared Error之和）来优化关键帧的位姿和地图点的位置，目标损失函数如下：

其中X表示关键帧的位姿+地图点位姿，z-z(X)是预测值和观测值之间的residual error，W是正定对称权重矩阵【W哪来的？】。

使用Levenberg-Marquardt梯度法来解优化问题。

为了限制计算开销，最后一个关键帧创建的地图点如果没有被两个以上关键帧观测到就会被剔除。

3.3 回环检测

鲁棒的回环检测对于减少SLAM系统的漂移而言非常关键。

视觉SLAM在回环检测环节常用词袋模型，但是对于雷达SLAM而言行不通，原因如下：

相比于光学图像，雷达图像的像素特征并不明显，这意味着雷达图像会出现大量相似的特征描述子。
雷达中的多路径反射问题会造成特征描述子模糊。
雷达传感器的微小旋转可能会使场景发生巨大的改变，导致描述子的histogram distribution发生较大扭曲。

鉴于此，我们采用另一种技术：

捕捉场景结构
利用雷达点云反射密度的空间特性。【？】

首先，将雷达图像(a)转换成点云。

(b): 局部最大值算法：在每个azimuth的读数中找到局部最大值来做峰值检测。但是，这个算法的问题是，由于散斑噪声speckle noise的存在，峰值会在整个雷达图像中随机分布，即使没有目标的地方也会出现峰值。

因此，我们提出了一种使用概率模型的点云生成算法（算法1）。

假设每个azimuth scan的peak power s都服从正态分布：
选择>均值μ且>标准差σ的峰值（降低假阳性）

从雷达图像中生成点云后，使用M2DP[31]（3D点云中使用的一种具有旋转不变形的全局描述子）作为点云的描述子，来进行回环检测。

M2DP：计算平面上点云的density signature，使用这些签名的left and right singular vectors作为点云的描述子。

3.4 位姿图优化

位姿图随着雷达的移动而逐渐建立。

一旦检测到回环，就计算当前帧和检测到的关键帧之间的相对变换，然后将变换添加到位姿图中作为回环约束。

计算方法：ICP[32] with RANSAC[33]

如果ICP收敛了，就对所有关键帧执行位姿图优化。

优化：使用g2o[34]库

对关键帧的位姿进行优化后，更新全局地图上的所有地图点。

当全部序列完成时，执行一个全局BA来细化地图。

4. 实验

实验包括定量实验和定性实验，用于评估大规模环境+多种天气情况下的RadarSLAM性能。

4.1 Quantitative Evaluation

4.2 Qualitative Evaluation

4.3 Computation Time

参考文献

[1] J. W. Marck, A. Mohamoud, E. vd Houwen, and R. van Heijster, “Indoor radar slam a radar application for vision and gps denied environments,” in 2013 European Radar Conference. IEEE, 2013.

[2] D. Vivet, P. Checchin, and R. Chapuis, “Localization and mapping using only a rotating fmcw radar sensor,” Sensors, 2013.

[3] F. Schuster, C. G. Keller, M. Rapp, M. Haueis, and C. Curio, “Landmark based radar SLAM using graph optimization,” in IEEE International Conference on Intelligent Transportation Systems. IEEE, 2016.

[4] M. Holder, S. Hellwig, and H. Winner, “Real-time pose graph slam based on radar,” in IEEE Intelligent Vehicles Symposium. IEEE, 2019.

[5] Narula, L., Iannucci, P.A. and Humphreys, T.E., 2020, April. Automotive-radar-based 50-cm urban positioning. In 2020 IEEE/ION Position, Location and Navigation Symposium (PLANS) (pp. 856867). IEEE.

[6] S. Cen and P. Newman, “Precise ego-motion estimation with millimeter-wave radar under diverse and challenging conditions,” in IEEE International Conference on Robotics and Automation. IEEE, 2018, pp. 1–8.

[7] S. H. Cen and P. Newman, “Radar-only ego-motion estimation in difficult settings via graph matching,” in International Conference on Robotics and Automation. IEEE, 2019, pp. 298–304.

[8] R. Aldera, D. De Martini, M. Gadd, and P. Newman, “What could go wrong? introspective radar odometry in challenging environments,” in IEEE Intelligent Transportation Systems Conference. IEEE, 2019.

[9] Y. S. Park, Y.-S. Shin, and A. Kim, “Pharao: Direct radar odometry using phase correlation,” in Proceedings of the IEEE International Conference on Robotics and Automation (ICRA). IEEE, May 2020.

[10] D. Barnes, M. Gadd, P. Murcutt, P. Newman, and I. Posner, “The oxford radar robotcar dataset: A radar extension to the oxford robotcar dataset,” in IEEE International Conference on Robotics and Automation, 2020.

[11] W. Maddern, G. Pascoe, C. Linegar and P. Newman, “1 Year, 1000km: The Oxford RobotCar Dataset,” The International Journal of Robotics Research (IJRR), 2016

[12] D. Barnes, R. Weston, and I. Posner, “Masking by moving: Learning distraction-free radar odometry from pose information,” arXiv preprint arXiv:1909.03752, 2019.

[13] R. Aldera, D. De Martini, M. Gadd, and P. Newman, “Fast radar motion estimation with a learnt focus of attention using weak supervision,” in 2019 International Conference on Robotics and Automation (ICRA). IEEE, 2019, pp. 1190–1196.

[14] S. Saftescu, M. Gadd, D. De Martini, D. Barnes and P. Newman. Kidnapped Radar: Topological Radar Localisation using RotationallyInvariant Metric Learning, in Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Paris, France, 2020

[15] M. Gadd, D. De Martini, and P. Newman, Look Around You: Sequence-based Radar Place Recognition with Learned Rotational Invariance, in IEEE/ION Position, Location and Navigation Symposium (PLANS), (Portland, OR, USA), 2020.

[16] Tang, T.Y., De Martini, D., Barnes, D. and Newman, P., 2020. RSLNet: Localising in Satellite Images From a Radar on the Ground. IEEE Robotics and Automation Letters, 5(2), pp.1087-1094.

[17] Tang, T.Y., De Martini, D., Wu, S. and Newman, P., 2020. SelfSupervised Localisation between Range Sensors and Overhead Imagery. arXiv preprint arXiv:2006.02108.

[18] Giseop Kim, Yeong Sang Park, Younghun Cho, Jinyong Jeong, Ayoung Kim, MulRan: Multimodal Range Dataset for Urban Place Recognition. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Paris, May 2020

[19] Jinyong Jeong, Younggun Cho, Young-Sik Shin, Hyunchul Roh and Ayoung Kim, Complex Urban Dataset with Multi-level Sensors from Highly Diverse Urban Environments. International Journal of Robotics Research, 38(6):642-657, 2019

[20] D. Barnes and I. Posner, “Under the radar: Learning to predict robust keypoints for odometry estimation and metric localisation in radar,” in IEEE International Conference on Robotics and Automation, 2020.

[21] M. Skolnik, “Radar handbook,” 1970. [22] S. Cen, “Ego-motion estimation and localization with millimeter-wave scanning radar,” Master’s thesis, University of Oxford, 2019.

[23] H. Bay, A. Ess, T. Tuytelaars, and L. Van Gool, “Speeded-up robust features (surf),” Computer vision and image understanding, vol. 110, no. 3, pp. 346–359, 2008.

[24] J. H. Challis, “A procedure for determining rigid body transformation parameters,” Journal of Biomechanics, vol. 28, pp. 733–737, 1995.

[25] M. J. M. M. Mur-Artal, Ra´ul and J. D. Tard´os, “ORB-SLAM: a versatile and accurate monocular SLAM system,” IEEE Transactions on Robotics, vol. 31, no. 5, pp. 1147–1163, 2015.

[26] Konc, J. and Janezic, D., An improved branch and bound algorithm for the maximum clique problem. proteins, 4(5), 2007.

[27] B. Triggs, P. F. McLauchlan, R. I. Hartley, and A. W. Fitzgibbon, “Bundle adjustmenta modern synthesis,” in International Workshop on Vision Algorithms. Springer, 1999, pp. 298–372.

[28] W. Churchill, “Experience based navigation: Theory, practice and implementation,” 2012.

[29] J. Zhang and S. Singh, “Loam: Lidar odometry and mapping in realtime,” in Proceedings of Robotics: Science and Systems Conference, 2014.

[30] J. Behley and C. Stachniss, “Efficient surfel-based SLAM using 3D laser range data in urban environments,” in Robotics: Science and Systems, 2018.

[31] L. He, X. Wang, and H. Zhang, “M2dp: A novel 3d point cloud descriptor and its application in loop closure detection,” in IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2016, pp. 231–237.

[32] P. J. Besl and N. D. McKay, “Method for registration of 3-d shapes,” in Sensor fusion IV: control paradigms and data structures, vol. 1611. International Society for Optics and Photonics, 1992, pp. 586–606.

[33] M. A. Fischler and R. C. Bolles, “Random sample consensus: a paradigm for model fitting with applications to image analysis and automated cartography,” Communications of the ACM, 1981.

[34] R. K¨ummerle, G. Grisetti, H. Strasdat, K. Konolige, and W. Burgard, “g2o: A general framework for graph optimization,” in IEEE International Conference on Robotics and Automation. IEEE, 2011.