Once the approximation has been made, any of the methods discussed in Section 8.2.2 can be used to compute a navigation function. An optimal navigation function can be easily computed using Dijkstra's algorithm from the goal. If each motion has unit cost, then a useful simplification can be made. Figure 8.4 describes a wavefront propagation algorithm that computes an optimal navigation function. It can be considered as a special case of Dijkstra's algorithm that avoids explicit construction of the priority queue. In Dijkstra's algorithm, the cost of the smallest element in the queue is monotonically nondecreasing during the execution of the algorithm. In the case of each motion having unit cost, there will be many states in the queue that have the same cost. Dijkstra's algorithm could remove in parallel all elements that have the same, smallest cost. Suppose the common, smallest cost value is . These states are organized into a wavefront, . The initial wavefront is , which represents the states in . The algorithm can immediately assign an optimal cost-to-go value of to every state that can be reached in one stage from any state in . These must be optimal because no other cost value is optimal. The states that receive cost can be organized into the wavefront . The unexplored neighbors of are assigned cost , which also must be optimal. This process repeats inductively from to until all reachable states have been reached. In the end, the optimal cost-to-go is computed in time, in which is the number of reachable grid states. For any states that were not reached, the value can be assigned. The navigation function shown in Figure 8.3 can actually be computed using the wavefront propagation algorithm.
Steven M LaValle 2012-04-20