Controlling a vehicle by producing a sequence of intermediate goals that includes specific goals, additional specific goals and optional goals, from a travel route for a prediction horizon. Testing a feasibility of each intermediate goal (IG), using a first model of motion of the vehicle and a first model of motion of the traffic, by the IG being achieved by satisfying traffic conditions and motion capabilities of the vehicle, after achieving the IG, a next goal of the specific goals and the additional specific goals can also be achieved. Compute a trajectory for each feasible IG, using a second model of motion of the vehicle and a second model of motion of the traffic, and compare each computed trajectory according to a numerical value determined by a cost function and determine satisfaction of constraints on the movement of the vehicle and the vehicle interaction with the road and the traffic.