Hierarchical Model Predictive Control For Trajectory Generation And Tracking In Highly Automated Vehicles