Non-Linear ZMP based State Estimation for Humanoid Robot Locomotion* Stylianos Piperakis and Panos Trahanias Abstract— This article presents a novel state estimation scheme for humanoid robot locomotion using an Extended Kalman Filter (EKF) for fusing encoder, inertial and Foot Sensi- tive Resistor (FSR) measurements. The filter’s model is based on the non-linear Zero Moment Point (ZMP) dynamics and thus, coupling the dynamic behavior in the frontal and the lateral plane. Furthermore, it provides state estimates for variables that are commonly used by walking pattern generators and posture balance controllers, such as the Center of Mass (CoM) and the linear time-varying Divergent Component of Motion (DCM) position and velocity, in the 3-D space. Modeling errors are taken into account as external forces acting on the robot in the acceleration level. In addition, an observability analysis for the non-linear system dynamics and the linearized discrete-time EKF dynamics is presented. Subsequently, by utilizing ground- truth data obtained from a vicon motion capture system with a NAO humanoid robot, we demonstrate the effectiveness and robustness of the proposed scheme contrasted to the linear filters, even in the case where disturbances are introduced to the system. Finally, the proposed approach is implemented and employed for feedback to a real-time posture controller, rendering a NAO robot able to walk on an outdoors inclined pavement. I. I NTRODUCTION Humanoid robot locomotion is a challenging task with many difficulties. Mainly, due to the non-linear multi-body dynamics along with the many Degrees of Freedoms (DoFs) the humanoid robots have, the under-actuation which oc- curs during the gait, and the unilateral type of contact the robots experience with the ground. The non-linear multi- body dynamics prohibit exact solutions to be obtained in real-time. Therefore, many researchers approximated those dynamics with simplified models that could describe the dynamic behavior of a humanoid while walking. However, those models are based on the assumption that the robot’s dynamics are decoupled in the frontal and the lateral plane, which is not true, especially when the robot exhibits highly dynamic motions. In addition, since the robot does not have a fixed base, it is under-actuated. Nevertheless, when the assumptions that a rigid type contact between the support leg and the ground along with sufficient friction exist, all the under-actuated DOFs vanish. Unfortunately, this is not the case in a realistic environment. Vertical displacement with respect to the ground can cause acceleration in the *This work has been partially supported by the EU FET Proactive grant (GA: 641100) TIMESTORM - Mind and Time: Investigation of the Temporal Traits of Human-Machine Convergence. The authors are with the Institute of Computer Science, Foundation for Research and Technology - Hellas (FORTH) and the Department of Computer Science, University of Crete, Heraklion, Crete, Greece. spiperakis@ics.forth.gr trahania@ics.forth.gr same direction which must be taken into consideration while planning or controlling the robot in order to avoid undesired ground reaction forces. In this paper, we propose a novel estimation scheme with an Extended Kalman Filter (EKF) which has its dynamics based on the non-linear Zero Moment Point (ZMP) [1] formulation, thus, effectively coupling the dynamic behavior in the frontal and lateral plane and fusing information from sensors that are widely available on humanoids, namely, encoders, Inertial Measurement Units (IMU), and Foot Sensi- tive Resistors (FSRs). This filter provides accurate estimates for variables that are commonly used by walking pattern generators and posture stabilization controllers, such as the Center of Mass (CoM) and the linear time-varying Divergent Component of Motion (DCM) [2] position and velocity, in the x, y, and z axes, as experimentally validated with a NAO humanoid robot under real world conditions. II. RELATED WORK Biped state-estimation plays an important role in realizing stable walking motions and in posture balance control [3]. Xinjilefu et al. [4] solved a Quadratic Program (QP) utilizing the robot’s full-body dynamics. The proposed approach was advantageous, in the sense that it did not require a state- space model as in the Kalman Filter (KF) case, could naturally handle equalities and inequalities as constraints, and consider modeling error in the state vector. However, due to the imposed constraints and the high-dimensionality the framework was computationally expensive for real-time execution, did not generalize since it was based on the robot’s dynamics and required force/torque sensors on robot’s joints. Stephens [5] used simplified models based on the the Linear Inverted Pendulum Model (LIPM) dynamics [6] for state- estimation in order to control the posture of the force- controlled Sarcos Primus humanoid. He was able to estimate modeling errors as incoming external forces, and possible CoM biases by fusing CoM and Center of Pressure (CoP) measurements from the joint encoders and the FSRs respec- tively. Nevertheless, he observed that there was a trade-off between disturbance estimation and state estimation, since time-varying disturbances demanded a carefully tuning of the noise covariances. Based on that approach, Xinjilefu and Atkeson [7] compared two KF schemes; one based on the LIPM dynamics and one based on robot’s planar dynamics. They observed that LIPM KF was simple to design and implement, easy to tune, robust to modeling errors, and can generalize to other robots, while, as expected, the Planar KF yielded more accurate estimates since it is based on a more