Focused Trajectory Planning for autonomous on-road driving

On-road motion planning for autonomous vehicles is in general a challenging problem. Past efforts have proposed solutions for urban and highway environments individually. We identify the key advantages/shortcomings of prior solutions, and propose a novel two-step motion planning system that addresses both urban and highway driving in a single framework. Reference Trajectory Planning (I) makes use of dense lattice sampling and optimization techniques to generate an easy-to-tune and human-like reference trajectory accounting for road geometry, obstacles and high-level directives. By focused sampling around the reference trajectory, Tracking Trajectory Planning (II) generates, evaluates and selects parametric trajectories that further satisfy kinodynamic constraints for execution. The described method retains most of the performance advantages of an exhaustive spatiotemporal planner while significantly reducing computation.