A control system of a vehicle, comprising: a memory to store data including driving mapped data and vehicle data; a hardware processor configured to produce a sequence of intermediate goals (in the following abbreviated as "SIG") from a travel route formed by a sequence of roads and turns for each prediction horizon, using the driving mapped data and the vehicle data, wherein the SIG includes specific goals which are specific turns at intersections that are specific in order to reach a final destination, additional specific goals that includes vehicle operational constraints according to driving rules, and optional goals; test a feasibility of each intermediate goal, using a first model of motion of the vehicle and a first model of motion of the traffic, based on a vehicle state and traffic conditions at a beginning of the prediction horizon, and that the intermediate goal is achieved by satisfying the traffic conditions and motion capabilities of the vehicle, after achieving the intermediate goal, a next goal among the specific goals and the additional specific goals can also be achieved; compute a trajectory for each feasible intermediate goal, 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 determines the satisfaction of constraints on the movement of the vehicle and the vehicle interaction with the road and the traffic; a vehicle controller to compute a control input to at least one actuator of the vehicle so that the vehicle motion produces a trajectory as close to the computed trajectory; and at least one actuator controller to execute the control input to the at least one actuator of the vehicle to modify the motion of the vehicle. a memory to store data including driving mapped data and vehicle data; a hardware processor configured to produce a sequence of intermediate goals (in the following abbreviated as "SIG") from a travel route formed by a sequence of roads and turns for each prediction horizon, using the driving mapped data and the vehicle data, wherein the SIG includes specific goals which are specific turns at intersections that are specific in order to reach a final destination, additional specific goals that includes vehicle operational constraints according to driving rules, and optional goals; test a feasibility of each intermediate goal, using a first model of motion of the vehicle and a first model of motion of the traffic, based on a vehicle state and traffic conditions at a beginning of the prediction horizon, and that the intermediate goal is achieved by satisfying the traffic conditions and motion capabilities of the vehicle, after achieving the intermediate goal, a next goal among the specific goals and the additional specific goals can also be achieved; compute a trajectory for each feasible intermediate goal, 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 determines the satisfaction of constraints on the movement of the vehicle and the vehicle interaction with the road and the traffic; test a feasibility of each intermediate goal, using a first model of motion of the vehicle and a first model of motion of the traffic, based on a vehicle state and traffic conditions at a beginning of the prediction horizon, and that the intermediate goal is achieved by satisfying the traffic conditions and motion capabilities of the vehicle, after achieving the intermediate goal, a next goal among the specific goals and the additional specific goals can also be achieved; compute a trajectory for each feasible intermediate goal, 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 determines the satisfaction of constraints on the movement of the vehicle and the vehicle interaction with the road and the traffic; a vehicle controller to compute a control input to at least one actuator of the vehicle so that the vehicle motion produces a trajectory as close to the computed trajectory; and at least one actuator controller to execute the control input to the at least one actuator of the vehicle to modify the motion of the vehicle.