Robust model-based trajectory planning for nonlinear systems