PaperlayerView exact revision on arXiv

Source-linked AI summary

Perceptive Locomotion through Nonlinear Model Predictive Control

Ruben Grandia, Fabian Jenelten, Shaohui Yang, Farbod Farshidian, Marco Hutter

arXiv:2208.08373v1cs.RO

TL;DR

Dynamic locomotion on rough terrain requires coordinated foothold placement, collision avoidance, and underactuated dynamics planning despite imperfect perception. The paper presents a real-time nonlinear MPC pipeline using local convex foothold constraints and precomputed terrain representations, and validates it on challenging scenarios with ANYmal. The controller achieves perceptive dynamic locomotion but remains limited by externally specified gait patterns and local optimization failures in some obstacle configurations.

  • Problem

    Coordinating precise foothold placement with contact constraints, system dynamics, and collision avoidance under perceptive uncertainty remains difficult for rough-terrain locomotion.

  • Method

    A perception, planning, and control pipeline uses nonlinear MPC with local convex foothold constraints, signed-distance-field collision avoidance, and precomputed terrain information.

  • Results

    The method enables perceptive dynamic locomotion across gaps, slopes, stepping stones, and obstacle courses in simulation and on the ANYmal quadruped platform.

  • Takeaways & Limitations

    Jointly modeling the complete joint configuration lets the controller optimize foot placement, knee collision avoidance, and underactuated dynamics in real time.

  • Takeaways & Limitations

    The gait pattern is externally given, and strong disturbances or thin, tall obstacles can cause execution failure or optimization to become stuck.

Abstract

from arXiv · show

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.

I. INTRODUCTION

Perceptive dynamic locomotion on rough terrain remains difficult because precise foothold placement must be coordinated with system dynamics, contact constraints, and collision avoidance. The paper addresses this with a real-time nonlinear MPC framework that jointly optimizes the robot’s motion while representing terrain through local geometric constraints.

  • Motivation: Rough terrain makes precise foot placement difficult, especially for negative obstacles and stepping stones.
  • Motivation: Terrain and system dynamics constrain contact location, force, and timing, while tightly integrating perception for coordinated foot placement remains an active problem.
  • Related work: Existing perceptive methods often select footholds before torso motion, limiting coordination and making kinematic limits and terrain collisions harder to include.
  • Related work: Joint torso-leg trajectory optimization removes engineered coordination but is often too slow online and can suffer from poor local minima on arbitrary terrain.
  • Proposed approach: The proposed framework jointly optimizes all robot degrees of freedom using nonlinear MPC, convex local foothold constraints, and a signed distance field for terrain collision avoidance.
  • Proposed approach: Multiple-shooting, sequential quadratic programming, real-time iteration, and filter-based line search support fast and reliable online solutions, with perception updated at 20 Hz and MPC running at 100 Hz.

B. Terrain representation

The paper represents rough terrain with local geometric approximations and uses them within a nonlinear MPC solver designed for real-time constrained optimization. Perception processing and solver design jointly address terrain non-convexity, discontinuity, and computational demands.

  • Terrain representation: Terrain is integrated through local convex regions rather than a global convex decomposition, avoiding mixed-integer foothold assignment while retaining nonlinear costs and dynamics.The local region is centered around a nominal foothold, reducing the geometric problem presented to the optimizer.
  • Terrain representation: The perception pipeline filters and classifies elevation maps, segments steppable areas into planes, and precomputes signed-distance and torso-reference information before optimization.These stages are organized as perception processing followed by integration into the optimal control problem.
  • Motion Optimization: Multiple shooting, real-time iteration, and a filter-based line search are combined to solve successive nonlinear MPC problems under real-time constraints.The filter-based line search manages nonlinear steps by monitoring both dynamics violations and cost reduction without relying on an arbitrary merit-function trade-off.
  • Filtering & Classification: Elevation maps support steppability classification using local inclination and roughness estimates computed from neighborhood covariance.A neighborhood of N = 9 uses a 2 cm normal-direction standard-deviation threshold and a maximum inclination of 35°.

B. Plane Segmentation

