3D ego-Motion Estimation Using low-Cost mmWave Radars via Radar Velocity Factor for Pose-Graph SLAM

Achieving general 3D motion estimation for all-visibility has been a key challenge in robotics, especially in extreme environments. The widely adopted camera and LiDAR-based motion estimation critically deteriorate under fog or smoke. In this letter, we devised a unique sensor system for 3D velocity estimation by compositing two orthogonal radar sensors. The proposed configuration allowed securing returns from the static objects on the ground. This work aimed at a realistic sensor deployment in a harsh environment by casing the bare sensor rig into a plastic box. As will be shown, the proposed velocity-based ego-motion estimation presented reliable performance over existing point matching-based methods, which degraded when measurement attenuates due to the casing. Furthermore, we introduce a novel radar instant velocity factor for pose-graph simultaneous localization and mapping (SLAM) framework and solve for 3D ego-motion in the integration with IMU. The validation reveals that the proposed method can be applied to estimate general 3D motion in both indoor and outdoor, targetting various visibility and the structureness in the environment.