Legged Robot State Estimation in Slippery Environments Using Invariant Extended Kalman Filter with Velocity Update

Sangli Teng, Mark Wilfried Mueller, Koushil Sreenath

TL;DR

This paper introduces a novel state estimation filter for legged robots navigating slippery terrain. By fusing inertial, leg kinematic, and vision-based velocity measurements within an Invariant Extended Kalman Filter (InEKF), the approach achieves robust and accurate state estimation even under significant slippage, a common failure point for existing methods.

What Problem Does This Paper Address?

Legged robots are increasingly expected to operate in complex, unstructured environments. A critical challenge in these scenarios is maintaining accurate state estimation (knowing the robot's position, orientation, and velocity) when the ground is slippery or unstable. Traditional state estimation methods often rely on assumptions of static contact, meaning they can fail dramatically when the robot's feet slip. While some approaches attempt to detect or reject slippage, they can struggle with consistent or large-magnitude slips. Furthermore, integrating visual information, which can provide valuable velocity cues, is often hampered by the need for precise camera calibration and susceptibility to lighting changes or texture loss. This paper aims to bridge this gap by developing a state estimator that is robust to slippage and can effectively leverage visual velocity measurements without requiring prior camera calibration.

Methods

The core of this work is an Invariant Extended Kalman Filter (InEKF). The authors extend the standard InEKF by:

  • Fusing multiple sensor modalities: They integrate data from the robot's Inertial Measurement Unit (IMU), leg kinematics (derived from joint encoders), and a tracking camera.
  • Vision-based velocity measurement: A key innovation is the use of a tracking camera (Intel Realsense T265) to provide velocity and rotational rate measurements. These are formulated as a "right-invariant observation," a technique well-suited for systems operating on Lie groups (like rotations).
  • Auto-calibration of camera pose: The misalignment between the robot's frame and the camera's frame is explicitly modeled as part of the state. This allows the filter to simultaneously estimate the robot's state and the camera's pose relative to the robot, eliminating the need for manual calibration.
  • Online noise parameter tuning: To handle the highly time-varying noise characteristics of camera measurements, especially during ground impacts, an online method is proposed to adaptively estimate the measurement covariance.
  • Invariant Error Formulation: The use of invariant errors and the concept of "group affine" systems allows for a linearized error dynamics model, which is crucial for the stability and performance of the InEKF.

Datasets

The paper evaluates the proposed method on experiments conducted with a Cassie bipedal robot walking over slippery terrain. The ground truth for evaluation is obtained using a motion capture system. No new datasets are introduced; rather, the focus is on demonstrating the performance of the proposed estimator in a challenging, real-world scenario.

Key Insights and Ideas

  • Slippage is a major challenge for legged robot state estimation: The paper clearly demonstrates how traditional methods struggle and diverge when faced with consistent slippage.
  • Vision-based velocity is a powerful cue, but needs careful integration: Leveraging velocity from a tracking camera, especially when formulated as a right-invariant observation, significantly improves state estimation robustness.
  • Auto-calibration of sensors is crucial for practical deployment: Explicitly modeling and estimating camera pose misalignment removes a significant practical hurdle and enhances robustness.
  • Adaptive noise estimation is vital for vision sensors: The time-varying nature of camera noise, particularly during dynamic maneuvers like walking on slippery surfaces, necessitates online covariance tuning for filter stability.
  • Observability limitations are inherent but manageable: The analysis reveals that absolute robot position and rotation around the gravity vector are unobservable. However, the filter demonstrates that the drifts in these unobservable states remain small and manageable, allowing for effective control.

Key Results

  • The proposed InEKF successfully estimates the robot's state (orientation, velocity, and position) with high accuracy, even during significant slippage events.
  • The filter demonstrates robustness, with estimated states converging to ground truth and maintaining consistency, unlike methods that discard slippage data or fail to adapt.
  • The auto-calibration of camera pose is successful, with the estimated camera pose converging to the calibrated value.
  • The estimated robot velocity and inclination are accurate, while the drifts in the unobservable states (absolute position and rotation around gravity) are shown to be small.
  • The method is computationally efficient, making it suitable for real-time control applications.

Limitations and Future Work

  • Unobservable States: The paper acknowledges that the absolute robot position and rotation around the gravity vector are inherently unobservable with the given sensor suite. While the filter manages these drifts, they are still present.
  • Camera Pose Observability: While the camera pose is estimated, its observability can be compromised under specific robot motions (e.g., zero linear and angular velocity).
  • Future Work: The authors suggest that further research could explore incorporating more sophisticated visual features or extending the approach to handle more complex terrains and dynamic maneuvers. The current work focuses on a specific type of tracking camera; future work might investigate other vision sensors.

Summary updated

Abstract

This paper proposes a state estimator for legged robots operating in slippery environments. An Invariant Extended Kalman Filter (InEKF) is implemented to fuse inertial and velocity measurements from a tracking camera and leg kinematic constraints. {\color{black}The misalignment between the camera and the robot-frame is also modeled thus enabling auto-calibration of camera pose.} The leg kinematics based velocity measurement is formulated as a right-invariant observation. Nonlinear observability analysis shows that other than the rotation around the gravity vector and the absolute position, all states are observable except for some singular cases. Discrete observability analysis demonstrates that our filter is consistent with the underlying nonlinear system. An online noise parameter tuning method is developed to adapt to the highly time-varying camera measurement noise. The proposed method is experimentally validated on a Cassie bipedal robot walking over slippery terrain. A video for the experiment can be found at https://youtu.be/VIqJL0cUr7s.
BibTeX
@inproceedings{teng2021legged,
  author = {Sangli Teng and Mark Wilfried Mueller and Koushil Sreenath},
  title = {{Legged Robot State Estimation in Slippery Environments Using Invariant Extended Kalman Filter with Velocity Update}},
  booktitle = {IEEE International Conference on Robotics and Automation},
  year = {2021},
  doi = {10.1109/icra48506.2021.9561313},
}
Figure