*4.2.2. D\* Lite with reset algorithm*

Unlike AD\*, D\* Lite with Reset (D\*LR) [16] partially solves the problem D\* Lite while still maintaining path optimality. The idea of D\*LR is simple; it decides flushing previous search data and starts searching from scratch when the replanning process is expensive.

D\*LR is a variant of D\* Lite; it inherits all the properties of D\* Lite. The main contribution of D\*LR is that it proposes two criteria to decide whether to incrementally replan path or calculate a fresh path using A\* at the position the robot detects changes. Let total traversed cell is *NT*; total cell of path that exists between consecutive detection incidents is *NP*, and the remaining path count is *NR* = *NP* − *NT*; the criteria are:

• **Ratio of traversed length:** The criteria measure how many percentages of the path the robot has moved between two consecutive positions that the robot detects environment changes. The ratio \_\_\_ *NT NP* is then compared with a threshold:

$$\frac{N\_r}{N\_p} < \infty \,. \tag{8}$$

If the ratio is greater than **threshold** ∝ when the robot perceives changes, it triggers the reset routine and starts searching from scratch. Intuitively, the position that robot detects changes is nearer the goal, the more likely replanning process is expensive.

• **Linear heuristic distance:** The criteria measure the complexity of the remaining path count *NR* between consecutive detection incidents. The method to measure the complexity is to use inflated heuristic function:

$$N\_{\mathbb{R}} \ge \varepsilon \* h \langle s\_{\gamma} \mid s\_{goal} \rangle \tag{9}$$

where *s* is the current position of the robot. If *NR* ≤ *ε* ∗ *h*(*s*,  *sgoal*), then the remaining path is simple enough; there is a chance that the new path is much more complex. Hence, the algorithm must plan over from scratch.

As can be seen, these criteria use only path information between consecutive detection incidents in order to estimate the amount of computation of replanning process comparing with planning over from scratch. The reason is that it is hard to predict propagation behaviour of OPEN list, because the state space is only partially known. Moreover, these criteria only work in high cluttered and complex environment, where environment changes usually block initial path and the new path is likely to be much longer than initial path. The pseudo code of D\*LR is presented in **Figure 11**.

The proposed criteria of D\*LR are not robust due to extensively replying on environmental assumptions. However, the algorithm can be improved if criteria that can robustly estimate computation of replanning process in any kind of environment are applied. If criteria are robust, D\*LR performance of each iteration is bounded by the complexity of A\*:

$$O(|V|) = O(|E|) = O(b^4) \tag{10}$$

#### **4.3. Optimality improvements: Field D\* algorithm**

At first, we set the suboptimal bound *ε* to be large enough in order to generate solution quickly (**Figure 10**, line 42). Unless environment changes are detected, AD\* iteratively decrease suboptimal bound *ε* to improve the solution as time allowed (**Figure 10**, lines 55–60); this phase

When changes are perceived, the suboptimal bound of solution is no longer guaranteed, especially the under-consistent cells, due to the fact that there could have shorter paths exist. Therefore, these under-consistent cells are placed in OPEN list with uninflated heuristic to ensure these cells propagate their inconsistences to neighbours first when ComputeOrImprovePath() function is called (**Figure 10**, lines 5 and 45–50). However, because of this effect, many under-consistent cells quickly rise to the top of OPEN list; they usually do not contribute to calculate new path in practice (e.g. objects cause under-consistent changes like human movement, etc.). Hence, AD\* tends to slow down when the path is near optimal.

When "significant cell changes are detected" (**Figure 10**, line 53), there is a high chance that the problem of D\* Lite occurs and the search tree may be corrupted heavily; the replanning process now is expensive; we can increase suboptimal bound *ε* to speed up the correction effort or start a new search from scratch. The problem is how to estimate "significant cell changes". This algorithm does not solve the mentioned problem of D\* Lite completely; it is just a trade-off between performance and path optimality. However, AD\* performs quickly in large-scaled map compared to other algorithms, in which the robot has significant time to

Unlike AD\*, D\* Lite with Reset (D\*LR) [16] partially solves the problem D\* Lite while still maintaining path optimality. The idea of D\*LR is simple; it decides flushing previous search

