The coordination of highly automated vehicles (or agents) in road intersections is an inherently nonconvex and challenging problem. In this paper, we propose a distributed motion planning scheme under reasonable vehicle-to-vehicle communication requirements. Each agent solves a nonlinear model predictive control problem in real time and transmits its planned trajectory to other agents, which may have conflicting objectives. The problem formulation is augmented with conditional constraints that enable the agents to decide whether to wait at a stopping line, if safe crossing is not possible. The involved nonconvex problems are solved very efficiently using the proximal averaged Newton method for optimal control (PANOC). We demonstrate the efficiency of the proposed approach in a realistic intersection crossing scenario.
Download