Multi-Rate Nonlinear Model Predictive Control for Wall-Supported Bipedal Locomotion of Quadrupedal Robots

Multi-Rate Nonlinear Model Predictive Control for Wall-Supported Bipedal Locomotion of Quadrupedal Robots

Paper: Multi-Rate Nonlinear Model Predictive Control for Wall-Supported Bipedal Locomotion of Quadrupedal Robots
Authors: Taizoon Chunawala, Jeeseop Kim, Kaveh Akbari Hamed (Virginia Tech / UTEP)
Link: arXiv:2607.01574　|　Code: Not available　|　Platform: Unitree A1 (RaiSim simulation)

One-line summary: Footstep planning and force control are unified into a single multi-rate NMPC framework, enabling a quadruped robot in wall-supported bipedal posture to achieve 2.9× the success rate of the Raibert heuristic on rough terrain and under external disturbances.

Background and Motivation

Quadrupedal robots in an upright bipedal posture can reach higher objects, negotiate obstacles that obstruct four-legged motion, and perform manipulation tasks requiring torso elevation. But maintaining dynamic balance on two legs in confined spaces—narrow corridors, disaster sites, industrial pipe networks—is nearly impossible given the robot's morphology: the support polygon is tiny, the center of mass is high, and the stability margin is razor-thin. This paper addresses "wall-supported bipedal locomotion," where the robot contacts a vertical wall surface with its front legs for additional support while walking on its rear legs, forming a multi-contact hybrid locomotion mode.

The challenges are multi-layered. First, the dynamics of an upright quadruped are hybrid, unstable, and high-dimensional: reaction forces between limbs and environment vary on millisecond timescales, while foot placements and contact transitions change on the slower gait-cycle timescale. This multi-rate structure is precisely what standard NMPC struggles with—it assumes all inputs update at a uniform rate. Second, confined environments demand tight coupling between locomotion and geometric constraints: safety envelopes, wall-contact reachability, and end-effector kinematic limits must all be considered within the same optimization. Third, underactuation, unilateral contact constraints, and the switching nature of dynamics make the trajectory optimization problem highly non-convex.

Existing approaches typically decouple: a high-level heuristic (e.g., the Raibert footstep formula) determines foothold locations, and a low-level MPC tracks the trajectory. The problem with decoupling is that heuristics cannot foresee future contact configuration changes. At high speeds on rough terrain, the distance covered per stance phase increases, and reactive feedback alone is insufficient to manage the coupled base-and-footstep dynamics. The paper's central question: can footstep planning be elevated from an external heuristic input to an internal dynamical state within the optimization framework, letting the controller directly reason about the coupling between foothold selection and CoM trajectory?

Core Method: Multi-Rate NMPC Framework

1. Multi-Rate System Modeling

The paper defines a general multi-rate discrete nonlinear system. The state $x\in\mathbb{R}^{n_x}$ comprises CoM position, velocity, Euler angles, and angular velocity. Control inputs are split into two classes: fast inputs $u(t)$ (environment reaction forces, ERFs, updatable every time step) and slow inputs $v(t)$ (footstep increments, updated only at the start of each gait cycle). The dynamics are:

$$x(t+1)=f\bigl(x(t),u(t),v(t)\bigr)$$

The piecewise-constant property of slow inputs is encoded via an indicator function:

$$v(t+1)=v(t)+\delta_{\mathcal{T}}(t+1)\,\Delta v(t)$$

where $\delta_{\mathcal{T}}(t+1)$ is the indicator of the switching instant set $\mathcal{T}$—it equals 1 only at gait-cycle boundaries and 0 otherwise. This formula precisely encodes the physical constraint that footstep locations update only at gait transitions, forming the core of the multi-rate framework.

2. Single Rigid Body (SRB) Dynamics Template

At the high level, the robot is simplified to a single rigid body model. The state is defined as:

$$x:=\mathrm{col}(p,\dot{p},\theta,\omega)\in\mathbb{R}^{12}$$

where $p\in\mathbb{R}^{3}$ is the CoM position, $\theta\in\mathbb{R}^{3}$ are Euler angles, and $\omega\in\mathbb{R}^{3}$ is the body-frame angular velocity. Environment reaction forces (ERFs) include both ground contact forces and wall contact forces—a key distinction from conventional GRFs. The SRB dynamics follow Newton-Euler equations:

$$\Sigma^{\mathrm{SRB}}:\begin{cases}\ddot{p}=\dfrac{f^{\mathrm{net}}}{m}-g_{0}\\[6pt]\dot{\theta}=A(\theta)\,\omega\\[6pt]\dot{\omega}=I^{-1}\bigl(R^{\top}(\theta)\,\tau^{\mathrm{net}}-\mathbb{S}(\omega)\,I\,\omega\bigr)\end{cases}$$

where $m$ is the total mass, $I$ is the body-frame inertia matrix, $\mathbb{S}(\cdot)$ is the skew-symmetric matrix operator, and $A(\theta)$ maps body angular velocity to Euler angle derivatives. The net force and torque are obtained by summing ERFs over all contact feet:

