Filtering-Linearization: A First-Order Method for Nonconvex Trajectory Optimization with Filter-Based Warm-Starting

TL;DR

This paper presents a novel first-order method with filter-based warm-starting for nonconvex trajectory optimization, significantly improving solution quality and convergence speed in multi-agent collision avoidance scenarios.

Contribution

It introduces a new approach combining particle filtering, clustering, and warm-starting for efficient nonconvex trajectory optimization.

Findings

Reduces objective function value by up to 96% in two-agent scenarios.

Achieves up to 98% reduction in six-agent scenarios.

Outperforms sequential quadratic programming and interior-point methods.

Abstract

Nonconvex trajectory optimization is at the core of designing trajectories for complex autonomous systems. A challenge for nonconvex trajectory optimization methods, such as sequential convex programming, is to find an effective warm-starting point to approximate the nonconvex optimization with a sequence of convex ones. We introduce a first-order method with filter-based warm-starting for nonconvex trajectory optimization. The idea is to first generate sampled trajectories using constraint-aware particle filtering, which solves the problem as an estimation problem. We then identify different locally optimal trajectories through agglomerative hierarchical clustering. Finally, we choose the best locally optimal trajectory to warm-start the prox-linear method, a first-order method with guaranteed convergence. We demonstrate the proposed method on a multi-agent trajectory optimization problem with linear dynamics and nonconvex collision avoidance. Compared with sequential quadratic programming and interior-point method, the proposed method reduces the objective function value by up to approximately 96\% within the same amount of time for a two-agent problem, and 98\% for a six-agent problem.