PaperlayerView exact revision on arXiv

Source-linked AI summary

Motion Planning for Quadrupedal Locomotion: Coupled Planning, Terrain Mapping and Whole-Body Control

Carlos Mastalli, Ioannis Havoutis, Michele Focchi, Darwin G. Caldwell, Claudio Semini

arXiv:2003.05481v4cs.ROcs.SYeess.SY

TL;DR

Terrain-blind reactive methods struggle with discontinuities, and terrain-aware planning must contend with non-convex terrain models. This paper couples terrain-aware foothold and horizontal-motion planning with stability-limited trunk adaptation; in stepping-stone trials, the coupled planner reached 90% success, over 30% above the decoupled planner.

  • Problem

    Terrain-blind reactive approaches cannot handle all terrain types, especially large discontinuities, and non-convex terrain models make optimization challenging.

    Source: 2) Preview schedule · p. 5, 3) Soft-constraints · p. 7

  • Method

    The method jointly plans footholds and horizontal motion using terrain cost maps, stochastic search, and trunk-attitude adaptation bounded by angular acceleration.

    Source: A. Contribution · p. 1, 2) Preview schedule · p. 5

  • Results

    Stepping-stone trial success reached 90%, up over 30% with respect to the decoupled planner.

    Source: 3) Increment of the success rate · p. 8

  • Takeaways & Limitations

    The framework was validated in experiments and simulations on terrains of progressively increasing complexity, including terrain elevation changes up to 12 cm.

    Source: §VIII · p. 13

    Including the terrain cost-map increases computation time from 23 s to 10–15 min in the tested planners.

    Source: 4) Computation time · p. 9

Abstract

from arXiv · show

Planning whole-body motions while taking into account the terrain conditions is a challenging problem for legged robots since the terrain model might produce many local minima. Our coupled planning method uses stochastic and derivatives-free search to plan both foothold locations and horizontal motions due to the local minima produced by the terrain model. It jointly optimizes body motion, step duration and foothold selection, and it models the terrain as a cost-map. Due to the novel attitude planning method, the horizontal motion plans can be applied to various terrain conditions. The attitude planner ensures the robot stability by imposing limits to the angular acceleration. Our whole-body controller tracks compliantly trunk motions while avoiding slippage, as well as kinematic and torque limits. Despite the use of a simplified model, which is restricted to flat terrain, our approach shows remarkable capability to deal with a wide range of non-coplanar terrains. The results are validated by experimental trials and comparative evaluations in a series of terrains of progressively increasing complexity.

I. INTRODUCTION

Legged robots struggle with complex terrain because terrain-blind reactive methods cannot handle all discontinuities, while terrain-aware trajectory optimization has often been limited to flat or simulated settings. The paper extends coupled planning with terrain modeling and trunk-attitude planning, then evaluates the framework against decoupled planning in experiments and simulations.

I. INTRODUCTION

  • Terrain-blind reactive methods cannot handle all terrain types, especially large discontinuities, while many trajectory-optimization methods omit terrain risk or focus on flat terrain and simulation.Terrain-risk models are often used only for foothold planning rather than incorporated into trajectory optimization.Source: §I · p. 1

A. Contribution

  • The paper adds log-barrier terrain modeling and a stability-constrained trunk-attitude planner to its coupled planning method, and compares it with decoupled planning.The framework integrates online terrain mapping, state estimation, and whole-body control for its experimental and simulation evaluations.Source: A. Contribution · pp. 1–2

II. RELATED WORK

  • Prior reactive gaited approaches can handle some irregular terrain, but complex geometries such as gaps, stairs, and rubble can quickly reach robot limits.Other strategies plan footholds for predefined challenging terrains, while many optimal-control approaches omit contact locations or timings.Source: §II · p. 2

A. Decoupled planning

  • The decoupled baseline selects footholds locally after constructing a body-state graph, then generates a dynamically stable body trajectory for that foothold sequence.Its horizontal CoM motion is represented by two fifth-order polynomials and computed with a cart-table model.Source: A. Decoupled planning · p. 2

