  Overtaking is one of the most challenging tasks in driving, and the current
solutions to autonomous overtaking are limited to simple and static scenarios.
In this paper, we present a method for behaviour and trajectory planning for
safe autonomous overtaking. The proposed method optimizes the trajectory by
simultaneously enforcing safety and minimizing intrusion onto the adjacent
lane. Furthermore, the method allows the overtaking to be aborted, enabling the
autonomous vehicle to merge back in the lane, if safety is compromised, because
of e.g. traffic in opposing direction appearing during the maneuver execution.
A finite state machine is used to select an appropriate maneuver at each time,
and a combination of safe and reachable sets is used to iteratively generate
intermediate reference targets based on the current maneuver. A nonlinear model
predictive controller then plans dynamically feasible and collision-free
trajectories to these intermediate reference targets. Simulation experiments
demonstrate that the combination of intermediate reference generation and model
predictive control is able to handle multiple behaviors, including following a
lead vehicle, overtaking and aborting the overtake, within a single framework.
