1 Introduction↩︎

Deployment of humanoid robots in real-world scenarios demands controllers that are both robust and reliable. The high-degree-of-freedom (DOF), hybrid, and underactuated dynamics, combined with the need to interact with diverse environments make reliable control of humanoids a significant challenge. Planning over the full robot state is computationally expensive, often rendering algorithms unsuitable for hardware deployment where the computational power may be limited and real-time control is essential. The hybrid dynamics necessitate careful reasoning about contact locations and timings, which nominally leads to large and challenging mixed-integer optimization problems.

The hybrid zero dynamics (HZD) framework [1] is based on finding stable periodic orbits for the full-order hybrid dynamical system through offline trajectory optimization, and then to stabilize the trajectories online. The method has been demonstrated across various different platforms [2]–[4]. Despite its strong theoretical foundation, the need for offline trajectory optimization makes the method less suitable in dynamic scenarios where online replanning is required.

Approaches involving whole-body model predictive control (MPC) benefit from future predictions to stabilize the underactuated humanoid dynamics. The use of whole-body MPC can lead to rich and complex behaviors [5], [6]. While whole-body MPC has been successfully deployed on hardware [7], solving the associated optimization problems are often computationally expensive, extensive tuning may be required, and it may be subject to local minima.

An alternative to using the full-order model is to instead employ a reduced-order model (ROM), such as the linear inverted pendulum (LIP) [8], hybrid LIP (HLIP) [9], angular LIP (ALIP) [10], or spring-loaded inverted pendulum (SLIP) [11]. A ROM is a reduced representation of the full-order model that captures the underlying core dynamics of locomotion. Stabilizing the ROM and embedding it within the full-order system stabilizes the whole-body dynamics. The lower state dimension and simplified dynamics makes ROM control approaches computationally efficient and well suited for real-time control. However, controllers using ROMs are less expressive as they nominally do not consider the full system dynamics such as arm swinging or turning.

1.1 Related Work↩︎

1.1.1 Foot Placement Strategies↩︎

Several works have studied use of ROMs to stabilize bipedal robots. In [12], MPC is used to optimize over step lengths based on the HLIP model to achieve global position tracking. The authors in [13] use MPC on the ALIP model to find the optimal step length to track a commanded velocity. In [14], a reparameterization of the ALIP model is used and include ankle torques to achieve stair-climbing. In these works, step periods are fixed to a constant value, leading to simpler optimization problems.

In addition to planning foot placement locations, incorporating step durations for optimization has also been investigated. In [15], a hierarchical control strategy is presented where a high-level MPC based on the LIP Flywheel (LIPF) model optimizes over ankle control, hip control, and step lengths, followed by a subsequent layer that optimizes step durations. The authors in [16] propose a nonlinear MPC (NMPC) controller based on the LIPF model and the divergent component of motion (DCM) error that optimizes over ankle control, hip control, step lengths, and step periods simultaneously. A heuristic stepping strategy is proposed in [17] where the nominal stepping period is increased based on the evolution of unstable states and body attitude changes in a high-level NMPC problem.

1.1.2 Upper Body Control↩︎

A key advantage of whole-body control is that it inherently reasons about torso and arm motion which has shown to give rise to emergent behaviors [6], [7], [18]. In contrast, several popular inverted pendulum variants exclude upper-body dynamics by design. The LIPF model can be used to strategize about upper body motion to stabilize the robot, as in [15]. Model Hierarchy Predictive Control (MHPC) has also shown promise in enhancing robustness through arm control [19]. The authors in [20] generate arm movement for a robot using zero moment point walking to find arm swinging motions to avoid torsional rotation of the feet around the z-axis.

1.2 Contributions↩︎

In this paper, we propose a hierarchical control framework for humanoid locomotion that aims at bridging the gap between fast, simplified ROM-based control and more complex whole-body control. At the high-level, we use NMPC on the step-to-step (S2S) dynamics of the ALIP model to efficiently optimize over step lengths, step periods, and ankle torques at 40 Hz. The resulting plans are then used to generate reference trajectories for a linear Single Rigid Body (SRB) MPC. We then show how the SRB model can be decomposed to incorporate arm and torso dynamics, while still keeping the dynamics constraints in the MPC linear. We demonstrate that adaptive stepping improves robustness and that upper-body control helps mitigate unwanted yaw rotations. An overview of the framework is shown in Fig. 2.

The hierarchical control structure allows the high-level planner to solve the nonconvex step length and step period optimization at a lower rate, while the mid-level controller is formulated as a linearly constrained MPC that can be executed at high rates. This separation ensures predictable solve times at the mid level while enabling more versatile step planning at the high level.

Our main contributions are:

1. A high-level NMPC formulation based on ALIP S2S dynamics that jointly optimizes over step lengths, step durations, and ankle torques.

2. A decomposition of the SRB model to include arm and torso dynamics, while maintaining linear dynamics.

3. Extensive simulation and hardware experiments to demonstrate the robustness of the proposed approach.

The remainder of the paper is organized as follows. Section 2 introduces the classical SRB model and the ALIP S2S dynamics. Section 3 details the NMPC formulation, the mapping from S2S plans to SRB references, and the decomposition of the SRB model to also include torso and arm movement. Section 4 presents simulation and hardware results, and in Section 5 we offer some concluding remarks. A video of the experiments is available at:
https://vimeo.com/1110476518

2 Preliminaries↩︎

2.1 Whole Body Dynamics↩︎

For a 3D humanoid robot we denote the configuration space as \(\mathbf{q} \in \mathcal{Q}\) and the velocities as \(\mathbf{v} \in \mathcal{V}\) . The whole body state of the robot is then represented as \(\mathbf{x}~=~[\mathbf{q}^\top, \mathbf{v}^\top]^{\top} \in \mathcal{X} \triangleq \mathcal{Q} \times \mathcal{V}\).

2.2 Single Rigid Body Dynamics↩︎

