According to one embodiment, an ADV is controlled according to a first trajectory planned during a first planning cycle. A control error is determined which represents a drifting error at a first location of the ADV at a first point in time at the end of the first planning cycle. A second point in time is selected on the first trajectory. A second trajectory is generated from a second location on the first trajectory corresponding o the second point in time as a starting location of the second trajectory for a second planning cycle as a next planning cycle. A segment of the first trajectory between the first point in time and the second point in time is combined with the second trajectory to generate a third trajectory for the second planning cycle. The ADV is driven and controlled according to the third trajectory corresponding to the second planning cycle.