SAFE TRAJECTORY TRACKING IN UNCERTAIN ENVIRONMENTS

The present disclosure relates to a method for trajectory planning for a vehicle. The method comprises obtaining a reference trajectory over a finite time horizon, where the reference trajectory comprises a speed reference over time for the finite time horizon. Further, the method comprises 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 comprises 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. Moreover, the method comprises generating a nominal trajectory for at least a portion of the finite time horizon based on a constraint controlled technique, where the nominal trajectory is dependent on the obtained reference trajectory and a terminal constraint. The terminal constraint defines that the nominal trajectory comprises the terminal state. The constraint controlled technique comprises a cost minimizing control strategy, and the back-up stop trajectory from the starting state to the final state is associated with zero cost.