Safety-critical Decision-making and Control for Autonomous Vehicles with Highest Priority

This paper proposes a comprehensive framework to enable autonomous vehicles (AVs) with the highest priority (e.g., ambulances) to perform lane change maneuvers timely and continuously to achieve speed benefits without compromising safety. This type of vehicles can override traffic rules and drive at high speeds when necessary. The proposed framework comprises three parts: a decision-making layer, a motion planning layer, and a safety filter for the controller. The discrete decisions are coordinated by a speed-oriented finite state machine (FSM). Once the decision has been made, model predictive control (MPC) is utilized for planning and control, ensuring safety guarantees. Safety filters are constructed as longitudinal and lateral constraints by decoupled and discrete control barrier functions (DCBFs), combined with MPC, making it a convex optimization problem. Specifically, the region of interest (ROI) is employed to determine the range of the activation of lateral constraints. The proposed framework is tested through comparative numerical simulations, demonstrating its ability to gain speed and ensure safety across randomly generated driving scenarios. Additionally, some emergency scenarios have been considered in the experiment.