4.2.3. Construct DM-Based Subgoal Graphs

By making use of the collision test equation shown in Equation (1), we modify the subgoal placement condition of the original construction algorithm; therefore. a collision-free subgoal graph for a certain safety radius *R* can be efficiently computed from the underlying DM.

Given an agent with safety radius *R* ≥ 0, Table 3 shows how to construct a subgoal graph, *Gs* := (*Vs*,*Es*), from the underlying DM. The entire construction process consists of two sequential phases, i.e., firstly placing collision-free subgoals on the corners of the expanded obstacle boundaries (line 45 to line 49), and secondly, adding edges to connect those subgoals which are mutually direct-h-reachable (line 50 to 53).

**Table 3.** The pseudocode for constructing DM-based subgoal graphs.

**ConstrucDMBasedSubGraph(safetyRadius** *R***)** 44. *Vs* <sup>←</sup> <sup>∅</sup>, *Es* <sup>←</sup> <sup>∅</sup> 45. **for each** *unblocked cells s* **do** 46. **for each** pair of perpendicular cardinal directions *c*<sup>1</sup> and *c*<sup>2</sup> **do** 47. **if** *C*(*R*,*s*+*c1*+*c2*)=1 **then** 48. **if** *C*(*R*,*s*+*c1*)=0∧*C*(*R*,*s*+*c2*)=0 **then** 49. *Vs* ← *Vs* {*s*} 50. **for each** s ∈ *Vs do* 51. *S*← GetDirectHReachable(*Vs*, *R*,s) 52. **for** e*ach s'*∈*S* **do** 53. *Es* ← *Es* {(*s,s'*)} 54. *Gs* ← (*Vs*,*Es*)

A time-consuming and important part in the algorithm shown in Table 3 is how to identify all direct-h-reachable subgoals from a given subgoal *s* (line 51), and this can be done by determining the direct-h-reachable area around *s*. As shown in Table 4 the algorithm proposed by Tansel Uras et al. [30] works in two steps. The first step (line 56 to 58) identifies the closest subgoals in each of the eight cardinal and diagonal directions, and the second step (line 59 to 70) incrementally finds out the other subgoals that can be reached via moves in two directions (i.e., either a diagonal direction or one of its two corresponded cardinal directions). In order to meet the strong requirements of Definition 5, we replace the Clearance(*R*, *V*, *s*, *d*) function in the original algorithm with SafeClearance(*R*, *V*, *s*, *d*), thus the resulting direct-h-reachable paths between two subgoals can be collision-free when taking safety radius *R* into consideration (e.g., the tests in line 73 ensure that every step of the incremental exploration is collision-free). For more detail about the function GetDirectHReachable, please refer to Tansel Uras' work [30].

**Table 4.** The pseudocode of determining the direct-h-reachable area of a given unblocked cell s.


4.2.4. Find Paths in DM-Based Subgoal Graphs

The algorithms shown in Table 5 are proposed by Tansel Uras et al. [30], illustrating how to search for a high-level path between two cells in the subgoal graphs. The function **FindHighLevelPath** is called to connect *s* and *s*' to the underlying subgoal graph, engage an A\* search to find a shortest collision-free path between *s* and *s'* from the updated subgoal graph (line 79), and to restore the original graph (line 80) before returning the resulting path (line 81). The function **ConnectToGraph** is called to identify all direct-h-reachable subgoals of *s* and *s*' (line 77 and line 78), and then add new edges between them into the graph (line 85)).

**Table 5.** The pseudocode for searching paths in DM-based subgoal graphs.


According to Tansel Uras' work, the real-time requirement is easy to meet because each segment of the resulting high-level path can be quickly refined by arbitrarily choosing one of the symmetric paths between its two direct h-reachable subgoals. However, this global planning strategy often fails in some dynamic environments in which some other moving agents may block the preplanned routes. To solve this problem, we introduce Learning Real-time A\* (LRTA\*), a real-time heuristic search algorithm [10], to take local changes into consideration and repeatedly plan and execute actions within a constant time interval during the runtime.

We illustrate the key idea of LRTA\* via pseudocode shown in Table 6. As long as the goal cell *s'* is not reached (line 87), the agent will follow the plan (line 88 and 89), learn (line 90), and execute the (91) cycle. The planning phase expands all the traversable, reachable cells within the range of a fixed look ahead range *R* and choose one cell *n* with the lowest *g*(*s*,*n*)+*h*(*n*,*s'*) as the immediate goal for the next move (line 89). During the learning part, the numeric value *h*(*sˆ*,*s'*) is updated to approach the real total cost, denoted by *h\**(*sˆ*,*s'*) (line 90). Finally, the agent moves by changing its current position towards the most promising cell discovered in the planning phase (line 91).

**Table 6.** The pseudocode for learning real time A\* search.


A drawback that prevents the application of LRTA\* is that agents who adopt this strategy could possibly be trapped in local minima [37]. As shown in Figure 8a, local minima often exist around certain terrain patterns such as concave regions, long distance barriers, and so on. Agents who use LRTA\* to search paths in these patterns would meaninglessly move back and forth, visit the same cells several times to correct their heuristic values before escaping from these regions (As shown in Figure 8c). However, the high-level path planned from the DM-based subgoal graph can provide an agent with waypoints which circumnavigate the collision regions (As shown in Figure 8b). Therefore, it can efficiently evade local minima by iteratively popping subgoals from the high-level path as the next waypoint to head for (as shown in Figure 8d).

**Figure 8.** Examples of using LRTA\* to search paths, with or without guidance of a high-level path provided by DM-based subgoal graph. (**a**) A DM with safety radius R=1.5 constructed on a concave blocked region; (**b**) The high-level path connecting the start and the goal cells can be searched from the DM-based subgoal graph; (**c**) The trajectory from the start cell to the goal cell searched by LRTA\*, without guidance of the subgoal graph; (**d**) Making use of the subgoal graph, LRTA\* visits much fewer cells when heading for the goal cell, and can efficiently deal with dynamical terrain changes which do not destroy connectivity.
