PaperlayerView exact revision on arXiv

Source-linked AI summary

Continuous-Time Safe Planning and Control for Agile Quadrupeds

Gage MacLin, Mason Peterson, Camilla Tabasso, Venanzio Cichella, Joshua Migdal, Tye Brady, Taskin Padir

arXiv:2610.35670v1cs.RO

TL;DR

Real-time quadruped planners can simplify kinematics to retain tractability, excluding lateral and heading-coupled motion, while sampled constraints do not certify the intervals between samples. This paper uses a nonconvex planner with composite Bernstein trajectories to certify velocity and clearance constraints continuously. In simulation it reaches goals faster than tested alternatives, and its full stack also runs onboard among moving obstacles.

Abstract

from arXiv · show

Agile quadruped navigation in dynamic environments demands trajectories that are both dynamically feasible and provably collision-free. Most real-time planners achieve tractability through a convex reduction that requires simplified kinematic models, discarding motion that the robot can readily execute, and certify safety only at discretization points, leaving open possible inter-sample collisions with moving obstacles. We present a real-time motion planning framework that retains the fully-actuated $SE(2)$ model of a quadruped, with its anisotropic, heading-coupled velocity bounds, and accepts the resulting nonconvexity. By parameterizing trajectories as composite Bernstein polynomials, we exploit the convex hull property to certify collision avoidance and the input bounds over the entirety of each planning horizon. Solved at online replanning rates, the optimization runs in closed loop, commanding body velocities to the locomotion controller. We validate the approach in simulation against ablations and external baselines, and in hardware experiments on a quadruped navigating among unknown moving obstacles, recovering both lateral and heading-coupled motion that simplified models exclude.

I. INTRODUCTION

The paper addresses how to plan for quadrupeds without discarding their lateral and heading-coupled motion, while certifying safety continuously. It uses a nonconvex receding-horizon planner and reports faster goal-reaching in simulation, alongside real-robot deployment.

I. INTRODUCTION

  • Simplified high-level models discard quadruped capabilities such as lateral translation and heading-dependent longitudinal and lateral velocity limits –.The paper notes that quadrupeds can execute these motion primitives.Source: §I · pp. 1–2
  • The approach accepts nonlinear kinematics and nonconvex hard collision constraints, whose computational cost the authors report as modest for online replanning.The formulation uses structure-exploiting transcriptions, sparse NLP solvers, and warm-started replanning.Source: §I · p. 2
  • The planner retains the fully actuated SE(2) model and certifies anisotropic velocity and collision constraints for all t using a receding-horizon NLP over composite Bernstein polynomials.The Bernstein convex-hull property supports continuous-trajectory certification.Source: §I · p. 2
  • In 50 closed-loop scenarios, the planner reached goals 1.6–2.7× faster than every tested kinematic ablation and external baseline, with the fewest collisions among moving obstacles.The contribution also reports onboard replanning at approximately 36 Hz.Source: §I · p. 2
  • The full stack ran onboard a quadruped through 22 consecutive traversals totaling 392 m among unknown moving robots, with perception, planning, and control operating in real time.The deployment commands body velocities to the locomotion controller.Source: §I · p. 2

II. RELATED WORKS

Prior quadruped navigation methods use simplified models or hierarchical pipelines, while flatness-based planning relies on constraints that remain convex in the flat output. Bernstein-based nonlinear optimization offers continuous-time path-constraint enforcement, with footprint and obstacle representations shaping the certificate's conservatism.

A. Quadruped Locomotion and Navigation

  • Safety-critical locomotion extensions enforce obstacle avoidance at discrete controller nodes and may require access to a whole-body control layer, whereas this planner certifies safety continuously at the planning layer through the velocity interface.Existing navigation frameworks often pair a global planner with a local optimizer over a simplified model.Source: A. Quadruped Locomotion and Navigation · p. 2

B. Flatness-Based Real-Time Planning

  • Flatness-based planners obtain tractability when constraints are convex in the flat output, but corridor approximations can be conservative and soft obstacle penalties do not guarantee strict clearance.This work instead retains the nonconvex SE(2) model and imposes hard clearance inequalities along the continuous trajectory.Source: B. Flatness-Based Real-Time Planning · p. 2

C. Bernstein-Based Nonlinear Trajectory Optimization

  • Bernstein convex-hull properties allow path constraints to be enforced over the trajectory, while composite polynomials capture sharp local features better than a single global polynomial.With circular obstacles and a disc robot footprint, the clearance constraints are polynomial; the circumscribing disc adds bounded excess clearance.Source: C. Bernstein-Based Nonlinear… · pp. 2–3

D. Learning-Based Approaches

Learning-based navigation can act directly on raw sensing but does not guarantee collision avoidance by construction. The paper instead plans directly over the quadruped's fully actuated SE(2) model, accepting nonconvexity where flat-output formulations cannot efficiently certify its velocity limits.