The SRB dynamics of the robot can be obtained using Newton-Euler dynamics \[\label{eq:newton95euler} \scalebox{0.9}{ \begin{bmatrix} \mathbf{F} \\ \mathbf{M} \end{bmatrix} = \begin{bmatrix} m_\textrm{COM} \boldsymbol{I}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\ \mathbf{0}_{3 \times 3} & \mathbf{I}_\textrm{COM} \end{bmatrix} \begin{bmatrix} \dot{\mathbf{v}}^\textrm{COM} \\ \dot{\boldsymbol{\omega}}^\textrm{COM} \end{bmatrix} + \begin{bmatrix} \mathbf{0}_{3 \times 1} \\ \left( \boldsymbol{\omega}^\textrm{COM} \right)^{\times} \mathbf{I}_\textrm{COM} \boldsymbol{\omega}^\textrm{COM} \end{bmatrix}. }\tag{1}\] \(\mathbf{F} \in \mathbb{R}^3\) and \(\mathbf{M} \in \mathbb{R}^3\) are the external forces and moments acting on the robot, represented in the inertial frame, \(m_\textrm{COM} \in \mathbb{R}\) and \(\mathbf{I}_\textrm{COM} \in \mathbb{R}^{3 \times 3}\) are the robot’s mass and inertia around the COM in the inertial frame. \(\mathbf{v}^\textrm{COM}\), \(\boldsymbol{\omega}^\textrm{COM} \in \mathbb{R}^3\) are the linear and angular velocity of the COM frame relative to the inertial frame. We represent the full SRB state as \[\label{eq:srb95state} \mathbf{x}^\textrm{SRB} = \begin{bmatrix} \boldsymbol{p}^\textrm{COM} & \boldsymbol{\Theta}^\textrm{COM} & \boldsymbol{v}^\textrm{COM} & \boldsymbol{\omega}^\textrm{COM} & g \end{bmatrix},\tag{2}\] where \(\mathbf{p}^\textrm{COM} \in \mathbb{R}^3\) and \(\boldsymbol{\Theta}^\textrm{COM} = [\phi \;\theta \;\psi]^\top \in \mathfrak{so}(3)\) is the COM frame position and orientation relative to the inertial frame. \(\phi\), \(\theta\), and \(\psi\) denote the roll, pitch, and yaw. We include the gravity vector in the state as it allows us to write the dynamics in affine form without adding a constant term \[\label{eq:continous95time95dynamics} \dot{\mathbf{x}}^\textrm{SRB} = \mathbf{A}^\textrm{SRB}(\mathbf{x}^\textrm{SRB}) \mathbf{x}^\textrm{SRB} + \mathbf{B}^\textrm{SRB}(\mathbf{x}^\textrm{SRB}, \mathbf{p}^\textrm{STF}) \mathbf{u}^\textrm{SRB}.\tag{3}\] In 3 , \(\mathbf{u}^\textrm{SRB} = [\mathbf{F}_\textrm{GRF,L}^\top \;\mathbf{M}_\textrm{GRF,L}^\top \;\mathbf{F}_\textrm{GRF,R}^\top \;\mathbf{M}_\textrm{GRF,R}^\top]^\top\) are the external forces and torques acting on the robot at the feet, and \(\mathbf{p}^\textrm{STF}\) is the stance foot (STF) location in the inertial frame. The expressions for \(\mathbf{A}^\textrm{SRB}(\mathbf{x}^\textrm{SRB})\) and \(\mathbf{B}^\textrm{SRB}(\mathbf{x}^\textrm{SRB}, \mathbf{p}^\textrm{STF})\) are omitted here for brevity but can be found in [21].

2.3 Step-to-Step ALIP Dynamics↩︎

The 3D ALIP model is composed of two LIP models, one in the sagittal plane and one in the frontal plane, where the states are given by \(\mathbf{x}^\textrm{ALIP} = [p_\textrm{x} \;L_\textrm{y} \;p_\textrm{y} \;L_\textrm{x}]^{\top}\). \(p_\textrm{x/y}\) is the horizontal COM x/y position relative to the stance foot, and \(L_\textrm{y/x}\) is the angular momentum around the stance foot about the y-axis and the x-axis [10]. The nominal ALIP model assumes a fixed height, \(p_\textrm{z}\), and that the angular momentum around the pre-impact swing foot is conserved during foot switching. Furthermore, if we have ankle torque \(\tau\) available, the continuous dynamics for the actuated ALIP model during a step are given by: \[\label{eq:alip95linear95dynamics} \scalebox{0.8}{ \begin{bmatrix} \dot{p}_\textrm{x} \\ \dot{L}_\textrm{y} \\ \dot{p}_\textrm{y} \\ \dot{L}_\textrm{x} \end{bmatrix} = \underbrace{ \begin{bmatrix} 0 & \frac{1}{m_\textrm{COM} p_\textrm{z}} & 0 & 0 \\ m_\textrm{COM} g & 0 & 0 & 0 \\ 0 & 0 & 0 & -\frac{1}{m_\textrm{COM} p_\textrm{z}} \\ 0 & 0 & -m_\textrm{COM} g & 0 \\ \end{bmatrix} }_{\mathbf{A}^\textrm{ALIP}} \underbrace{ \begin{bmatrix} p_\textrm{x} \\ L_\textrm{y} \\ p_\textrm{y} \\ L_\textrm{x} \end{bmatrix} }_{\mathbf{x}^\textrm{ALIP}} + \underbrace{ \begin{bmatrix} 0 & 0 \\ 1 & 0\\ 0 & 0 \\ 0 & 1 \end{bmatrix} }_{{\mathbf{B}^\textrm{ALIP}}} \underbrace{ \begin{bmatrix} \tau_\textrm{y} \\ \tau_\textrm{x} \end{bmatrix} }_{{\boldsymbol{\tau}}} }\tag{4}\]

