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