D\*LR is a variant of D\* Lite; it inherits all the properties of D\* Lite. The main contribution of D\*LR is that it proposes two criteria to decide whether to incrementally replan path or calculate a fresh path using A\* at the position the robot detects changes. Let total traversed cell is *NT*; total cell of path that exists between consecutive detection incidents is *NP*, and the remaining path

• **Ratio of traversed length:** The criteria measure how many percentages of the path the robot has moved between two consecutive positions that the robot detects environment

If the ratio is greater than **threshold** ∝ when the robot perceives changes, it triggers the reset routine and starts searching from scratch. Intuitively, the position that robot detects changes

< ∝ . (8)

is then compared with a threshold:

is nearer the goal, the more likely replanning process is expensive.

\_\_\_ *NT NP*

data and starts searching from scratch when the replanning process is expensive.

is exactly the same with ARA\*.

78 Advanced Path Planning for Mobile Entities

*4.2.2. D\* Lite with reset algorithm*

count is *NR* = *NP* − *NT*; the criteria are:

*NT NP*

changes. The ratio \_\_\_

Theorems of AD\* are described in detail in [12].

iteratively repair its path, and thus overall path is near optimal.

Although cell decomposition approximation is widely used to discretize C-space for searchbased algorithms due to its robustness (no prior environmental assumptions), this approximation intrinsically prevents search-based algorithm to produce optimal path. The search-based algorithms just allow to transition between cell centres, thus restricting robot traverse directions to increment of *π*\_\_ 4 . Moreover, the produced path involves many sharp turns and jerky segments in large map that makes robot difficult to move.

There were many approaches to cope with this problem. For instance, post-processing method that finds the furthest point P along the solution path for which a straight line path from P to robot position is collision-free and replaces the original path to P with this straight line. However, this method sometimes does not work and increases the path cost. Another approach is fast marching method [22]; this method incorporates interpolation step in planning step to produce low-cost interpolated path. Nonetheless, this method assumes that transition cost between grid cells is constant and does not have heuristic property like A\*; hence it is not applicable to outdoor environment, which requires fast path generating and non-uniform cost grid.

To incorporate incremental heuristic property of D\* Lite, the authors of Field D\* [3] embed linear interpolation method to the replanning process to generate "any-angle" optimal path


In this case, the edge, which resides on the boundary of two cells, has the edge cost equal to the minimum of cost of the two cells. Field D\* use linear interpolation to compute approxi-

**Figure 12.** Remapping state space graph vertices from cell centres to cell corners (a) Center Vertices, (b) Corner Vertices

*g*(*sy*) = *yg*(*s*2) + (1 − *y*)*g*(*s*1), (11)

\_\_\_\_\_\_\_\_\_

where *x* is the distance travel along the bottom edge from *s* before cutting through the centre

The interpretation from formulas (4) into ComputeCost() function is described in detail in [3]. This optimization approach can be plugged in any dynamic planner by replacing standard cost function between cell centres by function ComputeCost(). In addition, due to remapping

a distance *y* from *s*<sup>1</sup>

as

by using the path cost (cost from the node to

(**Figure 13**). Given the centre cell cost *c* and bottom cell cost

Search-Based Planning and Replanning in Robotics and Autonomous Systems

http://dx.doi.org/10.5772/intechopen.71663

81

(1 − *x*) <sup>2</sup> + *y*<sup>2</sup> + *g*(*s*2)*y* + *g*(*s*1)(1 − *y*)) (12)

. (see **Figure 13**).

 *s*2

) > g(<sup>2</sup>

) (c) Case g(<sup>1</sup>

. The subfigures illustrate possible

) and (d) Case path costs that

on the edge ⟶*s*<sup>1</sup> *<sup>s</sup>*<sup>2</sup>

to *sy*

**Figure 13.** Linear interpolation process to compute path cost of s using edge ⟶ *s*<sup>1</sup>

