Improved Approximation Accuracy for Nonconvex Trajectory Optimization via Trajectory Sensitivities

Trajectory optimization is valuable for a wide range of applications, from motion planning for mobile robots, to aircraft flight planning. However, nonlinear dynamic models lead to challenging nonconvex trajectory optimization problems. Many existing approaches formulate them as multistage programs and rely on derivatives of each stage to obtain a local approximation at each iteration, in which case quality of approximation when solving the optimization program has significant impact on convergence behavior. In this work, we develop a novel approach for obtaining improved local approximations when solving nonconvex trajectory optimization problems. By performing an input-to-state reformulation of system dynamics, we use trajectory sensitivities, which are derivatives of the entire system trajectory with respect to control inputs, to form local approximations. Local convergence guarantees for the proposed method are presented. The method is applied to generate trajectories for an autonomous vehicle, and is extended to include scenario with static obstacles. Simulation on a variety of reference paths show that the proposed method outperforms the traditional Sequential Quadratic Programming (SQP) in terms of local approximation accuracy, allowable trust-region radius, iterations to converge, and total solver time, and is less prone to failure when handling multiple obstacles with a complex reference.