Voronoi-based trajectory optimization for UGV path planning
rapidly - exploring random trees : a new tool for path planning .