A. The HyQ robot

  • HyQ is an 85 kg hydraulically actuated quadruped with onboard sensing and separate real-time control and non-real-time mapping and planning computers.The high-level whole-body controller runs at 250 Hz on the real-time computer.Source: A. The HyQ robot · p. 2

B. Framework components

The framework couples horizontal CoM and foothold planning with terrain mapping, trunk-attitude adaptation, and whole-body control. Its preview model separates horizontal CoM motion from trunk attitude, while angular-acceleration limits constrain adaptation to preserve stability.

B. Framework components

  • The framework uses terrain cost- and height-maps for horizontal motion and attitude planning, respectively, and a whole-body controller to track the resulting plans.Mapping and estimation provide local terrain maps and estimates of trunk pose and twist.Source: B. Framework components · pp. 2–3

IV. MOTION PLANNING

  • Coupled planning optimizes CoM motion and footholds together, incorporating terrain costs and trunk-attitude planning for elevation changes.The approach also optimizes step timing and uses simplified dynamics for foothold selection.Source: §IV · p. 3

A. Trajectory generation

  • The planner generates horizontal CoM trajectories and 2D foothold locations, then adapts trunk attitude to terrain elevation using maximum allowed angular accelerations.Leaving vertical CoM motion unspecified decouples CoM and trunk-attitude planning.Source: A. Trajectory generation · pp. 3–4

1) Preview model:

  • A cart-table-with-flywheel preview model decouples CoM motion from trunk attitude and predicts CMP shifts from trunk angular acceleration.The horizontal CoM trajectory is derived under an approximately linear CoP displacement within each phase.Source: 1) Preview model · p. 4
  • Trunk attitude adapts toward estimated support planes, but angular-acceleration bounds limit alignment to keep the CMP inside the support region.Cubic polynomial splines describe roll and pitch adaptation, which may span multiple phases when one phase would exceed the acceleration limits.Source: 1) Preview model · pp. 4–5
  • The preview model assumes constant trunk height and omits vertical CoM dynamics, using an estimated support area to obtain CoP height for non-coplanar footholds.The horizontal-frame vectors are expressed in a frame aligned with gravity.Source: 1) Preview model · pp. 4–5

2) Preview schedule:

The preview schedule represents locomotion as stance and swing phases whose control parameters shape the CoM trajectory. Foothold shifts are optimized within stance-frame bounds that preserve the support-region constraint.

  • Each preview phase is stance, with all legs on the ground, or swing, with at least one leg swinging; phase duration and swing-foot shift are control parameters.Source: 2) Preview schedule · p. 5
  • Foot-shifts are relative foothold locations in the stance frame, bounded by the foothold region; because they change the ZMP stability polygon, they make the optimization nonlinear.Source: 2) Preview schedule · p. 5, Figure 4 · p. 5
  • Horizontal CoM motion is set by phase durations and CoP displacements, while its height is held constant according to the estimated support polygon.Source: 2) Preview schedule · p. 5
  • The preview state sequence is generated from an initial state and control parameters, with each state recording CoM position and velocity, CoP, and the active-foot support region.Source: 2) Preview schedule · p. 5
  • Because terrain models can be non-convex and non-differentiable, they can create many local minima, making stochastic optimization a promising solver choice.Source: 2) Preview schedule · p. 5

B. Terrain cost-map

The terrain cost-map assigns each location a desirability cost from weighted geometric features, with normalized costs and piece-wise functions used to model risk.

  • The terrain cost-map quantifies how desirable a foot placement is at each location using geometric features including height deviation, slope, and curvature.Source: B. Terrain cost-map · p. 5
  • Feature costs are combined as a weighted linear combination, with the weights describing the relative importance of the features.Source: B. Terrain cost-map · p. 5
  • The total cost is normalized from 0 for minimum risk to 1 for maximum risk, and the feature costs use piece-wise functions resembling log-barrier constraints.Source: B. Terrain cost-map · pp. 5–6

1) Height deviation:

Height deviation is used to penalize footholds near large drop-offs, where execution inaccuracies may place the robot in gaps or banned areas.

  • The height-deviation feature is the standard deviation of terrain height in a defined neighborhood.Source: 1) Height deviation · p. 6
  • The cost penalizes footholds near large drop-offs, which is relevant for gaps and stepping stones because execution inaccuracies can lead into gaps or banned areas.Source: 1) Height deviation · p. 6

2) Slope:

Slope and curvature features encode foothold terrain risk, while finite-horizon optimization plans horizontal motion and footholds across future terrain conditions.

2) Slope:

  • Slope is estimated from neighborhood surface normals computed by PCA; larger slopes raise cost because they can increase slipping risk, even with a friction cone.The passage attributes residual risk to possible inaccuracies in friction coefficient or estimated surface normal; the approximately 70° worst-slope value was heuristically set from HyQ experience.Source: 2) Slope · p. 6, 3) Curvature · p. 6
  • The slope feature treats small slopes as approximately flat and zero-cost, while larger slopes increase cost up to the maximum feature value that defines the log-function barrier.Source: 2) Slope · p. 6

3) Curvature:

  • Curvature between 6 and 9 has zero cost and is preferred to flat terrain, whereas high curvature indicates narrow cracks and low curvature indicates edges where feet may get stuck or slip.The stated thresholds include c > 9 for cracks and c < −6 for edges.Source: 3) Curvature · p. 6

C. Horizontal trajectory optimization

1) Problem formulation:

The planner uses stochastic search to jointly handle user commands, energy, terrain, stability, and motion coupling. Its soft constraints encode terrain costs and CoM–CoP consistency while preserving stability within each phase.

1) Problem formulation:

  • CMA-ES searches a multi-objective cost landscape covering user velocity and direction, energy, terrain, CoP stability, and horizontal–vertical motion coupling.The method is selected to handle multiple local minima and discontinuous gradients introduced by the terrain cost-map.Source: 1) Problem formulation · p. 6

2) Cost functions:

  • Average walking velocity is used to track the user’s velocity command over the planning horizon, whose terminal state is the latest state considered.The supplied passage identifies the terminal CoM position and phase durations in this horizon formulation.Source: 2) Cost functions · p. 6
  • Estimated Locomotion Cost (ELC) approximates energy using a single point-mass kinetic-energy model because joint torques and velocities are unavailable during optimization.Minimizing ELC is intended to reduce energy consumption over a given terrain.Source: 2) Cost functions · p. 7

3) Soft-constraints:

  • Terrain costs weight feature values at candidate footholds; the 2 cm grid uses log-barrier features, and its cost-map is not guaranteed convex.The 2 cm resolution is described as a trade-off between computation time and information resolution.Source: 3) Soft-constraints · p. 7
  • A nonlinear constraint keeps the CoP inside a support polygon shrunk by margin r, with foothold positions included as decision variables.The constraint is defined from the foothold locations and CoP position.Source: 3) Soft-constraints · p. 7
  • A soft constraint penalizes artificial horizontal CoM displacement when the constant-height cart-table approximation poorly captures coupling with vertical motion.Here h is the cart-table height, while x and p denote CoM and CoP positions.Source: 3) Soft-constraints · p. 7
  • Stability and coupling constraints are enforced at each phase’s initial and terminal states; bounded foot shifts preserve a convex support hull, and penalties are quadratic.The method states these endpoint constraints suffice to guarantee stability and coupling throughout the phase.Source: 3) Soft-constraints · p. 7
  • The terrain cost-map is computed onboard, and its continuous costs range from blue for minimum cost to red for maximum cost.The map supports terrain-aware foothold cost evaluation from a foot-shift and CoM position.Source: Figure 5 · p. 7, 3) Soft-constraints · p. 7

V. WHOLE-BODY CONTROLLER

The whole-body controller uses full robot dynamics in an optimization to track trunk and swing-motion references while respecting contact, torque, and kinematic constraints. Feed-forward torques are combined with impedance-based feedback before commands reach the low-level torque controller.

