Abstract: Systems and method are provided for controlling a vehicle. In one embodiment, a method includes defining a region of interest and an intended path of the vehicle based on sensor data, and determining a set of predicted paths of one or more objects likely to intersect the region of interest within a planning horizon. The method further includes defining, within a spatiotemporal path space associated with the region of interest and the planning horizon, a set of obstacle regions corresponding to the set of predicted paths. Decision points for each of the obstacle regions are determined, and a directed graph is defined based on the plurality of decision points and a cost function applied to a set of path segments interconnecting the decision points. The directed graph is then searched to determine a selected path.