Contact-Aided Invariant Extended Kalman Filtering for Robot State Estimation (1904.09251v2)

Abstract: Legged robots require knowledge of pose and velocity in order to maintain stability and execute walking paths. Current solutions either rely on vision data, which is susceptible to environmental and lighting conditions, or fusion of kinematic and contact data with measurements from an inertial measurement unit (IMU). In this work, we develop a contact-aided invariant extended Kalman filter (InEKF) using the theory of Lie groups and invariant observer design. This filter combines contact-inertial dynamics with forward kinematic corrections to estimate pose and velocity along with all current contact points. We show that the error dynamics follows a log-linear autonomous differential equation with several important consequences: (a) the observable state variables can be rendered convergent with a domain of attraction that is independent of the system's trajectory; (b) unlike the standard EKF, neither the linearized error dynamics nor the linearized observation model depend on the current state estimate, which (c) leads to improved convergence properties and (d) a local observability matrix that is consistent with the underlying nonlinear system. Furthermore, we demonstrate how to include IMU biases, add/remove contacts, and formulate both world-centric and robo-centric versions. We compare the convergence of the proposed InEKF with the commonly used quaternion-based EKF though both simulations and experiments on a Cassie-series bipedal robot. Filter accuracy is analyzed using motion capture, while a LiDAR mapping experiment provides a practical use case. Overall, the developed contact-aided InEKF provides better performance in comparison with the quaternion-based EKF as a result of exploiting symmetries present in system.

• The paper introduces a novel InEKF method that integrates contact-inertial dynamics with Lie group theory for robust state estimation in legged robots.
• The method’s log-linear error dynamics and state-independent observation model enhance convergence and accuracy compared to traditional QEKF approaches.
• Empirical evaluations on a Cassie bipedal robot demonstrate the filter’s versatility and reliability for real-time autonomous robotic applications.

Contact-Aided Invariant Extended Kalman Filtering for Legged Robot State Estimation

The paper presents a sophisticated approach to state estimation in legged robots using a Contact-Aided Invariant Extended Kalman Filter (InEKF). The research highlights the necessity for precise pose and velocity estimation in legged robots to ensure stability and enable efficient execution of walking paths. Traditional techniques often rely on visual data or the fusion of kinematic and contact information with inertial measurements from an IMU. However, visual methods are hindered by varying environmental conditions, whereas traditional filtering methods may not fully exploit the underlying system symmetries.

The authors introduce a novel filtering approach using the theory of Lie groups and invariant observer design. This method integrates contact-inertial dynamics with forward kinematic corrections to estimate both pose and velocity, in addition to determining all current contact points. A pivotal outcome is the derivation of error dynamics that follow a log-linear autonomous differential equation. This result confers several advantages:

1. Observable state variables exhibit convergence behaviors with a domain of attraction that remains invariant with respect to the system's trajectory.
2. The linearized error dynamics and observation model are independent of the current state estimate, enhancing the filter's convergence properties.
3. The local observability matrix remains consistent with the underlying nonlinear system, preserving system integrity under state estimation processes.

The proposed InEKF is methodically evaluated through simulation and empirical experiments on a Cassie-series bipedal robot. The comparisons were conducted against a quaternion-based EKF (QEKF), which is a conventional approach in IMU-based state estimation. Findings demonstrate that the InEKF offers superior performance in terms of convergence and accuracy. The InEKF's ability to leverage the system symmetries results in improved consistency and reliability, particularly in scenarios with poor initial state estimates.

From an implementation standpoint, the InEKF is versatile, allowing for the inclusion of IMU biases, the dynamic addition or removal of contact points, and the formulation of both world-centric and robot-centric versions. This adaptability broadens the potential applications in complex robotic tasks that require robust real-time state estimation.

The implications of this research are significant for both theoretical and practical developments in robotics. Theoretically, it reinforces the effectiveness of integrating Lie groups and invariant observer design in state estimation. Practically, it provides a robust tool for developing autonomous legged robots, with potential applications in logistics, search and rescue, and household robotics.

Future research directions could explore enhancing the InEKF's resilience to more diverse terrain and environmental conditions, integrating with advanced motion planning algorithms, and further refining contact detection and manipulation in dynamic environments. The development of an open-source library paves the way for widespread adoption and further innovation in state estimation methodologies, potentially impacting a wide range of robotics applications.