Search-Based Planning and Replanning in Robotics and Autonomous Systems

Abstract

In this chapter, we present one of the most crucial branches in motion planning: search-based planning and replanning algorithms. This research branch involves two key points: first, representing traverse environment information as discrete graph form, in particular, occupancy grid cost map at arbitrary resolution, and, second, path planning algorithms calculate paths on these graphs from start to goal by propagating cost associated with each vertex in graph. The chapter will guide researcher through the foundation of motion planning concept, the history of search-based path planning and then focus on the evolution of state-of-the-art incremental, heuristic, anytime algorithm families that are currently applied on practical robot rover. The comparison experiment between algorithm families is demonstrated in terms of performance and optimality. The future of search-based path planning and motion planning in general is also discussed.

1. Introduction

Nowadays, as the rapid advances of computational power together with development of state-of-the-art motion planning (MP) algorithms, autonomous robots can now robustly plan optimal path in narrow configuration space or wide dynamic complex environment with high accuracy and low latency. These recent MP developments have a large impact in medical surgery, animation, expedition and many other disciplines. For instance, RRT [1] algorithm was applied for multi-arm surgical robot in [2]. Expedition robot GDRS XUV was implemented field D* any-angle path planner [3] that enables the robot to optimally move in harsh environment. D* [4] is implemented for Mars Rover prototypes and tactical mobile robots in [5]. Bug algorithms were implemented in multi-robot cooperation scenarios [6].

In general, the problem statement of MP can be generalised as follows: Given the initial defined world space and the robot’s configuration space, the MP algorithm must generate a series of consecutive collision-free configurations of the robot that connects start configuration and goal configuration. This series configuration must satisfy any inherent motion or non-motion constraints of the robot.

To cope with a wide range of environment characteristics, MP can be divided into two categories: gross MP and fine MP [7]. The gross MP concerns with the scenarios when world space is much wider than obstacles’ size and positional error of the robot, whereas the fine MP solves the planning problems in narrow space that requires high accuracy.

This manuscript presents the development of gross MP algorithm family, in particular search-based planning and replanning paradigm. The foundation concepts of MP, configuration space representations, and the position of mentioned paradigm in MP big picture is presented in Section 2. Section 3 describes historical basis of search-based algorithm family. Section 4 demonstrates the properties and pitfall of D* Lite, which is one of the most crucial algorithms to plan path in dynamic environment. After that, the variants of D* Lite, which improve D* Lite’s optimality and performance, are presented. To confirm the improvements, we provide experimental results of recent path planning algorithms and their comparisons in terms of performance and optimality in Section 5. Section 6 will discuss about the future development of MP and provide conclusion.

2. Motion planning concepts

This section will provide an overview of the basic elements that every MP problem must involve. These elements are configuration space of robot and obstacles, environment representation, MP method and search method. The mentioned factors must be analysed consecutively in order to apply suitable MP algorithm family for each scenario.

2.1. Classification of motion planning problems

There still does not exist unified MP algorithm that can robustly solve MP problems in any scenarios such as time optimality, path optimality, moving target, non-holonomic motion, etc. However, with the active recent development of MP, a variety of MP algorithm families are invented to deal with the mentioned scenarios. We will provide detail MP algorithm family classifications based on problem type and therefore demonstrate the location of search-based paradigm in MP.

Figure 1 describes the family tree of MP algorithms based on problem-type classification.

Figure 1.

Classification of MP algorithm families based on problem type; the deepest leaves of algorithm tree are representatives for their families.

As can be seen, MP with non-holonomic (velocity and kinodynamic) constraints, which is handled by sampling-based paradigm, is still an open research area due to the hardness of transforming high DOF robot and surroundings into configuration space. This configuration space problem has been proved to be NP hard, and computing configuration space operation has exponential lower bound [7]. Until recently, the mainstream of non-holonomic MP research is developed based on random rapidly exploring random tree planner (RRT). For example, heuristics property of A* [8] has been applied to RRT for faster trajectory convergence [9]. Fast Marching Square method was developed for non-holonomic car-like robot based on RRT that produces smoother trajectory than RRT [10].

