Overtaking decision and trajectory planning in highway via hierarchical architecture of conditional state machine and chance constrained model predictive control

Abstract

An overtaking trajectory planning algorithm is an essential part of autonomous vehicles, but maximizing trip efficiency (minimum travel time) while guaranteeing safety is non-trivial. In particular, to achieve optimal trajectory results in all situations using one algorithm is challenging because overtaking is a complex maneuver in which several behaviors are combined. In this paper, an overtaking algorithm that employs a finite state machine as a high-level decision maker and chance constrained model predictive control as a trajectory planner is proposed to optimize trip efficiency and ride comfort while guaranteeing safety. By combining two methods in a hierarchical structure, the proposed algorithm takes advantage of each method to realize optimality and real-time performance. Using the conditional state machine (CSM), algorithm classifies maneuver states that can ensure safety, and sets the optimal multi-vehicle constraints in each state. The chance constrained model predictive control (MPC) plans an optimal trajectory that considers the prediction uncertainty, safety, trip efficiency and ride comfort. To rigorously evaluate both trip efficiency and safety, the performance of the proposed overtaking algorithm is evaluated in a statistical manner for various level of service (LOS) scenarios. Simulation results show that the optimal trajectory was generated in a multi-vehicle situation while ensuring higher safety than the rule-based algorithm.