Plane segmentation converts classified steppable terrain into connected planar regions with explicit polygonal boundaries, including enclosed holes, for subsequent geometric reasoning.

  • Plane Segmentation: Connected components are evaluated as planes using normal-direction deviation, surface inclination, and minimum region-size criteria.The plane test accepts regions with σn ≤ 0.025, nz ≥ 0.87, and N ≥ 4.
  • Plane Segmentation: RANSAC recursively extracts smaller planes from connected regions that fail the initial planarity test.Cells not assigned to any plane are ultimately marked non-steppable.
  • Plane Segmentation: Each segmented region is represented by an outer polygon and additional polygons tracing enclosed holes.The contour is extracted from the elevation map and projected onto the fitted plane.
  • Signed Distance Field: Non-steppable cells receive a 2 cm vertical margin and one-cell dilation before signed-distance computation.The dilation horizontally inflates non-steppable areas by the map resolution, correcting underestimated terrain edges.
  • Motion Optimization: The resulting perception products provide plane-based terrain geometry, signed-distance information, and torso references for the nonlinear MPC formulation.The MPC is defined as a constrained nonlinear optimal-control problem over state and input trajectories.

A. Robot definition

The robot model includes floating-base motion, joint states, contact forces, and underactuated torso dynamics, with actuator bandwidth effects represented through system augmentation and frequency-dependent penalties.

  • Robot definition: The generalized coordinates and velocities describe the base pose and velocities, 12 joint positions and velocities, and contact forces for each leg.Base orientation uses Euler angles, while base angular and linear velocities are expressed in the body frame.
  • Robot definition: The model derives torso acceleration from the underactuated top six rows of full rigid-body dynamics, which govern achievable range of motion.The formulation retains inertial coupling terms between the legs and base in the torso dynamics.
  • Robot definition: Joint inertial coupling forces are set to zero so the legs are considered at velocity level and joint accelerations are removed from the formulation.Evaluating at a nominal configuration and zero joint velocity would yield the commonly used single-rigid-body simplification.
  • Robot definition: Low-pass system augmentation penalizes high-frequency contact-force and joint-velocity content to account for series-elastic actuator bandwidth limits.The auxiliary inputs associated with filtered contact forces and joint velocities are penalized instead of explicitly modeling actuator dynamics.
  • Robot definition: The continuous-time MPC model combines robot dynamics, system augmentation, coordinate transformations, and a constant disturbance-wrench parameter over the horizon.The resulting state and input vectors have dimensions x ∈ R48 and u ∈ R24.

E. Reference generation

Reference generation converts user commands and perceived terrain into base, foothold, swing, and force references, while the MPC cost and soft constraints regulate tracking, feasibility, contact, and collision behavior.

  • Reference generation: User velocity, angular-rate, and gait commands are converted into a full motion and contact-force reference before every MPC iteration.A torso reference layer and nominal height produce hip positions, which are fitted to a 6DoF base reference.
  • Reference generation: Nominal footholds are placed below the hips and adapted by projecting candidates onto nearby segmented planes subject to kinematic penalties.The penalty discourages excessive leg extension and footholds crossing to the opposite side of the body.
  • Reference generation: Swing references use quintic splines from liftoff to apex and apex to touchdown, with the apex selected to clear intervening terrain.Inverse kinematics converts the base and foot references into joint-position references.
  • Cost & Soft Inequality Constraints: The cost combines motion tracking, loop-shaping, and penalty terms for joint limits, foothold constraints, friction cones, and collision avoidance.Tracking errors cover the base and individual feet, while auxiliary-input costs regulate high-frequency forces and joint velocities.
  • Cost & Soft Inequality Constraints: Foot placement is represented by linear half-space inequalities formed from polygon edges and the touchdown-terrain normal.The polygon expands iteratively from the reference foothold until reaching the segmented boundary or risking non-convexity.
  • Cost & Soft Inequality Constraints: Collision avoidance evaluates the signed-distance field at collision-sphere centers and includes knees plus swing feet in the collision set.A shaping function relaxes constraints when a swing foot begins below an overestimated map and guides it smoothly toward free space.

G. Equality constraints

The controller imposes contact- and swing-phase equality constraints that regulate foot motion while preserving tangential foothold freedom during swing.

  • Swinging feet have zero contact force, while contacting feet have zero end-effector velocity.
  • During swing, only the normal component of reference tracking is enforced, leaving tangential foot placement unconstrained.
  • The normal vector is interpolated over time to accommodate different terrain orientations at liftoff and touchdown.
  • Algorithm 1 applies real-time iteration multiple-shooting MPC through discretization, quadratic approximation, constraint projection, QP solution, back substitution, and line search.

V. NUMERICAL OPTIMIZATION

