Robustness improvement of trajectory planning algorithms