PaperlayerView exact revision on arXiv

Source-linked AI summary

4D iRIOM: 4D Imaging Radar Inertial Odometry and Mapping

Yuan Zhuang, Binliang Wang, Jianzhu Huai, Miao Li

arXiv:2303.13962v2cs.RO

TL;DR

Lidar and cameras can struggle in degraded weather, while radar provides measurements useful in those conditions. iRIOM fuses radar and inertial data with scan-to-submap matching and loop closure; experiments report accurate odometry and consistent mapping on the authors’ and ColoRadar datasets.

  • Problem

    Lidar and cameras struggle in degraded environments such as rain and fog, while radar can provide measurements in those conditions.

    Source: §I · p. 1

  • Method

    iRIOM fuses radar ego-velocity and scan-to-submap matches with IMU data in an iterative EKF, with loop closure for revisits.

    Source: §I · p. 1

  • Results

    iRIOM achieved accurate odometry and consistent mapping with respect to EKFVIO and FastLIO-SLAM on the authors’ and ColoRadar datasets.

    Source: §V · p. 8

  • Takeaways & Limitations

    The authors report iRIOM as, to their knowledge, the first complete 4D radar-inertial SLAM system.

    Source: §I · pp. 1–2

Abstract

from arXiv · show

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.

I. INTRODUCTION

iRIOM combines radar ego-velocity, scan-to-submap matches, and 6D IMU data for odometry and mapping with loop closure. The paper reports favorable localization results and reviews related radar odometry and place-recognition work.

I. INTRODUCTION

  • iRIOM fuses radar-scan ego-velocity and scan-to-submap matches with 6D IMU data to estimate odometry and produce consistent maps.The authors describe iRIOM as a real-time imaging radar inertial odometry and mapping method.Source: §I · p. 1
  • The method uses graduated non-convexity for ego-velocity estimation and distribution-to-multi-distribution constraints for scan-to-submap point matches.The authors contrast these choices with RANSAC and scan-to-scan matching, respectively.Source: §I · p. 1
  • An iterative EKF fuses IMU data with radar constraints, while scancontext detects revisited places for pose-graph loop closure; the authors call this the first complete 4D radar-inertial SLAM system to their knowledge.The loop-closure method uses scancontext for place recognition.Source: §I · pp. 1–2
  • Tests on in-house and ColoRadar sequences report localization accuracy surpassing existing radar-inertial odometry approaches and approaching lidar-inertial SLAM approaches.Ablations report benefits from IMU data, ego-velocity measurements, and scan-to-submap matches.Source: §I · p. 2

II. RELATED WORK

  • The paper reviews work on radar odometry and radar-based place recognition.Its stated review covers 3D radar odometry, 4D radar inertial odometry, and radar-based place recognition.Source: §II · p. 2

A. 3D Radar Odometry

Prior radar odometry uses feature-based or direct scan matching, while 4D radar matching remains difficult because scans are sparse and noisy. iRIOM combines single-scan ego-velocity, scan-to-submap matching, and IMU data, then updates maps and closes loops.

A. 3D Radar Odometry

  • Conventional millimeter-wave radars estimate planar position from distance and azimuth but lack object heights; most radar odometry methods therefore convert data to planar images or 2D point clouds and match them.Source: A. 3D Radar Odometry · p. 2

B. 4D Radar Inertial Odometry

  • 4D imaging radar improves elevation resolution, but sparse point clouds and noise make stable, continuous, high-precision 3D scan matching difficult.The scans contain about 400 points per scan, and matching expands from a 2D to a 3D search space.Source: B. 4D Radar Inertial Odometry · p. 2
  • iRIOM combines single-scan ego-velocity with scan matching enhanced by a submap, addressing scan matching’s noise sensitivity and sparse-point breakdown alongside ego-velocity’s limited correction of vertical drift.The system uses scancontext for loop detection to remedy odometry drift.Source: B. 4D Radar Inertial Odometry · p. 2

A. Framework Overview

  • The framework propagates state with IMU data, estimates radar ego-velocity, finds scan-to-submap matches, and uses their covariance-weighted constraints to update state in an iterated Kalman filter.It transforms scans into the global frame to update local and global maps, then uses loop constraints in pose-graph optimization.Source: A. Framework Overview · p. 3

