LVI-Q: Robust LiDAR-Visual-Inertial-Kinematic Odometry for Quadruped Robots Using Tightly-Coupled and Efficient Alternating Optimization

TL;DR

LVI-Q tackles the drift and robustness challenges of multisensor odometry for quadrupeds by introducing an alternating optimization framework that tightly fuses LiDAR, visual, inertial, and kinematic data. It combines LIKO, a low-latency filter-based LiDAR-inertial-kinematic estimator using foot-preintegration and point-to-plane residuals, with VIKO, a sliding-window visual-inertial-kinematic optimizer that leverages depth-consistency from LiDAR and visual features. The key innovations are the foot-preintegration residuals integrated into the LIKO filter and the depth-consistency factor based on superpixel-grouped 3D-NDT distributions used in VIKO, enabling robust performance in challenging conditions. Extensive experiments across multiple datasets and platforms show that LVI-Q achieves lower drift and maintains real-time operation, demonstrating strong generalization and robustness for legged robotic navigation.

Abstract

Autonomous navigation for legged robots in complex and dynamic environments relies on robust simultaneous localization and mapping (SLAM) systems to accurately map surroundings and localize the robot, ensuring safe and efficient operation. While prior sensor fusion-based SLAM approaches have integrated various sensor modalities to improve their robustness, these algorithms are still susceptible to estimation drift in challenging environments due to their reliance on unsuitable fusion strategies. Therefore, we propose a robust LiDAR-visual-inertial-kinematic odometry system that integrates information from multiple sensors, such as a camera, LiDAR, inertial measurement unit (IMU), and joint encoders, for visual and LiDAR-based odometry estimation. Our system employs a fusion-based pose estimation approach that runs optimization-based visual-inertial-kinematic odometry (VIKO) and filter-based LiDAR-inertial-kinematic odometry (LIKO) based on measurement availability. In VIKO, we utilize the footpreintegration technique and robust LiDAR-visual depth consistency using superpixel clusters in a sliding window optimization. In LIKO, we incorporate foot kinematics and employ a point-toplane residual in an error-state iterative Kalman filter (ESIKF). Compared with other sensor fusion-based SLAM algorithms, our approach shows robust performance across public and longterm datasets.

Figures (9)

• Figure 1: Overview of our odometry system that combines multiple modalities from different sensors in the form of (a) point-to-plane factor where we minimize the distance from the current LiDAR scan to the map plane, (b) kinematic factor obtained from foot-preintegration method, (c) depth consistency factor by combining visual features and LiDAR point cloud, and (d) visual factor from tracking unique features on the image.
• Figure 2: The overall framework of LVI-Q with LIKO and VIKO which construct three major factors optimized in an alternating estimation framework comprising of error state iterative Kalman filter (ESIKF) and sliding window optimization. (a) Construction of the LiDAR point-to-plane residual through efficient voxel map management and plane modeling. (b) The kinematic factor determines the robot's stance by leveraging joint state propagation. Preintegrated foot measurements are utilized to optimize the current body pose. (c) Integration of the visual factor by tracking prominent image features and the depth consistency module derived from LiDAR point cloud data.
• Figure 3: Illustration of our point cloud division for parallel voxel map update. Each color indicates a group of points assigned to a specific thread for insertion into the voxel map. The blue arrow and red triangle represent the current robot pose and newly observed points, respectively. Multiple parallel threads are utilized to insert the new points into the voxel map according to their spatial division.
• Figure 4: Optimization scheme of LVI-Q, which alternates between VIKO and LIKO module depending on the available measurements. The factors are combined by utilizing sliding window optimization and ESIKF estimation.
• Figure 5: Visual feature consistency factor construction pipeline. (a) LiDAR point cloud utilized for depth consistency factor in the camera field of view. (b) Tracked visual feature points using FAST corner detector and KLT optical flow tracker on the image. (c) Projection of the LiDAR point cloud to the range image in the camera frame. (d) Result of the superpixel SEEDS bergh2012eccv algorithm to segment the image based on pixel information. (e) Grouped LiDAR points based on the superpixel segmentation shown by colored points. (f) Result of visual feature depth consistency according to its corresponding group. Yellow boxes with green dots indicate successful depth consistency factor generation (best viewed in color).