SAFE TRAJECTORY TRACKING IN UNCERTAIN ENVIRONMENTS

The present disclosure relates to a method for trajectory planning for a vehicle. The method includes obtaining a reference trajectory over a finite time horizon, where the reference trajectory includes a speed reference over time for the finite time horizon. Further, the method includes determining a back-up stop trajectory within the finite time horizon. The back-up stop trajectory has a starting state and terminating in a final state, where the final state is defined as a safe state. The method further includes forming a terminal set of states within the finite time horizon based on at least one predefined constraint, wherein the terminal set of states includes a terminal state that corresponds to the starting state for the back-up stop trajectory. Moreover, the method includes generating a nominal trajectory for at least a portion of the finite time horizon based on a constraint controlled technique.