Center of Mass Dynamics and Contact-Aided Invariant Filtering for Biped Robot State Estimation

Due to the complexity and uncertainty of the actual working environments, relying solely on proprioceptive sensors to obtain accurate floating base and center of mass (CoM) estimates is of great significance for biped robots. In this article, a biped locomotion state estimator aided by both CoM dynamics and leg forward kinematics is proposed. The main contribution of this estimator is the use of contact force measurements that are not considered in existing methods. Contact force measurements can be used to predict CoM motions and update the floating base estimates with CoM forward kinematics. Compared with the leg forward kinematics, the CoM dynamics prediction and the CoM forward kinematics update are more robust to contact slippage and highly dynamic motions. The simulation results show that the estimator proposed in this article improves the estimation accuracy of the floating base in the slippage direction under various reference speeds. In addition, the introduction of CoM dynamics enables the filter to directly output the CoM state in the world coordinate system, avoiding the complicated cascade design of the existing CoM state estimators.