Optimal trajectory planning and decision making in lane change maneuvers near a highway exit

This paper presents a control architecture for autonomous driving near a highway exit. In particular, detailed descriptions are provided for the two main blocks in the proposed architecture; a novel trajectory planner for lateral and longitudinal motion, and a novel decision manager for choosing the optimal target lane. We show that the trajectory planning can be realized by executing two sequential quadratic programs as two model predictive controllers, where one is planning the optimal trailing of a leading vehicle and the other is planning a lane change maneuver. Having the two optimal trajectories and a scalarized objective that considers safety, comfort, reference tracking and distance to the exit, the decision manager chooses one of the planed trajectories that is send to the vehicle. A case study is provided showing the effectiveness of the proposed control architecture.