In 4 , \(\tau_\textrm{y}\) and \(\tau_\textrm{x}\) are the ankle torques in the sagittal plane and frontal plane. The continuous dynamics during a single support phase can be compactly written as \[\label{eq:lti95dynamics} \dot{\mathbf{x}}^\textrm{ALIP}(t) = \mathbf{A}^\textrm{ALIP} \mathbf{x}^\textrm{ALIP}(t) + \mathbf{B}^\textrm{ALIP} \boldsymbol{\tau}(t).\tag{5}\] Given an initial state at time \(t = t_0\), a constant \(\boldsymbol{\tau}\), and \(T > 0\), we can flow the dynamics forward in time, and we have that the state at time \(t = t_0 + T\) is given by \[\label{eq:nonlinear95forward95propagation} \mathbf{x}^\textrm{ALIP}(t_0 + T) = \boldsymbol{\Phi}(T) \mathbf{x}^\textrm{ALIP} (t_0) + \boldsymbol{\Gamma}(T) \boldsymbol{\tau},\tag{6}\] where \(\boldsymbol{\Phi}\) and \(\boldsymbol{\Gamma}\) are given by \[\begin{align} \boldsymbol{\Phi}(T) &= \mathbf{e}^{\mathbf{A}^\textrm{ALIP} T}, \\ \boldsymbol{\Gamma}(T) &= \left( \int_{t_0}^{t_0 + T} \mathbf{e}^{\mathbf{A}^\textrm{ALIP}(t_0 + T - s)} \, ds \right) \mathbf{B}^\textrm{ALIP}. \end{align}\]

Let the state immediately before step \(k\), i.e. the pre-impact state, be denoted by \(\left(\mathbf{x}^\textrm{ALIP}_k\right)^-\) and the state right after impact at step \(k\), i.e. the post impact state, be denoted by \(\left(\mathbf{x}^\textrm{ALIP}_{k+1}\right)^+\). Given a step length \(\boldsymbol{\ell} = [\ell_\textrm{x} \;\ell_\textrm{y}]^\top\), we have that the pre-impact and post-impact states are related as follows \[\label{eq:alip95reset95map} \left(\mathbf{x}^\textrm{ALIP}_{k+1}\right)^+ = \left(\mathbf{x}^\textrm{ALIP}_k\right)^- + \mathbf{B}_d \boldsymbol{\ell}_k,\tag{7}\] where \(\mathbf{B}_d = [-1 \;0 \;-1 \;0]^{\top}\). Furthermore, from 6 we have that the pre-impact state at step \(k\) can be obtained from the post-impact state at step \(k\). We can then write: \[\label{eq:post95impact95to95pre95impact} \left(\mathbf{x}^\textrm{ALIP}_k \right)^- = \boldsymbol{\Phi}(T_k) \left(\mathbf{x}^\textrm{ALIP}_k \right)^+ + \boldsymbol{\Gamma}(T_k) \boldsymbol{\tau}_k.\tag{8}\] Inserting 8 into 7 gives us the S2S pre-impact dynamics \[\label{eq:nonlinear95s2s95dynamics} \mathbf{x}^\textrm{ALIP}_{k+1} = \boldsymbol{\Phi}(T_k) \mathbf{x}^\textrm{ALIP}_{k} + \boldsymbol{\Gamma}(T_k) \boldsymbol{\tau}_k + \mathbf{B}_\textrm{d} \boldsymbol{\ell}_k,\tag{9}\] where \(\mathbf{x}^\textrm{ALIP}_k\) is the pre-impact state at step \(k\).

2.4 Single Rigid Body Model Predictive Control↩︎

Given the current full-order state, \(\mathbf{x}_0\), a SRB reference trajectory, \(\hat{\bar{\mathbf{X}}}^\textrm{SRB} = \left\{ \bar{\mathbf{x}}_{n}^\textrm{SRB} \right\}_{n = 1}^{N}\), a stance foot trajectory, \(\hat{\bar{\mathbf{P}}}^\textrm{STF} = \left\{ \bar{\mathbf{p}}_{n}^{\textrm{STF}} \right\}_{n = 0}^{N-1}\), and a MPC time discretization sequence \(\Delta \hat{\mathbf{T}} = \left\{ \Delta t_n \right\}_{n = 0}^{N-1}\), we can linearize the SRB dynamics around the references at each timestep \(n\) in the horizon to obtain the linearized discrete dynamics \[\mathbf{x}_{n+1}^\textrm{SRB} = \mathbf{A}^\textrm{SRB}_n \mathbf{x}_{n}^\textrm{SRB} + \mathbf{B}^\textrm{SRB}_{n} \mathbf{u}_{n}^\textrm{SRB},\] where \(\mathbf{A}^\textrm{SRB}_{n}\) and \(\mathbf{B}^\textrm{SRB}_{n}\) are given by \[\mathbf{A}^\textrm{SRB}_{n} = \boldsymbol{I}_{13 \times 13} + {\Delta t}_{n} \mathbf{A}^\textrm{SRB }(\bar{\mathbf{x}}^\textrm{SRB}_n)\] \[\mathbf{B}^\textrm{SRB}_{n} = {\Delta t}_{n} \mathbf{B}^\textrm{SRB}(\bar{\mathbf{x}}^\textrm{SRB}_n, \bar{\mathbf{p}}^\textrm{STF}_n).\] At \(n = 0\) we discretize around the current state, \(\mathbf{x}_0\). Using the linearized discrete SRB dynamics and reference trajectory we can then formulate the following linear MPC problem \[\label{eq:srb95mpc} \begin{align} \min_{\mathbf{x}^\textrm{SRB}, \mathbf{u}^\textrm{SRB}} & \sum_{n = 0}^{N-1} \|{\mathbf{x}}_{n+1}^\textrm{SRB} - \bar{\mathbf{x}}_{n+1}^\textrm{SRB} \|_{\mathbf{Q}^\textrm{SRB}} + \| {\mathbf{u}_{n}^\textrm{SRB}} \|_{\mathbf{R}^\textrm{SRB}} \\ \text{s.t.} \quad & \mathbf{x}_{n+1}^\textrm{SRB} = \mathbf{A}^\textrm{SRB}_{n} \mathbf{x}_{n}^\textrm{SRB} + \mathbf{B}^\textrm{SRB}_{n} \mathbf{u}_{n}^\textrm{SRB}, n \in \{0, ..., N-1 \} \\ & \mathbf{C}_n^\textrm{SRB} \mathbf{u}_n^\textrm{SRB} \leq \mathbf{c}_n^\textrm{SRB},n \in \{0, ..., N-1 \} \\ & \mathbf{D}_n^\textrm{SRB} \mathbf{u}_{n}^\textrm{SRB} = \mathbf{0},n \in \{0, ..., N-1 \} \end{align}\tag{10}\]

