Decoupled state estimation for humanoids using full-body dynamics

Abstract

We propose a framework to use full-body dynam- ics for humanoid state estimation. The main idea is to decouple the full body state vector into several independent state vectors. Some decoupled state vectors can be estimated very efficiently with a steady state Kalman Filter. In a steady state Kalman Fil- ter, state covariance is computed only once during initialization. Furthermore, decoupling speeds up numerical linearization of the dynamic model. We demonstrate that these state estimators are capable of handling walking on flat ground and on rough terrain. I. INTRODUCTION Unlike fixed base robot manipulators, humanoid robots are high degree of freedom dynamical systems with a floating base that can move around in complex environments. State estimation is an integral part of controlling such a system. For a controller using floating base inverse dynamics to compute feed-forward torques, the state estimator needs to provide the location, orientation, and linear and angular velocities of the floating base, as well as the angle and angular velocity of each joint. In this paper, we propose a framework to use full-body dynamics for humanoid state estimation. Using full-body dynamics is currently too expensive to use in real time state estimation in the standard way. Our approach is to decouple the full body state vector into several independent state vectors. Each decoupled state vector can be estimated very efficiently by using a steady state Kalman Filter (KF). In a steady state KF, state covariance is computed only once during initialization. Furthermore, decoupling speeds up numerical linearization of the dynamic model. We trade partial information loss for a reduction in computational cost. This paper is organized as follows. In Section II, we will review some related work in state estimation using the KF. In Section III, we formulate the full body state estimation problem as several decoupled state estimation problems. Section IV describes the implementation of each state estimator. In Section V, we show simulation results with the Atlas robot (see Fig. 1) walking using the Gazebo simulator, and actual robot results. Section VI discusses future work and the last section concludes this paper.