Millimeter wave radar can measure distances, directions, and Doppler velocity for objects in harsh conditions such as fog. The 4D imaging radar with both vertical and horizontal data resembling an image can also measure objects' height. Previous studies have used 3D radars for ego-motion estimation. But few methods leveraged the rich data of imaging radars, and they usually omitted the mapping aspect, thus leading to inferior odometry accuracy. This paper presents a real-time imaging radar inertial odometry and mapping method, iRIOM, based on the submap concept. To deal with moving objects and multipath reflections, we use the graduated non-convexity method to robustly and efficiently estimate ego-velocity from a single scan. To measure the agreement between sparse non-repetitive radar scan points and submap points, the distribution-to-multi-distribution distance for matches is adopted. The ego-velocity, scan-to-submap matches are fused with the 6D inertial data by an iterative extended Kalman filter to get the platform's 3D position and orientation. A loop closure module is also developed to curb the odometry module's drift. To our knowledge, iRIOM based on the two modules is the first 4D radar inertial SLAM system. On our and third-party data, we show iRIOM's favorable odometry accuracy and mapping consistency against the FastLIO-SLAM and the EKFRIO. Also, the ablation study reveal the benefit of inertial data versus the constant velocity model, and scan-to-submap matching versus scan-to-scan matching.

翻译：毫米波雷达能够在雾等恶劣条件下测量物体的距离、方向和多普勒速度。兼具垂直与水平图像特征的四维成像雷达还可测量物体高度。已有研究利用三维雷达进行自运动估计，但极少方法充分利用成像雷达的丰富数据，且通常忽略建图功能，导致里程计精度欠佳。本文提出一种基于子图概念的实时成像雷达惯性里程计与建图方法iRIOM。为应对运动物体和多径反射，采用渐进非凸方法从单次扫描中稳健高效估计自速度；为衡量稀疏非重复雷达扫描点与子图点之间的匹配度，采用分布到多分布距离度量。通过迭代扩展卡尔曼滤波器将自速度与扫描-子图匹配结果融合六维惯性数据，获得平台的三维位置与姿态。同时设计闭环模块抑制里程计漂移。据我们所知，基于上述两模块的iRIOM是首个四维雷达惯性SLAM系统。在自有与第三方数据集上的实验表明，相较于FastLIO-SLAM和EKFRIO，iRIOM在里程计精度与建图一致性方面表现更优。此外，消融实验揭示了惯性数据相对于匀速运动模型、以及扫描-子图匹配相对于扫描-扫描匹配的优越性。