Unlike sampling-based paradigm, search-based paradigm, which represents for path planning algorithms, has a long history of evolution, from basic graph searching to dynamic motion planner with constraints. In this paradigm, robot is treated as point or scalar robot that is able to move in any direction at any time interval. Hence, the configuration obstacle space has the same dimension with the environment, and the generated trajectory is just a path in operating environment. Search-based paradigm is divided into time-invariant and time-variant environment categories. A* is the representative for time-invariant algorithm family; its cost function is incorporated with heuristic property for faster optimal path planning. When dealing with time-variant problem, although we can ensure the optimality and correctness of path solution, we cannot just rerun A* from the point that the robot detects changes in environment due to high latency. To efficiently path replanning in dynamic environment, incremental property is combined with heuristic property to develop D* Lite algorithm; this algorithm is the basis for future development of search-based replanning. Many variants of D* Lite for different MP problems are presented in Table 1.

Problem scenarios	Algorithms
Moving target	MTD* Lite [11]
Fast/suboptimal	Anytime D* [12], truncated D* Lite [13], anytime Truncated D* [14]
Any-angle movement	Field D* [3], incremental Phi* [15]
Performance improvement	D* Lite with Reset [16]

Table 1.

Different families of D* Lite variants.

The development of search-based algorithm family is described detail in Section 3 and Section 4.

2.2. Problem statement formulation

The general MP problem can be formulated as the following six terms:

1. State space: the configuration space of the robot transformed from physical space, W.

2. Boundary values: $x_{\mathit{\text{init}}}\in W$ and $X_{\mathit{\text{goal}}}\subset W$.

3. Collision detector:$D:W\to \left\{\mathit{\text{true}},\mathit{\text{false}}\right\}$ the function to detect whether the global constraints are satisfied from robot state $x$; it can output binary or real values.

4. Input space: a set U of input, which specifies a complete set of robot operation that affects the state $x$.

5. Incremental rules: a set of rules to transition state $x\left(t\right)$ to state $x\left(t+\mathrm{\Delta }t\right)$ when an operation is input over time interval, $\left\{u\left(t_c\right)\right|t<t_c<t+\mathrm{\Delta }t\}$.

6. Metric: a real-valued function $\rho :W\times W\to \left[0,+\infty \right)$ that defines the distance between two points in state space W.

General MP is viewed as a search for path (a series of configuration) in state space W that connects start configuration $x_{\mathit{\text{init}}}$ to goal configuration region $X_{\mathit{\text{goal}}}$. The robot is incorporated with a set of global constraints (small discrete headings, velocity, balancing, etc.). We denote $W_{\mathit{\text{free}}}$ as a set of configuration that satisfies global constraints, and the generated path must be in $W_{\mathit{\text{free}}}$. The incremental rules can be considered as discrete-time response system, and together with input space, it defines possible robot state transitions. Metric can affect heavily to the algorithm’s optimality and performance; it indicates the distance between pair of points in topological space. One can construct MP algorithm to deal with specific constraints in certain environment by following these basic terms.

2.3. Environment representation

This section will describe the transformation of world space to state space. This is the first step to formulate a MP algorithm; it creates an operation environment for MP algorithm and a way to represent physical world information as data structure in computer.

2.3.1. Configuration space (C-space) transformation

The world space (physical space) is where the robot and obstacles exist; it is a map of the practical world. However, we cannot apply directly MP algorithm to this space due to the hardness of representing orientation dimension and other parameters such as motion constraints on computer. Therefore, a C-space is needed, which incorporates all independent parameters that completely define the position of all points on the robot and specifies global constraints of the robot as Cartesian space. Figure 2 [17] shows a mapping between an effector of 2DOF robot arm and a set of possible two angle parameters that constitutes C-space of the effector.

