WE CLAIM:
[Claim 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 subset 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 the 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 subset; 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.

[Claim 2]
The method of claim 1, further comprising:
determining iteratively the control invariant subset using a backward-reachable set computation starting from a feasible set, wherein each iteration comprises:
determining the backward-reachable set, such that for each state within the backward-reachable set there is at least one control action maintaining the state of the second model within the feasible set for every modification of the state of the first model satisfying the first constraint; and
replacing the feasible set with the backward-reachable set, wherein the iterations are performed until a termination condition is met. [Claim 3]
The method of claim 2, wherein the termination condition specifies that a difference between the backward-reachable set and the feasible set is below a threshold. [Claim 4]
The method of claim 1, further comprising:
determining an admissible set of the states of the first model feasible in a future according to constraints on the first model including the first constraint;
determining iteratively the control invariant set starting from initial values of a first set of feasible states of the first model and feasible states of the second model and a second set formed by an the intersection of the first set with the admissible set, wherein a current iteration comprises:

constructing a third set by backward reachable computation of the first set while considering only the first constraint;
updating the first set to an intersection of the first set and the third set;
updating the second set to an intersection of the updated first set and the admissible set; and
determining the control invariant set as the updated second set if a difference between the updated second set and the second set updated during a previous iteration is less than a threshold. [Claim 5]
The method of claim 1, further comprising:
selecting a feasible command satisfying physical constraints on the motion of the vehicle;
estimating a transition of the vehicle from a current state to a future state according to the command; and
selecting the feasible command as the command for controlling the vehicle if a combination of the future state of the vehicle and the first element from the sequence of the states of the first model belongs to the control invariant set; and otherwise
selecting a different feasible command and repeating the estimating and the selecting steps. [Claim 6]
The method of claim 5, wherein the different feasible command is selected to reduce a distance between the future state determined by the different feasible command with the boarder of the control invariant set. [Claim 7]
The method of claim 1, further comprising:

formulating an optimization problem optimizing a performance of the vehicle subject to constraints including a combination of the future state of the vehicle and the first element from the sequence of the states of the first model belonging to the control invariant set; and
selecting the sequence of commands for moving the vehicle by solving the optimization problem. [Claim 8]
The method of claim 7, wherein the performance of the vehicle is one or combination of reducing lateral acceleration of the vehicle, reducing yaw rate of the vehicle, reducing lateral displacement from the desired trajectory, and reducing steering wheel actuation power. [Claim 9]
The method of claim 1, wherein the first constraint does not limit one of state variables of the first model. [Claim 10]
The method of claim 1, wherein the first constraint determines transitions between the states of the first model. [Claim 11]
The method of claim 1, wherein the first constraint includes one or combination of a constraint on a change of a curvature of the desired trajectory, and a constraint on a change of a yaw rate of transitioning the first model along the desired trajectory. [Claim 12]
The method of claim 1, further comprising:
determining the largest value of the first constraint allowing non¬empty control invariant set.

[Claim 13]
The method of claim 12, further comprising:
reducing the value of the first constraint while increasing the size of the control invariant set. [Claim 14]
A controller for controlling a vehicle, comprising:
a memory to store a first model of motion of vehicle, to store 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, to store a first constraint on the first model for moving along the desired trajectory of the vehicle, and to store 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 subset 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;
a supervisory controller to determine using the first model a portion of the desired trajectory satisfying the first constraint;
a vehicle controller to determine, 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 the 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 subset; and

an actuator controller to control the vehicle using at least one command from the sequence of commands. [Claim 15]
The controller of claim 14, wherein the vehicle controller is configured for
selecting a feasible command satisfying physical constraints on the motion of the vehicle;
estimating a transition of the vehicle from a current state to a future state according to the command; and
selecting the feasible command as the command for controlling the vehicle if a combination of the future state of the vehicle and the first element from the sequence of the states of the first model belongs to the control invariant set; and otherwise
selecting a different feasible command and repeating the estimating and the selecting steps. [Claim 16]
The controller of claim 15, wherein the different feasible command is selected to reduce a distance between the future state determined by the different feasible command with the boarder of the control invariant set. [Claim 17]
The controller of claim 14, wherein the vehicle controller is configured for
formulating an optimization problem optimizing a performance of the vehicle subject to constraints including a combination of the future state of the vehicle and the first element from the sequence of the states of the first model belonging to the control invariant set; and

selecting the sequence of commands for moving the vehicle by solving the optimization problem. [Claim 18]
The controller of claim 17, wherein the performance of the vehicle is one or combination of reducing lateral acceleration of the vehicle, reducing yaw rate of the vehicle, reducing lateral displacement from the desired trajectory, and reducing steering wheel actuation power. [Claim 19]
The controller of claim 14, wherein the first constraint includes one or combination of a constraint on a change of a curvature of the desired trajectory, and a constraint on a change of a yaw rate of transitioning the first model along the desired trajectory. [Claim 20]
A non-transitory computer readable memory embodied thereon a program executable by a processor for performing a method, the method comprising:
selecting from the 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 for moving exactly on the 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 subset 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 satisfying the first constraint;
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 the 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 subset; and
controlling the vehicle using at least one command from the sequence of commands.