Vehicle automated parking system and method

A system for parking a vehicle within a parking space, comprising:

• 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;
• a sensor operatively for sensing information indicative of the geometry of at least part of the parking space;
• and
• a processor coupled to the memory and configured for
• 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;
• determining, using a kinematic model of the vehicle, a set of kinematic subgraphs grown from a set of seeds corresponding to the set of waypoints to form 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;
• 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;
• and
• determining, using a dynamic model of the vehicle, a reference trajectory tracking the kinematic path as a function of time;
• and
• a controller for controlling the motion of the vehicle according to the reference trajectory.

A non-transitory computer readable storage medium embodied thereon a program executable by a processor for performing a method, the method comprising:

• determining a geometric path connecting an initial state of the vehicle with the target state of the vehicle without collision, wherein the geometric path includes a set of waypoints, each waypoint is defined by a position and orientation of the vehicle, and wherein the geometric path is determined using only geometry of the vehicle and the parking space;
• determining a kinematic graph having multiple nodes connected with kinematic edges, each node defines a state of the vehicle and each kinematic edge connecting two nodes defines a collision free kinematic path between the two nodes, wherein the nodes of the kinematic graph is grown iteratively from a set of seeds corresponding to the set of waypoints, and wherein the kinematic edge connects two nodes according to kinematics of the vehicle;
• 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 a reference trajectory tracking the kinematic path, wherein the reference trajectory is a function of time;
• and
• controlling the motion of the vehicle according to the reference trajectory.