Real-time trajectory planning for automated vehicle safety and performance in dynamic environments

Safe trajectory planning for high-performance automated vehicles in an environment with both static and moving obstacles is a challenging problem. Part of the challenge is developing a formulation that can be solved in real-time while including the following set of specifications: minimum time-to-goal, a dynamic vehicle model, minimum control effort, both static and moving obstacle avoidance, simultaneous optimization of speed and steering, and a short execution horizon. This paper presents a nonlinear model predictive control-based trajectory planning formulation, tailored for a large, high-speed unmanned ground vehicle, that includes the above set of specifications. The ability to solve this formulation in real-time is evaluated using NLOptControl, an open-source, direct-collocation based, optimal control problem solver in conjunction with the KNITRO nonlinear programming problem solver. The formulation is tested with various sets of the specifications. A parametric study relating execution horizon and obstacle speed indicates that the moving obstacle avoidance specification is not needed for safety when the planner has a small execution horizon and the obstacles are moving slowly. However, a moving obstacle avoidance specification is needed when the obstacles are moving faster, and this specification improves the overall safety without, in most cases, increasing the solve-times. The results indicate that (i) safe trajectory planners for high-performance automated vehicles should include the entire set of specifications mentioned above, unless a static or low-speed environment permits a less comprehensive planner; and (ii) the resulting formulation can be solved in real-time.