Closed-loop MPC with Dense Visual SLAM - Stability through reactive stepping

Walking gaits generated using Model Predictive Control (MPC) is widely used due to its capability to handle several constraints that characterize humanoid locomotion. The use of simplified models such as the Linear Inverted Pendulum allows to perform computations in real-time, giving the robot the fundamental capacity to replan its motion to follow external inputs (e.g. reference velocity, footstep plans). However, usually the MPC does not take into account the current state of the robot when computing the reference motion, losing the ability to react to external disturbances. In this paper a closed-loop MPC scheme is proposed to estimate the robot's real state through Simultaneous Localization and Mapping (SLAM) and proprioceptive sensors (force/torque). With the proposed control scheme it is shown that the robot is able to react to external disturbances (push), by stepping to recover from the loss of balance. Moreover the localization allows the robot to navigate to target positions in the environment without being affected by the drift generated by imperfect open-loop control execution. We validate the proposed scheme through two different experiments with a HRP-4 humanoid robot.