Abstract
Given a map and a target position, a robot must perform path planning in order to determine the best route from its current position to the target position. Three algorithms for path planning are presented: Dijkstra’s shortest path algorithm for a grid of cells, an algorithm for continuous maps and the A\(^{*}\) algorithm, an improvement of Dijkstra’s algorithm that uses heuristic functions. The chapter concludes with a description of the integration of path planning with obstacle avoidance.
Now that we have a map, whether supplied by the user or discovered by the robot, we can discuss path planning, a higherlevel algorithm. Consider a robot used in a hospital for transporting medications and other supplies from storage areas to the doctors and nurses. Given one of these tasks, what is the best way of going from point A to point B? There may be multiple ways of moving through the corridors to get to the goal, but there may also be short paths that the robot is not allowed to take, for example, paths that go through corridors near the operating rooms.
We present three algorithms for planning the shortest path from a starting position S to a goal position G, assuming that we have a map of the area that indicates the positions of obstacles in the area. Edsgar W. Dijkstra, one of the pioneers of computer science, proposed an algorithm for the shortest path problem. Section 10.1 describes the algorithm for a grid map, while Sect. 10.2 describes the algorithm for a continuous map. The A\(^{*}\) algorithm, an improvement on Dijkstra’s algorithm based upon heuristic methods, is presented in Sect. 10.3. Finally, Sect. 10.4 discusses how to combine a highlevel path planning algorithm with a lowlevel obstacle avoidance algorithm.
10.1 Dijkstra’s Algorithm for a Grid Map
Two versions of the algorithm are presented: The first is for grids where the cost of moving from one cell to one of its neighbors is constant. In the second version, each cell can have a different cost associated with moving to it, so the shortest path geometrically is not necessarily the shortest path when the costs are taken into account.
10.1.1 Dijkstra’s Algorithm on a Grid Map with Constant Cost
The algorithm continues iteratively: if a cell is marked n, its unmarked neighbors are marked with \(n+1\). When G is finally marked, we know that the shortest distance from S to G is n. Figure 10.3a shows the grid map after five iterations and Fig. 10.3b shows the final grid map after nine iterations when the goal cell has been reached.
10.1.2 Dijkstra’s Algorithm with Variable Costs
The grid on the left of Fig. 10.5 has some cells marked with a diagonal line to indicate that they are covered with sand and that the cost of moving through them is 4 and not 1. The shortest path, marked in gray, has 17 steps and also costs 17 since it goes around the sand.
The shortest path depends on the cost assigned to each cell. The right diagram shows the shortest path if the cost of moving through a cell with sand is only 2. The path is 12 steps long although its cost is 14 to take into account moving two steps through the sand.
Activity 10.1:

Construct a grid map and apply Dijkstra’s algorithm.

Modify the map to include cells with a variable cost and apply the algorithm.

Implement Dijkstra’s algorithm.

Create a grid on the floor.

Write a program that causes the robot to move from a known start cell to a known goal cell. Since the robot must store its current location, use this to create a map of the cells it has moved through.

Place some obstacles in the grid and enter them in the map of the robot.

10.2 Dijkstra’s Algorithm for a Continuous Map
What is the shortest path between vertex 2 representing the segment containing the starting point and vertex 10 representing the segment containing the goal? The result of applying Dijkstra’s algorithm is \(\textsf {\small S} \rightarrow 2\rightarrow 3\rightarrow 6\rightarrow 9\rightarrow 10 \rightarrow \textsf {\small G}\). Although this is the shortest path in terms of the number of edges of the graph, it is not the shortest path in the environment. The reason is that we assigned constant cost to each edge, although the segments of the map have various size.
Since each vertex represents a large segment of the environment, we need to know how moving from one vertex to another translates into moving from one segment to another. The right diagram in Fig. 10.6 shows one possibility: each segment is associated with its geometric center, indicated by the intersection of the dotted diagonal lines in the figure. The path in the environment associated with the path in the graph goes from the center of one segment to the center of the next segment, except that the start and goal positions are at their geometric locations. Although this method is reasonable without further knowledge of the environment, it does not give the optimal path which should stay close to the borders of the obstacles.
It is easy to see that the paths in the graph represent paths in the environment, since the robot can simply move from corner to corner. These paths are the shortest paths because no path, say from A to B, can be shorter than the straight line from A to B. Dijkstra’s algorithm gives the shortest path as \(S\rightarrow D\rightarrow F\rightarrow H\rightarrow G\). In this case, the shortest path in terms of the number of edges is also the geometrically shortest path.
Although this is the shortest path, a real robot cannot follow this path because it has a finite size so its center cannot follow the border of an obstacle. The robot must maintain a minimum distance from each obstacle, which can be implemented by expanding the size of the obstacles by the size of the robot (right diagram of Fig. 10.9). The resulting path is optimal and can be traversed by the robot.
Activity 10.2:

