Robust State Estimation for Legged Robots with Dual Beta Kalman Filter

TL;DR

A dual estimation framework that iteratively employs a parameter filter to estimate the leg length parameters and a state filter to estimate the robot's state and a partial measurement model for the parameter filter using the leg static equation is introduced.

Abstract

Existing state estimation algorithms for legged robots that rely on proprioceptive sensors often overlook foot slippage and leg deformation in the physical world, leading to large estimation errors. To address this limitation, we propose a comprehensive measurement model that accounts for both foot slippage and variable leg length by analyzing the relative motion between foot contact points and the robot's body center. We show that leg length is an observable quantity, meaning that its value can be explicitly inferred by designing an auxiliary filter. To this end, we introduce a dual estimation framework that iteratively employs a parameter filter to estimate the leg length parameters and a state filter to estimate the robot's state. To prevent error accumulation in this iterative framework, we construct a partial measurement model for the parameter filter using the leg static equation. This approach ensures that leg length estimation relies solely on joint torques and foot contact forces, avoiding the influence of state estimation errors on the parameter estimation. Unlike leg length which can be directly estimated, foot slippage cannot be measured directly with the current sensor configuration. However, since foot slippage occurs at a low frequency, it can be treated as outliers in the measurement data. To mitigate the impact of these outliers, we propose the beta Kalman filter (beta KF), which redefines the estimation loss in canonical Kalman filtering using beta divergence. This divergence can assign low weights to outliers in an adaptive manner, thereby enhancing the robustness of the estimation algorithm. These techniques together form the dual beta-Kalman filter (Dual beta KF), a novel algorithm for robust state estimation in legged robots. Experimental results on the Unitree GO2 robot demonstrate that the Dual beta KF significantly outperforms state-of-the-art methods.

Figures (6)

• Figure 1: (a) Foot slippage and dynamic deformation occur during the locomotion of legged robots. (b) Above: Comparison of the body velocity calculated using the existing measurement model bloesch2013state2 with the true velocity obtained from the motion capture system. The regions marked by thin black bars indicate significant spikes. The grey-shaded areas mark periods when the leg is off the ground, during which body velocity measurement is meaningless yang2022online. Below: Estimated trajectories from different algorithms, with our proposed Dual $\beta$-KF being the closest to the ground-truth trajectory.
• Figure 2: Important frame definitions and leg length parameters of Unitree GO2 robot.
• Figure 3: Framework overview of the Dual $\beta$-KF. Proprioceptive sensor data are input to the Dual $\beta$-KF, where a state-independent parameter filter first estimates the leg length parameters. These estimates are then used by a robust state filter to determine the robot's state.
• Figure 4: A Unitree Go2 robot walks with a trot gait in a laboratory equipped with motion capture system.
• Figure 5: Position estimation results for different algorithms in the simulation (left) and the real-world experiment (right) using the Unitree GO2 robot.