



This paper proposes a sampling based planning algorithm to control autonomous vehicles. We propose an improved Rapidly-exploring Random Tree which includes the definition of K- nearest points and propose a two-stage sampling strategy to adjust RRT in other to perform maneuver while avoiding collision. The simulation results show the success of the algorithm.