RIO-Vehicle: A Tightly-Coupled Vehicle Dynamics Extension of 4D Radar Inertial Odometry

Accurate and reliable localization in GNSS-denied environments is critical for autonomous driving. Nevertheless, LiDAR-based and camera-based methods are easily affected by adverse weather conditions such as rain, snow, and fog. The 4D Radar with all-weather performance and high resolution has attracted more interest. Currently, there are few localization algorithms based on 4D Radar, so there is an urgent need to develop reliable and accurate positioning solutions. This paper introduces RIO-Vehicle, a novel tightly coupled 4D Radar/IMU/vehicle dynamics within the factor graph framework. RIO-Vehicle aims to achieve reliable and accurate vehicle state estimation, encompassing position, velocity, and attitude. To enhance the accuracy of relative constraints, we introduce a new integrated IMU/Dynamics pre-integration model that combines a 2D vehicle dynamics model with a 3D kinematics model. Then, we employ a dynamic object removal process to filter out dynamic points from a single 4D Radar scan and perform scan-to-scan matching to obtain 4D Radar odometry. Furthermore, we introduce ground plane constraints to eliminate vertical error drift. In the backend, we add the IMU/Dynamics factor, ground plane factor, and 4D Radar odometry factor to the factor graph and obtain estimation results through sliding window-based optimization. Real-vehicle experiments confirm the reliability and accuracy of our proposed method.