Perceptive Locomotion through Nonlinear Model Predictive Control

TL;DR

We introduce a perception–planning–control pipeline that enables real-time, full DoF optimization for legged robots navigating rough terrain using nonlinear MPC with local convex foothold constraints. Terrain perception provides steppability planes, an SDF for collision avoidance, and a torso reference map, precomputed at 20 Hz to support online planning at 100 Hz. The approach jointly optimizes all joints and the torso, leveraging a multiple-shooting SQP solver with HPIPM and a filter-based line-search, validated extensively on the ANYmal platform in simulation and hardware. This method achieves dynamic climbing and robust locomotion across stairs, gaps, and stepping stones, demonstrating improved handling of underactuated dynamics and terrain contact while maintaining real-time performance.

Abstract

Dynamic locomotion in rough terrain requires accurate foot placement, collision avoidance, and planning of the underactuated dynamics of the system. Reliably optimizing for such motions and interactions in the presence of imperfect and often incomplete perceptive information is challenging. We present a complete perception, planning, and control pipeline, that can optimize motions for all degrees of freedom of the robot in real-time. To mitigate the numerical challenges posed by the terrain a sequence of convex inequality constraints is extracted as local approximations of foothold feasibility and embedded into an online model predictive controller. Steppability classification, plane segmentation, and a signed distance field are precomputed per elevation map to minimize the computational effort during the optimization. A combination of multiple-shooting, real-time iteration, and a filter-based line-search are used to solve the formulated problem reliably and at high rate. We validate the proposed method in scenarios with gaps, slopes, and stepping stones in simulation and experimentally on the ANYmal quadruped platform, resulting in state-of-the-art dynamic climbing.

Figures (20)

• Figure 1: ANYmal walking on uneven stepping stones. In the shown configuration, the top foothold is 60cm above the lowest foothold. The top right visualizes the internal terrain representation used by the controller.
• Figure 2: Schematic overview of the proposed method together with the update rate of each component.
• Figure 3: Perception pipeline overview. (A) The elevation map is filtered and classified into steppable and non-steppable cells [Section \ref{['sect:perc:filtering_and_classification']}]. All steppable areas are segmented into planes [Section \ref{['sect:perc:plane_segmentation']}]. After segmentation, the steppablity classification is refined. (B) A signed distance field [Section \ref{['sect:perc:signed_distance_field']}] and torso reference layer [Section \ref{['sect:perc:torso_reference_layer']}] are precomputed to reduce the required computation time during optimization. (C) Convex foothold constraints in \ref{['eq:perc:foothold_position_constraint']} are obtained from the plane segmentation. The signed distance field enables collision avoidance in \ref{['eq:perc:sdf_inequality']}, and the torso reference is used to generate height and orientation references [Section \ref{['sect:perc:reference_generation']}].
• Figure 4: An example of a segmented region represented by a non-convex outer polygon and two non-overlapping holes (drawn in black). Three different local convex approximations (drawn in orange) are shown that are found around query points with the iterative algorithm described in section \ref{['sect:perc:cost_definition']}.
• Figure 5: Overview of the coordinates frames and constraints used in the definition of the MPC problem. On the front left foot, a friction cone is shown, defined in the terrain frame $\mathcal{F}_T$. On the right front foot, a swing reference trajectory is drawn between the liftoff frame $\mathcal{F}_{T^-}$ and touchdown frame $\mathcal{F}_{T^+}$. Foot placement constraints are defined as a set of half-spaces in the touchdown frame. Stance legs have collision bodies at the knee, as illustrated on the right hind leg, while swing legs have collision bodies on both the foot and the knee, as shown on the left hind leg.