A Unified Framework Integrating Decision Making and Trajectory Planning Based on Spatio-Temporal Voxels for Highway Autonomous Driving

Intelligent decision making and efficient trajectory planning are closely related in autonomous driving technology, especially in highway environment full of dynamic interactive traffic participants. This work integrates them into a unified hierarchical framework with long-term behavior planning (LTBP) and short-term dynamic planning (STDP) running in two parallel threads with different horizon, consequently forming a closed-loop maneuver and trajectory planning system that can react to the dynamic environment effectively and efficiently. In LTBP, a novel voxel structure and the 'voxel expansion' algorithm are proposed for the generation of driving corridors in 3D configuration, which involves the prediction states of surrounding vehicles. By using Dijkstra search, the maneuver with minimal cost is determined in form of voxel sequences, then a quadratic programming (QP) problem is constructed for solving the optimal trajectory. And in STDP, another small-scaled QP problem is performed to track or adjust the reference trajectory from LTBP in response to the dynamic obstacles. Meanwhile, a Responsibility-Sensitive Safety (RSS) Checker keeps running at high frequency for real-time feedback to ensure security. Experiments on real data collected in different highway scenarios demonstrate the effectiveness and efficiency of our work.