Accurate Pseudospectral Optimization of Nonlinear Model Predictive Control for High-Performance Motion Planning

Nonlinear Model Predictive Control (NMPC) is an effective method for motion planning of automated vehicles, but the high computational load of numerical optimization limits its practical application. This paper designs an NMPC based motion planner and presents two techniques, named adaptive Lagrange discretization and hybrid obstacle avoidance constraints, to accelerate the numerical optimization by reducing the optimization variables and simplifying the non-convex constraints. Given the high nonlinearity of vehicle dynamics, the Lagrange interpolation is adopted to convert the state equation of vehicle dynamics and the objective function to ensure a preset accuracy but with less discretization points. An adaptive strategy is then designed to adjust the order of Lagrange polynomials based on the distribution of discretization error. Moreover, a hybrid strategy is presented to construct the constraints for obstacle avoidance by combing the elliptic and linear time-varying methods together. It can ensure driving safety and also make a good balance between computing load and accuracy. The performance of these techniques on accelerating the NMPC based motion planner is validated and analyzed by comparative numerical simulations and experimental tests under various scenarios. Compared with traditional methods, the results show that these techniques improve accuracy and efficiency by 74% and 60%, respectively.