A control system for controlling a motion of a vehicle to a target driving goal in a routing selected according to a desired destination of the vehicle, the control system comprising: a memory configured to store a first model including one or combination of a first motion model of the vehicle and a first traffic model of motion of a traffic in proximity of the vehicle and a second model including a second motion model of the vehicle and a second traffic model of motion of the traffic, wherein the first model is an approximation of the second model; and at least one processor coupled with stored instructions implementing modules of the control system, the modules comprising: a decision-maker configured to determine a sequence of intermediate goals (501, 502, 503, 504) leading to the target driving goal by optimizing the motion of the vehicle subject to the first model and tightened driving constraints formed by tightening driving constraints by a safety margin, wherein the driving constraints include mixed logical inequalities of temporal logic formulae specified by traffic rules and the routing, wherein the mixed logical inequalities define an area where the temporal logic formulae are satisfied, wherein the tightened driving constraints shrink the area by the safety margin, and wherein the safety margin is a function of a difference between the second model and the first model; a motion planner configured to determine a motion trajectory of the vehicle tracking the sequence of intermediate goals by optimizing the motion of the vehicle subject to the second model; and a controller configured to generate and submit control commands to at least one actuator of the vehicle to follow the motion trajectory. a memory configured to store a first model including one or combination of a first motion model of the vehicle and a first traffic model of motion of a traffic in proximity of the vehicle and a second model including a second motion model of the vehicle and a second traffic model of motion of the traffic, wherein the first model is an approximation of the second model; and at least one processor coupled with stored instructions implementing modules of the control system, the modules comprising: a decision-maker configured to determine a sequence of intermediate goals (501, 502, 503, 504) leading to the target driving goal by optimizing the motion of the vehicle subject to the first model and tightened driving constraints formed by tightening driving constraints by a safety margin, wherein the driving constraints include mixed logical inequalities of temporal logic formulae specified by traffic rules and the routing, wherein the mixed logical inequalities define an area where the temporal logic formulae are satisfied, wherein the tightened driving constraints shrink the area by the safety margin, and wherein the safety margin is a function of a difference between the second model and the first model; a motion planner configured to determine a motion trajectory of the vehicle tracking the sequence of intermediate goals by optimizing the motion of the vehicle subject to the second model; and a controller configured to generate and submit control commands to at least one actuator of the vehicle to follow the motion trajectory. a decision-maker configured to determine a sequence of intermediate goals (501, 502, 503, 504) leading to the target driving goal by optimizing the motion of the vehicle subject to the first model and tightened driving constraints formed by tightening driving constraints by a safety margin, wherein the driving constraints include mixed logical inequalities of temporal logic formulae specified by traffic rules and the routing, wherein the mixed logical inequalities define an area where the temporal logic formulae are satisfied, wherein the tightened driving constraints shrink the area by the safety margin, and wherein the safety margin is a function of a difference between the second model and the first model; a motion planner configured to determine a motion trajectory of the vehicle tracking the sequence of intermediate goals by optimizing the motion of the vehicle subject to the second model; and a controller configured to generate and submit control commands to at least one actuator of the vehicle to follow the motion trajectory.