V. WHOLE-BODY CONTROLLER

  • The trunk controller generates feed-forward joint torques for trunk and swing references while respecting friction, torque, and kinematic limits.An impedance controller computes feedback torques in parallel to address unpredictable events such as foot divergence during slippage.Source: §V · p. 7
  • The controller uses full robot dynamics and all degrees of freedom to distribute desired motion tasks across the joints.Joint dynamics matter for dynamic motions because their effects are no longer negligible for good tracking.Source: §V · pp. 7–8
  • A quadratic program chooses generalized accelerations and contact forces to track a reference centroidal wrench while regularizing the solution.Its constraints encode dynamics, stance and swing tasks, friction, torque limits, and kinematic limits.Source: §V · p. 8
  • Joint PD feedback torques are added to feed-forward torques, and the resulting command is sent to a low-level joint-torque controller.During nominal operation, the trunk controller’s feed-forward torques make the largest contribution.Source: §V · pp. 7–8
  • Terrain mapping supplies estimated friction and terrain normals at contacts, while state estimation fuses inertial, visual, and odometry data for base pose and velocity.Source: §V · p. 8

VI. EXPERIMENTAL RESULTS

The experiments compare coupled and decoupled planning on challenging terrain, then examine trunk-attitude modulation and terrain-cost weighting. The complete framework is also demonstrated on terrains with varied slopes and obstacles.

VI. EXPERIMENTAL RESULTS

  • The evaluation compares coupled and decoupled planners on stepping stones, a pallet, stairs, and a gap.The decoupled planner is the baseline for the comparison.Source: §VI · p. 8
  • The experiments assess how trunk-attitude modulation handles terrain-height variation and how terrain-cost-map weights change behavior without significantly affecting stability or ELC.Source: §VI · p. 8
  • The full framework—coupled planner, whole-body controller, terrain mapping, and state estimation—is demonstrated crossing varied slopes and obstacles.Source: §VI · p. 8

A. Motion planning: decoupled vs coupled approach

The coupled planner improves success on stepping stones and reports faster walking with fewer footholds, while the decoupled planner requires hand-tuned step durations. The terrain-aware search substantially increases computation time, but the coupled planner handles challenging height variations with a larger foothold region.

1) Decoupled planner setup:

  • The decoupled planner fixes swing and stance durations by terrain, and durations must be hand-tuned for each footstep sequence when stable CoM trajectories cannot be computed.The reported swing durations range from 0.5 to 0.7 s and stance durations from 0.05 to 1.4 s.Source: 1) Decoupled planner setup · p. 8

2) Coupled planner setup:

  • The coupled planner uses stochastic search without a warm-start trajectory and plans over one locomotion cycle, or four steps.The setup uses the same stability margin and angular-acceleration settings as the attitude planner.Source: 2) Coupled planner setup · p. 8

3) Increment of the success rate:

  • Stepping-stone success reaches 90%, up over 30% relative to the decoupled planner; walking speed rises 14%–63% and footholds fall 14% on average.The average foothold error is about 2 cm, half the decoupled-planner case; higher walking speeds and cost tuning explain the higher ELC.Source: 3) Increment of the success rate · pp. 8–9, Table I · p. 9

4) Computation time:

  • Including the terrain cost-map raises computation time from 23 s to 10–15 min, largely because stochastic search estimates the gradient.The decoupled planner instead uses tree search guided toward a shortest path rather than the safest one.Source: 4) Computation time · p. 9

5) Crossing challenging terrains:

  • A bounded foot-search region preserves kinematic feasibility for height differences up to 12 cm; the coupled planner expands the foothold region enough to cross the Fig. 6 terrains.The coupled region increases from 20 cm × 23.5 cm to 34 cm × 28 cm; the stair-climbing sequence uses 14 cm steps.Source: 5) Crossing challenging terrains · p. 9, Figure 7 · p. 10

B. Trunk attitude planning

The section reports attitude modulation, terrain-sensitive foothold selection, and whole-body tracking across challenging terrain. It also identifies limits in terrain-weight tuning and step-timing optimization.