*, s*<sup>2</sup> (b) Case g(<sup>1</sup>

) < g(<sup>2</sup>

 *s*2 .

*<sup>b</sup>*, we can compute the path cost of *s* using edge ⟶*s*<sup>1</sup> *<sup>s</sup>*<sup>2</sup>

*g*(*s*) = min*<sup>x</sup>*,*<sup>y</sup>* (*bx* + *c* √

cell to reach the right edge at the point *sy*

optimal path cost (a) Calculate g(*s*) based on *s*<sup>1</sup>

pass arbitrary point.

mately the cost of any point *sy*

and (c) Optimal Path intersected ⟶*s*<sup>1</sup>

where *y* is the distance from *s*<sup>1</sup>

goal) *g*(*s*1) and *g*(*s*2):

**Figure 11.** Pseudo code of D\*LR. Other functions such as Key(), UpdateVertex() and ComputePath() are the same with D\* Lite and thus are not presented.

that overcomes grid limitation in dynamic environment. The root cause of restriction of path optimality is the rule to transition between cell centres; the idea of Field D\* to solve this problem is to remap state space graph vertices to the corner of each cell (see **Figure 12**). The nodes *s* can be considered as sample points of continuous cost field, where the optimal path must pass one of the edges { <sup>⟶</sup>*s*<sup>1</sup> *<sup>s</sup>*<sup>2</sup> , ⟶*s*2 *s*3 , ⟶*s*3 *s*4 , ⟶*s*4 *s*5 , ⟶*s*5 *s*6 , ⟶*s*6 *s*7 , ⟶*s*7 *s*8 , <sup>⟶</sup>*s*<sup>8</sup> *<sup>s</sup>*1} that connects consecutive neighbours of *s*; the edge is ⟶*s*<sup>1</sup> *<sup>s</sup>*<sup>2</sup> in the picture's case.

Search-Based Planning and Replanning in Robotics and Autonomous Systems http://dx.doi.org/10.5772/intechopen.71663 81

**Figure 12.** Remapping state space graph vertices from cell centres to cell corners (a) Center Vertices, (b) Corner Vertices and (c) Optimal Path intersected ⟶*s*<sup>1</sup>  *s*2 .

In this case, the edge, which resides on the boundary of two cells, has the edge cost equal to the minimum of cost of the two cells. Field D\* use linear interpolation to compute approximately the cost of any point *sy* on the edge ⟶*s*<sup>1</sup> *<sup>s</sup>*<sup>2</sup> by using the path cost (cost from the node to goal) *g*(*s*1) and *g*(*s*2):

$$\mathcal{S}(\mathbf{s}\_y) = \mathcal{Y}\mathcal{S}\{\mathbf{s}\_2\} + (1 - \mathbf{y})\mathcal{S}\{\mathbf{s}\_1\} \tag{11}$$

where *y* is the distance from *s*<sup>1</sup> to *sy* (**Figure 13**). Given the centre cell cost *c* and bottom cell cost *<sup>b</sup>*, we can compute the path cost of *s* using edge ⟶*s*<sup>1</sup> *<sup>s</sup>*<sup>2</sup> as

$$\mathbf{g(s)} = \min\_{\mathbf{x}, \mathbf{y}} \left( b\mathbf{x} + c\sqrt{(1-\mathbf{x})^2 + y^2} + \mathbf{g(s\_2)}\mathbf{y} + \mathbf{g(s\_1)(1-y)} \right) \tag{12}$$

where *x* is the distance travel along the bottom edge from *s* before cutting through the centre cell to reach the right edge at the point *sy* a distance *y* from *s*<sup>1</sup> . (see **Figure 13**).

The interpretation from formulas (4) into ComputeCost() function is described in detail in [3]. This optimization approach can be plugged in any dynamic planner by replacing standard cost function between cell centres by function ComputeCost(). In addition, due to remapping

that overcomes grid limitation in dynamic environment. The root cause of restriction of path optimality is the rule to transition between cell centres; the idea of Field D\* to solve this problem is to remap state space graph vertices to the corner of each cell (see **Figure 12**). The nodes *s* can be considered as sample points of continuous cost field, where the optimal path must pass

**Figure 11.** Pseudo code of D\*LR. Other functions such as Key(), UpdateVertex() and ComputePath() are the same with

<sup>⟶</sup>*s*<sup>8</sup> *<sup>s</sup>*1} that connects consecutive neighbours

