Optimal Decision Making 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.