BPHTO

Barrier-Enhanced Parallel Homotopic Trajectory Optimization

for Safety-Critical Autonomous Driving

IEEE T-ITS 2024

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 parallel homotopic trajectory optimization (BPHTO) approach with the 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 BPHTO as a bi-convex optimization problem. Then constraint transcription and the 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 Parallel Homotopic 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 the over-relaxed ADMM iterations. We provide a comprehensive theoretical analysis of safety and computational efficiency.

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.  Visualization is performed using Python and RVIZ in ROS2.

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 in terms of 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, and 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.