1. A method for controlling a vehicle, comprising: selecting from a memory a first model of motion of vehicle, and a second model of the motion of the vehicle, wherein an order of the second model is higher that an order of the first model, wherein the order of a model is a number of state variables in the model; selecting from the memory a first constraint on the first model of motion of the vehicle for moving on a desired trajectory of the vehicle, and selecting a control invariant set joining states of the first model with states of the second model, wherein for each combination of the states within the control invariant set there is at least one control action to the second model that maintains the state of the second model within the control invariant set for every modification of the state of the first model satisfying the first constraint; determining, using the first model, a portion of the desired trajectory such that the first constraint is satisfied; determining, using the second model, a sequence of commands for moving the vehicle along the portion of the desired trajectory, such that the sequence of commands maintain a sequence of the states of the second model and a sequence of the states of the first model determined by the portion of the desired trajectory within the control invariant set; and controlling the vehicle using at least one command from the sequence of commands, wherein the steps of the method are performed using a processor operatively connected to the memory.