Hierarchical MPC-based Motion Planning for Automated Vehicles in Parallel Autonomy

Automated vehicles with parallel autonomy show advantages over fully automated vehicles and manual driving. This paper proposes a hierarchical motion planning method that mixes inputs of human drivers and the automated driving systems for automated vehicles in scenarios such as multi-lane roads and multi-intersections with dynamic obstacles. The proposed method comprises a reference path generator in the upper level and a nonlinear model predictive controller with mixed human-vehicle control in the lower level. The path planner considers dynamic obstacles, static obstacles, and human comfort to generate a reference path composed of splines with continuous curvatures in the upper level. In the lower level, the MPC generates a trajectory by tracking the reference path and optimizing the cost function containing inputs of drivers while avoiding both dynamic and static obstacles. The simulation verifies the efficacy and the computational tractability of the proposed method.