D. Learning-Based Approaches

  • Learning-based policies can command velocities from perception without an explicit obstacle model, but their collision avoidance is encouraged through training rather than guaranteed by construction.A model-based safety filter can be paired with a policy to provide reliable avoidance.Source: D. Learning-Based Approaches · p. 3

A. Fully-Actuated SE(2) Model

  • The fully actuated SE(2) model retains independent lateral velocity and heading, unlike a unicycle model that fixes lateral velocity to zero.Expressing its velocity bounds through flat outputs yields trigonometric and bilinear terms, so the paper formulates the problem directly on the SE(2) model as an NLP.Source: A. Fully-Actuated SE(2) Model · p. 3

B. Anisotropic Motion Constraints

The planner represents the quadruped's anisotropic, heading-coupled velocity limits rather than substituting a simplified model. It also imposes hard obstacle-clearance constraints continuously, with certification tied to the obstacle positions perceived at each solve.

B. Anisotropic Motion Constraints

  • The body-frame velocity envelope couples feasible translational motion to heading; a single-integrator overestimates lateral feasibility, while a unicycle excludes lateral motion.The envelope combines an elliptical bound with box bounds, including distinct longitudinal and lateral limits.Source: B. Anisotropic Motion Constraints · p. 3

C. Collision Avoidance

  • Each solve imposes hard obstacle-clearance constraints over the entire trajectory for the obstacle positions perceived at that solve, but motion between solves is uncertified.The obstacle set is treated quasi-statically and refreshed from perception; a positive clearance margin mitigates bounded tracking deviation within one replan cycle.Source: C. Collision Avoidance · p. 4

D. Receding-Horizon Trajectory Optimization

The planner replans over a bounded horizon toward an intermediate goal, using a terminal-position cost and yaw-rate smoothing. Its finite-dimensional NLP uses composite Bernstein trajectories to retain the fully actuated SE(2) model and certify velocity and collision constraints continuously.

D. Receding-Horizon Trajectory Optimization

  • Each solve plans from the pose predicted at solver completion, over a bounded horizon toward an intermediate goal supplied by the global planner.The horizon length is a decision variable within fixed minimum and maximum bounds.Source: D. Receding-Horizon Trajectory… · p. 4
  • The objective combines soft attraction to the goal position with yaw-rate smoothing; the bounded horizon favors maximum feasible progress while preserving a feasible halt-and-hold option.The planner delegates mission-level objectives to global guidance and certifies safety per solve.Source: D. Receding-Horizon Trajectory… · p. 4

IV. COMPOSITE BERNSTEIN TRAJECTORY OPTIMIZATION

  • Composite Bernstein trajectories transcribe the optimal-control problem into a finite-dimensional NLP while enabling continuous-time certification of velocity and collision constraints.The formulation retains the fully actuated SE(2) kinematics and accepts the resulting nonconvex constraints.Source: §IV · p. 4

A. Bernstein Preliminaries

Composite Bernstein polynomials represent trajectories as piecewise low-degree polynomials with control points. Their convex-hull, endpoint, and closure properties support continuous bounds and linear boundary or continuity constraints across segments.

A. Bernstein Preliminaries

  • A composite Bernstein polynomial joins segment polynomials at knots, and its control points provide the coefficients used to represent each segment.The Bernstein convex-hull property lets bounds on control points certify bounds over the whole interval.Source: A. Bernstein Preliminaries · p. 5
  • The convex-hull property certifies bounds over each interval, endpoint interpolation enforces boundary and knot conditions, and closure under arithmetic and calculus supports polynomial path constraints.These properties support continuous-time velocity and collision certificates.Source: A. Bernstein Preliminaries · p. 5

B. Trajectory Parameterization

  • Pose and input trajectories are parameterized as degree-N composite Bernstein polynomials over K segments with a bounded horizon.Uniformly spaced knots make the horizon duration one scalar decision variable, trading adaptive knot placement for fewer variables.Source: B. Trajectory Parameterization · p. 5
  • The trajectory is constrained to be C0-continuous at interior knots, and input continuity prevents jumps in commanded body velocities.Heading references and constraints can be imposed linearly on the heading control points, though those tasks are not evaluated here.Source: B. Trajectory Parameterization · p. 5

C. Enforcement of the Kinematics

The transcription approximates the quadruped kinematics by matching derivative control points to the vector field at pose and input control points. This allocates approximation error to kinematics while keeping velocity and collision constraints exact over the continuous trajectory.

C. Enforcement of the Kinematics

  • Because the rotation matrix makes the kinematics nonpolynomial, no polynomial trajectory satisfies them exactly, so approximation is unavoidable.The stated exceptions are constant heading or rotation in place.Source: C. Enforcement of the Kinematics · p. 5
  • The transcription matches each derivative control point to the vector field evaluated at the corresponding pose and input control points.Control points coincide with the trajectory only at segment endpoints, so these equalities constrain control points rather than polynomial values at node times.Source: C. Enforcement of the Kinematics · p. 5
  • Feasible transcription solutions approach continuous-problem solutions at rate O(1/K2N), while approximation is allocated to kinematics rather than continuous velocity and collision constraints.The paper treats kinematic approximation as more benign because the locomotion controller can absorb its tracking error; constraint violations could reach the robot as collisions or infeasible commands.Source: C. Enforcement of the Kinematics · p. 5