$$\begin{bmatrix}f^{\mathrm{net}}\\[3pt]\tau^{\mathrm{net}}\end{bmatrix}:=\sum_{\ell\in\mathcal{C}}\begin{bmatrix}f^{\ell}\\[3pt]\mathbb{S}(r^{\ell})\,f^{\ell}\end{bmatrix}$$

Here $r^{\ell}:=r^{\mathrm{foot},\ell}-p$ is the vector from CoM to contact foot $\ell$. The key innovation: the authors augment foot positions $r^{\mathrm{foot},\ell}$ into the state vector, evolving through the slow input $\Delta v$:

$$\Delta r^{\mathrm{foot},\ell}=\Delta v^{\ell}(t),\quad \ell\notin\mathcal{C}$$

This augmentation lets the NMPC directly reason about "how foothold selection affects future support polygon and stability margin," rather than treating footholds as exogenous independent variables.

3. Unified NMPC Optimization Problem

Based on the augmented state space, the authors construct a unified nonlinear MPC problem. The cost function includes terminal and stage costs for both fast and slow states:

$$\min \;\mathcal{L}_{\mathrm{terminal},x}(x_{t+N|t})+\mathcal{L}_{\mathrm{terminal},v}(v_{t+N|t})+\sum_{k=0}^{N-1}\Bigl[\mathcal{L}_{\mathrm{stage},x}(x_{t+k|t},u_{t+k|t})+\mathcal{L}_{\mathrm{stage},v}(v_{t+k|t},\Delta v_{t+k|t})\Bigr]$$

Constraints include the augmented dynamics, the piecewise-constant evolution of slow inputs, and time-varying input availability constraints $E_u(t+k)\,u_{t+k|t}=0$ and $E_{\Delta v}(t+k)\,\Delta v_{t+k|t}=0$. These binary matrices $E_u$ and $E_{\Delta v}$ zero out unavailable contact forces or footstep updates during different gait phases—for example, during swing phase the corresponding leg's contact force is constrained to zero. Notably, the framework uses the Raibert heuristic's computed step length as a reference $\Delta v^{\mathrm{ref}}$, meaning the heuristic is not fully replaced but embedded as a reference trajectory for the optimization.

4. Gait Design

The wall-supported bipedal locomotion gait cycle consists of three phases: a four-contact phase (50ms), a three-contact phase (30ms), and a double-contact phase (120ms). The sampling time is $T_s=10$ms, corresponding to 100Hz control frequency. The prediction horizon is $N=20$ steps, i.e., a 200ms prediction window—meaning the optimizer spans multiple gait phases. For example, when solving mid-four-contact-phase, the horizon covers the current four-contact phase, the subsequent three-contact and double-contact phases, and part of the next cycle's four-contact phase.

graph TD
  A[Four-contact
50ms: front on wall + rear on ground] --> B[Three-contact
30ms: one leg swinging]
  B --> C[Double-contact
120ms: two legs support]
  C --> D[Next cycle four-contact]
  D --> B
  style A fill:#4a90d9,stroke:#2c5f8a,color:#fff
  style B fill:#f5a623,stroke:#b97316,color:#fff
  style C fill:#7ed321,stroke:#4a8a14,color:#fff
  style D fill:#4a90d9,stroke:#2c5f8a,color:#fff

5. Low-Level Whole-Body Controller (WBC)

The optimal CoM, ERF, and foot trajectories from the high-level MR-NMPC are passed to a low-level nonlinear WBC. The WBC is based on the full-order floating-base Euler-Lagrange equations:

$$D(q)\,\ddot{q}+H(q,\dot{q})=B\,\tau+\sum_{\ell\in\mathcal{C}}\bigl(J^{\ell}(q)\bigr)^{\top}f^{\ell}$$

where $D(q)$ is the mass-inertia matrix, $H(q,\dot{q})$ collects Coriolis, centrifugal, and gravity terms, and $J^{\ell}(q)$ is the contact Jacobian. The WBC bridges reduced- and full-order models via virtual constraints $y(t,q):=y_a(q)-y_{\mathrm{des}}(t)$ and solves for joint torques through a convex quadratic program (QP):

$$\min_{(\tau,f,\delta)}\;\frac{\gamma_1}{2}\|\tau\|^2+\frac{\gamma_2}{2}\|f-f_{\mathrm{des}}\|^2+\frac{\gamma_3}{2}\|\delta\|^2$$

subject to output dynamics $\ddot{y}+K_D\dot{y}+K_P y=\delta$, zero stance-foot acceleration, and torque/force feasibility. The slack variable $\delta$ relaxes output dynamics when infeasible, ensuring numerical stability. Since $\ddot{y}$ and $\ddot{r}^{\mathrm{foot},\ell}$ are affine in $(\tau,f)$ under Lagrangian dynamics, the QP is convex and solvable in real time.

