A Hierarchical Framework of Decision Making and Trajectory Tracking Control for Autonomous Vehicles

Most of the existing research in the field of autonomous vehicles (AVs) addresses decision making, planning and control as separate factors which may affect AV performance in complex driving environments. A hierarchical framework is proposed in this paper to address the problem mentioned above in environments with multiple lanes and surrounding vehicles. Firstly, high-level decision making is implemented by a finite-state machine (FSM), according to the relative relationship between the ego vehicle (EV) and the surrounding vehicles. After the decision is made, a cluster of quintic polynomial equations is established to generate the path connecting the initial position to the candidate target positions, according to the traffic states of the EV and the target vehicle. The optimal path is selected from the cluster, based on the quadratic programming (QP) framework. Then, the speed profile is generated, based on the longitudinal displacement-time graph (S-T graph), considering the vehicle motion state constraints and collision avoidance. The smoothed speed profile is created through another QP formulation, in the space created by the dynamic-programming (DP) method. Finally, the planned path and speed profile are combined and sent to the lower level control module, which consists of the linear quadratic regulator (LQR) for lateral trajectory tracking control, and a double PID controller for longitudinal control. The performance of the proposed framework was validated in a co-simulation environment using PreScan, MATLAB/Simulink and CarSim, and the results demonstrate that the proposed framework is capable of addressing most ordinary scenarios on a structured road, with reasonable decisions and controlling abilities.