Mapping-Based Navigation

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.

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.

Dijkstra's Algorithm on a Grid Map with Constant Cost
Algorithm 10.1 is Dijkstra's algorithm for a grid map. The algorithm is demonstrated on the 5 × 6 cell grid map in Fig. 10.2a. There are three obstacles represented by the black cells. The algorithm incrementally marks each cell c with the number of steps needed to reach c from the start cell S. In the figures, the step count is shown as a number in the upper left hand corner of a cell. Initially, mark cell S with 0 since no steps are needed to reach S from S. Now, mark every neighbor of S with 1 since they are one step away from S; then mark every neighbor of a cell marked 1 with 2. Figure 10.2b shows the grid map after these two iterations of the algorithm.
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.
It is now easy to find a shortest path by working backwards from the goal cell G. In Fig.10.3b a shortest path consists of the cells that are colored gray. Starting with the goal cell at coordinate (4, 5), the previous cell must be either (4, 4) or (3, 5) since they are eight steps away from the start. (This shows that there is more than one shortest path.) We arbitrarily choose (3, 5). From each selected cell marked n, we choose a cell marked n − 1 until the cell S marked 0 is selected. The list of the selected cells is: (4, 5), (3, 5), (3, 4), (3, 3), (2, 3), (2, 2), (2, 1), (3, 1), (4, 1), (4, 0) . By reversing the list, the shortest path from S to G is obtained. Check that this is the same path we found intuitively ( Fig. 10.1b).
Example Figure 10.4 shows how Dijkstra's algorithm works on a more complicated example. The grid map has 16 × 16 cells and the goal cell G is enclosed within an obstacle and difficult to reach. The upper left diagram shows the grid map after three iterations and the upper right diagram shows the map after 19 iterations. The algorithm now proceeds by exploring cells around both sides of the obstacle. Finally, in the lower left diagram, G is found after 25 iterations, and the shortest path is indicated in gray in the lower right diagram. We see that the algorithm is not very efficient for this map: although the shortest path is only 25 steps, the algorithm has explored 256 − 25 = 231 cells!

Activity 10.1: Dijkstra's algorithm on a grid map
• Construct a grid map and apply Dijkstra's algorithm.
• Modify the map to include cells with a variable cost and apply the 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.

Dijkstra's Algorithm for a Continuous Map
In a continuous map the area is an ordinary two-dimensional geometric plane. One approach to using Dijkstra's algorithm in a continuous map is to transform the map into a discrete graph by drawing vertical lines from the upper and lower edges of the environment to each corner of an obstacle. This divides the area into a finite number of segments, each of which can be represented as a node in a graph. The left diagram of Fig. 10.6 shows seven vertical lines that divide the map into ten segments which are shown in the graph in Fig. 10.7. The edges of the graph are defined by the adjacency relation of the segments. There is a directed edge from segment A to segment B if A and B share a common border. For example, there are edges from node 2 to nodes 1 and 3 since the corresponding segments share an edge with segment 2.
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 S → 2 → 3 → 6 → 9 → 10 → 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  Figure 10.9 shows the graph formed by these nodes and edges. It represents all candidates for the shortest path between the start and goal locations.
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 → D → F → H → 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.

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.
The A * algorithm (pronounced "A star") is similar to the Dijkstra's algorithm, but is often more efficient because it uses extra information to guide the search. The A * algorithm considers not only the number of steps from the start cell, but also a heuristic function that gives an indication of the preferred direction to search. Previously, we used a cost function g(x, y) that gives the actual number of steps from the start cell. Dijkstra's algorithm expanded the search starting with the cells marked with the highest values of g(x, y). In the A * algorithm the cost function f (x, y) is computed by adding the values of a heuristic function h(x, y): We demonstrate the A * algorithm by using as the heuristic function the number of steps from the goal cell G to cell (x, y) without taking the obstacles into account. This function can be precomputed and remains available throughout the execution of the algorithm. For the grid map in Fig. 10.2a, the heuristic function is shown in Fig. 10.10a.
In the diagrams, we will keep track of the values of the three functions f, g, h by displaying them in different corners of each cell h g f . Figure 10.10b shows the grid map after two steps of the A * algorithm. Cells (3, 1) and (3, 0) receive the same cost f : one is closer to S (by the number of steps counted) and the other is closer to G (by the heuristics), but both have the same cost of 7.
It doesn't matter which of the nodes with value 9 is chosen: in either case, the algorithm reaches the goal cell (4, 5, 9).
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 non-obstacle cells, the A * algorithm explored only 17 cells.

A More Complex Example of the A * Algorithm
Let apply the A * algorithm to the grid map in Fig. 10.5. Recall that this map has sand on some of its cells, so the g function will give higher values for the cost of moving to these cells. The upper left diagram of Fig. 10.12 shows the g function as computed by Dijkstra's algorithm, while the upper right diagram shows the heuristic function h, the number of steps from the goal in the absence of obstacles and the sand. The rest of the figure shows four stages of the algorithm leading to the shortest path to the goal.
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.

Path Following and Obstacle Avoidance
This chapter and the previous ones discussed two different but related tasks: highlevel path planning and low-level obstacle avoidance. How can the two be integrated?
The simplest approach is to prioritize the low-level algorithm (Fig. 10.13). Obviously, it is more important to avoid hitting a pedestrian or to drive around a pothole than it is to take the shortest route to the airport. The robot is normally in its drive state, but if an obstacle is detected, it makes a transition to the avoid obstacle state. Only when the obstacle has been passed does it return to the state plan path so that the path can be recomputed. 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: Combining path planning and obstacle avoidance
• Modify your implementation of the line-following 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 line-following 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.

Summary
Path planning is a high-level 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.
Low-level obstacle avoidance must be integrated into high-level path planning.

Further Reading
Dijkstra's algorithm is presented in all textbooks on data structures and algorithms, for example, [1,Sect. 24.3]. Search algorithms such as the A * algorithm are a central topic of artificial intelligence [2, Sect. 3.5].