B. Radar Data Preprocessing

Radar preprocessing removes invalid returns and estimates platform ego-velocity from Doppler observations. IMU data propagates the system state and its uncertainty between measurements.

B. Radar Data Preprocessing

  • Relaxation filtering uses spatial distributions to remove radar noise while retaining sufficient point observations.The filter defines inliers using neighborhood counts and standard deviations against corresponding thresholds.Source: B. Radar Data Preprocessing · p. 3
  • For static world points, measured 1D Doppler velocity relates to radar ego-velocity through the point’s direction in the radar frame.This measurement model provides the basis for estimating platform velocity from a radar scan.Source: B. Radar Data Preprocessing · p. 3
  • GNC estimates radar-frame ego-velocity from sparse scans with potentially many moving points, and points below the residual threshold are retained as static.The non-minimal solver is described as typically more robust than RANSAC; the resulting noise covariance comes from the information matrix.Source: B. Radar Data Preprocessing · p. 3

C. IMU State Propagation

  • The IMU propagation state includes position, velocity, attitude, gravity, sensor biases, and radar-to-IMU extrinsics; the extrinsics can be estimated online or locked.The global frame is the IMU frame at system start, and gravity is estimated because its direction may not align with the global z-axis.Source: C. IMU State Propagation · p. 3
  • IMU data propagates the state and error covariance, with the error state defined relative to the state predicted by propagation.The state-minus operation uses a rotation-log difference for orientation, while other state components use the common minus operation.Source: C. IMU State Propagation · pp. 3–4

D. Ego-Velocity Measurements

The radar scan’s ego-velocity estimate is incorporated as a velocity measurement, with residual-based screening before the state update.

D. Ego-Velocity Measurements

  • Radar-frame ego-velocity estimated from a scan serves as the velocity measurement in the state update.Its residual and Jacobian are defined relative to the error state.Source: D. Ego-Velocity Measurements · p. 4
  • Measurements whose residual Mahalanobis distance exceeds the χ2 3,0.05 threshold are pruned at significance level 0.05.The remaining measurement update uses the state covariance and measurement covariance.Source: D. Ego-Velocity Measurements · p. 4
  • The update applies the state plus operation, which uses an exponential-map composition for orientation components.The operation is used to obtain the updated state and covariance.Source: D. Ego-Velocity Measurements · p. 4

E. Scan-to-Submap Matching Constraints

iRIOM matches each scan to a local submap and uses weighted point-match distances in an iterative state update. The submap approach is described as more stable and accurate than scan-to-scan matching.

E. Scan-to-Submap Matching Constraints

  • Scan-to-submap matching is described as more stable and accurate than scan-to-scan matching, which is less robust with sparse points or changing local scenes.The method uses an ikd-tree local map to facilitate real-time computation.Source: E. Scan-to-Submap Matching Constraints · p. 4
  • Distribution-to-distribution distances use local source and target geometry to weight residuals, making them resilient to noise in sparse radar scans.The stated motivation is that sparse scans make exact one-to-one matches difficult to ensure.Source: E. Scan-to-Submap Matching Constraints · p. 4
  • For each current-scan point, the method uses its five nearest submap points and weights the distance to their centroid by covariance.This distribution-to-multi-distribution weighting empirically improves scan-to-submap registration.Source: E. Scan-to-Submap Matching Constraints · p. 4
  • The scan-to-submap point matches update the state in an iterative Kalman filter, while the measurement residual and Jacobian are expressed relative to the state.The update is framed as a MAP estimation problem and constrains the state to remain near its initial estimate.Source: E. Scan-to-Submap Matching Constraints · p. 4

A. In-house Data

The in-house evaluation used a ground robot with a 4D radar and IMU across two outdoor sequences and one indoor sequence. Reference quality and scene conditions varied, particularly in the underground parking sequence.

