Continuous-Time Safe Planning and Control for Agile Quadrupeds

Gage MacLin, Mason Peterson, Camilla Tabasso, Venanzio Cichella, Joshua Migdal, Tye Brady, Taskin PadirarXiv 2026 · arXiv

TL;DR

This paper introduces a real-time motion planning framework for quadrupeds that provides continuous-time safety guarantees without sacrificing the robot's agile motion capabilities. By using composite Bernstein polynomials to parameterize trajectories, the authors solve a non-convex optimization problem that respects the robot's full $SE(2)$ kinematics and anisotropic velocity bounds, ensuring the robot remains collision-free throughout its entire path, not just at discrete time steps.

What Problem Does This Paper Address?

Navigating a quadruped through a crowded, dynamic environment (like a busy warehouse) requires a delicate balance between speed, agility, and safety. Most existing real-time planners take one of two shortcuts to stay fast:

  1. Model Simplification: They treat the robot as a simple point or a "unicycle," ignoring the quadruped's ability to sidestep (lateral motion) or its complex, heading-dependent speed limits.
  2. Discrete Safety Checks: They only check for collisions at specific points in time (e.g., every 0.1 seconds). This leaves "gaps" where a robot might collide with a moving obstacle between these checks.

The authors argue that these simplifications lead to overly conservative behavior, making robots slower and less efficient than they could be.

Methods

The core of their approach is a receding-horizon Nonlinear Program (NLP) that avoids the usual "convex reduction" shortcuts.

  • Full $SE(2)$ Kinematics: Instead of a unicycle model, they use the fully-actuated $SE(2)$ model. This allows the planner to utilize the robot's independent lateral velocity and yaw rate.
  • Anisotropic Velocity Bounds: Quadrupeds can usually run forward much faster than they can move sideways. The authors model this as an intersection of elliptical and box bounds in the robot's body frame (see Figure 3a).
  • Composite Bernstein Polynomials: This is the mathematical "secret sauce." By representing the trajectory as a sequence of Bernstein polynomials, they can leverage the convex hull property. This property states that a polynomial curve is always contained within the convex hull of its control points.
  • Continuous-Time Certification: Because of the Bernstein parameterization, if the planner ensures the control points satisfy safety and velocity constraints, the entire continuous curve is guaranteed to satisfy them. This eliminates the risk of inter-sample collisions.
The planning pipeline: (a) perception and goal setting, (b) solving the Bernstein NLP with hull certificates, and (c) execution on the hardware.

Datasets

The researchers evaluated their planner using:

  • Isaac Sim Benchmarks: A custom-generated field of 25 cylindrical obstacles. They tested both static conditions and dynamic conditions where obstacles moved laterally across the robot's path (see Figure 4).
  • Hardware Experiments: Real-world testing on a Boston Dynamics Spot robot navigating a field of five moving differential-drive robots.
  • Baselines: They compared their "holo" (holonomic) planner against kinematic ablations (unicycle and single-integrator models) and external baselines like a Control Barrier Function (CBF) safety filter and a Riemannian Motion Policy (RMP) controller.

Key Insights and Ideas

  • Nonconvexity is Manageable: While many researchers avoid non-convex optimization for real-time applications, this paper shows that with modern sparse solvers (like IPOPT) and a smart parameterization, it can run at 36Hz on edge hardware like an NVIDIA Jetson.
  • Lateral Motion Matters: By allowing the robot to move sideways while maintaining its heading, the planner reached goals significantly faster than unicycle-based planners, which have to turn their entire body to change direction.
  • Continuous Safety > Discrete Safety: The use of Bernstein polynomials provides a mathematical guarantee of safety across the entire time horizon, which is crucial when dealing with fast-moving obstacles where a small "gap" in safety checks could lead to a crash.

Key Results

  • Speed: In static simulations, the proposed planner reached the goal 1.6–2.7x faster than all tested baselines.
  • Safety: It sustained the highest success rate and fewest collisions in dynamic environments compared to external baselines.
  • Efficiency: The NLP solved in a median of 16.3 ms on an onboard Jetson Thor, enabling a high replanning rate of ~36 Hz.
  • Hardware Success: The system successfully completed 22 consecutive real-world traversals (392 meters total) among moving robots, demonstrating that the theoretical safety guarantees translate well to noisy, real-world sensing.
Spot navigating through moving obstacles. The magenta line shows the planned trajectory, which successfully weaves between moving robots.

Limitations and Future Work

  • Quasi-Static Obstacles: Currently, the planner treats obstacles as static snapshots during each solve. While the high replanning rate mitigates this, it doesn't explicitly account for obstacle velocity within a single planning horizon.
  • Footprint Conservatism: To keep the math polynomial, the robot is modeled as a disc. This adds some "dead space" (about 0.35m laterally), which might prevent the robot from squeezing through very tight gaps that its rectangular body could actually fit through.
  • Future Directions: The authors plan to extend the safety certificates to include obstacle trajectory predictions and to explicitly account for perception latency and tracking errors in the optimization.

Summary updated

Abstract

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.
BibTeX
@misc{maclin2026continuous,
  author = {Gage MacLin and Mason Peterson and Camilla Tabasso and Venanzio Cichella and Joshua Migdal and Tye Brady and Taskin Padir},
  title = {{Continuous-Time Safe Planning and Control for Agile Quadrupeds}},
  year = {2026},
  eprint = {2610.35670},
  archivePrefix = {arXiv},
  url = {https://arxiv.org/abs/2610.35670},
}
Figure