1. A method for controlling a vehicle, comprising: determining iteratively a motion of the vehicle from an initial location and a target location, wherein the motion is defined by state transitions connecting states of the vehicle, each state includes a location, a velocity and a heading of the vehicle, wherein the motion is determined iteratively until a termination condition is met, wherein an iteration comprises: determining a location between the initial location and the target location, wherein the location satisfies spatial constraints on locations of the vehicle; determining state transitions of the vehicle moved to the location from a set of neighboring locations determined during previous iterations, wherein each state transition satisfies constraints on a motion of the vehicle; selecting a neighboring location resulting in an optimal state transition of the vehicle from the state at the neighboring location to the state at the location; and updating a graph of state transitions of the vehicle determined during previous iterations with the optimal state transition; determining the motion of the vehicle as a sequence of state transitions connecting the initial location with the target location; and controlling the vehicle according to the determined motion, wherein steps of the method are performed by a processor of the vehicle.