Multi-Agent Trajectory Optimization With Dynamic Reachability Constraints

A multi-agent trajectory optimization method for differentially flat systems under the effect of uncertainties is proposed. Multi-agent and obstacle collision avoidance constraints, incorporating reachable sets of tracking errors, are formulated as dynamic reachability constraints. Based on exact feedback linearization, a flat system is presented in Brunovsky canonical form, along with a linear tracking error system. The reachability of tracking errors is then analyzed using set propagation techniques. The convergence of the reachable sets to a bounded invariant set is proved. A parameterized optimal trajectory is obtained by solving a linear quadratic minimal control problem based on the Brunovsky canonical form with interior-point constraints. The parameters are then optimized through a bilevel program, where the inner program is used to validate the dynamic reachability constraints. By leveraging the Karush–Kuhn–Tucker (KKT) optimality conditions, the bilevel program is transformed into a mathematical program with complementarity constraints, which is solved by a smoothing method. Numerical examples for trajectory optimization of quadrotors are provided.