Penalty boundary sequential convex programming algorithm for non-convex optimal control problems

A nonlinear optimal control problem with non-convex cost function and non-convex state constraints can be addressed by a series of convex programming to obtain numerical solutions in previous methods. However, a feasible initial solution is essential to ensure the convergence. In this paper, slack variables are added into the model to handle the infeasible initial point and are penalized in the cost. What is more, a new approximation point on the boundary of constraints is embraced in each iteration to increase the similarity to original problem and decrease number of iterations. Thus, a penalty boundary sequential convex programming algorithm is proposed, which is globally convergent to a Karush-Kuhn-Tucker (KKT) point of original problem under mild condition. The theoretical basis is guaranteed by a rigorous proof. Single UAV and multi-robots trajectory planning serve as simulations to verify the validity of the presented algorithm.