Embedded Robotics (Thomas Braunl, 2 ed, 2006)

Dijkstra’s Algorithm

Algorithm Start “ready set” with start node. In loop select node with shortest distance in every step, then compute distances to all of its neighbors and store path predecessors. Add current node to “ready set”; loop finishes when all nodes are included.

1. Init

Set start distance to 0, dist[s]=0, others to infinite: dist[i]=f (for izs), Set Ready = { } .

2. Loop until all nodes are in Ready

Select node n with shortest known distance that is not in Ready set Ready = Ready + {n} .

FOR each neighbor node m of n

IF dist[n]+edge(n,m) < dist[m] /* shorter path found */ THEN { dist[m] = dist[n]+edge(n,m);

pre[m] = n;

}

DE

DE

Figure 14.8: Dijkstra’s algorithm step 0 and 1

207

Dijkstra’s Algorithm

Example Consider the nodes and distances in Figure 14.8. On the left hand side is the distance graph, on the right-hand side is the table with the shortest distances found so far and the immediate path predecessors that lead to this distance.

In the beginning (initialization step), we only know that start node S is reachable with distance 0 (by definition). The distances to all other nodes are infinite and we do not have a path predecessor recorded yet. Proceeding from step 0 to step 1, we have to select the node with the shortest distance from all nodes that are not yet included in the Ready set. Since Ready is still empty, we have to look at all nodes. Clearly S has the shortest distance (0), while all other nodes still have an infinite distance.

For step 1, Figure 14.8 bottom, S is now included into the Ready set and the distances and path predecessors (equal to S) for all its neighbors are being updated. Since S is neighbor to nodes a, c, and d, the distances for these three nodes are being updated and their path predecessor is being set to S.

When moving to step 2, we have to select the node with the shortest path among a, b, c, d, as S is already in the Ready set. Among these, node c has the shortest path (5). The table is updated for all neighbors of c, which are S, a, b, and d. As shown in Figure 14.9, new shorter distances are found for a, b, and d, each entering c as their immediate path predecessor.

In the following steps 3 through 5, the algorithm’s loop is repeated, until finally, all nodes are included in the Ready set and the algorithm terminates. The table now contains the shortest path from the start node to each of the other nodes, as well as the path predecessor for each node, allowing us to reconstruct the shortest path.

Figure 14.10: Determine shortest path

Figure 14.10 shows how to construct the shortest path from each node’s predecessor. For finding the shortest path between S and b, we already know the shortest distance (9), but we have to reconstruct the shortest path backwards from b, by following the predecessors:

pre[b]=a, pre[a]=c, pre[c]=S

Therefore, the shortest path is: S o c o a o b

209

Potential Field Method

As it turns out, the currently shortest partial path is {S, c, a}, which we will now expand further:

•{S, c, a, b} with min. length 5 + 3 + 1 + 0 = 9

There is only a single possible expansion, which reaches the goal node b and is the shortest path found so far, so the algorithm terminates. The shortest path and the corresponding distance have been found.

Note This algorithm may look complex since there seems to be the need to store incomplete paths and their lengths at various places. However, using a recursive best-first search implementation can solve this problem in an elegant way without the need for explicit path storing.

The quality of the lower bound goal distance from each node greatly influences the timing complexity of the algorithm. The closer the given lower bound is to the true distance, the shorter the execution time.

14.6 Potential Field Method

References [Arbib, House 1987], [Koren, Borenstein 1991],

[Borenstein, Everett, Feng 1998]

Description Global map generation algorithm with virtual forces.

Required Start and goal position, positions of all obstacles and walls.

Algorithm Generate a map with virtual attracting and repelling forces. Start point, obstacles, and walls are repelling, goal is attracting; force strength is inverse to object distance; robot simply follows force field.

Example Figure 14.12 shows an example with repelling forces from obstacles and walls, plus a superimposed general field direction from start to goal.

Figure 14.12: Potential field

Figure 14.13 exemplifies the potential field generation steps in the form of 3D surface plots. A ball placed on the start point of this surface would roll

211

DistBug Algorithm

Goal

6

1

Start

Figure 14.14: Wandering standpoint

Problem The algorithm can lead to an endless loop for extreme obstacle placements. In this case the robot keeps driving, but never reaches the goal.

14.8 DistBug Algorithm

Reference [Kamon, Rivlin 1997]

Description Local planning algorithm that guarantees convergence and will find path if one exists.

Required Own position (odometry), goal position, and distance sensor data.

Algorithm Drive straight towards the goal when possible, otherwise do boundary-follow- ing around an obstacle. If this brings the robot back to the same previous collision point with the obstacle, then the goal is unreachable.

Below is our version of an algorithmic translation of the original paper.

Min_dist minimal distance to goal during boundary following

1. Main program Loop

“drive towards goal” /* non-blocking, proc. continues while driv. */ if P=G then {“success”; terminate;}

if “obstacle collision” { Hit = P; call follow; }

End loop

213

References

Figure 14.15, right, shows another example. The robot will stop boundary following at the first leave point, because its sensors have detected that it can reach a point closer to the goal than before. After reaching the second hit point, boundary following is called a second time, until at the second leave point the robot can drive directly to the goal.

Figure 14.16 shows two more examples that further demonstrate the DistBug algorithm. In Figure 14.16, left, the goal is inside the E-shaped obstacle and cannot be reached. The robot first drives straight towards the goal, hits the obstacle and records the hit point, then starts boundary following. After completion of a full circle around the obstacle, the robot returns to the hit point, which is its termination condition for an unreachable goal.

Figure 14.16, right, shows a more complex example. After the hit point has been reached, the robot surrounds almost the whole obstacle until it finds the entry to the maze-like structure. It continues boundary following until the goal is directly reachable from the leave point.

Leave

point

Figure 14.16: Complex Distbug examples

14.9 References

ARBIB, M., HOUSE, D. Depth and Detours: An Essay on Visually Guided Behavior, in M. Arbib, A. Hanson (Eds.), Vision, Brain and Cooperative Computation, MIT Press, Cambridge MA, 1987, pp. 129-163 (35)

ARKIN, R. Behavior-Based Robotics, MIT Press, Cambridge MA, 1998

215