Convergence-Guaranteed Trajectory Planning for a Class of Nonlinear Systems with Nonconvex State Constraints

In this article, we study the problem of trajectory planning for a class of nonlinear systems with convex state/control constraints and nonconvex state constraints with concave constraint functions. This corresponds to a challenging nonconvex optimal control problem. We present how to convexify the nonlinear dynamics without any approximation via a combination of variable redefinition and relaxation. We then prove that the relaxation is exact by designing an appropriate objective function. This exact relaxation result enables us to further convexify the nonconvex state constraints simply by linearization. As a result, an algorithm is designed to iteratively solve the obtained convex optimization problems until convergence to get a solution of the original problem. A unique feature of the proposed approach is that the algorithm is proved to converge and it does not rely on any trust-region constraint. High performance of the algorithm is demonstrated by its application to trajectory planning of UAVs and autonomous cars with obstacle avoidance requirements.