where \(\mathbf{C}_n^\textrm{SRB} \mathbf{u}_n^\textrm{SRB} \leq \mathbf{c}_n^\textrm{SRB}\) and \(\mathbf{D}_n^\textrm{SRB} \mathbf{u}_{n}^\textrm{SRB} = \mathbf{0}\) denote the inequality and equality constraints at node \(n\). The inequality constraints can include constraints such as friction cone constraints and force limits, while the equality constraint are often used to ensure zero force and moments from the swing foot. The MPC problem in 10 is a quadratic program (QP), which is suitable for real-time control.

3 Method↩︎

This section outlines our hierarchical control framework. Section 3.1 introduces the high-level NMPC planner, which computes optimal step periods and lengths based the ALIP S2S dynamics. Section 3.2 describes how the S2S trajectories are converted into SRB references. In Section 3.3, we decompose the SRB-MPC formulation to include arm movement and torso rotation. In Section 3.4, we show how the optimization solutions are mapped to joint commands.

3.1 Step-to-Step ALIP Nonlinear Optimization Problem↩︎

3.1.1 Nonlinear MPC Formulation↩︎

The objective of the high-level planner is to find optimal step length and step period trajectories based on the current state of the robot to make it walk at a commanded velocity with a desired step period. Optimizing step lengths and step periods over the full-order dynamics is computationally expensive, hence we use the S2S ALIP dynamics from 9 instead. Based on the ALIP dynamics we formulate the following optimization problem where we attempt to find the optimal step length and step periods for a horizon of \(K\) steps: \[\label{eq:nonlinear95mpc} \begin{align} \min_{\mathbf{x}^\textrm{ALIP}, \boldsymbol{\ell}, \mathbf{T}, \boldsymbol{\tau}} \quad & \sum_{k = 0}^{K-1} \| \tilde{\mathbf{x}}^\textrm{ALIP}_{k+1} \|_{\mathbf{Q}_\textrm{x}} + \| \tilde{{\boldsymbol{\ell}}}_{k} \|_{\mathbf{R}_{\ell}} + \| \tilde{{T}}_{k} \|_{{R}_T} + \| {\boldsymbol{\tau}}_{k} \|_{\mathbf{R}_{\tau}} \\[0.5em] \text{s.t.} \quad & \textrm{Eq.}~\eqref{eq:nonlinear95s2s95dynamics},k \in \{1, \dots, K{-}1\} \\ & {\mathbf{x}}^\textrm{ALIP}_\textrm{lb} \leq \mathbf{x}^\textrm{ALIP}_k \leq {\mathbf{x}}^\textrm{ALIP}_\textrm{ub},k \in \{1, \dots, K\} \\ & {\boldsymbol{\ell}}_\textrm{lb} \leq \boldsymbol{\ell}_k \leq {\boldsymbol{\ell}}_\textrm{ub},k \in \{0, \dots, K{-}1\} \\ & {T}_\textrm{lb} \leq T_k \leq {T}_\textrm{ub},k \in \{0, \dots, K{-}1\} \\ & {\boldsymbol{\tau}}_\textrm{lb} \leq \boldsymbol{\tau}_k \leq {\boldsymbol{\tau}}_\textrm{ub},k \in \{0, \dots, K{-}1\} \\ & \mathbf{x}^\textrm{ALIP}_1 = \boldsymbol{\Phi}(T_0 {-} t_\textrm{curr}) \, \mathbf{x}^\textrm{ALIP}_\textrm{0} + \boldsymbol{\Gamma}(T_0 {-} t_\textrm{curr}) \, \boldsymbol{\tau}_0 \end{align}\tag{11}\]

In 11 , the tilde \(\tilde{(\cdot)}\) denotes the difference between the actual value and the desired value. This gives us \(\tilde{\mathbf{x}}^\textrm{ALIP}_k = \mathbf{x}^\textrm{ALIP}_k - {\mathbf{x}}^\textrm{ALIP,des}_k\), \(\tilde{\boldsymbol{\ell}}_k = \boldsymbol{\ell}_k - {\boldsymbol{\ell}}_k^\textrm{des}\), and \(\tilde{T}_k = T_k - {T}_k^\textrm{des}\). \(\mathbf{x}^\textrm{ALIP}_\textrm{0}\) is the current ALIP state of the robot, and \(t_\textrm{curr}\) is the time since the beginning of the current step. For the NLP in 11 we have that the cost is quadratic and all the constraints are linear, except for the dynamics constraints from 9 where nonlinearity is introduced by considering optimization of \(T_k\).

3.1.2 Obtaining Desired S2S Parameters↩︎

