Heterogeneous Traffic Intersection Coordination Based on Distributed Model Predictive Control with Invariant Safety Guarantee

Abstract

This paper considers the heterogeneous traffic intersection where both Human Driven Vehicles (HDVs) and Connected and Automated Vehicles (CAVs) exist. In such a dynamic environment, CAVs must act in a way such that safety is guaranteed at all times, which is challenging due to the unpredictable nature of human behavior. To guarantee safety, in this paper we consider the worst-case behavior of HDVs by constructing the forward reachable set and ensuring collision avoidance against the forward reachable set within the CAV's planning horizon. To ensure safety at all times, a maximal invariant safe set is designed and used as a terminal constraint such that within this set there is always admissible control for CAVs to react against all possible future behavior of other vehicles safely. Finally, we propose to solve the intersection coordination problem within a Distributed Model Predictive Control (DMPC) framework where all pairwise safety constraints among CAVs are decoupled by prioritization. As a result, each CAVs solves a Mixed Integer Quadratic Programming (MIQP) problem considering collision avoidance with all CAVs of higher priority and with all HDVs. We give theoretical proof of the recursive feasibility of our proposed DMPC formulation and practical invariant safety guarantees. The resulting solution is evaluated in simulation and shows that our coordination framework can provide invariant safe coordination in a heterogeneous traffic intersection.