On-line trajectory generation for safe and optimal vehicle motion planning

This paper presents a framework for motion planning of autonomous vehicles, it is characterized by its efficient computation and its safety guarantees. An optimal control based approach generates comfortable and physically feasible maneuvers of the vehicle. Therefore, a combined optimization of the lateral and longitudinal movements in street-relative coordinates with carefully chosen cost functionals and terminal state sets is performed. The collision checking of the trajectories during the planning horizon is also performed in street-relative coordinates. It provides continuous collision checking, which covers nearly all situations based on an algebraic solution and has a constant response time. Finally, the problem of safety assessment for partial trajectories beyond the planning horizon is addressed. Therefore, the Inevitable Collision States (ICS) are used, extending the safety assessment to an infinite time horizon. To solve the ICS computation nonlinear programming is applied. An example implementation of the proposed framework is applied to simulation scenarios that demonstrates its efficiency and safety capabilities.