Figure 2.

Configuration space of 2DOF robot arm that represents a set of collision-free angles in white and specific object collided in colours (a) Workspace, (b) C-space.

After computing the C-space, all MP problems are basically reduced to finding a series of configuration that connects start configuration and goal configuration. In other word, the problem is reduced to finding a path for a point robot from start to goal. The number of parameters that defines robot position is the dimension of C-space. The method to compute C-space is mentioned in [7].

For simplicity, to follow the scope of this chapter, we will treat C-space of point robot the same as world space; the reason is that search-based paradigm deals with holonomic MP problem in which the size of robot is neglected compared to operating environment.

2.3.2. Continuous to discrete approximation

After transforming world space to C-space, we still cannot apply search-based algorithms to C-space. The problem is that search-based algorithms like A* or D* Lite work on graph-like structures; hence, applying search-based algorithms on continuous C-space is intractable. However, other MP algorithm families such as sampling based can apply directly to C-space. Unfortunately, the path optimality and performance of sampling-based algorithms are currently worse than state-of-the-art search-based algorithms.

There are two main approaches to discretize C-space into graph-like structure:

• Cell decomposition

• Roadmap

In cell decomposition approach, we divide C-space into eight-connected square grid environment with arbitrary resolution. Then we colour all cells that intersect with obstacle configuration with black, and other free cells are white. Figure 3 illustrates this approach.

Figure 3.

Cell decomposition approach (a) Original Objects, (b) Encoded Objects into cells.

This approximation has limited assumptions on obstacle configuration. Therefore, the approach is used widely in practice. However, there is no concept of path optimality, because we can infinitely divide C-space into smaller squares. It is a trade-off between optimality and computation. Cell decomposition in high dimensions is also expensive; it has exponential growth in PSPACE.

In roadmaps approach, the idea is avoiding scanning the entire C-space by computing an undirected graph with “road” edges that are guaranteed to be collision-free. The main methods of this approach are visibility graph [17] and Voronoi diagrams. The examples of the two methods are demonstrated in Figure 4.

Figure 4.

Path topologies of visibility graph and Voronoi diagram methods in roadmap approach (a) Visibility Graph method, (b) Voronoi method.

As can be seen, this approach generates fewer vertices than cell decomposition approach. Visibility graph method tends to generate with vertices that are the vertices of obstacles; this property leads to finding shortest path. However, the visibility graph’s roadmaps are close to obstacles; collision is inevitable due to some movement error. Voronoi diagram solves the problem by generating roadmaps that keep robot as far away as possible from obstacles.

Despite this approach constructs efficiently graph representation for search-based algorithm; it is difficult to compute in higher dimension or non-polygonal environment. The approach also can be unstable in dynamic scenarios; small changes in obstacles can lead to large changes in graph.

In the following sections, we use cell decomposition approach for search-based algorithms due to its clarity to describe the operation of search-based algorithms and its feasibility to apply in practice.

3. Search-based planning on time-invariant environment

This section demonstrates one of the most well-known algorithms in graph search family: A*. The A* algorithm’s properties are also examined and utilised to use in different cases.

3.1. A* algorithm

There are three main properties of A* [8] that are inherited from historical graph search algorithms:

• Search tree: a search tree $T$, which root is the starting cell, stores expanded cells as branches. This tree is capable to extract path to starting cell from any expanded cell $s$ in the map. A* inherits this tree from breadth-first search algorithm.

• Uniform cost search: This property includes a data structure $g\left(s\right)$ that stores the cost to travel from starting cell to any cell $s$ in the map, which is formulated as

$$\mathrm{f}\left(\mathrm{s}\right)=g\left(s\right),$$E1