A. In-house Data

  • The robot carried a Continental ARS548 4D radar scanning at 15 Hz, and its built-in IMU data were fused with radar data.The reference trajectory was obtained from a Bynav X1-5H GNSS/INS module in open-sky areas.Source: A. In-house Data · p. 5
  • The reference was centimeter-level GNSS RTK/INS for sequence 1, while sequence 2 included occasional short GNSS blockages but reported good INS poses.Both trajectories began and ended at the same pose.Source: A. In-house Data · p. 5
  • Sequence 1 was a six-minute outdoor basketball-court run with large angular velocity at turns and a centimeter-level GNSS RTK/INS reference.The run reached 3 m/s and started and stopped at the same pose.Source: A. In-house Data · p. 5
  • Sequence 3 was a five-minute underground parking run with smooth surfaces, occlusions, and many multipath “ghost” points.The robot reached 2 m/s, and its GNSS/INS solution was somewhat unreliable.Source: A. In-house Data · p. 5

B. In-house Data Results

On the in-house sequences, iRIO outperformed EKFRIO in position accuracy, and iRIOM’s loop closure further reduced drift. iRIOM’s accuracy was comparable to FastLIO-SLAM, and it ran in real time on a consumer laptop.

B. In-house Data Results

  • iRIO had much better position accuracy than EKFRIO, while iRIOM further reduced Z-direction drift and achieved accuracy comparable to FastLIO-SLAM.Table I reports statistics for EKFRIO, iRIO, and iRIOM on sequences 1–3.Source: B. In-house Data Results · p. 6
  • On sequence 2, iRIO’s scan-to-submap matching yielded significantly lower trajectory closure error and RMSEs than EKFRIO, while loop closure reduced trajectory closure error by an order of magnitude.Despite the matching improvement, iRIO retained substantial Z-direction drift in the large scene.Source: B. In-house Data Results · p. 6, Figure 4 · p. 6
  • iRIOM ran in real time on a consumer laptop with the ARS548 dataset’s approximately 15 Hz radar scans.Scan-to-submap matching, including the iterative EKF update, was the most costly part; runtime also varied with scene, running faster indoors than outdoors.Source: B. In-house Data Results · p. 6

C. Ablation Studies

With loop closure disabled, the ablations compare inertial prediction, radar ego-velocity, and scan-to-submap constraints, separately and together. Their results show the value and limitations of each update source.

C. Ablation Studies

  • Using both velocity and scan-to-submap constraints produced no failures across repeated runs and largely reduced mean closure errors and trajectory RMSEs.Table III reports the ablation statistics for sequences 1–3.Source: Table III · p. 7, C. Ablation Studies · p. 7
  • With ego-velocity alone, closure errors were large, especially in Z, and position APE RMSE exceeded 2 m.Source: Table III · p. 7, C. Ablation Studies · p. 7
  • Scan-to-submap-only updates often failed from incorrect or failed matches, but successful runs had relatively high location accuracy and reduced Z drift on smaller sequences.Source: Table III · p. 7, C. Ablation Studies · p. 7
  • Without IMU data, more than 3 of 10 runs failed to estimate a complete trajectory, while constant-velocity prediction reduced localization accuracy.Source: C. Ablation Studies · p. 7
  • Scan-to-five-scans matching improved on scan-to-scan but remained less accurate than submaps; scan-to-submap matches also reduced mapping blur versus velocity-only updates.Larger temporal windows increased computation while their accuracy benefit flattened out.Source: C. Ablation Studies · p. 7, Figure 7 · p. 7

D. ColoRadar Dataset Results

Experiments on ColoRadar indoor and outdoor sequences indicate that iRIOM generalizes while achieving high-accuracy localization and mapping.

D. ColoRadar Dataset Results

V. CONCLUSIONS

iRIOM combines radar and inertial measurements for 3D odometry and mapping, and the authors report accurate odometry and consistent mapping on their and the ColoRadar datasets. They identify it as the first real-time 3D odometry and consistent mapping system using 4D radar data.

V. CONCLUSIONS

  • iRIOM fuses IMU data, radar-scan ego-velocity, and scan-to-submap point matches for 3D radar-inertial odometry and mapping.Source: §V · p. 8
  • Experiments on the authors’ and ColoRadar datasets showed accurate odometry and consistent mapping compared with EKFVIO and FastLIO-SLAM.Source: §V · p. 8
  • Ablations validated benefits from IMU data, scan-to-submap matching, and loop closure; future work proposes additional constraints and sensor modalities.The proposed additions include the Manhattan-world assumption, ground planes, and cameras.Source: §V · p. 8

Related papers

Loading 2303.13962v2…