A Novel LiDAR-IMU-Odometer Coupling Framework for Two-Wheeled Inverted Pendulum (TWIP) Robot Localization and Mapping with Nonholonomic Constraint Factors

Yanwu Zhai, Songyuan Zhang2022 · DOI

TL;DR

This paper introduces a novel framework for robust localization and mapping of Two-Wheeled Inverted Pendulum (TWIP) robots. By coupling LiDAR, IMU, and odometer data within a factor graph, and importantly, introducing new nonholonomic constraint factors for odometry and ground interactions, the system achieves significantly improved accuracy, especially in challenging vertical motion scenarios.

What Problem Does This Paper Address?

Simultaneous Localization and Mapping (SLAM) is crucial for mobile robots operating in unknown environments. While various sensor fusion approaches exist, accurately localizing and mapping robots with unique dynamics, like the Two-Wheeled Inverted Pendulum (TWIP) robot, remains a challenge. TWIP robots, due to their inherent instability and reliance on dynamic balancing, experience complex motion that is not easily captured by standard SE(2) or even SE(3) parameterizations alone. Furthermore, real-world environments often present uneven terrain and motion disturbances that can degrade the performance of traditional SLAM systems. This paper aims to address these limitations by developing a robust SLAM framework specifically tailored for TWIP robots, accounting for their nonholonomic constraints and dynamic balancing behavior.

Methods

The core of this work is a factor graph-based SLAM framework that fuses data from LiDAR, Inertial Measurement Unit (IMU), and wheel encoders (odometry). The novelty lies in the introduction of specific "nonholonomic constraint factors" designed to better model the TWIP's motion.

  • LiDAR Pose Factor: Standard LiDAR feature extraction (plane and edge points) is used, with matching against a global map to estimate the robot's pose.
  • IMU Pre-Integration Factor: IMU measurements (accelerometer and gyroscope) are pre-integrated between keyframes to reduce computational load and provide motion constraints. This factor also optimizes for IMU bias.
  • Odometry Pre-Integration Factor: Wheel encoder data, which provides 2D planar motion information, is also pre-integrated. This is a key nonholonomic factor, as it only constrains parts of the robot's pose.
  • Ground Constraint Factor: This is another novel nonholonomic factor. It models the robot's interaction with the ground, assuming an approximately flat surface. It accounts for motion disturbances (like vibrations) using a zero-mean Gaussian model, acknowledging that real-world terrain is not perfectly smooth.
  • Loop Closure Factor: Standard loop closure detection is implemented to correct for accumulated drift.

The framework utilizes SE(3) pose parameterization and represents rotations using Lie algebra for efficiency and to avoid gimbal lock issues. The system is optimized using a Gauss-Newton method.

Datasets

The authors collected their own datasets for evaluation, as no suitable open datasets for TWIP robots with calibrated LiDAR-IMU-Odometry were available. Two primary datasets were used:

  • Dataset Corridor: An indoor scene featuring a long, straight corridor.
  • Dataset Outdoor: An outdoor scene with a structured environment.

Ground truth for these datasets was obtained offline by using downward-viewing cameras and an attitude sensor, and then optimizing the reprojection error and pose sensor constraints in a batch process.

Key Insights and Ideas

  • TWIPs need specialized SLAM: Standard SLAM approaches struggle with the unique dynamics of TWIP robots, particularly their balancing and non-planar movements.
  • Nonholonomic constraints are crucial: Explicitly modeling the partial constraints imposed by odometry and ground interaction (nonholonomic factors) significantly improves accuracy, especially in the vertical (z) and pitch dimensions.
  • Ground constraints matter: Even on "approximately flat" ground, disturbances exist. A probabilistic ground constraint factor helps bound vertical motion and improve overall pose estimation.
  • Lie algebra for rotations: Using Lie algebra for rotation representation in optimization is more efficient and robust than Euler angles.

Key Results

The proposed LiDAR-IMU-Odometer framework demonstrated superior performance compared to state-of-the-art LiDAR-based SLAM algorithms (Fast_lio2, Lio_sam, Loam) and a pure odometry baseline.

  • Indoor Corridors: The proposed method achieved significantly lower RMSE in all principal parameter directions (x, y, pitch, yaw) compared to other methods. Notably, the z-axis estimation was well-bounded around zero, unlike other algorithms.
  • Outdoor Structured Environment: The algorithm continued to outperform others, with particularly significant improvements in the z-axis direction. The ground constraints were highlighted as crucial for improving vertical pose estimation in environments where LiDAR has limited vertical field of view.

Limitations and Future Work

  • Limited to approximately flat ground: The current ground constraint model assumes an approximately flat surface.
  • Future Work: The authors express interest in extending the framework to handle TWIP robots on undulating roads, which would require more sophisticated ground interaction modeling.

Summary updated

BibTeX
@misc{zhai2022novel,
  author = {Yanwu Zhai and Songyuan Zhang},
  title = {{A Novel LiDAR-IMU-Odometer Coupling Framework for Two-Wheeled Inverted Pendulum (TWIP) Robot Localization and Mapping with Nonholonomic Constraint Factors}},
  year = {2022},
  doi = {10.3390/s22134778},
}
Figure