This video introduces Kinematic-ICP, a novel LiDAR odometry approach designed to enhance ego-motion estimation for wheeled mobile robots operating on planar surfaces. Traditional LiDAR odometry methods often produce "jumpy" or inconsistent pose estimations, while wheel odometry suffers from drift. Kinematic-ICP addresses this by incorporating the robot's kinematic model directly into the iterative closest point (ICP) optimization framework. It utilizes smooth wheel odometry as an initial guess and refines it with LiDAR data by optimizing for the robot's kinematic control inputs (e.g., linear and angular displacements) rather than a full 6D pose. This ensures a locally consistent and smooth trajectory that aligns with the robot's physical motion capabilities. The approach also features an adaptive regularization mechanism to dynamically weight the influence of LiDAR measurements and wheel odometry, improving robustness in challenging environments like feature-poor corridors. Experimental results demonstrate that Kinematic-ICP achieves superior accuracy and smoothness compared to existing LiDAR odometry and fusion methods, even when tested on large-scale warehouse environments and uneven outdoor terrains, and has been successfully deployed in real-world robotic fleets.
Kinematic-ICP: Enhancing LiDAR Odometry with Kinematic Constraints
0:01 Introduction to Kinematic-ICP: Benedikt Mersch introduces Kinematic-ICP, a new method to enhance LiDAR odometry by leveraging the kinematic constraints of wheeled mobile robots.
0:10 Robot Odometry Challenges: Wheeled mobile robots (e.g., Husky, warehouse robots) often operate on flat surfaces and require accurate ego-motion (odometry). The video highlights that wheel odometry is smooth but prone to drift, while LiDAR odometry (like KISS-ICP) is globally more consistent but can exhibit "jumps" or discontinuities locally.
2:12 Importance of Smooth Odometry: The speaker emphasizes that odometry should always evolve smoothly without discrete jumps, citing ROS Enhancement Proposal 105. Large jumps in LiDAR odometry can imply unrealistic velocities for the robot.
2:40 Goal of Kinematic-ICP: The objective is to efficiently estimate odometry that is locally consistent, smooth, and accurate, without merely fusing different odometry sources.
It starts with the wheel odometry as an initial (smooth) guess for the robot's pose.
LiDAR measurements are then aligned with a local point cloud map.
Instead of estimating a full 6-degree-of-freedom pose correction, Kinematic-ICP optimizes for the control inputs of a kinematic model (e.g., forward motion and rotation for a differential drive robot). This fundamentally ensures the resulting motion is smooth and physically plausible.
4:12 Improved Trajectory Quality: Visual comparisons demonstrate that Kinematic-ICP produces a trajectory that is both highly accurate (close to the reference) and remarkably smooth, rectifying the issues observed in both wheel and standard LiDAR odometry.
Key Contributions and Performance (from paper/video):
Accuracy & Robustness: Kinematic-ICP consistently outperforms wheel odometry and state-of-the-art LiDAR odometry systems (like KISS-ICP, EKF, Fuse) in terms of relative and absolute errors across various challenging environments (warehouses, outdoor parks).
Adaptive Regularization: The system dynamically adjusts the weighting between LiDAR measurements and wheel odometry based on their consistency. This is crucial for robustness in "featureless" or ambiguous environments (e.g., long corridors) and avoids manual tuning.
Computational Efficiency: Unlike approaches that fuse separate LiDAR and wheel odometry estimates, Kinematic-ICP directly processes both, leading to higher computational efficiency (e.g., 100 Hz on a single-core CPU vs. 10 Hz for Fuse).
Real-world Deployment: The method has been successfully deployed in the Dexory fleet of robots operating in warehouses worldwide, showcasing its practical applicability and reliability.
Open-Source Availability: The Kinematic-ICP implementation is open-source and available on GitHub, supporting ROS2 (Humble, Iron, Jazzy).