The continuous optimal-control problem is discretized over a prediction-horizon grid, producing finite-dimensional states, inputs, costs, and constraints for nonlinear MPC.

  • The continuous control signal is parameterized over horizon subintervals, with nodes aligned to gait transitions.
  • A zero-order-hold input parameterization and numerical integration convert continuous dynamics into a discrete-time representation.
  • The nonlinear MPC problem defines costs and constraints on the grid of discretized nodes.
  • State and input sequences are collected into the decision vector w = [X^T, U^T]^T for the resulting nonlinear program.
  • F(w) collects initial-state and dynamics constraints, whereas G(w) collects general equality constraints.

B. Sequential Quadratic Programming (SQP)

The SQP solver makes the online MPC update tractable by forming a convex quadratic subproblem, eliminating equality constraints, and exploiting optimal-control sparsity.

  • SQP applies Newton-type iterations to KKT conditions and computes updates by solving a potentially non-convex quadratic program.
  • A positive-semidefinite Hessian makes the QP convex and efficiently solvable, motivating an approximate positive-semidefinite Hessian.
  • The approximation uses a Generalized Gauss-Newton form for least-squares tracking costs and convexity of soft-constraint penalties.
  • Full-row-rank equality constraints are eliminated through a change of variables before solving the QP.
  • Because constraints are node-local, projection preserves optimal-control sparsity and permits parallel computation, while the projected QP needs one Riccati-based iteration.

E. Line-Search

The online controller combines filter-based step acceptance with whole-body and reactive control to handle constraint violations, model execution, and contact mismatches.

  • E. Line-Search: The filter line search accepts updates when they improve either constraint satisfaction or the cost, rather than combining both into one merit metric.
  • E. Line-Search: When constraint violation is high, the search focuses on reducing constraints; when low, it focuses on minimizing cost.
  • E. Line-Search: The online implementation removes recovery strategies and second-order corrections because only one iteration is performed per problem.
  • E. Line-Search: The line search backtracks the step size until an update is accepted or the minimum step size is reached.
  • A. Event based execution: Measured contact states can differ from planned states, so reactive behaviors provide feedback to the MPC layer.
  • A. Event based execution: For a missing planned contact, the foot follows a downward regaining motion and the MPC replans from a closed-contact state.

VII. RESULTS

The evaluation validates the perception pipeline, collision-aware optimization, and solver behavior on ANYmal across simulated and hardware settings. Precomputed terrain representations support fast perceptive queries, while collision avoidance and line search improve feasible motion execution.

  • Experimental setup: 20 Hz elevation mapping on an onboard GPU supports asynchronous 100 Hz MPC and 400 Hz control and state estimation.The perception and control components run at different rates on ANYmal’s onboard hardware.
  • Experimental protocol: The experiments keep perception, MPC, and whole-body-control parameters constant across simulation and hardware, after simulation initialization and hardware fine-tuning.The controller is evaluated in isolated perception tests, design-choice studies, and challenging simulation and hardware experiments.
  • Perception pipeline: Plane segmentation handles slopes, steps, curved surfaces, and bounded rough terrain while allowing holes that exclude locally unsuitable regions.A global convex decomposition would produce many more regions.
  • Perception pipeline: Below 10 ms, local signed distance fields typically containing fewer than 10^5 voxels enable fast collision queries during 20 Hz map updates.Construction scales linearly with SDF size, while query time is nearly constant until cache capacity is exceeded.
  • Collision avoidance: 35 cm box traversal succeeds with knee-collision avoidance, which alters swing trajectories and adjusts the base pose and footholds for stepping.The unified optimization also places feet away from the step during descent to avoid knee collisions.
  • Solver behavior: During collision-sensitive stepping, line search reduces the stepsize when constraint violation rises, balancing cost reduction against constraint satisfaction.Most accepted steps use a full stepsize of 1.0, but nonlinear collision penalties require smaller steps near the box edge.

2) Model selection:

