Enhancing Safety-Critical Autonomous Driving: Integrated Decision-Making and Planning With Barrier-Enhanced Homotopic Parallel Trajectory Optimization 

Lei Zheng, Rui Yang, Michael Yu Wang, and Jun Ma 

Abstract

Enforcing safety while preventing overly conservative behaviors is essential for autonomous vehicles to achieve high task performance. In this paper, we propose a barrier-enhanced homotopic parallel trajectory optimization (BHPTO) approach with over-relaxed alternating direction method of multipliers (ADMM) for real-time integrated decision-making and planning. To facilitate safety interactions between the ego vehicle (EV) and surrounding vehicles, a spatiotemporal safety module exhibiting bi-convexity is developed on the basis of barrier function. Varying barrier coefficients are adopted for different time steps in a planning horizon to account for the motion uncertainties of surrounding HVs and mitigate conservative behaviors. Additionally, we exploit the discrete characteristics of driving maneuvers to initialize nominal behavior-oriented free-end homotopic trajectories based on reachability analysis, and each trajectory is locally constrained to a specific driving maneuver while sharing the same task objectives. By leveraging the bi-convexity of the safety module and the kinematics of the EV, we formulate the BHPTO as a bi-convex optimization problem. Then constraint transcription and over-relaxed ADMM are employed to streamline the optimization process, such that multiple trajectories are generated in real time with feasibility guarantees. Through a series of experiments, the proposed development demonstrates improved task accuracy, stability, and consistency in various traffic scenarios using synthetic and real-world traffic datasets.

What does Barrier-Enhanced Homotopic Parallel Trajectory Optimization (BPHTO) do?

The primary objective of the BPHTO is to seamlessly integrate discrete decision-making maneuvers with continuous trajectory variables for safety-critical autonomous driving.  The algorithm operates in real-time, optimizing trajectories of autonomous vehicles to ensure safety, stability, and proactive interaction with uncertain human-driven vehicles across various driving tasks, utilizing over-relaxed ADMM iterations. We provide a comprehensive theoretical analysis of safety robustness, demonstrating the asymptotic convergence of the EV from an unsafe state to a safe state in the sense of Lyapunov stability.

Results

We demonstrate our method in different scenarios (see below)

Implementation: Developed in C++ and Robot Operating System 2, running on an Ubuntu 22.04 system with an AMD Ryzen 5 5600G CPU. 

Safe Navigation in Cluttered Static Environments 

Navigating the EV safely while maintaining a desired speed in static conditions with dense obstacles. The number of driving lanes is set to 5 with a width of 3.75  m. The target cruise velocity is 15 m/s.   

Adaptive Cruise in Dynamic Traffic 

This task requires the EV to safely cruise at an average speed of  15 m/s to navigate through dense traffic, where HVs exhibit multi-modal behaviors.

NGSIM Dataset

Constrained High-speed Racing Scenario 

The EV is tasked with accelerating to a high speed and demonstrating rapid decision-making and adaptability under dynamic traffic scenarios. The key objective is to evaluate the EV's capabilities concerning driving stability and efficiency within a limited time frame. The desired motion is confined to the bottom three driving lanes. The initial and target racing speeds are 15 m/s and 22 m/s, respectively..

Real-time Performance Tests

Authors

Lei Zheng           Rui Yang        Michael Yu Wang     Jun  Ma              

Lei Zheng, Rui Yang, and Jun  Ma are with The Hong Kong University of Science and Technology, China.

Michael Yu Wang is with the School of Engineering, Great Bay University, Dongguan, China.