ADMM-based Continuous Trajectory Optimization in Graphs of Convex Sets

TL;DR

This paper introduces an ADMM-based numerical solver for continuous trajectory optimization in non-convex environments, combining polynomial parameterization and a spatio-temporal graph approach for improved search and convergence.

Contribution

It presents a novel ADMM-based method that jointly optimizes discrete and continuous variables, enhancing trajectory quality and robustness in non-convex settings.

Findings

Accesses a larger search space than decoupled methods

Ensures reliable convergence from naive initializations

Achieves superior trajectories in complex environments

Abstract

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 spatial and continuous temporal 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.