Optimal Estimation of the Centroidal Dynamics of Legged Robots

François Bailly, Justin Carpentier, Philippe Souères

TL;DR

This paper introduces a novel Maximum A Posteriori (MAP) estimation framework, MAPE-DDP, for accurately estimating the centroidal dynamics of legged robots. By formulating the problem as a quadratic cost minimization solved with Differential Dynamic Programming (DDP), the method significantly reduces estimation errors and ensures dynamic consistency, outperforming existing Kalman and complementary filters.

What Problem Does This Paper Address?

Controlling legged robots, especially in complex multi-contact scenarios, relies heavily on accurately estimating their "centroidal dynamics." This refers to the robot's motion projected onto its center of mass (CoM), encompassing its position, linear momentum, and angular momentum. Traditional methods often struggle with noisy sensor data and model inaccuracies, leading to suboptimal control and potential instability. Existing approaches like Extended Kalman Filters (EKF) and recursive complementary filters have limitations, particularly in handling the inherent complexities of legged robot dynamics and the need for accurate state estimation, especially for unobserved states. This paper aims to bridge this gap by providing a more robust and accurate estimation method.

Methods

The core of the paper is the MAPE-DDP (Maximum A Posteriori - Differential Dynamic Programming) framework. The problem of estimating the centroidal state is formulated as a Maximum A Posteriori (MAP) estimation problem. This means finding the state trajectory that is most probable given the observed measurements and the robot's dynamic model.

This MAP problem is then transformed into a quadratic cost minimization problem. The cost function balances two key aspects:

  1. Dynamic Consistency: How well the estimated state trajectory adheres to the robot's underlying physical dynamics.
  2. Measurement Consistency: How closely the estimated states match the actual sensor measurements.

To solve this minimization problem efficiently, the authors employ Differential Dynamic Programming (DDP). DDP is an iterative algorithm that uses second-order Taylor expansions to approximate the cost-to-go function and iteratively refines the control inputs (in this case, the unknown dynamics) to minimize the cost. A novel aspect introduced is a method for retrieving the initial state conditions, inspired by parameter-dependent DDP, which is crucial for accurate estimation from the start of a trajectory.

The paper also briefly describes two alternative methods for comparison:

  • Extended Kalman Filter (EKF): A standard technique for estimating states in nonlinear systems, but it relies on linearizing the dynamics and doesn't inherently back-propagate information from future states.
  • Recursive Complementary Filtering (REC): An approach that fuses kinematic and kinetic data in the spectral domain, aiming to correct for low-frequency errors.

Datasets

The paper uses two primary sources of data for evaluation:

  • Simulated HRP-2 Robot Walking Data: Ground truth data from a simulated HRP-2 robot walking motion is used. This allows for a precise error analysis by comparing the estimators' outputs to the known true states. The simulation data is also intentionally noised to mimic realistic experimental conditions, including biases in mass distribution, force/torque sensor noise, and kinematic defects.
  • Real Experimental HRP-2 Robot Walking Data: Kinematic and kinetic data from actual walking experiments with the HRP-2 robot (from a previous work, [16]) are used to demonstrate the estimator's effectiveness in real-world scenarios.

Key Insights and Ideas

  • MAP Formulation for Dynamics Estimation: Framing centroidal dynamics estimation as a MAP problem allows for a principled way to incorporate both sensor measurements and the robot's physical laws.
  • DDP's Advantage in Back-propagation: Unlike EKF, DDP's backward pass inherently uses information from future states to refine past estimates. This is crucial for improving accuracy, especially when initial conditions are uncertain.
  • Importance of Initial State Estimation: The paper highlights that accurately estimating the initial state is critical for the overall trajectory estimation. Their novel DDP extension addresses this directly.
  • Exploiting Full Dynamics: The MAPE-DDP method leverages the complete system dynamics, whereas other methods might rely on simplified models or only specific parts of the dynamics.
  • Unified Framework: The MAPE-DDP approach provides a single framework that can handle various types of noise and model uncertainties, leading to more consistent and accurate estimations.

Key Results

  • Reduced Estimation Error: The MAPE-DDP estimator significantly reduced estimation errors compared to EKF and REC, particularly for unobserved states like linear and angular momenta.
  • Dynamic Consistency: The proposed method ensures that the estimated state trajectory respects the robot's underlying dynamics, leading to more physically plausible results.
  • Effective Noise Filtering: MAPE-DDP demonstrated strong performance in filtering out both high-frequency sensor noise and low-frequency model errors (e.g., from mass distribution inaccuracies).
  • Real-time Potential: While the Python implementation was not optimized, the convergence time (around 4.5 seconds for a 12.5-second motion) is promising for real-time applications with optimized code.
  • Superiority over EKF and REC: In both simulated and experimental conditions, MAPE-DDP consistently outperformed EKF and REC, especially in terms of smoothness and accuracy for various centroidal state components.

Limitations and Future Work

  • Computational Cost: While promising, the current Python implementation is not optimized for real-time performance. Future work includes implementing the estimator in C/C++ for faster execution.
  • Moving Horizon: To achieve real-time control, a moving horizon estimation scheme needs to be implemented, requiring a thorough investigation of the speed-accuracy trade-off.
  • Comparison with Kalman Smoothing: The authors suggest comparing MAPE-DDP with Kalman smoothing algorithms, as they share similarities in back-propagating measurement information.
  • Integrating Complementary Filtering: Exploring the inclusion of complementary filtering techniques within the MAPE framework could potentially leverage the strengths of both approaches.
  • Control Integration: The ultimate goal is to embed this estimation block into control architectures for bipedal and quadrupedal robots to improve stability and recovery from disturbances.

Summary updated

Abstract

International audience
BibTeX
@inproceedings{bailly2021optimal,
  author = {François Bailly and Justin Carpentier and Philippe Souères},
  title = {{Optimal Estimation of the Centroidal Dynamics of Legged Robots}},
  booktitle = {IEEE International Conference on Robotics and Automation},
  year = {2021},
  doi = {10.1109/icra48506.2021.9561993},
}

Loading related content...

Figure