Agile Decision-Making and Safety-Critical Motion Planning for Emergency Autonomous Vehicles

Efficiency is critical for autonomous vehicles (AVs), especially emergency AVs. However, most existing methods focus on regular vehicles, overlooking the different strategies required by emergency vehicles to address the challenge of maximizing efficiency while ensuring safety. In this paper, we propose an Integrated Agile Decision-Making with Active and Safety-Critical Motion Planning System (IDEAM). IDEAM focuses on enabling emergency AVs, such as ambulances, to actively achieve efficiency in dense traffic scenarios with safety in mind. Firstly, the speed-centric decision-making algorithm named the long short-term spatio-temporal graph-centric decision-making (LSGM) is given. LSGM comprises conditional depth-first search (C-DFS) for multiple path generation as well as methods for speed gains and risk evaluation for path selection, which presents a robust algorithm for high efficiency and safety consideration. Secondly, with an output path from LSGM, the motion planner reconsiders environmental conditions to decide constraint states for the final planning stage, among which the lane-probing state is designed for actively attaining spatial and speed advantage. Thirdly, under the Frenet-based model predictive control (MPC) framework with final constraints state and selected path, the safety-critical motion planner employs decoupled discrete control barrier functions (DCBFs) and linearized discrete-time high-order control barrier functions (DHOCBFs) to model the constraints associated with different driving behaviors, making the optimization problem convex. Finally, we extensively validate our system using scenarios from a randomly synthetic dataset, which reveal that IDEAM improves average route progress by approximately 5.25% to 12.93% and increases average speed by about 4.5% to 9.8% compared to the benchmark method, demonstrating its capability to achieve speed benefits and assure safety simultaneously. Simulation video is available at: https://www.youtube.com/watch?v=873BZoQSf-Q. Our implementation code is available at https://github.com/YimingShu-teay/IDEAM.git