H-RO-MPC

Abstract

As humanoid robots enter real-world environments, ensuring robust locomotion across diverse environments is crucial. This paper presents a computationally efficient hierarchical control framework for humanoid robot locomotion based on reduced-order models -- enabling versatile step planning and incorporating arm and torso dynamics to better stabilize the walking. At the high level, we use the step-to-step dynamics of the ALIP model to simultaneously optimize over step periods, step lengths, and ankle torques via nonlinear MPC. The ALIP trajectories are used as references to a linear MPC framework that extends the standard SRB-MPC to also include simplified arm and torso dynamics. We validate the performance of our approach through simulation and hardware experiments on the Unitree G1 humanoid robot. In the proposed framework the high-level step planner runs at 40 Hz and the mid-level MPC at 500 Hz using the onboard mini-PC. Adaptive step timing increased the push recovery success rate by 36%, and the upper body control improved the yaw disturbance rejection. We also demonstrate robust locomotion across diverse indoor and outdoor terrains, including grass, stone pavement, and uneven gym mats.

Method

Control Diagram

In the high-level planner, NMPC is used on the S2S dynamics of the ALIP model to generate a step plan. The step plan is then converted into a SRB reference trajectory. The reference trajectory is passed to a linear MPC problem that optimizes ground reaction forces and the upper body outputs. The output references together with the forces and torques from the MPC are passed to the joint mapper which converts all the references into joint commands that are sent to the robot.

Decomposed Single Rigid Body (DSRB) Model

The nominal SRB model for locomotion does not consider the upper body dynamics explicitly, but lumps them together with the legs as a single rigid body. Here we decompose the single rigid body of the robot into four components: the legs, torso, and arms. This decomposition allows us to generate internal forces and moments acting on the base of the robot to better stabilize the walking. By constraining the arm motion to a linear trajectory, we can still maintain the linearity of the MPC problem.

Experiments

Push Recovery

Sagittal View

Frontal View

To demonstrate the robustness of the framework we performed hardware push recovery experiments on the Unitree G1 humanoid robot. We performed multiple experiments where the robot had to react to both disturbances in the sagittal and frontal planes. The videos and figure below shows how the robot adjusts both its step timing and step length to recover from the disturbances and get back to its nominal walking gait.

Gait Tiles — Gait tiles showing step timing and length adaptation during push recovery.

Terrain Traversal

Grass

Pavement

Indoor grass

Gym mats

The robustness of the robot was further demonstrated through various terrain traversal experiments. Due to the hierarchical nature of the framework, we were able to run the full control stack on the onboard mini-PC of the robot. The high level planner runs at 40 Hz and the mid-level MPC at 500 Hz. For the state estimation we used an a strap-down EKF that fuses IMU and VIO data from an Intel Realsense T265 camera.

Robustness Analysis

Upper Body Control: We quantified the advantages of including upper body dynamics in the MPC by performing push recovery experiments. While walking, yaw disturbances were applied to the torso of the robot to evaluate its robustness. The plot shows the yaw angle after recovering from the disturbance for both the SRB-MPC and DSRB-MPC. Adaptive Step Frequency: We also evaluated the robustness benefits of utilizing adaptive step timing during locomotion. To investigate this we performed a series of push recovery experiments where forces where applied to the base of the robot in the x and y direction. The results show that using adaptive step timing increased the push recovery success rate by 36% compared to when a fixed step timing.

Poster

Citation

If you find this work useful, please consider citing it as:

        
          @article{ghansah2025hierarchical,
            title={Hierarchical Reduced-Order Model Predictive Control for Robust Locomotion on Humanoid Robots},
            author={Ghansah, Adrian B and Esteban, Sergio A and Ames, Aaron D},
            journal={arXiv preprint arXiv:2509.04722},
            year={2025}
          }

Hierarchical Reduced-Order Model Predictive Control for Robust Locomotion on Humanoid Robots

Project Overview