Experimental Results

Simulations are conducted in the RaiSim physics engine using the Unitree A1 platform. The high-level MR-NMPC uses the CasADi framework with the IPOPT interior-point solver, achieving a mean solve time of 8.26ms (std 0.87ms) on an Intel Core i9-12900F (64GB RAM), guaranteeing 100Hz operation.

Figure 1: Wall-supported locomotion using MR-NMPC (top) vs. Raibert heuristic baseline (bottom). Green box denotes the safety envelope; red box indicates a violation.

Comparative Benchmark

The baseline is the classical Raibert heuristic with step adjustment $\Delta x=\frac{\dot{x}T_s}{2}+k_v(\dot{x}-\dot{x}_{\mathrm{des}})$, where $k_v=\sqrt{h/g}$. Under nominal conditions (flat terrain, no disturbances), both controllers achieve nearly identical reference velocity ($v_{\mathrm{des}}=0.8$ m/s) and base orientation tracking, establishing a fair comparison baseline.

Dimension	Raibert Heuristic	MR-NMPC
Flat terrain velocity tracking	0.8 m/s (met)	0.8 m/s (met)
Rough terrain success rate	OK at low speed, sharp drop at high speed	2.9× success rate at high speed
Disturbance recovery	50N disturbance → instability	50N disturbance → stable
Rear foot placement	Fixed symmetric constraint	Dynamic support polygon expansion/contraction

Figure 6: Survival probability across 250 randomized rough terrains. The Raibert heuristic degrades sharply at higher velocities; MR-NMPC maintains high success.

Adaptive Footstep Placement and Stability

In obstacle tests with stacked wooden blocks (2cm tall, 14cm wide, up to 3 layers, covering 70% of the track), the MR-NMPC maintains stability via a 20-step prediction horizon. The stability mechanism is analyzed through longitudinal foot offsets:

$$r^{f}_{x}=p^{\mathrm{FR}}_{x}-x_{\mathrm{CoM}},\quad r^{r}_{x}=p^{\mathrm{RR}}_{x}-x_{\mathrm{CoM}}$$

The key finding: MR-NMPC and Raibert differ little in front-foot placement, but significantly in rear-foot placement. The NMPC proactively modulates rear-foot positions to accommodate undulating surfaces—by decoupling front and rear step lengths, the optimizer dynamically expands or contracts the support polygon, generating corrective restorative torques unattainable under Raibert's fixed-symmetry constraints.

Figure 5: (a) CoM velocity with Raibert (blue) vs. MR-NMPC (red), desired 0.8 m/s. (b) Front and rear foot positions relative to CoM.

Disturbance Rejection

A 50N sinusoidal force is applied from $t=8$s to $9$s. The Raibert heuristic responds to the velocity spike with long step lengths that trigger joint-limit saturation and subsequent instability. MR-NMPC leverages its prediction horizon to identify an optimal sequence of smaller, asymmetric steps, mitigating the disturbance while respecting all safety and kinematic constraints.

Scenario	Disturbance/Condition	Raibert	MR-NMPC
Flat terrain	No disturbance	Stable tracking	Stable tracking
Wooden blocks	250 random terrains	High failure at speed	2.9× success rate
Sinusoidal force	50N, 8-9s	Joint saturation → fall	Asymmetric small steps → stable
Hardware	Preliminary	—	60s+ stable standing

Limitations

Author-stated: In preliminary hardware experiments, velocity estimation noise causes forward pitching, preventing the stable limit-cycle behavior observed in simulation. The authors attribute this to insufficient state estimation accuracy rather than controller design.

Analysis: Only simulation validation is provided; hardware experiments are preliminary (60-second static standing), lacking full locomotion comparison data. While 250 randomized terrain trials are statistically sufficient, all simulations use the same physics engine (RaiSim), leaving the sim-to-real gap inadequately assessed. The slow input considers only longitudinal (x-direction) footstep and step length, simplifying lateral footstep planning—this limits applicability in environments requiring lateral adjustment. The wall contact model assumes ideal rigid surfaces; real wall friction and deformation are not discussed.

Conclusion and Future Work

The core contribution elevates footstep planning from an external heuristic decision to an internal state variable within the NMPC framework. Through the indicator-function-encoded multi-rate structure, fast inputs (ERFs) and slow inputs (footstep increments) are jointly solved in one optimization, enabling the controller to anticipate future contact configurations and proactively adjust the support polygon. The 2.9× success rate improvement stems not from finer force control but from "foothold foresight"—precisely what the Raibert heuristic lacks at high speeds. The low-level WBC's convex QP design ensures real-time feasibility of full-order dynamics. Preliminary hardware results, though limited by state estimation, validate the framework's real-time deployability.

Future directions include Kalman filtering for sensor fusion, enlarging front-foot contact area for wall-support stability, and perception-aware planning for terrain geometry and contact uncertainty.

Elevating footholds from computed parameters to optimized states is the key step toward letting predictive control truly take over multi-contact locomotion.