Robust High-Speed State Estimation for Off-road Navigation using Radar Velocity Factors

Abstract

Enabling robot autonomy in complex environments for mission critical application requires robust state estimation. Particularly under conditions where the exteroceptive sensors, which the navigation depends on, can be degraded by environmental challenges thus, leading to mission failure. It is precisely in such challenges where the potential for FMCW radar sensors is highlighted: as a complementary exteroceptive sensing modality with direct velocity measuring capabilities. In this work we integrate radial speed measurements from a FMCW radar sensor, using a radial speed factor, to provide linear velocity updates into a sliding-window state estimator for fusion with LiDAR pose and IMU measurements. We demonstrate that this augmentation increases the robustness of the state estimator to challenging conditions present in the environment and the negative effects they can pose to vulnerable exteroceptive modalities. The proposed method is extensively evaluated using robotic field experiments conducted using an autonomous, full-scale, off-road vehicle operating at high-speeds (~12 m/s) in complex desert environments. Furthermore, the robustness of the approach is demonstrated for cases of both simulated and real-world degradation of the LiDAR odometry performance along with comparison against state-of-the-art methods for radar-inertial odometry on public datasets.

Figures (5)

• Figure 1: Figure shows the robotic vehicle during a field test conducted in a desert environment. The 4km trajectory is annotated with linear and rotational speeds achieved during the experiment. The barren and structure--less environment proved challenging for LiDAR odometry and lead to failure during sharp turns (marked by A). Bottom row shows the environment through images captured by on-board cameras.
• Figure 2: Visualization of the different sensor frames on the vehicle as well as an example of the radar return from a static object and the resulting rdmap.
• Figure 3: Illustration visualizing the connections in the factor graph, including factors from the IMU, LiDAR odometry, and radar connected to pose, linear velocity, and IMU bias states along with a marginalization prior for the sliding-window of the smoother.
• Figure 4: Figures comparing the performance of different methods versus GPS in the jpl East Lot experiment including nominal performance (a) as well as the added noise experiments (b,c) where the noise standard deviation is 1m.
• Figure 5: Figures comparing the performance of different methods versus GPS in the jpl Helendale experiment including position (a), forward velocity in body-frame (b), and lateral velocity in body-frame estimates (c).