Hybrid Contact Preintegration for Visual-Inertial-Contact State Estimation Using Factor Graphs

Maani Ghaffari, Ross Hartley, Maani Ghaffari Jadidi, Lu Gan, Jiunn-Kai Huang, Jessy W. Grizzle, Ryan M. Eustice

TL;DR

This paper introduces a novel "hybrid contact preintegration" method for legged robots, enabling more robust and accurate state estimation by effectively handling frequent changes in foot-ground contact. By preintegrating contact information over arbitrary contact switches, the method significantly reduces computational complexity and improves estimation accuracy, especially during vision system failures.

What Problem Does This Paper Address?

Estimating the state (position, orientation, velocity) of legged robots is crucial for tasks like navigation and control. This is challenging because legged robots interact with their environment through intermittent and dynamic foot-ground contact, which can cause significant drift in traditional odometry systems. Existing methods often struggle with the frequent switching of contact points, requiring the addition of new variables to the state estimation graph each time contact is made or broken. This increases computational load and complexity, especially for robots with multiple contact points. The paper aims to address this by developing a more efficient and robust way to incorporate contact information into state estimation, even when contact states change rapidly.

Methods

The core contribution is the hybrid contact preintegration technique. This method extends the concept of preintegration (used for IMUs) to handle contact information. Instead of re-linearizing the contact model every time contact changes, the paper proposes to "preintegrate" the contact measurements over periods that can include multiple contact switches.

Key aspects of the method include:

  • Factor Graph Framework: The problem is framed within a factor graph, a powerful tool for representing and solving state estimation problems by modeling states as nodes and measurements as factors.
  • Hybrid Dynamics Model: The contact dynamics are modeled as a hybrid system, accounting for both continuous motion while contact is maintained and discrete transitions when contact switches occur.
  • Preintegration: The method preintegrates the contact dynamics over time, effectively summarizing the effect of contact measurements between keyframes. This avoids re-computing the entire contact model at every time step.
  • Arbitrary Contact Switches: The "hybrid" aspect allows the preintegration to handle an arbitrary number of contact switches within a single preintegrated factor, significantly reducing the number of variables needed in the optimization problem.
  • Forward Kinematics Factor: A new factor is introduced to model the forward kinematics of the robot, relating joint encoder measurements to the end-effector pose. This factor accounts for encoder noise.
  • Rigid Contact Factor: This factor relates consecutive contact poses, incorporating the preintegrated contact measurements and noise.

Datasets

The experiments were conducted using data collected from a Cassie-series robot in an indoor laboratory environment. Ground truth trajectories were obtained using a Vicon motion capture system. The dataset includes stereo images (20 Hz), IMU data (750 Hz), and joint encoder data from the robot (400 Hz). No new benchmarks were introduced; rather, existing datasets were used for evaluation.

Key Insights and Ideas

  • Contact is Key for Robustness: The paper strongly demonstrates that incorporating contact information is crucial for robust state estimation in legged robots, especially when other sensors (like vision) fail.
  • Preintegration is Not Just for IMUs: The success of preintegration for IMUs can be extended to other dynamic measurements like contact, offering significant computational benefits.
  • Handling Contact Switches Efficiently: The core innovation of handling arbitrary contact switches within a single factor is a major breakthrough for reducing the complexity of state estimation for legged robots.
  • Vision Dropout is a Real Problem: The experiments highlight how susceptible visual odometry can be to environmental conditions (like motion blur) and sensor failures, underscoring the need for complementary sensors.

Key Results

  • Improved Odometry Accuracy: The Visual-Inertial-Contact (VIC) odometry system, which includes the proposed hybrid contact factor, consistently outperformed Visual-Inertial (VI) and Inertial-Contact (IC) systems in terms of relative position error and drift.
  • Robustness to Vision Failure: During simulated "vision dropout" periods, the covariance of the robot's base pose for the VIC system grew much less significantly compared to the VI system, demonstrating its enhanced robustness.
  • Reduced Computational Complexity: By reducing the number of variables required, the hybrid preintegration method leads to a more efficient estimator.
  • Terrain Factor Potential: The paper shows that by treating contact poses as state variables, it's possible to introduce "terrain factors" (e.g., fixing the z-coordinate of the contact frame on flat ground) to further improve estimation accuracy, particularly in the vertical direction.

Limitations and Future Work

  • Limitations:
    • The current implementation assumes a binary contact sensor.
  • The "terrain factors" experiment used a very simple prior (flat ground), and more complex terrain mapping would be beneficial.
  • The paper mentions that the derivation of the noise term for arbitrary contact switches can become complex, though it simplifies with iterative propagation.
  • Future Work:
    • Incorporating loop closure constraints into the factor graph to further improve long-term state estimation and mapping.
  • Investigating the utility of "terrain factors" more deeply, allowing robots to correct their state based on detected contact with an estimated map.
  • Extending the approach to handle more complex contact scenarios and potentially more sophisticated contact sensors.

Summary updated

Abstract

The factor graph framework is a convenient modeling technique for robotic state estimation where states are represented as nodes, and measurements are modeled as factors. When designing a sensor fusion framework for legged robots, one often has access to visual, inertial, joint encoder, and contact sensors. While visual-inertial odometry has been studied extensively in this framework, the addition of a preintegrated contact factor for legged robots has been only recently proposed. This allowed for integration of encoder and contact measurements into existing factor graphs, however, new nodes had to be added to the graph every time contact was made or broken. In this work, to cope with the problem of switching contact frames, we propose a hybrid contact preintegration theory that allows contact information to be integrated through an arbitrary number of contact switches. The proposed hybrid modeling approach reduces the number of required variables in the nonlinear optimization problem by only requiring new states to be added alongside camera or selected keyframes. This method is evaluated using real experimental data collected from a Cassie-series robot where the trajectory of the robot produced by a motion capture system is used as a proxy for ground truth. The evaluation shows that inclusion of the proposed preintegrated hybrid contact factor alongside visual-inertial navigation systems improves estimation accuracy as well as robustness to vision failure, while its generalization makes it more accessible for legged platforms.
BibTeX
@inproceedings{ghaffari2018hybrid,
  author = {Maani Ghaffari and Ross Hartley and Maani Ghaffari Jadidi and Lu Gan and Jiunn-Kai Huang and Jessy W. Grizzle and Ryan M. Eustice},
  title = {{Hybrid Contact Preintegration for Visual-Inertial-Contact State Estimation Using Factor Graphs}},
  booktitle = {IEEE/RSJ International Conference on Intelligent Robots and Systems},
  year = {2018},
  doi = {10.1109/iros.2018.8593801},
}
Figure