Lazier graph-based path planning for autonomous navigation

Patent metadata

Field	Value
Publication number	US-10054447-B2
Application number	US-201615239664-A
Country	US
Kind code	B2
Filing date	Aug 17, 2016
Priority date	Aug 17, 2016
Publication date	Aug 21, 2018
Grant date	Aug 21, 2018

Abstract

A navigation method comprises receiving data modeling an environment, finding an optimal path to a goal in the environment, and reporting the optimal path to a navigation system. An autonomous vehicle comprises a path-finding system configured to receive data modeling an environment to be traversed and to find an optimal path to a goal. The vehicle further comprises a navigation system configured to receive the optimal path and to formulate drive data for driving the vehicle along the optimal path. The vehicle further comprises a drive system configured to receive the drive data and to drive the vehicle along the optimal path. A navigation device comprises a path-finding system configured to receive data modeling an environment and to find an optimal path to a goal. The navigation device further comprises a display configured to receive the optimal path and to represent the optimal path in a human-readable format.

First claim

The invention claimed is:

1. A navigation method comprising:
receiving data modeling an environment to be traversed, the data representing one or more obstacles within the environment;
defining a navigation graph structure corresponding to the environment, wherein the navigation graph structure is a lattice, the navigation graph structure including a beginning node B and a goal node G;
including the beginning node B in an open-node set O;
selecting, from the open-node set O, a node S of lowest cost;
if S differs from the goal node G, then determining whether the navigation graph structure includes a node N neighboring S, where N is not already included in the open-node set O;
if the navigation graph structure includes the node N, then determining whether N is obstacle-free and determining whether an edge E connecting N and S is obstacle-free, and, if both N and E are obstacle-free, then inserting N into the navigation graph, generating and inserting E into the navigation graph, updating a cost of N, including N in the open-node set O, and returning to said determining whether the navigation graph structure includes a node N;
if the navigation graph structure includes no node N neighboring S, where N is not already included in O, then excluding S from the open-node set O, including S in a closed-node set C, and returning to said selecting a node S of lowest cost;
if S coincides with the goal node G, then finding an optimal path from B to G, including searching each node in C for a parent node in C; and
reporting the optimal path to a navigation system to guide traversal of the environment.

2. The method of claim 1 further comprising:
if both N and E are obstacle-free, then determining whether an edge E′ connecting N and an ancestor node of S is obstacle-free, and, if both N and E′ are obstacle-free, then generating E′, wherein updating the cost of N includes updating in view of E′.

3. The method of claim 2 wherein the edge E is generated when E is obstacle-free but E′ is not obstacle-free.

4. The method of claim 1 wherein defining the navigation graph structure includes, for each of a plurality of navigation nodes of the navigation graph structure, defining a position of that node.

5. The method of claim 4 wherein defining the navigation graph structure does not include, for any of the plurality of navigation nodes of the navigation graph structure, evaluating a cost of the node, determining whether the node intersects an obstacle, or constructing an edge between that and any other node of the navigation graph structure.

6. The method of claim 1 wherein the navigation graph structure is adaptable to represent a topographic map of the environment.

7. The method of claim 1 wherein no previously evaluated node is deleted responsive to occlusion by the one or more obstacles.

8. The method of claim 1 wherein no previously constructed edge is deleted responsive to intersection of the one or more obstacles.

9. An autonomous vehicle comprising:
a path-finding system including a processor and an associated nonvolatile memory, the nonvolatile memory holding instructions that cause the processor to:
receive data modeling an environment to be traversed, the data representing one or more obstacles within the environment,
define a navigation graph structure corresponding to the environment, wherein the navigation graph structure is a lattice, the navigation graph structure including a beginning node B and a goal node G,
including the beginning node B in an open-node set O,
select, from the open-node set O, a node S of lowest cost,
if S differs from the goal node G, then determine whether the navigation graph structure includes a node N neighboring S, where N is not already included in the open-node set O,
if the navigation graph structure includes the node N, then determine whether N is obstacle-free and determine whether an edge E connecting N and S is obstacle-free, and, if both N and E are obstacle-free, then insert N into the navigation graph, generate and insert E into the navigation graph, update a cost of N, include N in the open-node set O, and return to said determine whether the navigation graph structure includes a node N,
if the navigation graph structure includes no node N neighboring S, where N is not already included in O, then exclude S from the open-node set O, include S in a closed-node set C, and return to said select a node S of lowest cost, and
if S coincides with the goal node G, then find an optimal path from B to G, including searching each node in C for a parent node in C;
a navigation system configured to receive the optimal path as navigation data and to formulate drive data for driving the vehicle along the optimal path; and
a drive system configured to receive the drive data and to drive the vehicle along the optimal path pursuant to the drive data.

10. The vehicle of claim 9 wherein the instructions further cause the processor to:
if both N and E are obstacle-free, then determine whether an edge E′ connecting N and an ancestor node of S is obstacle-free, and, if both N and E′ are obstacle-free, then generate E′, wherein updating the cost of N includes updating in view of E′, wherein the edge E is generated when E is obstacle-free but E′ is not obstacle-free.

11. The vehicle of claim 9 wherein the drive system includes a land- or water-contacting drive element.

12. The vehicle of claim 9 further comprising a machine-vision system.

13. The vehicle of claim 12 wherein the machine-vision system is configured to detect the one or more obstacles, and wherein at least some of the data modeling the environment is received from the machine-vision system.

14. The vehicle of claim 9 further comprising an inertial-measurement unit.

15. The vehicle of claim 14 wherein the inertial-measurement unit is configured to detect the one or more obstacles, and wherein at least some of the data modeling the environment is received from the inertial-measurement unit.

16. The vehicle of claim 9 , further comprising a communication system, wherein at least some of the data modeling the environment is received from the communication system.

17. A navigation device comprising:
a path-finding system including processor and an associated nonvolatile memory, the nonvolatile memory holding instructions that cause the processor to:
receive data modeling an environment to be traversed, the data representing one or more obstacles within the environment,
define a navigation graph structure corresponding to the environment, wherein the navigation graph structure is a lattice, the navigation graph structure including a beginning node B and a goal node G,
add the beginning node B to an open-node set O,
select, from the open-node set O, a node S of lowest cost,
if S differs from the goal node G, then determine whether the navigation graph structure includes a node N neighboring S, where N is not already included in the open-node set O,
if the navigation graph structure includes the node N, then determine whether N is obstacle-free and determine whether an edge E connecting N and S is obstacle-free, and, if both N and E are obstacle-free, then insert N into the navigation graph, generate and insert E into the navigation graph, update a cost of N, include N in the open-node set O, and return to said determine whether the navigation graph structure includes a node N,
if the navigation graph structure includes no node N neighboring S, where N is not already included in O, then exclude S from the open-node set O, include S in a closed-node set C, and return to s

Assignees

• Sharp Laboratories America Inc

Inventors

• Hong Soonhac

Classifications

• G01C21/3446

  Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags or using precalculated routes · CPC title

• G05D1/0212

  with means for defining a desired trajectory (involving a plurality of land vehicles G05D1/0287) · CPC title

• B25J9/1664

  characterised by motion, path, trajectory planning · CPC title

• Y10S901/01

  Mobile robot · CPC title

• G01C21/343

  Calculating itineraries (travelling salesman problem G06Q10/04; optimisation of routes G06Q10/047) · CPC title