Real-Time Trajectory Planning for Autonomous Urban Driving: Framework, Algorithms, and Verifications

Abstract

This paper focuses on the real-time trajectory planning problem for autonomous vehicles driving in realistic urban environments. To solve the complex navigation problem, we adopt a hierarchical motion planning framework. First, a rough reference path is extracted from the digital map using commands from the high-level behavioral planner. The conjugate gradient nonlinear optimization algorithm and the cubic B-spline curve are employed to smoothen and interpolate the reference path sequentially. To follow the refined reference path as well as handle both static and moving objects, the trajectory planning task is decoupled into lateral and longitudinal planning problems within the curvilinear coordinate framework. A rich set of kinematically feasible path candidates are generated to deal with the dynamic traffic both deliberatively and reactively. In the meanwhile, the velocity profile generation is performed to improve driving safety and comfort. After that, the generated trajectories are carefully evaluated by an objective function, which combines behavioral decisions by reasoning about the traffic situations. The optimal collision-free, smooth, and dynamically feasible trajectory is selected and transformed into commands executed by the low-level lateral and longitudinal controllers. Field experiments have been carried out with our test autonomous vehicle on the realistic inner-city roads. The experimental results demonstrated capabilities and effectiveness of the proposed trajectory planning framework and algorithms to safely handle a variety of typical driving scenarios, such as static and moving objects avoidance, lane keeping, and vehicle following, while respecting the traffic rules.