Systems and methods are provided for controlling a vehicle. In one embodiment, a method includes receiving vehicle and object environment data. A search graph is generated based upon the received data. The search graph contains a grid of points for locating objects and is used to determine a desired trajectory for the vehicle.