4D Radar-Inertial SLAM based on Factor Graph Optimization

SLAM (Simultaneous Localization and Mapping) plays a key role in autonomous driving. Recently, 4D Radar has attracted widespread attention because it breaks through the limitations of 3D millimeter wave radar and can simultaneously detect the distance, velocity, horizontal azimuth and elevation azimuth of the target with high resolution. However, there are few studies on 4D Radar in SLAM. In this paper, RI-FGO, a 4D Radar-Inertial SLAM method based on Factor Graph Optimization, is proposed. The RANSAC (Random Sample Consensus) method is used to eliminate the dynamic obstacle points from a single scan, and the ego-motion velocity is estimated from the static point cloud. A 4D Radar velocity factor is constructed in GTSAM to receive the estimated velocity in a single scan as a measurement and directly integrated into the factor graph. The 4D Radar point clouds of consecutive frames are matched as the odometry factor. A modified scan context method, which is more suitable for 4D Radar’s sparse and noisy point clouds and field of view, is proposed to detect possible loops. Different from the common front-end odometry and back-end optimization structure, we implement the whole SLAM system including odometry, loop detection and graph optimization in the factor graph. We compared our method with some mainstream methods such as EKFRIO on our own and public datasets. At the same time, ablation experiments were also carried out to illustrate the role of 4D Radar velocity factor and odometry factor. Experiments have shown that our proposed SLAM method can converge the bias of accelerometers and gyroscopes well and has excellent accuracy.