Given a velocity command \(\mathbf{v}^\textrm{cmd} = (v_\textrm{x}^\textrm{cmd}, v_\textrm{y}^\textrm{cmd}, \omega_\textrm{z}^\textrm{cmd})\), and a desired step period \({T}^\textrm{des}\), we calculate the desired step lengths as \[\begin{align} \tag{12} {\ell}_\textrm{x}^{\textrm{des}} = v_\textrm{x}^\textrm{cmd} {T}^\textrm{des},\\ \tag{13} \ell_\textrm{y}^{\textrm{des}} = v_\textrm{y}^\textrm{cmd} {T}^\textrm{des} + \ell_\textrm{y}^\textrm{offset} \gamma, \end{align}\] where \(\ell_\textrm{y}^\textrm{offset}\) is the nominal desired step width, and \(\gamma \in \{-1, 1\}\) indicates the step direction based on the stance foot. We decide the desired pre-impact position and velocity based on the HLIP model as discussed in [9], where we set the single support step period to \(T_\textrm{SSP} = {T}^\textrm{des}\), and we set the double support step period to zero, i.e. \(T_\textrm{DSP} = 0\). For the x-direction, we use a period-1 orbit, and for the y-direction we use a period-2 orbit. This gives us: \[\begin{bmatrix} {\mathbf{x}}^\textrm{HLIP,des} \\ {\mathbf{y}}^\textrm{HLIP,des} \end{bmatrix} = \begin{bmatrix} \textrm{Period1}(T_\textrm{SSP}, T_\textrm{DSP}, {p}_\textrm{z}^\textrm{des}, v_\textrm{x}^\textrm{cmd}) \\ \textrm{Period2}(T_\textrm{SSP}, T_\textrm{DSP}, {p}_\textrm{z}^\textrm{des}, v_\textrm{y}^\textrm{cmd}, \ell_\textrm{y}^\textrm{offset}) \end{bmatrix},\] where \(\bar{\mathbf{x}}^\textrm{HLIP,des} = [{p}_\textrm{x}^\textrm{HLIP,des} \;{v}_\textrm{x}^\textrm{HLIP,des}]^\top\) and \({\mathbf{y}}^\textrm{HLIP,des} = [{p}_\textrm{y}^\textrm{HLIP,des} \;{v}_\textrm{y}^\textrm{HLIP,des}]^\top\) are the desired x and y HLIP states.

Next, we convert the HLIP pre-impact references into ALIP pre-impact references the following way \[{!}{ \begin{bmatrix} {p}_\textrm{x}^\textrm{ALIP,des} \\ {L}_\textrm{y}^\textrm{ALIP,des} \\ {p}_\textrm{y}^\textrm{ALIP,des} \\ {L}_\textrm{x}^\textrm{ALIP,des} \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 &m_\textrm{COM} p_\textrm{z}^\textrm{des} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & -m_\textrm{COM} p_\textrm{z}^\textrm{des} \end{bmatrix} \begin{bmatrix} {p}_\textrm{x}^\textrm{HLIP,des} \\ {v}_\textrm{x}^\textrm{HLIP,des} \\ {p}_\textrm{y}^\textrm{HLIP,des} \\ {v}_\textrm{y}^\textrm{HLIP,des} \end{bmatrix}. }\]

3.1.3 Obtaining the current state↩︎

When solving the nonlinear MPC problem in 11 , we set \(\mathbf{x}_0^\textrm{ALIP}\) to the current ALIP state of the robot. The current position state of the ALIP model can be directly calculated based on the current COM position relative to the stance foot. The current angular momentum in the sagittal and frontal plane we approximate based on the current state as \[\begin{align} \tag{14} L_y &= m_\textrm{COM} p_\textrm{z} v_\textrm{x} - m_\textrm{COM} p_\textrm{x} v_\textrm{z} + I_\textrm{COM,y} \omega_\textrm{y}, \\ \tag{15} L_x &=-m_\textrm{COM} p_\textrm{z} v_\textrm{y} + m_\textrm{COM} p_\textrm{y} v_\textrm{z} + I_\textrm{COM,x} \omega_\textrm{x}. \end{align}\]

Where we assume that the inertial matrix is diagonal, and that \(I_\textrm{COM,x}\) and \(I_\textrm{COM,y}\) correspond to the inertia about the robot’s COM around the x and y-axis.

3.2 SRB Reference Generation↩︎

3.2.1 S2S References↩︎

Solving the NMPC from 11 yields the following S2S trajectories: \(\hat{\bar{\mathbf{X}}}^\textrm{ALIP} = \left\{ \bar{\mathbf{x}}_k^\textrm{ALIP} \right \}_{k = 1}^{K}\),
\(\hat{\bar{\mathbf{L}}} = \left \{\bar{\boldsymbol{\ell}}_k \right \}_{k=0}^{K-1}\), \(\hat{\bar{\mathbf{T}}} = \left\{ \bar{{T}}_k \right \}_{k=0}^{K-1}\), and \(\hat{\bar{\boldsymbol{\tau}}}= \left\{ \bar{\boldsymbol{\tau}}_k \right \}_{k=0}^{K-1}\).

Next, by using the commanded turning rate, \(\omega_\textrm{z}^\textrm{cmd}\), we can obtain the stance foot yaw orientation for each step relative to the current stance foot yaw orientation \(\hat{\boldsymbol{\Psi}} = \left \{ \psi_k^\textrm{STF} \right \}_{k=0}^{K-1}\).

Using the S2S solution trajectories, we can calculate the robot’s stance foot position at each step \(k\), relative to the current stance foot position, \(\mathbf{p}^\textrm{STF}_0 = \mathbf{0}\), i.e. the inertial frame: \[\label{eq:stf95position} \bar{\mathbf{p}}_{\textrm{xy,}k}^\textrm{STF} = \sum_{i=1}^{k} \mathbf{R}_\textrm{z}(\psi_{i-1}^\textrm{STF}) \boldsymbol{\ell}_i, \quad k \in \{1, \dots, K{-}1\}\tag{16}\] In 16 , \(\mathbf{R}_\textrm{z}\) denotes a simple rotation around the z-axis.

3.2.2 SRB References↩︎

