ADMM-based Continuous Trajectory Optimization in Graphs of Convex Sets
Authors: Lukas Pries, Jon Arrizabalaga, Zachary Manchester, Markus Ryll
Organizations: Autonomous Aerial Systems Lab, Dep. of Aerospace and Geodesy, TU Munich, Germany · Department of Aeronautics and Astronautics, Massachusetts Institute of Technology, USA
This paper presents a numerical solver for computing continuous trajectories in non-convex environments. Our approach relies on a customized implementation of the Alternating Direction Method of Multipliers (ADMM) built upon two key components: First, we parameterize trajectories as polynomials, allowing the primal update to be computed in closed form as a minimum-control-effort problem. Second, we introduce the concept of a spatio-temporal allocation graph based on a mixed-integer formulation and pose the slack update as a shortest-path search. The combination of these ingredients results in a solver with several distinct advantages over the state of the art. By jointly optimizing over both discrete and continuous domains, our method accesses a larger search space than existing decoupled approaches, enabling the discovery of superior trajectories. Additionally, the solver's structural robustness ensures reliable convergence from naive initializations, removing the bottleneck of complex warm starting in non-convex environments.
Figures & tables
Fig. 1 : Example trajectories generated by ACTOR: a quadrotor minimum-snap trajectory (left) and high-order continuous trajectories through a non-convex maze (right).
Fig. 2 : The allocation graph assigns segments (red) to convex sets (blue) and is constructed from the convex space decomposition and its corresponding intersection graph.
Fig. 3 : Effects of the ADMM updates on the trajectory. (a) The primal update yields a continuous trajectory. (b) The slack update projects each segment onto all convex sets. (c) A shortest-path search in the weighted allocation graph determines the segment allocation. (d) The dual update enforces consistency between primal and slack via a gradient ascent step.
Fig. 4 : Illustrative examples of challenging non-convex environments highlighting the benefits of our method.
Fig. 5 : Continuous trajectories in a point-cloud environment.
Fig. 6 : Left: Trajectories in sequences of sets. Right: Runtime as a function of the number of sets for different solvers.
Fig. 7 : Bounded velocity and acceleration profile and convergence for the min-snap trajectory in Fig. 1 .