Draw a larger version of the map in Fig. 10.8. Measure the length of each segment. Now apply Dijkstra’s algorithm to determine the shortest path.

Create your own continuous map, extract the visibility graph and apply Dijkstra’s algorithm.
10.3 Path Planning with the A\(^{*}\) Algorithm
Dijkstra’s algorithm searches for the goal cell in all directions; this can be efficient in a complex environment, but not so when the path is simple, for example, a straight line to the goal cell. Look at the top right diagram in Fig. 10.4: near the upper right corner of the center obstacle there is a cell at distance 19 from the start cell. After two more steps to the left, there will be a cell marked 21 which has a path to the goal cell that is not blocked by an obstacle. Clearly, there is no reason to continue to explore the region at the left of the grid, but Dijkstra’s algorithm continues to do so. It would be more efficient if the algorithm somehow knew that it was close to the goal cell.
All the cells in the upper right of the grid are not explored because cell (1, 3) has f value 11 and that will never be the smallest value. While Dijkstra’s algorithm explored all 24 nonobstacle cells, the A\(^{*}\) algorithm explored only 17 cells.
A More Complex Example of the A\(^{*}\) Algorithm
Already from the middle left diagram, we see that it is not necessary to search towards the top left, because the f values of the cells above and to the left of S are higher than the values to the right of and below S. In the middle right diagram, the first sand cell has a value of 13 so the algorithm continues to expand the cells with the lower cost of 12 to the left. In the bottom left diagram, we see that the search does not continue to the lower left of the map because the cost of 16 is higher than the cost of 14 once the search leaves the sand. From that point, the goal cell G is found very quickly. As in Dijkstra’s algorithm, the shortest path is found by tracing back through cells with lower g values until the start cell is reached.
Comparing Figs. 10.4 and 10.12 we see that the A\(^{*}\) algorithm needed to visit only 71% of the cells visited by Dijkstra’s algorithm. Although the A\(^{*}\) algorithm must perform additional work to compute the heuristic function, the reduced number of cells visited makes the algorithm more efficient. Furthermore, this heuristic function depends only on the area searched and not on the obstacles; even if the set of obstacles is changed, the heuristic function does not need to be recomputed.
Activity 10.3:

Apply the A\(^{*}\) algorithm and Dijkstra’s algorithm on a small map without obstacles: place the start cell in the center of the map and the goal in an arbitrary cell. Compare the results of the two algorithms. Explain your results. Does the result depend on the position of the goal cell?

Define other heuristic functions and compare the results of the A\(^{*}\) algorithms on the examples in this chapter.
10.4 Path Following and Obstacle Avoidance
The strategy for integrating the two algorithms depends on the environment. Repairing a road might take several weeks so it makes sense to add the obstacle to the map. The path planning algorithm will take the obstacle into account and the resulting path is likely to be better than one that is changed at the last minute by an obstacle avoidance algorithm. At the other extreme, if there are a lot of moving obstacles such as pedestrians crossing a street, the obstacle avoidance behavior could be simply to stop moving and wait until the obstacles move away. Then the original plan can simply be resuming without detours.
Activity 10.4:

Modify your implementation of the linefollowing algorithm so that the robot behaves correctly even if an obstacle is placed on the line. Try several of the approaches listed in this section.

Modify your implementation of the linefollowing algorithm so that the robot behaves correctly even if additional robots are moving randomly in the area of the line. Ensure that the robots do not bump into each other.
10.5 Summary
Path planning is a highlevel behavior of a mobile robot: finding the shortest path from a start location to a goal location in the environment. Path planning is based upon a map showing obstacles. Dijkstra’s algorithm expands the shortest path to any cell encountered so far. The A\(^{*}\) algorithm reduces the number of cells visited by using a heuristic function that indicates the direction to the goal cell.
Path planning is based on a graph such as a grid map, but it can also be done on a continuous map by creating a graph of obstacles from the map. The algorithms can take into account variables costs for visiting each cell.
Lowlevel obstacle avoidance must be integrated into highlevel path planning.
10.6 Further Reading
References
 1.Cormen, T.H., Leiserson, C.E., Rivest, R.L., Stein, C.: Introduction to Algorithms, 3rd edn. MIT Press, Cambridge (2009)Google Scholar
 2.Russell, S., Norvig, P.: Artificial Intelligence: A Modern Approach, 3rd edn. Pearson, Boston (2009)Google Scholar
Copyright information
This chapter is distributed under the terms of the Creative Commons Attribution 4.0 International License (http://creativecommons.org/licenses/by/4.0/), which permits use, duplication, adaptation, distribution and reproduction in any medium or format, as long as you give appropriate credit to the original author(s) and the source, provide a link to the Creative Commons license and indicate if changes were made. The images or other third party material in this chapter are included in the work’s Creative Commons license, unless indicated otherwise in the credit line; if such material is not included in the work’s Creative Commons license and the respective action is not permitted by statutory regulation, users will need to obtain permission from the license holder to duplicate, adapt or reproduce the material.