Vehicle automated parking system and method

Patent metadata

Field	Value
Publication number	US-9969386-B1
Application number	US-201715402353-A
Country	US
Kind code	B1
Filing date	Jan 10, 2017
Priority date	Jan 10, 2017
Publication date	May 15, 2018
Grant date	May 15, 2018

Abstract

A method and a system for an automated parking determines, 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 and determines, using a kinematic model of the vehicle, a set of kinematic subgraphs forming a kinematic graph having multiple nodes connected with kinematic edges. Each waypoint defines a position and orientation of the vehicle, 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. A kinematic path is selected form the kinematic graph and a reference trajectory tracking the kinematic path as a function of time is determined using a dynamic model of the vehicle. The motion of the vehicle is according to the reference trajectory.

First claim

The invention claimed is:

1. 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;
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;
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.

2. The method of claim 1 , further comprising:
growing the kinematic graph iteratively from a set of seeds corresponding to the set of waypoints.

3. The method of claim 1 , further comprising:
determining, using at least one sensor of the vehicle, the geometry of at least part of the parking space; and
selecting the geometry of the vehicle from a memory, wherein the sensor and the memory are operatively connected to the processor.

4. The method of claim 1 , wherein the determining the geometric path comprises:
building a geometric graph connecting the initial node with the target node through a set of collision free nodes, each pair of nodes in the geometric graph is connected with a collision free edge determined using only geometry of the vehicle and the parking space; and
selecting a set of nodes from the geometric graph forming the set of waypoints of the geometric path.

5. The method of claim 4 , further comprising:
sampling a point in the state space of the parking lot to produce a sampled state;
rejecting the sampled state if all states of the nodes of the geometrical graph are within a non-reachable area of the sampled state; and otherwise determining a nearest node of the geometric graph having a state nearest to the sampled state;
adding a node for the sampled state to the geometric graph and connecting the added node with the nearest node via an edge if the edge is collision free; and
repeating the sampling, the rejecting, the determining, and the adding until the initial node is connected to the target node.

6. The method of claim 5 , wherein the geometric graph includes an initial tree starting from the initial node toward the target node and a target tree starting from the target node toward the initial nodes, and wherein the nearest node includes one or combination of a first nearest node from the initial tree and a second nearest node from the target tree.

7. The method of claim 4 , further comprising:
determining for the initial node and each non-leaf node of the geometric graph a minimal cost of reaching the target node; and
selecting a set of nodes connecting the initial node with the target node with the minimal cost determined for the initial node to form the set of waypoints of the geometric path.

8. The method of claim 7 , further comprising:
removing the leaf nodes except the initial node and the target node from the geometric graph; and
removing the edges connecting the removed leaf nodes from the geometric graph.

9. The method of claim 1 , wherein the determining the kinematic graph comprises:
sampling the parking space in proximity to the set of waypoints to produce a sampled state;
connecting the sampled state with at least one tree starting from at least one waypoint using the kinematic edge to update the tree if the kinematic edge is collision free; and
repeating the sampling and the connecting until a termination condition is met, wherein the termination condition includes one or combination of the formation of the connected geometric graph, a number of paths connecting the initial node and the target node on the kinematic graph is above a threshold, a number of nodes connecting trees of neighboring waypoints.

10. The method of claim 9 , wherein the sampling is performed biasedly using a probability distribution of states of the parking space having a mean and a covariance as a function of the states of the waypoints.

11. The method of claim 9 , further comprising:
selecting two adjacent waypoints X i , X i+1 from the set of waypoints X 0 , . . . , X M ;
forming a subgraph SG i+1 which connects a subgraph SG i to the waypoint X i+1 through a set of collision-free nodes and kinematic edges, wherein SG i is the subgraph determined during a previous iteration for two adjacent waypoints X i−1 , X 1 ; and
repeating the selecting and the forming for 0≤i≤M−1.

12. The method of claim 11 , further comprising
initializing the subgraphs using an initial tree and a target tree wherein the start tree includes the subgraph SG i including nodes corresponding to geometric waypoints {X 0 , . . . , X i }, and the target tree includes the node corresponding to the waypoint X i+1 ;
sampling biasedly a new state X from the state space of the vehicle kinematic model in proximity to waypoints X i , X i+1 ;
adding the new state X to at least one of the initial tree and the target tree if the vehicle can follow a collision-free kinematic path from the new state to a state in the initial tree and the target tree;
repeating the sampling and adding until the new state X is added to both the initial tree and the target tree.

13. The method of claim 12 , further comprising
constructing a probability distribution function P(X|X i , X i+1 ) according to the waypoints X i , X i+1 and the geometry of the vehicle and the parking space;
sampling the new state X from the state space according to the probability distribution function P(X|X i , X i+1 ); and
repeating the sampling until the sampled state is collision-free.

14. The method of claim 1 , further comprising
determining optimal cost of each node of the kinematic graph, wherein the optimal cost of a node represents a best estimated cost of moving the vehicle from the node to the target node through edges of the kinematic graph; and
determining by policy iteration the initial kinematic path represented by a set of kinematic waypoints and associated edges connecting the initial state with the target state.

15. The method of claim 14 , further comprising:
initializing the initial kinematic path P 0 to include a current state X c as an initial state X 0 of the first kinematic waypoint;
determining a next state Xn of the current state such that an edge E(Xc, Xn) between the next state and the current state produce the minimum cost from the current state to the target state;
adding the next state Xn and the edge E(Xc,Xn) to the initial kinematic path P0={P0,Xn,E(Xc,Xn)}; and
repeating the determinin

Assignees

• Mitsubishi Electric Res Laboratories Inc

Inventors

• Wang Yebin
• Jha Devesh

Classifications

• B62D15/027

  Parking aids, e.g. instruction means · CPC title

• B60W2554/00

  Input parameters relating to objects · CPC title

• G06N5/01

  Dynamic search techniques; Heuristics; Dynamic trees; Branch-and-bound · CPC title

• B60W30/06 Primary

  Automatic manoeuvring for parking · CPC title

• B60W2510/207

  Oversteer or understeer · CPC title