A method for parking a vehicle within a parking space, wherein the method uses a processor coupled to a memory storing a geometry of the vehicle, a map of the parking space, a kinematic model of the vehicle, and a dynamic model of the vehicle, wherein the processor is coupled with stored instructions implementing the method, wherein the instructions, when executed by the processor carry out at least some steps of the method, comprising: determining, using the geometry of the vehicle and the map of the parking space, a collision free geometric path connecting an initial state of the vehicle with a target state of parked vehicle through a set of waypoints, each waypoint defines a position and orientation of the vehicle; and determining, using a kinematic model of the vehicle, a set of kinematic subgraphs forming a kinematic graph having multiple nodes connected with kinematic edges, each kinematic subgraph connects a pair of neighboring waypoints of the geometric path, each node defines a state of the vehicle, and each kinematic edge connecting two nodes defines a collision free kinematic path connecting the two nodes according to kinematics of the vehicle; characterized in that the method further comprises: selecting, from the kinematic graph, a kinematic path connecting an initial node corresponding to the initial state of the vehicle with a target node corresponding to the target state of the vehicle through a set of intermediate nodes; determining, using a dynamic model of the vehicle, a reference trajectory tracking the kinematic path as a function of time; and controlling the motion of the vehicle according to the reference trajectory. determining, using the geometry of the vehicle and the map of the parking space, a collision free geometric path connecting an initial state of the vehicle with a target state of parked vehicle through a set of waypoints, each waypoint defines a position and orientation of the vehicle; and determining, using a kinematic model of the vehicle, a set of kinematic subgraphs forming a kinematic graph having multiple nodes connected with kinematic edges, each kinematic subgraph connects a pair of neighboring waypoints of the geometric path, each node defines a state of the vehicle, and each kinematic edge connecting two nodes defines a collision free kinematic path connecting the two nodes according to kinematics of the vehicle; characterized in that the method further comprises: selecting, from the kinematic graph, a kinematic path connecting an initial node corresponding to the initial state of the vehicle with a target node corresponding to the target state of the vehicle through a set of intermediate nodes; determining, using a dynamic model of the vehicle, a reference trajectory tracking the kinematic path as a function of time; and controlling the motion of the vehicle according to the reference trajectory. selecting, from the kinematic graph, a kinematic path connecting an initial node corresponding to the initial state of the vehicle with a target node corresponding to the target state of the vehicle through a set of intermediate nodes; determining, using a dynamic model of the vehicle, a reference trajectory tracking the kinematic path as a function of time; and controlling the motion of the vehicle according to the reference trajectory. <EOS>