A practical trajectory planning framework for autonomous ground vehicles driving in urban environments

This paper presents a practical trajectory planning framework towards fully autonomous driving in urban environments. Firstly, based on the behavioral decision commands, a reference path is extracted from the digital map using the LIDAR-based localization information. The reference path is refined and interpolated via a nonlinear optimization algorithm and a parametric algorithm, respectively. Secondly, the trajectory planning task is decomposed into spatial path planning and velocity profile planning. A closed-form algorithm is employed to generate a rich set of kinematically-feasible spatial path candidates within the curvilinear coordinate framework. At the same time, the velocity planning algorithm is performed with considering safety and smoothness constraints. The trajectory candidates are evaluated by a carefully developed objective function. Subsequently, the best collision-free and dynamically-feasible trajectory is selected and executed by the trajectory tracking controller. We implemented the proposed trajectory planning strategy on our test autonomous vehicle in the realistic urban traffic scenarios. Experimental results demonstrated its capability and efficiency to handle a variety of driving situations, such as lane keeping, lane changing, vehicle following, and static and dynamic obstacles avoiding, while respecting traffic regulations.