Nonlinear programming for multi-vehicle motion planning with homotopy initialization strategies

Multi-vehicle motion planning (MVMP) is a critical decision-making module in intelligent transportation systems. Compared to the decentralized MVMP methods, centralized MVMP methods are beneficial in being generic and complete, because information of all the vehicles is simultaneously considered. This study formulates the MVMP problems as centralized optimal control problems. These problems are parameterized into nonlinear programming (NLP) problems for the convenience of numerical solution. In solving those NLPs, the main challenges lie in the large scale of collision-avoidance constraints, and the high nonlinearity of vehicle kinematics. The typical NLP solvers are inefficient in directly handling such difficulties. It is widely known that the initialization has a significant influence on the NLP solving behavior. Therefore, homotopy initialization strategies are developed in this work to generate the initial guess. The main idea of homotopy is that simplified subproblems are solved in a sequence such that each subproblem is closer to the original problem; the solution to each subproblem serves as the initial guess to facilitate the solving process of the next subproblem. This process continues until the original problem is solved. The efficiency of the proposed initialization strategies is verified via numerical experimentation and theoretical analysis.