Five-point smoothing algorithm trajectory optimization