In the past years a number of SAE level 2 driving automation systems have come available in commercial vehicles. An example is Volvo's Pilot Assist, which provides a more comfortable journey for the driver as certain parts of the dynamic driving task are taken away, but it still requires the driver to be attentive in order to supervise the system and resume vehicle operation if necessary. Car manufacturers and high-tech companies all around the world are working on the development of an Automated Driving System (ADS) of level 3 or higher where the driver does not need to supervise, but have so far not been able to introduce any to the public. Operating a vehicle on an empty highway is relatively easy, but roads are getting more and more congested so dense traffic driving needs to be accounted for as well.

Unsupervised driving does not only require that a system itself operates the vehicle correctly, but requires that it is able to cope with mistakes of other drivers. In order to take these into account as well as other unpredictable events, current driving automation systems tend to adopt large longitudinal margins with respect to surrounding traffic to use their ability to brake to avoid a collision. In dense traffic situations this conservative behaviour prohibits the ADS from making lane changes.

This thesis was setup in cooperation with Volvo Cars / Zenuity to develop a lane change path planning algorithm that can plan active lane changes in dense traffic, by pushing into a gap without compromising the ability to avoid a possible collision.

Earlier work at Volvo suggested considering the ability of a vehicle to not only brake, but also to make an evasive maneuver. If an evasion maneuver is available the necessary safety margins are reduced considerably compared to a `braking-only' scenario.

In this work, the evasive maneuver was modeled and a constraint formulation was developed that if met, guarantees the availability of the designed evasion maneuver. This constraint translates to a state-dependent safety zone which the ADS should keep the vehicle clear off at all times.

Model Predictive Control allows the implementation of such a safety zone by considering it as a constraint on the optimisation, guaranteeing that an evasion maneuver is available along any point on the prediction horizon. This negates the necessity to plan all possible evasion maneuvers separately, and it was therefore chosen as the path planning method.

Implementation of the safety zone constraint on multiple surrounding vehicles, spread over multiple lanes in a Model Predictive Control framework, together with the requirements of being able to overtake other vehicles within the prediction horizon, necessitated the development of a novel 3-step algorithm. In this algorithm three Optimal Control Problems are solved consecutively to plan a path which positions the ego-vehicle optimally in the targeted gap without breaching the safety zones.

To be able to implement the algorithm in real-time, a computationally expedient vehicle model is chosen and the OCP's are solved using state-of-the-art structure exploiting Interior Point solvers, generated as efficient C-code using FORCES Pro.

With this setup, computation times of $\sim$30[ms] are achieved for a 10[s] prediction horizon sampled at 100[ms]. The algorithm is demonstrated in simulation and shown to be able to find the point of maximum lateral intrusion, to operate on the edge of the safety zone constraint, and to avoid a collision when it is being cut off from behind or when the vehicle in front comes to a sudden stop.

The results show that vehicle operation by an ADS is possible in dense traffic without compromising safety. By using the developed safety zones and novelty solution framework the necessary margins can be reduced, increasing the availability of the lane change maneuver. Furthermore the control strategies necessary for implementation are shown to be applicable in real-time, paving the way for implementation in real vehicles. This way the research contributes to the next step in developing a usable Automated Driving System.