The results show that this algorithm can control vehicle to follow the pre-given trajectory accurately, and good robustness and adaptability are also shown.