B. Trunk attitude planning

  • The cart-table model neglects angular dynamics; its flywheel extension supports stable attitude changes, while the planner limits angular acceleration to keep CMP inside the support region.The attitude planner adapts trunk orientation to the estimated support region at each phase.Source: B. Trunk attitude planning · p. 10
  • 0.11 rad/s^2 was the maximum diagonal angular-acceleration limit; HyQ successfully modulated trunk roll and pitch over one locomotion cycle of four steps.The attitude planner aligns the trunk with the support plane using cubic polynomial splines.Source: B. Trunk attitude planning · p. 10, Figure 8 · p. 10

C. Motion planning and terrain mapping

  • At a 25 cm gap, increasing terrain weight by 200% prevented crossing; reducing it let the coupled planner select footholds nearer the gap border.The terrain weight affects foothold selection, not stability or ELC; computational cost was not affected by terrain geometry.Source: C. Motion planning and terrain mapping · p. 10, Figure 9 · p. 11

D. Whole-body control, state estimation and terrain mapping

  • In simulation, the controller climbed ramps up to 20 degrees under friction conditions similar to real experiments (µ = 0.7) and handled unpredictable contact interactions.The coupled cart-table planner neglects non-coplanar contact and the friction cone; the whole-body controller tracked planned motion without violating friction, torque, or kinematic constraints.Source: D. Whole-body control, state… · pp. 10–11
  • Terrain surface normals are estimated online from vision; tracking errors under state-estimation and terrain-mapping errors mainly came from low-frequency corrections of the estimated pose.The trials used a friction coefficient of 0.7, described as a conservative estimate of real contact conditions.Source: D. Whole-body control, state… · p. 11

A. Motion planning: decoupled vs coupled approach

  • Coupled planning uses dynamics in foothold selection and step duration, handling varied terrain heights more easily than decoupled planning, but with longer computation time.The continuous cost-map covers a broader range of challenging environments; decoupled planning is more prone to kinematic-limit collisions on elevated gaps.Source: A. Motion planning: decoupled vs… · p. 11
  • Step-timing optimization showed no clear experimental benefit; the cart-table model omits friction and torque constraints that may require slower motion.The authors identify low friction and torque-limit risk as cases where slower timing could help satisfy both constraints.Source: A. Motion planning: decoupled vs… · p. 11
  • 208 flat-terrain steps and 50 non-flat-terrain steps were tested in simulation; planning four steps ahead sufficed, with no significant improvement at six or eight.Unexpected events can compromise stability and may require replanning.Source: A. Motion planning: decoupled vs… · p. 11

B. Trunk attitude planning

The attitude-planning method and terrain cost-map support locomotion over challenging terrain, with reported results spanning foothold selection, slopes, state estimation, and increasingly complex experimental trials.

B. Trunk attitude planning

  • On the combined ramp-gap-step course, controller torque stayed within limits despite planned motion reaching them at t =14 sec; tracking error rose near x =1.25 m, then fell.Source: Figure 11 · p. 12, Figure 10 · p. 12
  • The attitude planner heuristically adapts trunk orientation under mild stability assumptions, supplementing cart-table CoP estimates that omit angular motion; fuller planning needs limb dynamics and future events.The paper relates CoM-applied torques to CoP displacement and a stability margin.Source: B. Trunk attitude planning · pp. 11–12

C. The effect of the terrain cost-map

D. Considering terrain with slopes

  • Experiments demonstrated navigation across a wide range of terrain slopes without planning-level friction-cone stability; controller constraints help mitigate poor tracking.The whole-body optimization includes friction-cone and foot-loading constraints.Source: D. Considering terrain with slopes · p. 13

E. Terrain mapping and state estimation

  • State drift between high-frequency proprioceptive and low-frequency exteroceptive updates affected experimental performance, so controller compliance was reduced by increasing proportional gains.Source: E. Terrain mapping and state estimation · p. 13

VIII. CONCLUSION

  • The framework combines log-barrier terrain costs and attitude planning to optimize horizontal CoM motion and footholds, with experiments and comparative evaluations across increasingly complex terrains.The full dynamic model is used in the real-time whole-body controller to avoid slippage and torque and kinematic limits.Source: §VIII · p. 13
Loading 2003.05481v4…