Optimal Trajectory Planning and Robust Tracking Using Vehicle Model Inversion