  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.
