A method determines iteratively a motion of the vehicle from an initial location and a target location. An iteration of the method determines a location between the initial location and the target location that satisfies spatial constraints on locations of the vehicle and determines state transitions of the vehicle moved to the location from a set of neighboring locations determined during previous iterations. The method selects a neighboring location resulting in an optimal state transition of the vehicle and updates a graph of state transitions of the vehicle determined during previous iterations with the optimal state transition. The motion of the vehicle is determined a sequence of state transitions connecting the initial location with the target location and the vehicle is controlled according to the determined motion.