The model-selection and locomotion experiments examine dynamics fidelity, solver reliability, contact feedback, gait generality, and obstacle-course traversal. Together, they show how the integrated controller handles terrain interactions and multiple motion patterns under stated feasibility limits.

  • Model selection: Using SRBD omits the backward CoM shift caused by changing leg configurations because torso dynamics use a constant nominal joint configuration.The comparison keeps the rest of the controller identical.
  • Solver comparison: Multiple-shooting avoids iLQR’s inherently single-threaded rollout line search, which takes four times longer per line search in the compared implementation.The stated difference disappears for hybrid multiple-shooting-iLQR variants.
  • Solver comparison: iLQR satisfies dynamics constraints through nonlinear rollouts but diverges at 63% during a large leap and 78% near hind-leg singularity in the stepping-stone scenario.The reported failures are sudden and also occur regularly on hardware under imperfect maps, real dynamics, and disturbances.
  • Contact feedback: Contact feedback triggers a reactive downward trajectory for late touchdown and immediately replans from the sensed contact location for early touchdown.The tested terrain mismatch is 10 cm above actual terrain for late contact and 5 cm below it for early contact.
  • Stairs: The controller autonomously progresses, repeats, or skips steps across stairs with 18.5 cm rise and 24 cm run across trot, pace, dynamic-walk, and static-walk gaits.No gait- or stair-specific parameters or control modes are used.
  • Obstacle course: Fast trotting at 0.8 m s−1 traverses an obstacle course containing slopes, gaps, stepping stones, and rough terrain, while transverse gallop remains infeasible on the more complex course.Gallop feasibility is limited by system torque and slope friction constraints.
  • Obstacle course: The compared perceptive RL controller crosses unstructured terrain but falls between stepping stones and off a narrow passage in the more challenging sections.The reported failures occur when starting from the left in the obstacle course.

C. Hardware

Hardware experiments validate perceptive dynamic locomotion across ramps, gaps, steps, stepping stones, and stairs, while timing measurements support the controller’s real-time update rates. The experiments also expose limitations involving externally specified gait patterns, local planning, thin obstacles, and perceptive estimation during flight phases.

  • Hardware experiments: ANYmal traversed a ramp, gap, and large step, then walked across uneven stepping stones and up and down outdoor stairs.The stepping stones measured 20x20 cm with 20 cm height increments; the outdoor stairs had a 16 cm rise and 29.5 cm run.
  • Computation: The perception pipeline kept up with 20 Hz elevation-map updates, despite terrain classification and plane segmentation dominating map-update computation.RANSAC refinement was the largest contributor and could produce high worst-case computation times.
  • Computation: 100 Hz MPC updates were achievable on average, although model evaluation dominated computation and rare worst-case times were attributed mainly to scheduling variance.The QP solve was the second-largest MPC computation component, despite parallelization across four cores.
  • Limitations: The controller’s gait pattern is externally given and adapted only during early and late touchdown, so strong disturbances can cause failure.The authors identify changing the stepping pattern and adding reinforcement-learning-based tracking as possible robustness improvements.
  • Limitations: Independent foothold-plane selection for each leg can place all feet on one stair tread, producing fragile support and potential self-collisions.The method targets local motion planning and will climb over gaps or obstacles when commanded rather than autonomously navigating around them.
  • Limitations: Optimization can get stuck at very thin, tall obstacles because horizontal SDF gradients provide little local guidance for avoidance.The reported example uses a structure 5 cm wide and 20 cm high; sampling-based recovery or learning-based initialization is suggested for future work.
  • Limitations: During gallop and trot experiments with flight phases, perceptive information was disabled because impact artefacts and state-estimation drift made the elevation map unusable.The robot instead estimated the ground plane from a history of contact points.

APPENDIX A SIGNED DISTANCE FIELD COMPUTATION

The appendix computes a signed distance field for a 2.5D elevation map by combining vertical and horizontal distance transforms. It corrects cell-center distance calculations so obstacle borders are represented between discrete map cells.

  • Distance-field construction: The vertical transform is simplified because elevation maps have trivial distance along the z-direction.The algorithm starts with the z-direction, then applies two-dimensional distance transforms for each height in parallel.
  • Border correction: Using Euclidean distances between cell centers creates discontinuous SDF gradients at obstacle borders, so distances are measured to cell borders instead.The distance function uses the map resolution r when computing center-to-border distances.
  • Vertical transform: f_z returns the one-dimensional distance transform in the z-direction for each horizontal position, using the evaluated elevation h(x′, y′).Its closed form distinguishes points above the elevation surface from points at or below it.
  • Signed distance field: The full signed distance field combines distances to obstacles and obstacle-free space, with gradients computed by finite differences across layers, columns, and rows.This produces both positive and negative parts of the SDF.
  • Distance-field construction: The distance field minimizes horizontal cell distances plus the elevation-map-based vertical distance over discrete map cells.The computation uses D(x, y, z) = min over {x′, y′} ∈ M of d(x, x′) + d(y, y′) + f_z(x′, y′, z).
Loading 2208.08373v1…