Continuous-Time LiDAR/IMU/Radar Odometry for Accurate and Smooth State Estimation via Tightly Coupled Integration

For advanced autonomous systems, precise and consistent ego-motion estimation is critical to ensure system safety and performant closed-loop control. Unfortunately, traditional discrete-time approaches generally rely on interpolation to fuse asynchronous measurements, which would inevitably lose accuracy in high-speed motions. To this end, we propose a tightly coupled LiDAR/IMU/Radar odometry tailored for accurate and smooth state estimation using continuous-time trajectory optimization. Specifically, a novel coarse-to-fine dynamic initialization method is first proposed to obtain system states, IMU biases, and optional intersensor spatiotemporal parameters without requiring specific artificial targets and movements. After performing data association for LiDAR scans and outlier removal for Radar scans, LiDAR point-to-plane factor, angular velocity factor, linear acceleration factor, and Radar Doppler velocity factor are jointly minimized in a factor graph framework to derive optimal estimates. A series of dedicated experiments on the real-world data sets were conducted to evaluate the proposed method. The results demonstrate that the proposed method exhibits superior navigation performance in complex environments than the state-of-the-art methods in terms of position, velocity, and attitude.