where $\mathrm{f}\left(\mathrm{s}\right)$ is the priority of cell in open list $O$; the smaller the $\mathrm{f}\left(\mathrm{s}\right)$, the higher the priority. The open list $O$ handles processing expanding cells, and therefore this property prioritises expanding cells with less cost to travel. A* inherits this property from Dijkstra’s algorithm.

• Heuristic: a rule to guide expanding search towards goal cell. This rule is formulated:

$$\mathrm{f}\left(\mathrm{s}\right)=h\left(s\right),$$E2

where $h\left(s\right)$ is the heuristics function for each cell s that indicates the closeness from cell s to goal. $h\left(s\right)$ can be Euclidean distance or Manhattan distance function in this case. In addition, $h\left(s\right)$ must satisfy admissible property:

$$h\left(s\right)\le \mathit{\text{cost}}\left(s,s^{\prime }\right)+h\left(s^{\prime }\right),$$E3

for any successor $s^{\prime }$ of $s$ to ensure path optimality. A* inherits this property from greedy best-first search.

Figure 5 illustrates each property of A* when they are applied to search for goal:

Figure 5.

Operation demonstration of properties of A* and A* itself (a) Map, (b) Uniform cost search, (c) Greedy Best-First Search and (d) A*.

The total expanded cells in each algorithm constitutes for their performance (e.g. how many cells are processed before path is found). As can be seen, Dijkstra’s algorithm has the worst performance due to lack of guidance to expand search; it just expands uniformly to all directions. Greedy best-first search has the best computation; however, it does not guarantee the shortest path like Dijkstra’s algorithm, because its search is trap in local minima shown in the picture. A* has both computation and optimality advantages over these old algorithms by combining uniform cost search rule to guarantee path optimality and heuristic rule of greedy best-first search to guide search process towards goal. Both rules can be combined and formulated as priority function:

$$\mathrm{f}\left(\mathrm{s}\right)=g\left(s\right)+h\left(s\right)$$.E4

Intuitively, one could think $\mathrm{f}\left(\mathrm{s}\right)$ is an estimated cost to travel from start cell to goal through concerning cell s. Hence, A* expands towards cells that have least cost travel (Figure 6, line 11).

Figure 6.

Pseudo code of A* algorithm.

The pseudo code for A* is shown in Figure 6.

3.2. Anytime A*: path suboptimal bound (ARA*) algorithm

In practice, the performance issue is more critical; time for robot to “think” before making decision is limited. Therefore, a path planner, which has these properties, is essential:

• Quickly producing a suboptimal solution and then gradually improving its solution as time allowed by reusing its previous search effort as much as possible

• Having control over the suboptimal bound and hence indicating a bound of processing time of each search iteration

We introduce the algorithm that is well-suited for this scenario: ARA* [18].

Basically, ARA* is developed from A*; it inherits all intrinsic properties of A*. The idea to quickly plan suboptimal path is derived from inflated heuristics function [18] by a factor $\epsilon$. The search is greedier to provide solution faster, and the solution is proven to be bounded:

$$g^{\ast }\left(s\right)\le g\left(s\right)\le \epsilon \ast g^{\ast }\left(s\right),$$E5

where $g^{\ast }\left(s\right)$ is the optimal path cost from start to $s$.

The pseudo code for ARA* is shown in Figure 7.

Figure 7.

Pseudo code of ARA* algorithm.

To understand the behaviours of ARA*, we must keep in mind that ARA* violates admissible property—$h\left(s\right)\le \mathit{\text{cost}}\left(s,s^{\prime }\right)+h\left(s^{\prime }\right)$—for any successor $s^{\prime }$ of $s$. ARA* modifies A* $f\left(s\right)$ function by inflating heuristics function $h\left(s\right)$:

$$f\left(s\right)=g\left(s\right)+\epsilon \ast h\left(s\right)$$.E6

Hence, the computed path is no longer optimal. Moreover, each search iteration is no longer guaranteed to expand searching each cell at most once like A* due to decreasing $\epsilon$. However, to maintain efficiency and ensure suboptimal bound, ARA* introduces INCONS list to store local inconsistent cells as specified function:

$$g\left(s^{\prime }\right)>{min}_{s^{\prime \prime }\in \mathit{\text{pred}}\left(s^{\prime }\right)}\left(\mathit{\text{cost}}\left(s^{\prime },s^{\prime \prime }\right)+g\left(s^{\prime \prime }\right)\right),$$E7

(Figure 7, line 13) that already are expanded once and processes these cells in the next search iteration.

In general, ARA* executes consecutive search iterations with decreasing suboptimal bound; each search does not recalculate consistent cells from previous search. Therefore, the path improvement process is efficient. Theoretical properties of ARA* is described in [18].

4. Search-based replanning on time-varying environment

In real-world application, there is often a scenario that the robot initially does not know a priori information about its surroundings. We cannot encode the world space information each time the robot runs, because it is expensive, tedious, and infeasible due to rapid changes in practice. To maintain collision-free path, one can naively rerun A* to replan the shortest path from the point that the robot detects changes. However, this naïve approach will waste computation by reprocessing cells that are irrelevant to compute a new path and hence increase idle time between each search. This section will demonstrate search-based algorithms to solve mentioned problem in time-variant environment.

4.1. Incremental heuristic algorithm: D* Lite algorithm

4.1.1. D* Lite algorithm

In goal-directed navigation task, with cell decomposition approximation, the robot always observes a limited range of eight connected grids. The robot is able to move in eight directions with cost one, and it assumes that unknown cells are traversable. The robot follows the initial calculated path to goal and encounters blockage cells; it must be able to process only cells that are relevant to compute the new path. The challenge is to find these relevant cells. Figure 8 illustrates this idea.

Figure 8.

MP simulation on grid environment (a) Initial path, (b) Reset A* and (c) D* Lite.

Note that grey cells (in Figure 8) are expanded cells to compute initial path or new path when robot detects blockage cell in purple at position yellow cell. Darker grey cells are processed multiple times. As can be seen, total expanded cells in replanning process of D* Lite is 61, whereas expanded cells of rerunning A* are 75.

D* Lite [19] is developed directly from Lifelong Planning A* (LPA*) [20] for applying on mobile robot, which is a combination of Dynamic SWSF-FP [21] and A* [8]. Therefore, D* Lite possesses these properties:

• Reverse search: Unlike A*, D* Lite expands its search from goal; $h\left(s\right)$ now indicates the closeness from cell s to start cell. $g\left(s\right)$ now also stores estimated distance from goal. After searching is finished, the path from start to goal is generated by iteratively moving from cell $s$ towards neighbour cells $s^{\prime }$ that have the lowest sum $g\left(s\right)+\mathit{\text{cost}}\left(s,s^{\prime }\right)$ in greedy style.

• Heuristics: D* Lite inherits this property from A* with admissible rule. Thus, D* Lite maintains path optimality by expanding heuristically towards start cell.

• Incremental: D* Lite inherits incremental search property from Dynamic SWSF-FP; it reuses information from previous search to repair path in a series of similar searches, which is much efficient than calculating path from scratch.

The pseudo code for D* Lite is shown in Figure 9.

Figure 9.

Pseudo code of D* Lite algorithm.

In general, the pseudo code of D* Lite maintains three invariants:

• Invariant 1: $\mathit{rhs}\left(s\right)=\left\{\begin{array}{c}0\mathit{\text{if}}s=s_{\mathit{\text{start}}}\\ {\mathit{min}}_{s^{\prime }\in \mathit{\text{Pred}}\left(s\right)}\left(g\left(s^{\prime }\right)+\mathit{\text{cost}}\left(s,s^{\prime }\right)\right)\mathit{\text{otherwise}}\end{array}\right.$.

• Invariant 2: OPEN list contains exactly only local inconsistent cells $g\left(s\right)\ne \mathit{rhs}\left(s\right)$.

• Invariant 3: Priority value of cells in OPEN list is equal to its $\mathit{Key}\left(s\right)$.

At the first run, D* Lite is exactly like A*. It guarantees to expand cells at most twice in each search routine due to the concept of one-step look-ahead estimated goal distance r $\mathit{hs}\left(s\right)$ that is inherited from LPA*. r $\mathit{hs}\left(s\right)$ leads to the terms of over-consistent cell $g\left(s\right)>\mathit{rhs}\left(s\right)$ and under-consistent cell $g\left(s\right)<\mathit{rhs}\left(s\right)$. Intuitively, these concepts help propagating the inconsistency of cells to their neighbours. To maintain Invariants 1 and 2, ComputePath() function updates rhs-values of changed cells, checks their consistency and decides their membership of OPEN list accordingly. Invariant 3 is maintained by updating the OPEN list keys while expanding (Figure 9, lines 17–18). ComputePath() stops when the smallest key of OPEN list is less than $\mathit{Key}\left(s_{\mathit{\text{start}}}\right)$ or $s_{\mathit{\text{start}}}$ is consistent; this criteria indicates that cell expansion has reached target $s_{\mathit{\text{start}}}$. Theorems of D* Lite are described detail in [19].

4.1.2. Pitfall of D* Lite

Despite being an effective replanner for dynamic environment, D* Lite does have a big pitfall for certain circumstances. In fact, D* Lite is designed to be implemented in mobile robot with range sensors, in which the environment changes are perceived near the robot (the starting cell). In other word, the changes occurred at the perimeter of expansion. Therefore, D* Lite just propagates inconsistencies in a small area near the search front; the replanning process is efficient. However, the problem arises when we combine other sensors (e.g. UAV, satellite, etc.) to detect environment changes in further area near the goal. Intuitively, we can imagine a valley where $g\left(s\right)$ of each cell substitutes for its height; the goal cell has the lowest height (the bottom of the valley), and robot position (start cell) is always at valley’s edge. Suddenly, there is a change in height near the goal; D* Lite has to give enormous effort to correct the continuity of the valley slope from the bottom to the surface. Because of the overhead of storing g-value information, the correction effort now is more expensive than starting the search from scratch. This is a big limitation for multi-sensor-based robot system; the problem also makes D* Lite unreliable in high-dimensional state space.

This behaviour leads us to a problem statement: The location of environment changes with respect to goal position makes an enormous difference to efficiency of D* Lite. This problem is also addressed by the author of D* Lite in [12] as open question. In other papers, mathematical approach is used to study this pitfall in [16]. Unfortunately, the problem is still not solved thoroughly; however, there are approaches to partly overcome this pitfall in certain situation that will be presented in the following section.

4.2. Performance improvements

This section describes variants of D* Lite that partially solves the mentioned pitfall of D* Lite. Hence, these state-of-the-art algorithms improve the computation factor of D* Lite.

4.2.1. Anytime dynamic A* (AD*) algorithm

As one of the prominent properties of D* Lite, it maintains the optimality of solution paths. However, in real-world application, optimal paths are difficult to calculate due to the complexity and uncertainty of environment within available time; the paths are also quickly to become out of date because of dynamic surroundings. Moreover, the state space, which encodes global motion constraints of the robot, tends to be high dimensions. With these difficulties, D* Lite becomes unreliable when implementing in real robot.

Anytime Dynamic A* (AD*) [12], which is a combination of D* Lite and ARA* algorithm, is a trade-off between computation and path optimality. It sacrifices the shortest path to quickly generate suboptimal solution to cope with imperfect information and dynamic environment. AD* inherits these properties from its parent algorithms:

• Anytime: AD* uses inflated heuristic function to increase the greedy factor that expands aggressively towards goal while still maintaining path suboptimal bounds $\epsilon$. In addition, AD* also is capable to reuse information from previous search to improve its path. This property is inherited from ARA* to cope with complex planning scenario.

• Incremental heuristic: AD* has the ability to efficiently identify relevant cells that contribute to replan a new path when environment changes are detected. However, the heuristic function in this property is not inflated to guarantee the suboptimal bound in replanning process. This property is inherited from D* Lite to cope with dynamic environment.

In general, AD* executes a series of similar searches with decreasing suboptimal bounds to generate a series of paths with improved bounds. As environment changes are detected, the locally inconsistent cells are placed in OPEN list with uninflated heuristic keys, and AD* processes these cells to correct the outdated path. The pseudo code for AD* is shown in Figure 10.

Figure 10.

Pseudo code of AD* algorithm.

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

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. Theorems of AD* are described in detail in [12].

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 $\epsilon$ 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 iteratively repair its path, and thus overall path is near optimal.

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 $N_T$; total cell of path that exists between consecutive detection incidents is $N_P$, and the remaining path count is $N_R=N_P-N_T$; 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 $\frac{N_T}{N_P}$ is then compared with a threshold:

$$\frac{N_T}{N_P}<\propto .$$E8

If the ratio is greater than threshold$\propto$ 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 $N_R$ between consecutive detection incidents. The method to measure the complexity is to use inflated heuristic function:

$$N_R>\epsilon \ast h\left(s,s_{\mathit{\text{goal}}}\right),$$E9

where s is the current position of the robot. If $N_R\le \epsilon \ast h\left(s,s_{\mathit{\text{goal}}}\right)$, 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.

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.

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\left(\left|V\right|\right)=O\left(\left|E\right|\right)=O\left(b^d\right)$$E10

4.3. Optimality improvements: Field D* algorithm

Although cell decomposition approximation is widely used to discretize C-space for search-based 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 $\frac{\pi }{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 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 $\left\{\overrightarrow{s_1{s}_2},\overrightarrow{s_2{s}_3},\overrightarrow{s_3{s}_4},\overrightarrow{s_4{s}_5},\overrightarrow{s_5{s}_6},\overrightarrow{s_6{s}_7},\overrightarrow{s_7{s}_8},\overrightarrow{s_8{s}_1}\right\}$ that connects consecutive neighbours of $s$; the edge is $\overrightarrow{s_1{s}_2}$ in the picture’s case.

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₁ s₂.

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 $s_y$ on the edge $\overrightarrow{s_1{s}_2}$ by using the path cost (cost from the node to goal) $g\left(s_1\right)$ and $g\left(s_2\right)$:

$$g\left(s_y\right)=\mathit{yg}\left(s_2\right)+\left(1-y\right)g\left(s_1\right),$$E11

where $y$ is the distance from $s_1$ to $s_y$ (Figure 13). Given the centre cell cost $c$ and bottom cell cost $b$, we can compute the path cost of $s$ using edge $\overrightarrow{s_1{s}_2}$ as

$$g\left(s\right)=\underset{x,y}{min}\left(\mathit{bx}+c\sqrt{{(1-x)}^2+y^2}+g\left(s_2\right)y+g\left(s_1\right)\left(1-y\right)\right)$$E12

Figure 13.

Linear interpolation process to compute path cost of s using edge ⟶ s₁ s₂. The subfigures illustrate possible optimal path cost (a) Calculate g(s) based on s₁, s₂ (b) Case g(𝑠₁) < g(𝑠₂) (c) Case g(𝑠₁) > g(𝑠₂) and (d) Case path costs that pass arbitrary point.

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 $s_y$ a distance $y$ from $s_1$. (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 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*; optimised versions are presented in [3].

Figure 14.

Pseudo code of Field D*.

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.

5. Experimentation

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 optimality.

5.1. Evaluation method

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 $\epsilon$ suboptimal bound and optimal path of other algorithms is also measured to visualise the trade-off between optimality and computation.

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 and vice versa) as it moves along the initial path (see Figure 15).

Figure 15.

Simulated environment on our framework.

5.2. Evaluation results

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 (optimal path).

Figure 16 shows the speedup result throughout the evolution from Replanning A* to AD* with different $\epsilon$ suboptimal bounds as well as the trade-off of AD*. The environment is initially generated randomly obstacles that occupy 25% of the map. The initial map is then input to the robot. While the robot is moving, we randomly change the cell states that are 15% of the map, thus forcing the robot to replan its path whenever it detects environmental changes.

Figure 16.

Comparison between search-based algorithms on partially known environment with increasing map scale in terms of computation and path solution.

As can be seen, AD* has the highest performance that has least total expanded cells in replanning process; the higher the suboptimal bound, the better the performance. The reason is AD* is inflated its heuristic function to make it greedier in expanding cells towards goal. It is interesting that path solution of AD* is not much longer than optimal path. As the scale of map is increasing, the path between map corner is longer to travel, and thus, the robot is given enough time to improve its solution (path ratio with $\epsilon =6.0$ is gradually converged to the one $\epsilon =3.0$ at 1.016).

D*LR slightly improves the performance of D* Lite; it is because D*LR relies on computation differences between Replanning A* and D* Lite. In fact, the pitfall of D* Lite rarely happens in scenarios that the robot detects changes near its position. Replanning A* does not have incremental property and thus uses the highest computation.

The data confirms the fact that AD*, in average throughout the increasing map scale, improves 125% and 194% performance compared to D* Lite with $\epsilon =3.0$ and $\epsilon =6.0$, respectively. The path produces by AD* only 1% longer than optimal path in average.

Figure 17 describes the evaluation case on unknown environment. The environment is initially generated with random obstacles that occupy 15% of the map. The robot does not know the initial conditions; it will replan its path whenever it detects obstacles that do not exist in its map.

Figure 17.

Comparison between search-based algorithms on unknown environment with increasing map scale in terms of computation and path solution.

For unknown environment scenario, D*LR performs significantly better than D* Lite as increasing map scale. The reason is that if the replanned path is much longer than the initial path, which is the common case in unknown environment, the replanning process of D* Lite is also expensive. AD* still has the least computation compared to old search-based algorithm; it reduces drastically the computation of D* Lite with 845% better performance, in the case $\epsilon =10.0$, while still maintains good path solution.

6. Conclusion

In practice, motion planning algorithms can be implemented on top of navigation layer such as simultaneous localization and mapping (SLAM) for autonomous robot. While navigation layer enables the robot to perceive surrounding information and its position relative to the surroundings, motion planning layer gives the robot abilities to plan a path in surrounding environment and make decision to avoid obstacles. Because of that fact, navigation and motion planning are always paired up to enable autonomous robot to operate in dynamic and complex environment.

This chapter is a guide to comprehend the foundation of motion planning, in particular, search-based path planning algorithms. In this chapter, we present the steps to develop and formulate a motion planning problem. We also describe the evolution branches of motion planning and then focus on the development of search-based algorithm family. Each algorithm in search-based family is invented to cope with increasing demands in performance or solution quality, for the robot to operate in more complex scenarios. To reinforce the revolution statement of state-of-the-art search-based algorithms, we provide a computation and optimality comparison between search-based algorithms on partially known and unknown environment. Based on the data, we conclude that Anytime Dynamic A* is the most suitable algorithm that enables the robot to operate in cluttered and fast changing scenario.

Until recently, the mainstream of motion planning development is to enhance the performance of search-based algorithm and their solution optimality by modifying cell decomposition method. There are signals that the trajectory planning paradigm is starting to be active research field after being frozen for a decade. We expect that the future development of trajectory planning will robustly incorporate motion constraints with higher optimality and better computation. The ultimate goal of motion planning field is giving robot spatial decision planning converging to human ability.