Methods of refining a planned trajectory of an autonomous vehicle are disclose. For multiple cycles as the vehicle moves along the trajectory, the vehicle will perceive nearby objects. The vehicle will use the perceived object data to calculate a set of candidate updated trajectories. The motion planning system will measure a discrepancy between each candidate updated trajectory and the current trajectory by: (i) determining waypoints along each trajectory; (ii) determining distances between at least some of the waypoints; and (iii) using the distances to measure the discrepancy between the updated trajectory and the current trajectory. The system will use the discrepancy to select, from the set of candidate updated trajectories, a final updated trajectory for the vehicle to follow.