Kinematic-ICP: Enhancing LiDAR Odometry with Kinematic Constraints for Wheeled Mobile Robots Moving on Planar Surfaces

TL;DR

Kinematic-ICP addresses the mismatch between LiDAR-based odometry and wheeled robot kinematics by embedding a unicycle-like planar motion model directly into the ICP optimization. It uses the wheel odometry as an initial guess and applies a kinematic correction ${\hat{T}}_{t} \boxplus \Delta \mathbf{u} = {\hat{T}}_{t} \mathrm{Exp}(f(\Delta \mathbf{u}))$ with $\Delta \mathbf{u} \in \mathbb{R}^2$, augmented by adaptive regularization that balances LiDAR and wheel data via $\beta_t = {\chi}({\mathbf{T}}_{t-1} {\mathbf{O}}_{t})$. The approach demonstrates improved accuracy over wheel odometry and standard LiDAR-ICP variants across large warehouse and outdoor scenarios, while maintaining real-time performance (≈100 Hz) and enabling deployment in production (Dexory fleet) with open-source code. This work offers a practical pathway to robust, kinematically consistent odometry for planar wheeled robots in feature-poor environments.

Abstract

LiDAR odometry is essential for many robotics applications, including 3D mapping, navigation, and simultaneous localization and mapping. LiDAR odometry systems are usually based on some form of point cloud registration to compute the ego-motion of a mobile robot. Yet, few of today's LiDAR odometry systems consider domain-specific knowledge or the kinematic model of the mobile platform during the point cloud alignment. In this paper, we present Kinematic-ICP, a LiDAR odometry system that focuses on wheeled mobile robots equipped with a 3D LiDAR and moving on a planar surface, which is a common assumption for warehouses, offices, hospitals, etc. Our approach introduces kinematic constraints within the optimization of a traditional point-to-point iterative closest point scheme. In this way, the resulting motion follows the kinematic constraints of the platform, effectively exploiting the robot's wheel odometry and the 3D LiDAR observations. We dynamically adjust the influence of LiDAR measurements and wheel odometry in our optimization scheme, allowing the system to handle degenerate scenarios such as feature-poor corridors. We evaluate our approach on robots operating in large-scale warehouse environments, but also outdoors. The experiments show that our approach achieves top performances and is more accurate than wheel odometry and common LiDAR odometry systems. Kinematic-ICP has been recently deployed in the Dexory fleet of robots operating in warehouses worldwide at their customers' sites, showing that our method can run in the real world alongside a complete navigation stack.

Figures (3)

• Figure 1: Given the locally consistent but globally drifting wheel odometry of a mobile robot, our approach refines the odometry using LiDAR data and a kinematic model of the platform. The depicted trajectory computed by our approach is roughly 10 km.
• Figure 2: Leica total station prose tracking reference system
• Figure 3: Qualitative comparison of odometry methods on Warehouse Small. Wheel odometry (top left) is smooth but drifts over time. WO + 3D KISS-ICP (bottom left) lacks smoothness and accuracy. WO + 2D KISS-ICP (top right) improves consistency but remains inaccurate. Our approach (bottom right) combines sensor data for smooth and accurate odometry estimation