one of the edges { <sup>⟶</sup>*s*<sup>1</sup> *<sup>s</sup>*<sup>2</sup>

D\* Lite and thus are not presented.

80 Advanced Path Planning for Mobile Entities

of *s*; the edge is ⟶*s*<sup>1</sup> *<sup>s</sup>*<sup>2</sup>

, ⟶*s*2 *s*3 , ⟶*s*3 *s*4 , ⟶*s*4 *s*5 , ⟶*s*5 *s*6 , ⟶*s*6 *s*7 , ⟶*s*7 *s*8 ,

in the picture's case.

**Figure 13.** Linear interpolation process to compute path cost of s using edge ⟶ *s*<sup>1</sup>  *s*2 . The subfigures illustrate possible optimal path cost (a) Calculate g(*s*) based on *s*<sup>1</sup> *, s*<sup>2</sup> (b) Case g(<sup>1</sup> ) < g(<sup>2</sup> ) (c) Case g(<sup>1</sup> ) > g(<sup>2</sup> ) and (d) Case path costs that pass arbitrary point.

graph vertices into cell corners, we also need to change finding cell centre neighbours to a pair of corner nodes as illustrated in **Figure 13**. Once the path costs of necessary nodes are computed, the path is generated by starting from the initial node and iteratively finds, using linear interpolation, the optimal node on the neighbor cell boundary to move next. The pseudo code of Field D\* and modifications in red colour are shown in **Figure 14**. Note that the differences between D\* Lite and Field D\* are highlighted in red. The function Key(), ComputePath() are the same as D\* Lite and thus is not presented. This pseudo code is a basic version of Field D\*;

Search-Based Planning and Replanning in Robotics and Autonomous Systems

http://dx.doi.org/10.5772/intechopen.71663

83

Field D\* inherits all properties of D\* Lite; it combines linear interpolation method to compute path from any point inside cell, not just corners or cell edges. This feature is crucial for robot to get back on track if the actuator execution is faulty. Moreover, Field D\* is not subjected to

In this section, using our path planning framework, we demonstrate the evaluation comparison between algorithms in search-based family in terms of performance and path

To visualise the evolution in computation of search-based algorithms, we compare the replanning computation of D\* Lite, Anytime Dynamic A\* and D\* Lite with Reset. The purpose of the comparison is to demonstrate the performance improvements of D\* Lite variants in order to apply on robot that operates in complex and dynamic environment. However, since the planning time depends on the implementation and machine configuration, we therefore choose the amount of cell expansion in each replanning iteration of search-based algorithm to be standard performance measurement of the mentioned algorithms. This method is independent on machine specifics and actual implementation and therefore firmly accurately shows the enhancement of this evaluation. The path solution ratio between AD\* with different *ε* suboptimal bound and optimal path of other algorithms is also measured to visualise the

The experiments are conducted on our 2D simulation engine. The state space is a 2D grid cell with uniform resolution [23]. The conceptual robot in this simulation has two-cell-unit range and its own known grid map to detect environment changes (unblocked cell to blocked cell

We evaluate the performance and path solution of search-based algorithms in two scenarios: partially known and unknown 2D grid environment with uniform resolution. The total expanded cells are averaged based on total replanning processes on each simulation instance, with 95% confident. The path solution of each algorithm is counted as the total cells that the robot has traversed from corner to corner of the map. We decrease the suboptimal bound of AD\* for 0.1 per step the robot travels until the suboptimal bound reaches 1.0

direction restriction; hence, it produces much shorter and smoother path.

optimised versions are presented in [3].

**5. Experimentation**

**5.1. Evaluation method**

**5.2. Evaluation results**

(optimal path).

trade-off between optimality and computation.

and vice versa) as it moves along the initial path (see **Figure 15**).

optimality.


**Figure 14.** Pseudo code of Field D\*.

between D\* Lite and Field D\* are highlighted in red. The function Key(), ComputePath() are the same as D\* Lite and thus is not presented. This pseudo code is a basic version of Field D\*; optimised versions are presented in [3].

Field D\* inherits all properties of D\* Lite; it combines linear interpolation method to compute path from any point inside cell, not just corners or cell edges. This feature is crucial for robot to get back on track if the actuator execution is faulty. Moreover, Field D\* is not subjected to direction restriction; hence, it produces much shorter and smoother path.
