Path Planning as Heuristic Graph Search

Pavel Yurlov

Department of Computer Science

National Research University – Higher School of Economics

Moscow, Russia

[email protected]

Abstract — This paper provides a summary of the presentation 1 on some basic path planning methods. First, the problem is defined, and it is shown that it consists of two parts: (1) construction of a graph and (2) search for a close-to-optimal path in that graph. Then, several solutions for each step are outlined, their advantages and disadvantages discussed.

Keywords — path planning, path finding.

INTRODUCTION

Path planning is the finding of a path from the current pose to a goal pose. There are numerous applications of it in robotics, video games, unmanned vehicles, etc. Path planning must be very fast (because it is often done on the spot) and must generate realistic paths (due to its use in applications).

There are two types of constraints: environmental, e.g. obstacles, and dynamics or kinematics. The generated path should: (1) be feasible, i.e. believable or realistic; (2) minimise cost, e.g. time, distance, etc.

The task consists of two parts. First, a graph representing the environment is constructed, then it is searched for a path. Depending on the algorithm, these two steps may be interleaved.

GRAPH CONSTRUCTION

Configuration Space Transform

A configuration is legal if it does not intersect itself, obstacles, etc. Configuration space (C-space) is the set of all legal configurations.

C-space may be transformed by expanding all obstacles by the radius r of the movable object. The planning then can be done for a point instead of a complex figure, which is much faster. However, the expansion itself is O(n) time-consuming.

There are two basic ways in which a graph may be constructed to represent C-space: (1) cell decomposition and (2) skeletonisation.

CELL DECOMPOSTITION

Cell decomposition can be exact and approximate. The exact decomposition is done by overlaying convex exact polygons over the free C-space. This solution is too expensive, which is why the presentation focuses on approximate decomposition.

Approximate cell decomposition is performed by discretising the C-space, i.e. overlaying uniform grid over it. Such solution is very simple, hence its popularity, but it is expensive, especially in high-dimensional spaces.

There arises a problem of partially blocked cells. If made untraversable, an existing path may not be found. If made traversable, the generated path may be invalid.

The first solution is to make the discretisation very fine, but it is too expensive, especially in high-dimensional spaces.

The second solution is to make the discretisation adaptive. For instance, edges can connect not only immediate neighbours but also neighbours’ neighbours. In the latter case the path is smoother, but it is also costlier.

Using motion primitives

In order to generate more realistic, e.g. smooth, paths, connections between cells may be restricted by specifying certain motion primitives 2. Each state in such a graph represents not only location, but other parameters such as orientation, velocity, etc., as well.

On the one hand, such lattice-based graph is sparse and contains only feasible paths. On the other hand, there may be too few edges and an existing optimal path may not be found.

SKELETONISATION

Skeletonisation involves construction of a unidimensional representation of the C-space. Its independence from the latter’s dimension makes it cost-efficient.

The presentation treats of four possible ways of skeletonisation.

Visibility graph

This approach is based on the idea that the shortest path consists of straight segments. Hence, the graph’s vertices are a set of the current pose, obstacles’ vertices and the goal pose. This heuristic ignores dynamics constraints and the potential danger of moving too close to the obstacles. It also does not work if the obstacles are not polygonal.

Voronoi diagrams

A Voronoi diagram is the set of all points that are equidistant to the nearest obstacles. The algorithm is as follows: (1) compute a Voronoi diagram, (2) add two shortest segments connecting it to the current pose and the final pose, (3) search the resulting graph for the shortest path.

This approach prioritises keeping the obstacles at bay (safety in applications) over path optimality (in terms of distance).

Probabilistic road map

As it is clear from the name, this is a randomised algorithm. First, generate enough legal configurations, add initial and final configurations, then add edges in between those that are easy to connect.

On the one hand, it is a very simple and fast algorithm. On the other hand, it may return an extremely suboptimal path and encounter difficulties with narrow passages (it is hard to randomly generate a configuration that would fit into heavily restricted environment).

Navmeshes

A navigation mesh is a collection of two-dimensional convex polygons that define a traversable area. Polygons’ centres are represented as graph vertices. Navigation meshes can be created either manually or automatically. This approach is very popular in video games, but it can result in suboptimal paths with no guarantee on suboptimality.

PATH FINDING

Once a graph is constructed, it has to be searched for the most optimal path.

A* algorithm

For every relevant state s compute g(s) = the cost of the shortest path from sstart to s found so far and h(s) = an (under) estimate of the cost of the shortest path from s to sgoal. h(s) is a heuristic function. It must be: (1) admissable: for every state s, h(s) ? min_cost(s, sgoal), and (2) consistent: h(sgoal) = 0 and for every s ? sgoal, h(s) ? cost(s, succesor(s)) + h(succesor(s)). Admissability follows from consistency and consistency often follows from admissability.

Pseudo-code:

Main function:

1: g(sstart) = 0

2: ?s ? sstart g(s) = +?

3: OPEN = {sstart} #set of candidates for expansion

4: CLOSED = {} #set of expanded states

5: ComputePath()

6: publish solution

ComputePath function:

1: while sgoal is not expanded (i.e. sgoal ? CLOSED) then

2: remove s with the smallest f(s) = g(s) + h(s) from OPEN

3: CLOSED = CLOSED + {s}

4: for ?s` = successor(s), s` ? CLOSED, then

5: if g(s`) ; g(s) + cost(s, s`) then

6: g(s`) = g(s) + cost(s, s`)

7: OPEN = OPEN + {s`}

8: end if

9: end for

10: end while

Comparison of A*, weighted A* and Dijkstra’s algorithms

A* algorithm expands states in the order of f = g + h values. (Or ? = 1, see below.)

Weighted A* algorithm expands states in the order of f = g + ?h values, ? > 1.

Dijkstra’s algorithm expands states in the order of f = g values. (Or ? = 0.)

The larger ?, the greater is the bias towards states that are closer to the goal state and the more is algorithmic efficiency prioritised over path optimality.

CONCLUSION

The presentation 1 provides a clear and useful overview of several basic techniques of solving the task of path planning via heuristic graph search. It does not contain original research but serves as a very fine introduction to the field of motion planning.

REFERENCES

Maxim Likhachev, “Basic path finding”. Autumn 2011. Online. Available: Carnegie Melon University, http://www.cs.cmu.edu/~maxim/classes/CIS15466_Fall11/lectures/basicpathfind_cis15466.pdf. Accessed: March 22, 2018.

Steven M. LaValle, “Motion primitives,” in Planning Algorithms. New York: Cambridge University Press, 2006. Online. Available: Planning Algorithms, http://planning.cs.uiuc.edu/node738.html. Accessed: March 22, 2018.

1200 words.