Let the time of node \(n\) be given by \(t_n\). Each node \(n\) is associated with a step \(k_n\), whose pre-impact time is given by \(t_{k_n}^\textrm{impact}\). We then have that the time-until-impact (T2I) for node \(n\) is given by \(t_{n}^\textrm{T2I} = t_{k_n}^\textrm{impact} - t_n\). To obtain the desired ALIP state at node \(n\), we flow the continuous ALIP dynamics from 6 backward in time from the pre-impact state as follows: \[\label{eq:nonlinear95backward95propagation} \bar{\mathbf{x}}^\textrm{ALIP}_n = \boldsymbol{\Phi}(-t_n^\textrm{T2I}) \bar{\mathbf{x}}^\textrm{ALIP}_{k_n} + \boldsymbol{\Gamma}(-t_n^\textrm{T2I}) \boldsymbol{\tau}_{k_n}.\tag{17}\] The COM xy position relative to the current stance foot frame can be obtained through the following transformation: \[\bar{\mathbf{p}}_{\textrm{xy,}n}^\textrm{COM} = \bar{\mathbf{p}}_{k_n}^\textrm{STF} + \mathbf{R}_\textrm{z}(\psi_{k_n}) \bar{\mathbf{p}}_{\textrm{xy},n}^\textrm{ALIP},\] and the reference velocity at node \(n\) is given by \[\bar{\mathbf{v}}^\textrm{COM}_{\textrm{xy},n} = \mathbf{R}_\textrm{z}(\psi_{k_n}^\textrm{STF}) \bar{\mathbf{v}}_{\textrm{xy,} k_n}^\textrm{HLIP}.\] We then compute the SRB reference at node \(n\) as follows: \[\label{eq:x95srb95ref} \scalebox{0.95}{ \bar{\mathbf{x}}_n^\textrm{SRB} = \begin{bmatrix} (\bar{\mathbf{p}}_{\textrm{xy,}n}^\textrm{COM})^\top \; p_\textrm{z}^\textrm{des} \; \bar{\phi} \; \bar{\theta} \; \bar{\psi}_n^\textrm{COM} \; (\bar{\mathbf{v}}_{\textrm{xy,}n}^\textrm{COM})^\top \; \mathbf{0}_{3 \times 1}\;\; \bar{\omega}_{\textrm{z,} n}^\textrm{cmd} \; g \end{bmatrix}^\top }\tag{18}\]

None

3.3 Linear MPC Formulation↩︎

In the classical SRB-MPC formulation for legged systems, the input vector \(\mathbf{u}^\textrm{SRB}\) only contains forces and moments applied to the robot due to contact with external objects [21], [22]. Here we expand upon the classical legged SRB-MPC framework, by also incorporating the arms and torso into the MPC formulation while still ensuring linear dynamics.

3.3.1 Decomposing the SRB Model↩︎

We model the humanoid as four parts; the lower body (LB), the torso (TR), and the left (LA) and right (RA) arms. We model the left and right arm as point masses, \(m_\textrm{LA}\) and \(m_\textrm{RA}\), which are located at \(\mathbf{r}_\textrm{LA}\) and \(\mathbf{r}_\textrm{RA}\). We assume that the torso can rotate relative to the lower body via a z rotation, and that the arms are connected to the torso. In addition to the ground reaction forces and moments available in the standard SRB model, our proposed model also allows us to generate forces acting on the robot’s torso and lower body by accelerating the arms, and a z moment pair can be generated between the lower body and the torso by applying a moment in the torso z actuator. Figure 4 illustrates the proposed model.

3.3.2 Dynamics↩︎

If we accelerate one of the arm masses, e.g. the left arm, relative to the COM by \(\mathbf{a}_\textrm{LA} \in \mathbb{R}^3\), we effectively apply a force \(\ThisStyle{\setbox 0=\SavedStyle\mathbf{F} \def\useanchorwidth{T}\stackon[-4.2\LMpt]{\SavedStyle\mathbf{F}}{\,\rule{0pt}{7\LMpt}\smash{\stackon[-1.989\LMpt]{ \SavedStyle\mkern--2.4mu\rule[1.388\LMex]{\dimexpr\wd 0-2.5pt}{.36\LMpt} \kern-6.0\LMpt\mathchar"017E} {\rotatebox{180}{\SavedStyle\mkern--2.4mu\rule[1.388\LMex]{\dimexpr\wd 0-2.5pt}{.36\LMpt} \kern-6.0\LMpt\mathchar"017E}}}}}_\textrm{LA} = m_\textrm{LA} \mathbf{a}_\textrm{LA}\) to the arm and in return an equal and opposite force \(\mathbf{F}_\textrm{LA} = - \ThisStyle{\setbox 0=\SavedStyle\mathbf{F} \def\useanchorwidth{T}\stackon[-4.2\LMpt]{\SavedStyle\mathbf{F}}{\,\rule{0pt}{7\LMpt}\smash{\stackon[-1.989\LMpt]{ \SavedStyle\mkern--2.4mu\rule[1.388\LMex]{\dimexpr\wd 0-2.5pt}{.36\LMpt} \kern-6.0\LMpt\mathchar"017E} {\rotatebox{180}{\SavedStyle\mkern--2.4mu\rule[1.388\LMex]{\dimexpr\wd 0-2.5pt}{.36\LMpt} \kern-6.0\LMpt\mathchar"017E}}}}}_\textrm{LA}\) is applied to the robot at \(\mathbf{r}_\textrm{LA}\). When applying \({\mathbf{F}}_\textrm{A}\) at \(\mathbf{r}_\textrm{LA}\), the following forces and moments are applied to the rest of the robot \[\label{eq:arm95dynamics95on95robot} \begin{bmatrix} \mathbf{F}_\textrm{LA} \\ \mathbf{M}_\textrm{LA} \end{bmatrix} = \begin{bmatrix} \mathbf{F}_\textrm{LA} \\ (\mathbf{r}_\textrm{LA})^{\times} \mathbf{F}_\textrm{LA} \end{bmatrix}.\tag{19}\] From 19 we have that the moments generated by the arms on the robot are nonlinear due to the coupling between the \(\mathbf{r}_\textrm{LA}\) vector and the applied force \(\mathbf{F}_\textrm{LA}\), which is unsuitable for linear MPC. However, 19 can be made linear with respect to the forces and arm positions if we constrain the motion of the arm mass to move along a single axis.

