Using topological structure for path planning in semi-structured environments

• US 8,392,117 B2
• Filed: 05/22/2009
• Issued: 03/05/2013
• Est. Priority Date: 05/22/2009
• Status: Active Grant

• Current Assignee: Toyota Motor Corporation
• Original Assignee: Toyota Motor Engineering & Manufacturing North America Incorporated (Toyota Motor Corporation)
• Inventors: Dolgov, Dmitri A., Thrun, Sebastian
• Primary Examiner(s): Zanelli, Michael J.
• Application Number:
  US12/471,079
• Publication Number:
  US 20100299013A1
• Time in Patent Office: 1,383 Days
• Field of Search: None
• US Class Current: 701/533
• CPC Class Codes: G05D 1/0274 using mapping information stored in a memory device navigation using map-matching G01C21/30

Abstract

A method of creating a lane network of a semi-structured environment for a vehicle, assigning a corresponding cost function to each of a plurality of coordinates of the semi-structured environment using the lane network and a state of the vehicle, the state of the vehicle corresponding to a coordinate location of the vehicle and an angular orientation of the vehicle, and determining an obstacle-free path on the semi-structured environment using the cost function of each of the plurality of coordinates.

Claims

1. A method, comprising:

creating a lane network of an environment for a vehicle using a processor of a computing device and storing the lane network in electronic memory of the computing device;

calculating a cost function for each of a plurality of coordinates of the environment using the processor from the lane network, a goal position of the vehicle and a state of the vehicle, the state of the vehicle including a location and an angular orientation of the vehicle; and

determining an obstacle-free path for the vehicle from the location to the goal position by minimizing a calculated cost of the obstacle-free path, the calculated cost being a sum of the cost functions of the coordinates that the obstacle-free path intersects.

2. The method recited in claim 1, wherein the calculating includes calculating a first cost function for each of the plurality of coordinates based on a distance between the each of the plurality of coordinates and the goal position, and then modifying the first cost function with a modifying function corresponding to the lane network and the state of the vehicle.

3. The method recited in claim 2, wherein the calculated cost of the obstacle-free path is defined as

4. The method recited in claim 3, wherein the minimizing the calculated cost

5. The method recited in claim 1, wherein

the determining includes determining a plurality of obstacle-free paths between the location of the vehicle and the goal position,

each of the plurality of obstacle-free paths has a calculated cost, and

an obstacle-free path of the plurality of obstacle-free paths having the lowest calculated cost is recorded onto the electronic memory.

6. The method recited in claim 1, wherein the determining includes smoothing the obstacle-free path.

7. The method recited in claim 1, wherein the environment is a semi-structured environment.

8. A method of planning a path for a vehicle, comprising:

accessing a lane network of an environment stored in electronic memory of a computing device, and determining a location and a goal location of the vehicle within the environment using a processor of the computing device;

calculating a cost function for a plurality of coordinates of the environment between the location and the goal location using the processor, the cost function based on the lane network, the goal location and a state of the vehicle, the state of the vehicle including the location and an angular orientation of the vehicle; and

determining an obstacle-free path of the environment by minimizing a sum of the cost functions corresponding to the coordinates which intersect the obstacle-free path between the location and the goal location using the processor and recording the determined obstacle-free path onto the electronic memory.

9. A computer readable medium including computer executable instructions, wherein the instructions, when executed by a processor of a computing device, cause the processor to perform a method comprising:

creating a lane network of an environment for a vehicle using the processor of the computing device and storing the lane network in electronic memory of the computing device;

calculating a cost function for each of a plurality of coordinates of the environment using the processor from the lane network, a goal position of the vehicle and a state of the vehicle, the state of the vehicle including a location and an angular orientation of the vehicle; and

determining an obstacle-free path for the vehicle from the location to the goal position by minimizing a calculated cost of the obstacle-free path, the calculated cost being a sum of the cost functions of the coordinates that the obstacle-free path intersects.