Detection of topological structure from sensor data with application to autonomous driving in semi-structured environments

• US 8,384,776 B2
• Filed: 04/22/2009
• Issued: 02/26/2013
• Est. Priority Date: 04/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): Perungavoor, Sath V
• Assistant Examiner(s): LUO, KATE H
• Application Number:
  US12/428,222
• Publication Number:
  US 20100274430A1
• Time in Patent Office: 1,406 Days
• Field of Search: None
• US Class Current: 348/116
• CPC Class Codes: G01C 21/20 Instruments for performing navigational calculations G01C21/24, G01C21/26 take precedence

Abstract

A method of creating an obstacle-free diagram using topological sensor data to form a graph corresponding to a driving path, transforming the graph using discrete heuristics, locally smoothing a plurality of edges of the graph after the transforming, and globally smoothing the graph after the locally smoothing. Transforming includes deleting an edge of the graph, merging two intersections of the graph into a single intersection, and collapsing a plurality of edges forming a loop into a single edge. Locally smoothing includes smoothing a lane segment of the graph by minimizing a sum of a function of smoothness and a function of distance. Globally smoothing includes defining an intersection potential and minimizing a sum of the function of smoothness, the function of distance, a function of intersections and a function of direction.

Claims

1. A method, comprising:

creating an obstacle-free diagram using sensor data gathered from a topological sensor configured to detect obstacles within a surrounding environment;

forming a graph from the obstacle-free diagram which corresponds to a driving path of the surrounding environment and recording the graph onto electronic memory of a computing device;

transforming the graph using a processor of the computing device to apply discrete heuristic transformations to the graph;

locally smoothing a plurality of edges of the graph using the processor after the transforming;

globally smoothing the graph using the processor after the locally smoothing;

creating a lane network corresponding to the graph after the globally smoothing by using a processor to create two lanes substantially equidistant and parallel to the graph; and

displaying the lane network using a display device.

2. The method recited in claim 1, wherein the discrete heuristic transformations include deleting an edge of the graph, merging two intersections of the graph into a single intersection, and collapsing a plurality of edges forming a loop into a single edge.

3. The method recited in claim 1, wherein the locally smoothing includes smoothing a lane segment of the graph by minimizing a sum of a function of smoothness and a function of distance.

4. The method recited in claim 3, wherein the graph is defined as G=V, E having vertices V and edges E, where V={x_i}={x_i, y_i} and lane segments Sε

V are a collection of connected nodes between two intersections of the graph, and the function of smoothness is defined as

5. The method recited in claim 3, wherein the graph is defined as G=V, E having vertices V and edges E, where V={x_i}={x_i, y_i}, o_i is a location of an obstacle in an obstacle map 0 which is closest to a location x, on the graph, d_max is a predetermined maximum road width, and the function of distance is defined as

6. The method recited in claim 3, wherein the globally smoothing includes defining an intersection potential and minimizing a sum of the function of smoothness, the function of distance, a function of intersections and a function of direction.

7. The method recited in claim 1, wherein the obstacle-free diagram is a generalized Voronoi diagram.

8. A method, comprising:

creating an obstacle-free diagram using sensor data gathered from a topological sensor configured to detect obstacles within a surrounding environment;

forming a graph from the obstacle-free diagram which corresponds to a driving path of the surrounding environment and recording the graph onto electronic memory of a computing device;

transforming the graph using a processor of the computing device to apply discrete heuristic transformations to the graph;

locally smoothing a plurality of edges of the graph using the processor after the transforming; and

globally smoothing the graph using the processor after the locally smoothing, wherein

the locally smoothing includes smoothing a lane segment of the graph by minimizing a sum of a function of smoothness and a function of distance, and

the globally smoothing includes defining an intersection potential and minimizing a sum of the function of smoothness, the function of distance, a function of intersections and a function of direction.

9. The method recited in claim 8, wherein the graph has intersections I and neighbor sets N(i), where iε

I, kε

N(i), Δ

_ij=x_i−

x_j is a vector between adjacent nodes i and j, and the function of intersections is defined as

10. The method recited in claim 8, wherein the graph is defined as G=V, E having vertices V and edges E, where V=[x_i]={x_i, y_i} and lane segments Sε

V are a collection of connected nodes between two intersections of the graph, o, is a location of an obstacle in an obstacle map 0 which is closest to a location x, on the graph,

11. The method recited in claim 8, further comprising:

creating a lane network corresponding to the graph after the globally smoothing by using a processor to create two lanes substantially equidistant and parallel to the graph; and

displaying the lane network using a display device.

12. A method, comprising:

creating a graph corresponding to a driving path of an environment using an obstacle-free diagram corresponding a topology of the environment;

transforming the graph using a processor of a computing device to apply discrete heuristic transformations to the graph;

locally smoothing a plurality of edges of the graph using the processor after the transforming;

globally smoothing the graph using the processor after the locally smoothing;

creating a lane network corresponding to the graph after the globally smoothing by using a processor to create two lanes substantially equidistant and parallel to the graph; and

displaying the lane network using a display device.

13. The method recited in claim 12, wherein the discrete heuristic transformations include deleting an edge of the graph, merging two intersections of the graph into a single intersection, and collapsing a plurality of edges forming a loop into a single edge.

14. The method recited in claim 12, wherein the locally smoothing includes smoothing a lane segment of the graph by minimizing a sum of a function of smoothness and a function of distance.

15. The method recited in claim 14, wherein the globally smoothing includes defining an intersection potential and minimizing a sum of the function of smoothness, the function of distance, a function of intersections and a function of direction.

16. The method recited in claim 12, wherein the obstacle-free diagram is a generalized Voronoi diagram.

17. A method, comprising:

creating a graph corresponding to a driving path of an environment using an obstacle-free diagram corresponding a topology of the environment;

transforming the graph using a processor of a computing device to apply discrete heuristic transformations to the graph;

locally smoothing a plurality of edges of the graph using the processor after the transforming; and

globally smoothing the graph using the processor after the locally smoothing, wherein

the locally smoothing includes smoothing a lane segment of the graph by minimizing a sum of a function of smoothness and a function of distance, and

the globally smoothing includes defining an intersection potential and minimizing a sum of the function of smoothness, the function of distance, a function of intersections and a function of direction.

18. The method recited in claim 17, further comprising:

creating a lane network corresponding to the graph after the globally smoothing by using a processor to create two lanes substantially equidistant and parallel to the graph; and

displaying the lane network using a display device.