Safe trajectory tracking in uncertain environments

A computer-readable storage medium storing one or more programs configured to be executed by one or more processors of a vehicle control system, the one or more programs comprising instructions for performing the method for trajectory planning for a vehicle, the method comprising:

• obtaining a reference trajectory over a finite time horizon, the reference trajectory comprising a speed reference over time for the finite time horizon;
• determining a back-up stop trajectory within the finite time horizon, the back-up stop trajectory having a starting state and terminating in a final state, the final state being defined as a safe state;
• forming a terminal set of states within the finite time horizon based on at least one predefined constraint, wherein the terminal set of states comprises a terminal state that corresponds to the starting state for the back-up stop trajectory;
• generating a nominal trajectory for at least a portion of the finite time horizon based on a constraint controlled technique, the nominal trajectory being dependent on the obtained reference trajectory and a terminal constraint, wherein the terminal constraint defines that the nominal trajectory comprises the terminal state;
• and
• wherein the constraint controlled technique comprises a cost minimizing control strategy, and wherein the back-up stop trajectory from the starting state to the final state is associated with zero cost.

A control device for trajectory planning for a vehicle, the control device comprising control circuitry configured to:

• obtain a reference trajectory over a finite time horizon, the reference trajectory comprising a speed reference over time for the finite time horizon;
• determine a back-up stop trajectory within the finite time horizon, the back-up stop trajectory having a starting state and a final state, the final state being defined as a safe state;
• form a terminal set of states within the finite time horizon based on at least one predefined constraint, wherein the terminal set of states comprises a terminal state that corresponds to the starting state for the back-up stop trajectory;
• generate a nominal trajectory for at least a portion of the finite time horizon based on a constraint controlled technique, the nominal trajectory being dependent on the obtained reference trajectory and a terminal constraint, wherein the terminal constraint defines that the nominal trajectory comprises the terminal state;
• and
• wherein the constraint controlled technique comprises a cost minimizing control strategy, and wherein the back-up stop trajectory from the starting state to the final state is associated with zero cost.