In this paper we are mainly interested in using the arms to stabilize the yaw of the robot’s base, i.e. the pelvis, hence we choose to only allow movement of the arm mass along the x-axis, \(\mathbf{e}_\textrm{x,LA}\). We therefore allow \(r_\textrm{LA,x}\) to move, while we keep \(r_\textrm{LA,y}\) and \(r_\textrm{LA,z}\) fixed. Furthermore, to keep the arm movement fixed in the y and z-direction, we also have that \(F_\textrm{LA,y} = 0\) and \(F_\textrm{LA,z} = 0\). With these constraints enforced we have that the arm forces simplify 19 into the following forces and moments acting on the robot

\[\label{eq:arm95dynamics95on95robot95simple} \mathbf{u}_\textrm{LA} = \begin{bmatrix} F_\textrm{LA,x} \\ M_\textrm{LA,y} \\ M_\textrm{LA,z} \end{bmatrix} = \begin{bmatrix} F_\textrm{LA,x} \\ r_\textrm{LA,z} F_\textrm{LA,x} \\ - r_\textrm{LA,y} F_\textrm{LA,x}\\ \end{bmatrix}.\tag{20}\]

In addition to the forces and moments generated from the arms, our proposed decomposition also allows us to rotate the torso relative to the lower body about the z-axis. We denote the torso yaw relative to the lower body as \(\psi_\textrm{TR}\). Furthermore, since the lower and upper body are connected via an actuator located in the lower body, we can apply a torque \(\tau_\textrm{TR}\) to the torso, which equivalently leads to an equal and opposite torque \(\ThisStyle{\setbox 0=\SavedStyle\tau \def\useanchorwidth{T}\stackon[-4.2\LMpt]{\SavedStyle\tau}{\,\rule{0pt}{7\LMpt}\smash{\stackon[-1.989\LMpt]{ \SavedStyle\mkern--2.4mu\rule[1.388\LMex]{\dimexpr\wd 0-2.5pt}{.36\LMpt} \kern-6.0\LMpt\mathchar"017E} {\rotatebox{180}{\SavedStyle\mkern--2.4mu\rule[1.388\LMex]{\dimexpr\wd 0-2.5pt}{.36\LMpt} \kern-6.0\LMpt\mathchar"017E}}}}}_\textrm{TR} = -\tau_\textrm{TR}\) being applied to the lower body. By limiting the arm motion to one direction and including the torso z actuation, we define the Decomposed SRB (DSRB) state and input as \[\begin{align} \tag{21} \mathbf{x}^\textrm{DSRB} &= \left[ (\mathbf{x}^\textrm{SRB})^\top \; \psi_\textrm{TR} \; p_\textrm{LA} \; p_\textrm{RA} \; \dot{\psi}_\textrm{TR} \; v_\textrm{LA} \; v_\textrm{RA} \right]^\top,\\ \tag{22} \mathbf{u}^\textrm{DSRB} &= \left[ (\mathbf{u}^\textrm{SRB})^\top \; \tau_\textrm{TR} \; F_\textrm{LA,x} \; F_\textrm{RA,x} \right]^\top, \end{align}\] and the corresponding linear dynamics as shown in 3. In 3, \(\boldsymbol{\alpha}_\textrm{LA} = \left[ 0 \;\frac{r_\textrm{LA,z}}{I_\textrm{COM,y}} \;0 \right]^\top\), \(\boldsymbol{\alpha}_\textrm{RA} = \left[ 0 \;\frac{r_\textrm{RA,z}}{I_\textrm{COM,y}} \;0 \right]^\top\), \(\mathbf{e}_\textrm{x} = [1 \;0 \;0]^\top\), \(\mathbf{e}_\textrm{z} = [0 \;0 \;1]^\top\), and \(I_\textrm{LB,z}\) and \(I_\textrm{TR,z}\) is the inertia around the z-axis for the lower body and torso, respectively. Since the robot is divided into the lower body and torso, \(I_\textrm{COM,z}\) in \(\mathbf{B}^\textrm{SRB}\) is replaced by \(I_\textrm{LB,z}\) in \(\mathbf{B}^\textrm{SRB*}\).

Similarly as for the standard SRB dynamics from 3 , we can discretize the DSRB dynamics and solve a linear optimization problem at the same form as in 10 .

3.4 Low Level Control↩︎

The joint position commands, \(\mathbf{q}_\textrm{p}^\textrm{cmd}\), are calculated using inverse kinematics based on the current state and the DSRB trajectory. For the swing foot trajectory we use the method proposed in [23] to ensure smooth blending between the current state and the target state.

We calculate the feed-forward joint torques by using the Jacobian from the COM to the components of \(\mathbf{u}^\textrm{DSRB}\), together with the first control input from the MPC controller \[\label{eq:jacobian} \mathbf{q}_\tau^\textrm{cmd} = \mathbf{J}^\top \mathbf{u}_0^\textrm{DSRB}\tag{23}\] In 23 we expand \(\mathbf{u}_0^\textrm{DSRB}\) to also enforce that the arm forces in the y and z-direction equal zero, i.e.
\(F_\textrm{LA,y} = F_\textrm{LA,z} = F_\textrm{RA,y} = F_\textrm{RA,z} \equiv 0\). Even though we optimize over ankle torques in the S2S ALIP dynamics, we do not pass those references directly to the robot, but instead use the values obtained from 23 .

4 Experimental Results↩︎

We validate our control framework through simulation and hardware experiments on the 29-DOF, 35 kg Unitree G1 humanoid robot. We first demonstrate through simulation experiments how incorporating the upper body in the mid-level MPC enhances the robot’s ability to reject external yaw-moment disturbances. Next, in Section 4.3 we show how the adaptive step frequency improves the stability of the robot when being exposed to various external disturbances. Finally, in Section 4.4 we demonstrate the robustness of the robot through hardware experiments, where we show how the nonlinear step-planner modifies the step periods and step lengths when the robot is pushed.