D. Continuous Constraint Certification

The Bernstein parameterization certifies nonconvex polynomial path constraints over the whole horizon through its convex-hull and closure properties. The velocity envelope is represented as a polynomial on each segment, whose control-point constraints certify satisfaction throughout the segment.

D. Continuous Constraint Certification

  • The convex-hull and closure properties certify nonconvex path constraints over the entire horizon when those constraints are polynomial in the trajectory.This includes the paper's velocity-envelope and collision constraints.Source: D. Continuous Constraint Certification · p. 5

1) Velocity envelope:

  • The elliptical velocity constraint becomes a degree-2N polynomial on each segment, and bounding its Bernstein control points certifies it for all times.The box bounds are certified directly by bounding the input control points.Source: 1) Velocity envelope · p. 5

2) Collision avoidance:

Obstacle clearance is imposed as a hard constraint over the continuous trajectory, not merely at sampled instants. The resulting structured NLP is warm-started between replans and evaluated in simulation and hardware.

2) Collision avoidance:

  • For every obstacle and trajectory segment, nonnegative clearance-polynomial control points certify clearance across the full interval without a sampling grid, safe corridor, or soft penalty.The constraints are hard constraints on the continuous trajectory relative to the obstacle set for the current solve.Source: 2) Collision avoidance · pp. 5–6

E. Resulting NLP and Real-Time Implementation

  • The NLP uses 6K(N + 1) trajectory control points and one bounded horizon-time variable, with block-banded constraints and warm starts from the preceding solution.The first solve uses a straight-line position interpolation and zero values otherwise; the reported planners use K = 2 and N = 5.Source: E. Resulting NLP and Real-Time… · p. 6

V. EXPERIMENTS

  • Evaluation proceeds from closed-loop simulation against model ablations and external baselines to a hardware deployment with real sensing.The hardware stack runs onboard Spot, while simulations use identical seeded scenarios for planner comparisons.Source: §V · p. 6

A. Simulation Setup

The evaluation compares the full planner with model ablations and external approaches in matched static and moving-obstacle scenarios. The full planner is faster than the tested alternatives, while safety and success vary with obstacle motion and model choice.

A. Simulation Setup

  • The 20 m task uses 25 seeded cylindrical obstacles, with dynamic obstacles crossing laterally at 0.5–1.0 m/s and perceived positions updating at 15 Hz.All planners receive the same local goals and active obstacle set, and measured solve time advances the simulation before a new plan deploys.Source: A. Simulation Setup · p. 6

B. Envelope and Model Ablations

  • Roughly 1.7× faster at comparable safety, holo reaches the goal sooner than conservative int-vy; int-vx times out on six static seeds, while uni takes about 60% longer.The comparisons use 50 shared seeds; under moving obstacles, heading-coupled lateral motion also buys some safety.Source: B. Envelope and Model Ablations · p. 6, Table I · p. 7
  • Five dynamic seeds caused collisions for every tested planner, indicating scenes with little room to escape within the velocity bounds.On other seeds, the more conservative int-vy cleared all four holo collisions, including two grazes under 3 N.Source: B. Envelope and Model Ablations · p. 6

C. External Baseline Comparison

  • Roughly 2.5× and 2.7× faster than layered and rmp, respectively, holo also sustains the fewest collisions under motion.In static trials, layered fails by timeout and rmp by collision; in dynamic trials, holo sustains the fewest collisions.Source: C. External Baseline Comparison · p. 7, Table I · p. 7

D. Hardware Demonstration

The full planning stack runs onboard Spot under depth-camera perception and executes coupled longitudinal–lateral motion among moving obstacles. Hardware trials demonstrate online operation, while the paper identifies gaps between per-solve certificates and closed-loop safety.

D. Hardware Demonstration

  • Across 22 traversals totaling 392 m, Spot completed 20 successfully and made contact with an obstacle in two.The trials crossed a field of five differential-drive robots and produced 13,331 planner solves.Source: D. Hardware Demonstration · p. 7
  • The continuous-time certificate applies to obstacles as perceived at each solve, not their motion between replans, perception error, or tracking error beyond the clearance margin.The reported contacts are likely attributable to this quasi-static assumption.Source: D. Hardware Demonstration · pp. 7–8
  • The onboard NLP solved in 16.3 ms median and 37.9 ms at p95, sustaining a 36 Hz mean replan rate with real sensing.The deployment used obstacle estimates affected by sensing noise and latency absent from ground-truth simulation.Source: D. Hardware Demonstration · p. 8

VI. CONCLUSION

  • The conclusion identifies obstacle prediction, perception latency, and tracking error as future extensions to close residual gaps between per-solve and closed-loop safety.The full planner certifies velocity and collision constraints along the continuous trajectory.Source: §VI · p. 8

Related papers

Loading 2610.35670v1…