Graph based search algorithms are the most commonly used algorithms for path planning of mobile robots.
Among them the most popular one is A*, which finds complete and optimal path in static environments.
The A* graph search algorithm is based on a path cost function g, which represents the total cost from the start node of the search to the current node, and a heuristic function h, which estimates
but never overestimates
the cheapest solution for achieving the goal node from the current node in the (x, y) grid map search space.
Such a heuristic function is called admissible and optimistic.
The total cost function f = g + h determines order of expanding nodes in state space as illustrated in Fig. 1.
Space searching by A*
Optimal path is noted with arrows from the start to the goal node. Contours are determined by equal values of the f-function, where
f [fmin, fmax].
When following any path from the start node, value of the f-function never decreases, which is true if heuristic exhibits monotonicity.
The most often used heuristic function is Euclidean distance from the start node to the current node.
However, the Euclidean distance is computationally inefficient since calculation of the square root function for each node expansion demands floating point arithmetics. In order to alleviate this problem a heuristic that uses integer arithmetics is proposed. This is possible in occupancy grid maps because they enable transition costs to be described as integer multiplies. For example, if each cell of a grid is regarded as a node of the path graph, 10 can be used for straight transition and 14 for diagonal transition. We used the heuristic:
a = max( | xG - xN | , | yG - yN | ),
b = min( | xG - xN | , | yG - yN | ),
h(N) = 14b + 10(a - b),
where (xN, yN) are the coordinates of the current node N and (xG, yG) of the goal node G.
This heuristic exhibits monotonicity because it fulfilles the triangular inequality property.
A* search fans out from the start node, expanding neighbour nodes within the contours of increasing f-value until the goal node is reached or all possible free neighbours (free of obstacle) are exhausted upon which the algorithm declares no path found.
In a dynamic environment the global path must be completely replanned at each servo tick, which may cause A* algorithm to perform poorly since it does not use search information from previous iterations. Therefore, minimum path criterion may not be optimal in the sense of minimum time.
Therefore, D* graph search algorithm should be used, which allows updating of only those nodes along the path that actually change due to sensor measurements.
The D* algorithm is similar to A* in the case of initial off-line path planning. The main distinction from A* algorithm is that D* search fans out not from the start node but from the goal node.
Evaluation function g here represents the total path cost from the current node to the goal node. It is computed from the goal node since the search started from goal. Nodes are expanded within the contours of increasing g-value, where
g [gmin, gmax],
until the start node is reached or all possible states of the environments are exhausted upon which the algorithm declares no path found, as depicted in Fig. 2.
Space searching by D*
Initial search by D* algorithm sets pointers from each state in the searched area to the next state. Optimal paths to the goal from every state in the expanded area of the environment are computed simply following the pointers.
Further on-line execution of the algorithm relies on sensor information from the vicinity of the robot environment. Any discrepancy that is discovered from the earlier information about the environment initiate algorithm execution. New path is then determined redirecting the pointers locally.
Including the heuristic in the remaining cost of the path to the start node leads to the focused D* algorithm variant.
Space searching by FD*
Heuristic is chosen similar to (1) with the coordinates of the start node
S(xS, yS) instead of the goal node
Nodes are expanded according to increasing values of total cost function f = g + h and contours are focused around the optimal path but narrowly around the start node, as is depicted in Fig. 3,
therefore similar to A* algorithm but opposite according to start and goal node. The name focused D* algorithm is given accordingly.