Optimal Decision Making for Automated Vehicles Using Homotopy Generation and Nonlinear Model Predictive Control

To navigate complex driving scenarios, automated vehicles must be able to make decisions that reflect higher-level goals such as safety and efficiency, leveraging the vehicle's full capabilities if necessary. We introduce an architecture that is capable of handling combinatorial decision making and control with a high fidelity vehicle model. This is accomplished by solving a nonlinear model predictive control optimization for each maneuver variant, or homotopy, identified in the drivable space. These locally optimal solutions are then evaluated on a criterion that reflects high-level objectives. Experimental results on a full-scale vehicle demonstrate this architecture's effectiveness in an overtaking scenario with oncoming traffic that requires the ego vehicle to decide whether to pass before or after the oncoming traffic passes.