A Novel Trajectory Optimization for Affine Systems: Beyond Convex-Concave Procedure

Abstract

Trajectory optimization problems under affine motion model and convex cost function are often solved through the convex-concave procedure (CCP), wherein the non-convex collision avoidance constraints are replaced with its affine approximation. Although mathematically rigorous, CCP has some critical limitations. First, it requires a collision-free initial guess of solution trajectory which is difficult to obtain, especially in dynamic environments. Second, at each iteration, CCP involves solving a convex constrained optimization problem which becomes prohibitive for real-time computation even with a moderate number of obstacles, if long planning horizons are used.In this paper, we propose a novel trajectory optimizer which like CCP involves solving convex optimization problems but can work with an arbitrary initial guess. Moreover, the proposed optimizer can be computationally upto a few orders of magnitude faster than CCP while achieving similar or better optimal cost. The reduced computation time, in turn, stems from some interesting mathematical structures in the optimizer which allows for distributed computation and obtaining solutions in symbolic form. We validate our claims on difficult benchmarks consisting of static and dynamic obstacles.