An Efficient Algorithm for Optimal Trajectory Generation for Heterogeneous Multi-Agent Systems in Non-Convex Environments

D. Reed Robinson1, Robert T. Mar2, Katia Estabridis3, Gary Hewer2

  • 2Naval Air Warfare Center Weapons Division
  • 3NAVY



Interactive Session


10:30 - 13:00 | Tue 22 May | podG | TuA@G

Motion Planning 1

Full Text


Generating optimal trajectories for many vehicles in real-time becomes extremely challenging when including realistic dynamic models and time-varying obstacle constraints. We present a novel method to efficiently produce time-optimal collision-free trajectories in complex non-convex maze-like environments while enforcing nonlinear constraints on velocity, acceleration, jerk, and snap. The approach combines two complementary numerical techniques for optimal control: level set reachability analysis and pseudospectral orthogonal collocation. Applied in a multi-agent prioritized planning framework, the methodology allows for heterogeneous sizes, dynamics, endpoint conditions, and individualized optimization objectives, while achieving linear scaling in computation time relative to the number of vehicles. Simulation examples are shown with up to 26 static obstacles and 32 quadcopters in a tightly constrained space with average computation times of 1 to 3.5 seconds per vehicle, depending on scenario complexity.

Additional Information

No information added


No videos found


8 heterogeneous quadcopters in a maze

  • Smooth, time-optimal trajectories avoid static/dynamic obstacles without restriction to predefined corridors or waypoints
  • Combines strengths of complementary optimal control methods: level sets, and psuedospectral orthogonal collocation
  • Prioritized sequential planning results in computation that scales linearly in the number of vehicles
  • Reliably and accurately handles maze-like environments and complex nonlinear dynamics