4.1 Implementation Details↩︎

The high level NMPC planner was implemented using Casadi’s [24] C++ interface and the IPOPT solver [25]. We solve the NMPC at a fixed frequency of 40 Hz. The rest of the controller, i.e. the trajectory generator, the mid-level MPC, and the low-level command mapper all run at 500 Hz. We utilize the Eigen library [26] to perform linear algebra operations and Pinocchio [27] to compute kinematics and dynamics. The simulation experiments were conducted using the MuJoCo physics engine [28]. Both in simulation and on hardware we run a strap down inertial navigation system [29] at 500 Hz to estimate the base pose and twist state of the robot, where we receive IMU and joint measurements at 500 Hz, and pose and twist measurements at 200 Hz.

4.2 Upper Body-Assisted Disturbance Rejection↩︎

To demonstrate the effectiveness of our proposed mid-level DSRB controller over the nominal SRB controller, we first make the robot walk forward at a fixed speed of 0.3 m/s. In each test we apply a \(M_\textrm{z}\) moment in the range 30 – 150 Nm for 0.05 s to the robot’s torso, and then after the robot is stabilized, we record the yaw of the base, i.e., the pelvis. As shown in Fig. 5, the DSRB-MPC consistently reduces yaw deviations over the SRB-MPC, where the difference in error on average grows with the magnitude of the yaw disturbances. When incorporating upper body control the robot can more effectively reject external yaw moments, as torso moment and arm forces can be used to mitigate pelvis rotation. When disabling the upper body control, the robot relies more on the friction from the stance foot.

4.3 Adaptive Step-Frequency Stabilization↩︎

We evaluated the benefit of optimizing over step periods and step lengths, as opposed to only step lengths, by performing push recovery tests. In each test, the robot was walking in place and then an external force (\(F_\textrm{x}\): –600 – 600 N, \(F_\textrm{y}\): 0 – 400 N) was applied for 0.1 s. If the robot was still walking after being pushed the test was deemed a success. For each force combination we performed the test 5 times, where the force was applied at different times to account for varying step phases. Figure 6 (top) visualizes the results when using a fixed stepping period of 0.35 s, and Fig. 6 (bottom) when using an adaptive step period in the range 0.25 – 0.5 s, with a desired step period of 0.4 s. Step period adaptation increased the number of successful recoveries by 36 %, despite having a lower average step frequency, and the controller withstood larger disturbances in the x and y-direction. The step adaptation allows the NMPC to search over a larger set of potential solutions to stabilize the robot. In particular, when the robot is pushed, it is often advantageous to step quickly to recover faster.

4.4 Hardware Validation↩︎

4.4.1 Hardware Setup↩︎

The hardware experiments were run using a Minisforum EM780 computer, a low-power small-scale computer equipped with an AMD Ryzen 7 7840U CPU, and 32 GB of RAM. The onboard computer was powered by a power bank strapped to the robot, and communicated with the robot’s internal computer over Ethernet. The onboard computer received joint state and IMU measurements from the robot, and pose and twist measurements from an onboard Intel RealSense T265 camera mounted to the torso.

4.4.2 Push Recovery Experiments↩︎

To investigate the robustness of our proposed walking controller we conducted multiple push recovery experiments. Two of these experiments are presented in Fig. 7. In these experiments the robot was walking in place and then suddenly subjected to an external push force. In Fig. 7a) the disturbance is acting in the x-direction, and in 7d) the disturbance is acting in the y-direction. The corresponding plots, Fig. 7b) and Fig. 7c), show how the base velocity, step periods, and step lengths adaptively change in the two experiments.

In the sagittal push recovery experiment we see that after the push occurs at \(t = 2.63\) s, the velocity in the x-direction increases quickly. We can see that the controller responds by taking a couple of long steps to stabilize the robot rapidly. This can also be seen from snapshot \(t=\)​2.47 – 3.30 s in Fig. 7a) where the robot takes two long steps.

In the frontal push recovery experiments the robot is exposed to two forces in the y-direction, first at \(t = 1.63\) s and then at \(t = 5.43\) s. In both cases the y-velocity quickly increases after the pushes and the robot immediately takes longer and faster steps to stabilize itself, before going back to its nominal stepping pattern.

4.4.3 Locomotion Across Diverse Terrains↩︎

In addition to the push recovery experiments we also conducted various other walking experiments. These experiments included walking over rough terrain indoors (Fig. 1c), blindly stepping down from large heights (Fig. 1d), outdoor walking on stone pavement (Fig. 1b), and walking on grass (Fig. 1a). In these hardware experiments we disabled the upper body control.

5 Conclusion↩︎

In this paper, we presented a hierarchical control framework for robust humanoid locomotion based on reduced-order models. The high-level planner utilizes NMPC on the ALIP model to jointly optimize over step periods and step lengths, and its S2S plan is converted into an SRB trajectory tracked by a linear MPC. We proposed a decomposed SRB model that incorporates arm and torso dynamics while maintaining linear constraints in the MPC. The method was demonstrated through simulation and hardware experiments, showing that upper-body control improves yaw disturbance rejection and that step adaptation increases robustness.

A limitation of the current framework is that the high-level ALIP model does not capture the richer torso and arm dynamics represented in the DSRB model. Future work will explore closer integration of the planners and extensions to more dynamic gaits such as running and hopping.

References↩︎

---

1. \(^{1}\)A. B. Ghansah, and A. D. Ames are with the Department of Control and Dynamical Systems, California Institute of Technology, Pasadena, CA 91125, USA, {aghansah, ames}@caltech.edu
   \(^{2}\)S. A. Esteban and A. D. Ames are with the Department of Mechanical and Civil Engineering, California Institute of Technology, Pasadena, CA 91125, USA, {sesteban, ames}@caltech.edu↩︎

2. This research is supported by Technology Innovation Institute (TII).↩︎