Hierarchical Model Predictive Control for Trajectory Generation and Tracking in Highly Automated Vehicles