Open Access
This article is
 freely available
 reusable
Appl. Sci. 2019, 9(10), 2029; https://doi.org/10.3390/app9102029
Article
Speed Optimization for Incremental Updating of GridBased Distance Maps
College of Systems Engineering, National University of Defense Technology, Changsha 410073, China
^{*}
Author to whom correspondence should be addressed.
Received: 2 April 2019 / Accepted: 13 May 2019 / Published: 16 May 2019
Abstract
:In the context of robotics and game AI, gridbased Distance Maps (DMs) are often used to fulfill collision checks by providing each traversable cell maximal clearance to its closest obstacle. A key challenge for DMs’ application is how to improve the efficiency of updating the distance values when cell states are changed (i.e., changes caused by newly inserted or removed obstacles). To this end, this paper presents a novel algorithm to speed up the construction of DMs on planar, eightconnected grids. The novelty of our algorithm, Canonical Ordering Dynamic Brushfire (CODB), lies in two aspects: firstly, it only updates those cells which are affected by the changes; secondly, it employs the strategy of Canonical Ordering from the fast path planning community to guide the direction of the update; therefore, the construction requires much fewer cell visits and less computation costs compared to previous algorithms. Furthermore, we propose algorithms to compute DMbased subgoal graphs. Such a spatial representation can be used to provide highlevel, collisionfree roadmaps for agents with certain safety radius to engage fast and rational path planning tasks. We present our algorithm both intuitively and through pseudocode, compare it to competing algorithms in simulated scenarios, and demonstrate its usefulness for realtime path planning tasks.
Keywords:
distance map; incremental algorithms; canonical ordering; path planning; subgoal graph1. Introduction
In the context of collision check and path planning in robotics and game AI, the Distance Map (DM) has been widely used as a consistent model to encode the search space [1,2,3,4,5]. In a gridbased environment with regions of blocked cells, a corresponding DM can be constructed to provide each cell a maximal clearance value, which registers the distance from itself to the nearest obstacle. Thus, a DM can help an agent (e.g., a NonPlayer Character (NPC) in the video game or a robot in the real world) with a certain safety radius to efficiently search out collisionfree paths and to avoid obstacles in motion. Figure 1 presents a DM constructed in an indoor environment.
In many practical applications, the underlying environments that an agent maneuvers in are often dynamic; therefore, it is necessary to reconstruct their corresponding DMs whenever changes of cell states are observed (e.g., an obstacle is inserted, removed, reshaped, or transferred). Since such changes usually occur within a relatively neighboring area around the agent, only portions of the previously constructed DM need repair. To make use of this localized feature, existing algorithms such as Dynamic Brushfire [6] and its subsequent variants [7,8] aim to speed up the reconstruction by launching a wavefront from the source of the state changes to incrementally repair the distance values, rather than reconstructing the whole DM from scratch. With such a localized mechanism, only those cells that are actually affected by the wavefront need to be handled; thus, in most cases, the computation costs can be efficiently reduced.
However, for all of the previously proposed algorithms, the propagation of the wavefronts simply expands all the neighbors of a processed cell without preference, and then inserts the newly affected neighbors into a priority queue, so as to prepare for the next round propagation. Such indiscriminate expansion results in a much longer priority queue and thus becomes an efficiency bottleneck. In order to reduce the number of elements which need to be sorted by the priority queue, a searching strategy, Canonical Ordering, is introduced by us to systematically choose a single route from the equivalent propagation paths. Figure 2 shows how our algorithm propagates the wavefront form the source blocked cell (denoted as yellow tiles), visiting each affected cell only once (where the red arrows denotes the propagation directions of the wavefronts). We choose the successors of an expanded cell, s, by following two basic rules described in Nathan R. Sturtevant and Steve Rabin’s paper [9]. That is, for a cell s, with the propagation direction c that was previously used to reach s, (1) If c to arrive at s is one of the four cardinal directions, the only legal direction at s for the next round propagation is c; (2) If the c to arrive at s is one of the four diagonal directions which can be decomposed into two perpendicular cardinal components c_{1} and c_{2}, the legal directions at s for the next round propagation are c, c_{1}, and c_{2}. We name our algorithm Canonical Ordering Dynamic Brushfire (abbreviated to CODB in the rest of this paper).
Since a DM stores the maximal clearance for each traversable cell to its nearest obstacle, operations such as collision checks can be simplified to instant lookup queries (As shown in Equation (1)). For instance, if an agent with safety radius R> 0 is located on cell s, then the result of the collision check is determined by Equation (1) as below (where 1 denotes collision detected and 0 denotes collisionfree):
$$C\left(R,s\right)=\{\begin{array}{c}1\\ 0\end{array}\begin{array}{c}if\text{}\left(RDM\left(s\right)\right)\ge 0\\ if\text{}\left(RDM\left(s\right)\right)0\end{array}$$
To make better use of this feature, we furthermore propose algorithms to construct DMbased subgoal graphs. The resulting subgoal graphs are sparse but adequate highlevel roadmaps to enable agents who possess safety radius to search out collisionfree paths in real time. In order to reduce the possibility of replanning caused by dynamic terrain changes, we introduce Learning Real Time A* (LRTA*) [10], an algorithm for planning immediate moves at runtime, to drive the agents between subgoals connected by the highlevel paths. Since each segment of the highlevel paths is proved to be direct hreachable, irrational behaviors such as trapping in local minima can be eliminated when LRTA* is iteratively applied between the directhreachable subgoals (see the definition of direct hreachable in Section 4.1.3).
There is another space representation; i.e., gridbased Voronoi diagrams can be used as a sparse model to help agents to maximize its distance to the obstacle cells. Actually, current algorithms for building gridbased Voronoi graphs can also obtain a corresponding distance map in which each cells keeps the distance to its nearest obstacle cell [11,12,13]. Although a Voronoi diagram can provide an agent with a sparser search space, its drawback is also prominent. For instance, in MultiAgent Pathfinding Problems (MAPF) [14,15,16,17,18,19], a group of coordinated agents share the same Voronoi edges as their search space, thus a dense cluster of conflicts may occur, which needs to resolve during the planning process. Different from gridbased Voronoi Diagram, DMbased subgoal graphs don’t conservatively compress the search space in unnecessary narrow channels. Furthermore, the search space between each pair of the directhreachable subgoals commonly reserve more spaces than Voronoi edges for a group of coordinated agents to resolve conflicts.
We provide three main contributions in this paper. Firstly, we present an algorithm, Canonical Ordering Dynamic Brushfire (CODB), to speed up the incremental update of gridbased Distance Maps (DMs). Secondly, we propose algorithms to compute DMbased subgoal graphs which are used to provide highlevel, collisionfree roadmaps for agents with certain safety radius. Thirdly, we verify that under the guidance of the subgoal graphs, realtime search algorithms such as LRTA* can effectively avoid local minima; therefore, the resulting trajectories can successfully coincide with the optimal solutions searched by A*. We present our algorithms both intuitively and through pseudocode, compare them to current approaches on typical scenarios, and demonstrate their usefulness for fast path planning tasks.
The outline of this paper is as follow: Section 2 discusses related studies on DMs, Canonical Ordering, and subgoal graphs; Section 3 gives preliminaries and notations; Section 4 presents our algorithms both intuitively and through pseudocode; Section 5 compares CODB to other algorithms and tests the usefulness of DMbased subgoal graph for fast path planning tasks. This paper ends with conclusions in Section 6.
2. Related Work
2.1. GridBased Distance Maps
In the context of robotics and game AI, the gridbased DM is a popular spatial representation applied in navigation and motion planning tasks. The principal component of the recent approaches for constructing or reconstructing gridbased DMs is the wellknown Brushfire algorithm [20]. Intuitively, Brushfire launches wavefronts to propagate changes of maximal clearance (i.e., changes caused by insertion or deletion of obstacle cells), updating distance values from the source of the change, and terminates when the change does not affect any more cells. Brushfire represents the OPEN list as a priority queue to incrementally record the affected cells and propagate the wavefronts. The priority of an element in the OPEN queue is determined by its newly updated distance and all these elements are popped up in increasing priorities. Sequentially, new cells which are adjacent to the popped one are tested, among which, newly updated cells will again be inserted into the OPEN list so that the propagation continues.
Kalra et al. [6], in their fundamental work, proposed a dynamic version of Brushfire algorithm, Dynamic Brushfire, to incrementally update gridbased DMs by propagating two kinds of wavefronts named “lower” and “raise” which start at newly blocked or freed cells, respectively; therefore, the update can be constrained within local areas. However, the wavefronts launched by Dynamic Brushfire roughly accumulate 8connected grid steps to approximate maximal clearance, which overestimates the true Euclidean distances and would possibly lead to either a collision risk or overly conservative movements. To this end, Scherer et al. [21] proposed a method to propagate obstacle locations rather than counts of the grid steps, which reduces the absolute overestimation error below an upper bound of 0.09 pixel units. In the method proposed by Cuisenaire and Macq [22], the shortest distance at which this propagation error can occur is 13 pixels, which yields a maximum relative error of 0.69%. Regarding propagating obstacle references, Lau et al. proposed an approach to provide the location of the closest obstacle rather than just the distance to it, which can be appealing for collision check tasks [23]. Moreover, Lau et al. extended their method to 3D by adding the possibility to limit the propagated distances to maintain online feasibility in large open spaces and outdoors as proposed by Scherer et al.
Although these dynamic algorithms are fast and efficient for dealing with local changes, they just indiscriminately expand all the adjacent cells surrounding a currently processed cell, which results in a lot of redundant cell visits and scales up the size of the OPEN list and restricts the overall efficiency of the algorithm. We introduce the Canonical Ordering strategy in our work to prune the search space.
2.2. Canonical Ordering
The idea of applying Canonical Ordering as a speedup technique for realtime pathfinding systems that operate on regular grids was proposed by Daniel Harabor [24] and N Pochter [25]. As mentioned in the literature, searching in grids often becomes overwhelmed by a high degree of path symmetry, which accounts for a major part of the computational costs. Two paths are viewed as symmetric if (1) they have the same start and goal cells; (2) they are of the same length; and (3) their respective sequences of moves (i.e., cardinal or diagonal moves) can be reordered into the other. With symmetries in the grids, a search task will explore multiple cells for multiple times from those symmetric paths and this severely undermines the efficiency.
To break such symmetries, an online algorithm called Jump Point Search (JPS) [26] was presented by Daniel Harabor et al. to apply Canonical Ordering to recursively prune redundant successors and selectively expand only certain cells, called jump points. Canonical Ordering is essentially a special case of partial orderings among all the symmetric paths and prefers the diagonalfirst ones to other alternatives. We say that a path has the diagonalfirst property if there is no straightdiagonal turning point can be mutated into a diagonalstraight one of the same length constrained by the obstacles. By its virtue, JPS visits much fewer cells than traditional searching strategies; therefore, it answers a path query averagely faster than A* by an order of magnitude. After that, this algorithm’s performance was further improved by a preprocessing based strategy and addition of Bounding Boxes, resulting in the algorithms JPS+ [26] and JPS+BB [27]. As an automatic move pruning technique for singleagent search [28], Canonical Ordering can not only be used in grids, but can also be built on general graphs and considerably reduce the number of cells generated by an A* search [29].
The outstanding performance of the Canonical Ordering strategy in compressing search space for realtime pathfinding algorithms provides us with a novel method to guide the direction of the wavefronts which propagate the distance changes, making it possible to speed up the construction of gridbased DMs.
2.3. Subgoal Graphs
A subgoal graph is a kind of sparse spatial representation which can be precomputed by abstracting the skeletons of the underlying grids into undirected graphs. The algorithm which accounts for computing subgoal graphs was proposed by Tansel Uras et al. [30]. It firstly introduces the basic version of the subgoal graph, called Simple Subgoal Graphs (SSGs). On grid maps, SSGs are constructed by placing subgoals at the convex corners of obstacles and connecting directhreachable subgoals in the preprocessing stage. Two subgoals are mutually directhreachable if all the optimal paths between them are valid and traverse no other subgoals.
SSGs reduce the search space and accelerate the process of finding shortest paths by abstracting the key points of the grid maps and reducing all symmetric paths between subgoals into only one edge. It can be proved that, for any given start and goal cells which are reachable, there is a shortest path that can be divided into segments between subgoals. Each segment connects directhreachable subgoals. Therefore, when finding shortest paths on SSGs, one first connects the start and goal vertices to their directhreachable subgoals in SSGs, and then searches the modified graphs via A* to get the shortest highlevel path, which consists of a sequence of subgoals between the start and goal vertices. By refining the shortest highlevel path, one can get the shortest path between start and goal on the grid map.
Moreover, a Twolevel Subgoal Graph (TSG) is constructed from a SSG by partitioning the subgoals into global and local ones and only the global ones belong to the TSG. When removing the local subgoals, one has to add some extra edges so that the shortest paths between global subgoals remain the same. Finding the shortest path on a TSG is similar to finding a shortest path on a SSG. One first connects the start and goal vertices to their directhreachable subgoals, among which the local ones should be temporarily connected to the TSG. Then, the search and refining processes are executed. It should be noted that the refining process of TSGs is slower than SSGs because TSGs connect hreachable subgoals instead of directhreachable subgoals. But, TSGs still find optimal paths faster due to the smaller search space. To reduce the search space even further, Tansel Uras et al. generalized the idea of partitioning [31,32], created a hierarchy among the vertices, and repeatedly divided the highestlevel subgoals into global and local subgoals and increased the level of global subgoals by one. The resulting graphs are called NLevel Subgoal Graphs.
Subgoal Graphs can be used not only to find grid paths, but also to find anyangle paths which are shorter and more realistic [33,34,35]. Tansel Uras and Sven Koenig [36] exploited the similarities between Subgoal Graphs and visibility graphs and used Subgoal Graphs to quickly find anyangle paths with some small modifications. Their algorithm is up to two orders of magnitude faster than Theta*, a wellknown anyangle path planning algorithm.
All the above algorithms only take regular grids as input and build subgoal graphs which do not take physical radius into considerations. Therefore, the paths planned from the resulting subgoal graphs would possibly be unable to meet the collisionfree requirements in practical application. In this paper, we attempt to modify the existing algorithm so that it can make use of the maximal clearance values provided by the gridbased DMs and to build new type of subgoal graph that can efficiently search collisionfree paths.
3. Preliminaries and Notation
All the algorithms studied in this paper work in planar, eightconnected grid maps with regions of obstacles consisting of blocked cells. A wavefront launched by a cell state change can propagate from one cell to its neighbor in any cardinal or diagonal directions, and the length of the cardinal and diagonal moves are 1 and $\sqrt{2}$, respectively. We follow the definition of octile distance to compute the heuristic distance between any two cells in a grid map, i.e., the distance between cells s and s’ is computed by following Equation (2):
$$dist\left(s,{s}^{\prime}\right)=\sqrt{2}\times min\left(dx,dy\right)+\leftdxdy\right$$
In Equation (2), dx and dy denote the differences of the 2D coordinates of cell s and s’. For each cell s, obst_{s} maintains coordinates of the obstacle cell s_{o} to which s is currently closest, and dist_{s} maintains the distance between s and s_{o}. The notation dir_{s} maintains the direction along with which s would propagate the wavefront. The options of dir_{s} include four cardinal directions (denoted as left = 1, up = 3, right = 5, and down = 7), four diagonal directions (denoted as upleft = 2, upright = 4, downright = 6, and downleft = 8), and full directions (denoted as fulldir = 0, declaring that s is the source of the wavefront; thus, all the eight directions need to be considered in the next round propagation). The notation raise_{s} shows if the wavefront on s is a raise wavefront or a lower one (the differences between raise and lower wavefronts are explained in Section 4.1.1). Given a cell s, a direction d, and an integer k, the notation s’ = s + kd denotes a cell s’ that is reached from s by moving k steps along d. For two perpendicular directions c_{1} and c_{2}, we have d=c_{1}+c_{2} to denote that the sum of c_{1} and c_{2} results in their corresponding diagonal direction d.
4. The Methodology
4.1. Algorithm Intuition
Figure 3 shows the flowchart describing the main steps of CODB and its application in realtime pathfinding tasks.
4.1.1. Lower and Raise Wavefronts
Following the basic ideas of Dynamic Brushfire algorithms, CODB employs two kinds of wavefront, lower and raise wavefronts, to incrementally update DMs. CODB keeps a priority queue (denoted as OPEN) to sort the cells to be explored by the wavefronts. A cell’s priority is determined by its dist_{s} value and is dequeued with increasing priority values. When a cell is popped from OPEN, the distance change on it will be propagated to its adjacent cells and any inconsistent cells (i.e., cells which are affected by the wavefront) are again put on OPEN, so as to prepare for the next round of propagation.
As shown in Figure 4a, when an obstacle cell (red dot denoted as s) is newly inserted into the center of the grids, it firstly sets dist_{s} as 0 and obst_{s} as its own coordinates; then, a so called “lower wavefront” is launched to propagate a distance reduction to its adjacent cells (as shown in Figure 4b). This propagation emanates from s, the source of the change, and terminates when the distance change no longer affects anymore cells (as shown in Figure 4c,d, the wavefront encounters cells which keeps equal clearance to other obstacle cells and thus failed to continue the propagation.). Finally, new boundaries of the updated DM are reached and the distance values of all affected cells are updated (As shown in Figure 4e).
On the other hand, when an obstacle cell is removed (e.g., as Figure 5a shows, we again remove the center obstacle cell s from the map), all the cells whose obst and dist value are computed based on s become invalid; therefore, a so called “raise wavefront” is launched to reset these invalid cells, declaring that they can be then updated by other lower wavefronts (as shown in Figure 5b). The “raise wavefront” terminates at the boundaries between s and other obstacle cells (as shown in Figure 5c), then lower wavefronts on the other sides of the boundaries are permitted to continue their propagation until the invalid region is again submerged (as shown in Figure 5d) and Figure 5e).
4.1.2. Propagation in Canonical Ordering
To speed up the construction efficiency, we introduce the concept of Canonical Ordering to guide the propagation of wavefronts. The application of Canonical Ordering has been recently discussed to speed up bestfirst search in gridsbased pathfinding tasks in [24]. Different from the classical Dynamic Brushfire algorithm, which expands all the eight directions at every step, Canonical Ordering can eliminate redundant, symmetric paths from the source of state change, which accounts for the main part of the speed improvement.
As shown in Figure 6a, in an open grid map without obstacles, a wavefront launched by a cell (denoted as s) on the left top can follow three different optimal paths to propagate its distance change to the traversable cell (denoted as g) on the right bottom. The only difference between these paths relies on the permutation of actions of moving right one cell and moving downright two cells. It is clear to see that the number of possible paths grows exponentially if we enlarge the grid map and keep g in the right bottom corner. Since the existing Dynamic Brushfire algorithm generates the same cell along different paths, such propagation would trigger large numbers of pushing and sort ascending operations in OPEN list, which can be computationally expensive.
To eliminate these redundancies, Canonical Ordering insists on a preference rule, i.e., distance changes first propagate diagonally before propagating cardinally whenever possible along a path (e.g., as shown in Figure 6a, by following this rule, only the gray path is tested.). With such a preference, a source cell in an open grid map returns a unique path from itself to each affected cell. As shown in Figure 6b a wavefront launched from the center cell (denoted as a yellow cell) draws lines diagonally and then branches vertically and horizontally, extending from the diagonals. Since every affected cell owns exactly one path from the source cell, the propagation is no longer a graph, but a tree.
4.1.3. DMBased Subgoal Graph
In order to introduce the concept of maximal clearance provided by DMs into the precomputing algorithms of subgoal graphs, we extend the formal definitions of the traditional subgoal graph as follows:
Definition 1.
For two cells s and s’, let dx and dy be the respective distances between s and s’ along the x and y axes. The shortest trajectory between s and s’ is a permutation of exactly min(dx,dy) diagonal and dxdx cardinal moves, for a total of max(dx,dy) moves.
Definition 2.
Given a safety radius R, a collisionfree path between two cells s and s’ is the shortest trajectory in which each cell s^{^} is collisionfree, i.e., C(R,s^{^})=0.
Definition 3.
Given a safety radius R, an unblocked cell s is a collisionfree subgoal if there are two perpendicular cardinal directions c_{1} and c_{2} such that C(R,s+c_{1}+c_{2})=1, C(R,s+c_{1})=0 and C(R,s+c_{2})=0.
Definition 4.
Given a safety radius R, two cells s and s’ are hreachable if there is a collisionfree path of octile length h(s,s’) between them. Two hreachable cells are safehreachable if all shortest trajectories between them are also paths. Two safehreachable cell s and s’ are directhreachable if none of the shortest paths between them contains a subgoal s’’ $\notin $ {s,s’}.
Definition 5.
Given a safety radius R, an DMbased subgoal graph, Gs =(R,Vs,Es), on its corresponded grids, is a highlevel, undirected graph, where Vs is the set of collisionfree subgoals and Es is the set of edges connecting directhreachable subgoals, and the length of the edges is the octile distances between the subgoals they connect.
The process of constructing an DMbased subgoal graph can be intuitively divided into two steps: (1) placing collisionfree subgoals at the corners of the expanded obstacle boundary to circumnavigate the collision regions (e.g., the orange circles shown in Figure 7a); (2) adding edges between those subgoals which are mutually directhreachable (e.g., the green edges shown in Figure 7b). Given a start and a goal cell (e.g., the blue and red discs shown in Figure 7b), we first connect them to their respective directhreachable subgoals and then plan a highlevel shortest path by executing A* on the updated subgoal graph (e.g., the white trajectory shown in Figure 7b). We can refine each segment of the highlevel path by arbitrarily selecting an hreachable path between two connecting subgoals, and then orderly appending these refined paths together. Moreover, as shown in Figure 7c,d, for agents with different safety radius, their corresponding DMbased subgoal graph, according to Definition 5, can set aside adequate clearance; therefore, the paths found from the resulting graphs can be collisionfree.
4.2. Algorithm Pseudocode
4.2.1. Initiate the DM
Table 1 presents pseudocode for the initialization of a DM, including three functions, i.e., (1) initialize() to set aside certain space for required data structures and then initial their values; (2) setObstacleCell(o) to register a newly inserted obstacle cell; and (3) removeObstacleCell(o) to reset an removed obstacle cell. Upon initialization, all the cells are set to be traversable and undetermined, declaring that there is no obstacle cell, neither in the map, nor in finite distance (lines 1 to 7). When a cell o is marked as an obstacle cell by calling setObstacleCell(o), it sets dist_{o} as 0 and refers to itself as the closest obstacle cell, i.e., obst_{o} = o (lines 8 and 9). Conversely, when o is freed by calling removeObstacleCell(o), the function resetCell(o) resets it to the initial values, i.e., dist_{o} = $\infty $ and obst_{o} = ∅ (line 11), and raise_{o} is set as true (line 12). The function insert (OPEN, c, d) inserts a cell c into OPEN with a priority value d, or updates the priority if c is already in OPEN.
4.2.2. Update the DM
Table 2 presents pseudocode for updating the DM. The function update() orderly pops the next unprocessed cell s with the lowest dist_{s} until the OPEN queue is empty (lines 14 and 15). If s is cleared and is not yet propagated by a raise wavefront, the function raise() is called (lines 16 and 17). However, if s has a valid closest obstacle cell, the function lower() is called (lines 18 to 20).
Newly inserted obstacle cells call function lower() to launch a lower wavefront to propagate the reduction of dist and obst values from the currently popped cell s to its affected cells (lines 26 to 29). The lower wavefront continues when the distance value (denoted as d which is computed in line 24) between obst_{s} and the newly expanded cell n holds the trend of distance reduction (being determined in line 25). Simultaneously, newly freed cells call function raise() to launch a raise wavefront, resetting the cells whose closest obstacle cell was the freed one (line 33 to 36). The raise wavefront terminates when it reaches those cells whose closest obstacle is valid; thus, a lower wavefront launched by n is generated (line 37). During the interwoven of these two wavefronts, inconsistent neighbors affected by the processed cell are again put on OPEN (line 29 and 38), thus the propagation continues.
Instead of propagating a wavefront along with full directions, we introduce a function findSuccessors(s) to filter cells by employing the rule of canonical ordering (line 21 and 30). As illustrated in Figure 6b, only the successors of a cell in the direction of the arrows are chosen as the candidates of the next round of propagation, while the others are ignored. For a cell n conducting a wavefront from one of its adjacent cell s, function direction(s,n) determines the direction from s to n and accordingly returns the value of dir_{n} (line 28 and 36).
4.2.3. Construct DMBased 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 collisionfree subgoal graph for a certain safety radius R can be efficiently computed from the underlying DM.
Given an agent with safety radius R $\ge $ 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 collisionfree 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 directhreachable (line 50 to 53).
A timeconsuming and important part in the algorithm shown in Table 3 is how to identify all directhreachable subgoals from a given subgoal s (line 51), and this can be done by determining the directhreachable 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 directhreachable paths between two subgoals can be collisionfree when taking safety radius R into consideration (e.g., the tests in line 73 ensure that every step of the incremental exploration is collisionfree). For more detail about the function GetDirectHReachable, please refer to Tansel Uras’ work [30].
4.2.4. Find Paths in DMBased Subgoal Graphs
The algorithms shown in Table 5 are proposed by Tansel Uras et al. [30], illustrating how to search for a highlevel 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 collisionfree 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 directhreachable subgoals of s and s’ (line 77 and line 78), and then add new edges between them into the graph (line 85)).
According to Tansel Uras’ work, the realtime requirement is easy to meet because each segment of the resulting highlevel path can be quickly refined by arbitrarily choosing one of the symmetric paths between its two direct hreachable 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 Realtime A* (LRTA*), a realtime 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).
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 highlevel path planned from the DMbased 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 highlevel path as the next waypoint to head for (as shown in Figure 8d).
5. Experiments and Results
In this section, we employ statistical methods to compare our algorithm with other competing methods on certain simulated scenarios. We also demonstrate the usefulness of the DMbased subgoal graphs to realtime path planning tasks. Our algorithms are implemented in C++, running on an Intel® Core(TM) i74790 CPU @ 3.60GHz.
5.1. Comparison to Other Algorithms
We compare CODB with other three competing algorithms (i.e., Brushfire, Dynamic Brushfire, and algorithms proposed by Boris Lau et al. [23] (we abbreviate it as BL below)) which are discussed in Section 2 in four typical scenarios as shown in Figure 9.
We set aside four grid maps (from Figure 9a to Figure 9d) which have the same size (100 × 100) while allowing different proportions of the obstacles (i.e., fully reconstruction, 75%, 50%, and 25% respectively) to randomly change their positions or shapes. After each set of changes, we use the above mentioned four algorithms to construct or reconstruct the DMs. We run each algorithm on each scenario 100 times, and the comparisons of the performances among these algorithms are shown in Table 7 and Figure 10 (i.e., the average computation time scaled in milliseconds and its variance) and Table 8 and Figure 11 (i.e., the maximal size of the priority queue OPEN during construction and its variance).
For the first scenario in which full reconstructions are inevitable since all the obstacles are moved or reshaped, the extra operations that enable local repairs make Dynamic Brushfire (8.61 ms) and BL (7.22 ms) slower than their static counterpart, Brushfire (6.34 ms). However, due to the integration of Canonical Ordering strategy, CODB can substantially prune the search space; thus, it still maintains an obvious advantage over the other three algorithms in time efficiency (4.26 ms), even in the case of full reconstructions. As for the other three scenarios, as the proportion of the dynamic obstacles decreases, dynamic algorithms gradually manifest their superiority in speed. Among the three dynamic algorithms, CODB is also faster than the others due to fewer cell visits, reducing the computation time by at least 50%.
5.2. Application Tests on RealTime Pathfinding
This work is motivated by the need to engage realtime and collisionfree pathfinding tasks for agents with a certain safety radius. Such agents often maneuver in dynamic, crowded environments, and have to decide their next move in a limited amount of time. In order to demonstrate the usefulness of our algorithm in such scenarios, two agents (denoted as agent X and agent Y) working in a grid map of size 200 × 200 (as shown in Figure 12) are simulated. For each round of testing, both of the two agents are assigned the same start and goal cells located on the top right and left bottom of the grid map, respectively. The resulting paths of Agent X and Y are respectively shown in Figure 12a,b as light green trajectories; by contrast, the optimal path searched by the classical path planning algorithm, A*, is marked as red trajectories. Agent X simply adopts LRTA* to directly search and move in the underlying DM, while agent Y shown in Figure 12b would firstly search out a highlevel path from the DMbased subgoal graph, and then engages LRTA* to search and move between the segments of the path. In order to gain the average performance, we run the test of each of the ten pairs of start and goal cells 100 times.
For agent X, applying LRTA* to engage a realtime search can help the agent successfully avoid collisions with obstacles, but it is very easy for the underlying DM to generate local minima around typical locations such as concaves, narrow channels, long distance barriers, and so on. To the cells within these locations, the occurrence of the local minima results in an increase in errors between the default heuristic values and the actual values. Therefore, LRTA* has to recheck these cells many times to incrementally correct their heuristic values (as shown in line 90, Table 6). Only in this way can the heuristic depressions be gradually filled up and finally drive the agent to escape from the local minima. Unfortunately, such runtime correction incurs meaningless movements (As the regions labeled as A, B, and C in Figure 12a) which are unacceptable to the requirements of engaging reasonable behavior.
As for agent Y, a highlevel path (as the red path shown in both Figure 12a,b, is firstly searched out from the DMbased subgoal graph by executing the algorithms proposed in Table 5 (in order to make the figures clear, we only retain the subgoals and do not draw the directhreachable edges). The resulting path provides a list of sparse and directhreachable waypoints to circumnavigate the collision regions. Based on the path, agent B uses LRTA* at runtime to search and move along the path; therefore, it can efficiently avoid local minima and terminate meaningless movements.
6. Conclusions
In this paper, we present an algorithm, Canonical Ordering Dynamic Brushfire (CODB), to speed up the incremental updating of gridbased Distance Maps (DMs). CODB only updates those cells which are affected by local changes, and it employs the strategy of Canonical Ordering to guide the search direction; therefore, the algorithm requires much fewer cell visits and less computation costs compared to its competing approaches. Furthermore, we propose algorithms to compute DMbased subgoal graphs which are used to provide highlevel, collisionfree roadmaps for agents with certain safety radius to engage fast and rational path planning tasks. We present our algorithms both intuitively and through pseudocode, compare them to current approaches on typical scenarios, and demonstrate their usefulness for fast path planning tasks.
Author Contributions
Conceptualization, L.Q., Y.H. and J.Z.; Data curation, L.Q.; Formal analysis, L.Q.; Funding acquisition, Q.Y.; Methodology, L.Q., Y.H. and J.Z.; Project administration, Q.Y.; Resources, Q.Y.; Software, L.Q.; Supervision, Q.Y.; Writing—original draft, L.Q.; Writing—review & editing, L.Q., Y.H. and J.Z.
Funding
This work described in this paper is sponsored by the National Natural Science Foundation of China under Grant No. 61473300.
Acknowledgments
This work described in this paper is sponsored by the National Natural Science Foundation of China under Grant No. 61473300. We appreciate the fruitful discussion with the Sim812 group: Qi Zhang, Kai Xu, Weilong Yang, and Cong Hu.
Conflicts of Interest
The authors declare no conflict of interest. The founding sponsors had no role in the design of the study; in the collection, analysis, or interpretation of data; in the writing of the manuscript, and in the decision to publish the results.
References
 Chen, L.; Chuang, H.Y. A Fast Algorithm for Euclidean Distance Maps of a 2D Binary Image. Inf. Process. Lett. 1994, 51, 25–29. [Google Scholar] [CrossRef]
 Yazici, A.; Kirlik, G.; Parlaktuna, O.; Sipahioglu, A. A Dynamic Path Planning Approach for Multirobot SensorBased Coverage Considering Energy Constraints. IEEE Trans. Cybern. 2017, 44, 305–314. [Google Scholar] [CrossRef] [PubMed]
 Borgefors, G. Distance Transformations in Digital Images. Comput. Vis. Gr. Image Process. 1988, 34, 344–371. [Google Scholar] [CrossRef]
 Fabbri, R.; Costa, L.D.F.; Torelli, J.C.; Bruno, O.M. 2D Euclidean Distance Transform Algorithms: A Comparative Survey. ACM Comput. Surv. 2008, 40, 1–44. [Google Scholar] [CrossRef]
 Charrow, B.; Kahn, G.; Patil, S.; Liu, S.; Goldberg, K.; Abbeel, P.; Michael, N.; Kumar, V. InformationTheoretic Planning with Trajectory Optimization for Dense 3D Mapping. Robot. Sci. Syst. 2015, 11. [Google Scholar]
 Kalra, N.; Ferguson, D.; Stentz, A. Incremental Reconstruction of Generalized Voronoi Diagrams on Grids. Robot. Auton. Syst. 2009, 57, 123–128. [Google Scholar] [CrossRef]
 Lau, B.; Sprunk, C.; Burgard, W. Efficient GridBased Spatial Representations for Robot Navigation in Dynamic Environments. Robot. Auton. Syst. 2013, 61, 1116–1130. [Google Scholar] [CrossRef]
 Qin, L.; Yin, Q.; Zha, Y.; Peng, Y. Dynamic Detection of Topological Information from GridBased Generalized Voronoi Diagrams. Math. Probl. Eng. 2013, 2013, 1–11. [Google Scholar] [CrossRef]
 Sturtevant, N.R.; Rabin, S. Canonical Orderings on Grids. In Proceedings of the TwentyFifth International Joint Conference on Artificial Intelligence, IJCAI 2016, New York, NY, USA, 9–13 July 2016; pp. 683–689. [Google Scholar]
 Bulitko, V.; Lee, G. Learning in RealTime Search: A Unifying Framework. J. Artif. Intell. Res. 2006, 25, 119–157. [Google Scholar] [CrossRef]
 Lucet, Y. New Sequential Exact Euclidean Distance Transform Algorithms Based on Convex Analysis. Image Vis. Comput. 2009, 27, 37–44. [Google Scholar] [CrossRef]
 Schouten, T.E.; Den Broek, E.L. Fast Exact Euclidean Distance (FEED): A New Class of Adaptable Distance Transforms. IEEE Trans. Pattern Anal. Mach. Intell. 2014, 36, 2159–2172. [Google Scholar] [CrossRef] [PubMed]
 Tsardoulias, E.G.; Serafi, A.T.; Panourgia, M.N.; Papazoglou, A.; Petrou, L. Construction of Minimized Topological Graphs on Occupancy Grid Maps Based on GVD and Sensor Coverage Information. J. Intell. Robot. Syst. 2014, 75, 457–474. [Google Scholar] [CrossRef]
 Wang, K.H.C.; Botea, A. Scalable MultiAgent Pathfinding on Grid Maps with Tractability and Completeness Guarantees. In Proceedings of the ECAI—European Conference on Artificial Intelligence DBLP, Lisbon, Portugal, 16–20 August 2017. [Google Scholar]
 Sharon, G.; Stern, R.; Felner, A.; Sturtevant, N.R. ConflictBased Search for Optimal MultiAgent Pathfinding. Artif. Intell. 2015, 219, 40–66. [Google Scholar] [CrossRef]
 Sigurdson, D.; Bulitko, V.; Yeoh, W.; Hernandez, C.; Koenig, S. MultiAgent Pathfinding with RealTime Heuristic Search. In Proceedings of the 2018 IEEE Conference on Computational Intelligence and Games (CIG), Maastricht, The Netherlands, 14–17 August 2018; pp. 1–8. [Google Scholar]
 Li, J.; Harabor, D.; Stuckey, P.; Ma, H.; Koenig, S. Symmetry Breaking Constraints for Gridbased MultiAgent Path Finding. In Proceedings of the National Conference on Artificial Intelligence, Honolulu, HI, USA, 27 January–1 February 2019. [Google Scholar]
 Ma, H.; Wagner, G.; Felner, A.; Li, J.Y.; Kumar, T.K.S.; Koenig, S. MultiAgent Path Finding with Deadlines. In Proceedings of the International Joint Conference on Artificial Intelligence, Wellington, New Zealand, 10–12 December 2018; pp. 417–423. [Google Scholar]
 Boyarski, E.; Felner, A.; Stern, R.; Sharon, G.; Tolpin, D.; Betzalel, O.; Shimony, E. ICBS: Improved ConflictBased Search Algorithm for MultiAgent Pathfinding. In Proceedings of the International Conference on Artificial Intelligence, Buenos Aires, Argentina, 25–31 July 2015. [Google Scholar]
 Rao, N.S.V.; Stoltzfus, N.; Iyengar, S.S. A “retraction” method for learned navigation in unknown terrains for a circular robot. IEEE Trans. Robot. Autom. 1991, 7, 699–707. [Google Scholar] [CrossRef]
 Scherer, S.; Ferguson, D.; Singh, S. Efficient Cspace and cost function updates in 3D for unmanned aerial vehicles. In Proceedings of the 2009 IEEE International Conference on Robotics and Automation, Kobe, Japan, 12–17 May 2009. [Google Scholar]
 Cuisenaire, O.; Macq, B. Fast Euclidean Distance Transformation by Propagation Using Multiple Neighborhoods. Comput. Vis. Image Underst. 1999, 76, 163–172. [Google Scholar] [CrossRef]
 Lau, B.; Sprunk, C.; Burgard, W. Improved Updating of Euclidean Distance Maps and Voronoi Diagrams. Intell. Robot. Syst. 2010, 281–286. [Google Scholar]
 Harabor, D.; Botea, A. Breaking Path Symmetries on 4Connected Grid Maps. In Proceedings of the Sixth AAAI Conference on Artificial Intelligence and Interactive Digital Entertainment, AIIDE 2010, Stanford, California, CA, USA, 11–13 October 2010. [Google Scholar]
 Pochter, N.; Zohar, A.; Rosenschein, J.S.; Felner, A. Search Space Reduction Using Swamp Hierarchies. In Proceedings of the TwentyFourth AAAI Conference on Artificial Intelligence, California, CA, USA, 11–15 July 2010. [Google Scholar]
 Harabor, D.; Grastien, A. Improving Jump Point Search. In Proceedings of the TwentyFourth International Conference on Automated Planning and Scheduling, New Hampshire, NH, USA, 21–26 June 2014. [Google Scholar]
 Rabin, S.; Sturtevant, N.R. Combining Bounding Boxes and Jps to Prune Grid Pathfinding. In Proceedings of the Thirtieth AAAI Conference on Artificial Intelligence, Phoenix, Arizona, 12–17 February 2016. [Google Scholar]
 Harabor, D.; Grastien, A. Online Graph Pruning for Pathfinding on Grid Maps. In Proceedings of the TwentyFifth AAAI Conference on Artificial Intelligence, California, CA, USA, 7–11 August 2011. [Google Scholar]
 Sturtevant, N.R. Generalizing JPS Symmetry Detection: Canonical Orderings on Graphs. In Proceedings of the Ninth Annual Symposium on Combinatorial Search, New York, NY, USA, 6–8 July 2016. [Google Scholar]
 Uras, T.; Koenig, S.; Hernández, C. Subgoal Graphs for Optimal Pathfinding in EightNeighbor Grids. In Proceedings of the TwentyThird International Conference on Automated Planning and Scheduling, Rome, Italy, 10–14 June 2013. [Google Scholar]
 Uras, T.; Koenig, S. Identifying Hierarchies for Fast Optimal Search. In Proceedings of the TwentyEighth AAAI Conference on Artificial Intelligence, Quebec City, QC, Canada, 27–31 July 2014. [Google Scholar]
 Uras, T.; Koenig, S. Understanding Subgoal Graphs by Augmenting Contraction Hierarchies. In Proceedings of the International Joint Conference on Artificial Intelligence, Macao, China, 28 September 2018; pp. 1506–1513. [Google Scholar]
 Harabor, D.D.; Grastien, A.; Öz, D.; Aksakalli, V. Optimal AnyAngle Pathfinding in Practice. J. Artif. Intell. 2016, 56, 89–118. [Google Scholar] [CrossRef]
 Hormazábal, N.; Díaz, A.; Hernández, C.; Baier, J.A. Fast and Almost Optimal AnyAngle Pathfinding Using the 2k Neighborhoods. In Proceedings of the Tenth Annual Symposium on Combinatorial Search, Pittsburgh, PA, USA, 16–17 June 2017. [Google Scholar]
 Uras, T.; Koenig, S. An Empirical Comparison of AnyAngle PathPlanning Algorithms. In Proceedings of the Annual Symposium on Combinatorial Search, Ein Gedi, The Dead Sea, Israel, 11–13 June 2015. [Google Scholar]
 Uras, T.; Koenig, S. Speedingup anyangle pathplanning on grids. In Proceedings of the TwentyFifth International Conference on Automated Planning and Scheduling, Jerusalem, Israel, 7–11 June 2015. [Google Scholar]
 Hu, Y.; Zhang, Q.; Qin, L.; Yin, Q. Escaping Depressions in LRTS with Wall Following Method. In Proceedings of the 2017 9th International Conference on Intelligent HumanMachine Systems and Cybernetics (IHMSC), Hangzhou, China, 26–27 August 2017. [Google Scholar]
Figure 1.
A distance map (DM) constructed from an indoor environment. The yellow regions consist of blocked cells (obstacles). Visually speaking, for each of these traversable cells in the unblocked areas, the father it is away from the nearest obstacle, the higher its brightness is.
Figure 3.
The flowchart describing the process of building a DM, building a DMbased subgoal graph, engaging a highlevel path planning task, and executing Learning Real Time A* (LRTA*) to fulfill a lowlevel navigation task.
Figure 4.
Lower wavefront propagation. (a) Insert a new obstacle cell; (b) Launch a lower wavefront; (c) Propagate the distance change; (d) Reach edges of affected area; (e) New DM is reconstructed.
Figure 5.
Raise wavefront propagation. (a) Remove the center obstacle; (b) Launch a raise wavefront; (c) Propagate the resetting to the invalid edges; (d) Continue the lower wavefront from other sides; (e)The invalid region is again submerged.
Figure 6.
Canonical Ordering in wavefront propagation. (a) The possible paths to propagate a wavefront from cell S on lefttop to cell G on bottomright. Among these paths, only the gray one is tested when following the preference rule of Canonical Ordering; (b) By following the preference rule of Canonical Ordering, the expansion of a wavefront denoted as red arrows is launched from the center yellow cell and visits each affected cell only once.
Figure 7.
Steps of constructing DMbased subgoal graphs with different safety radius. (a) Step1: placing subgoals (R=1); (b) Step2: connecting directhreachable subgoals (R=1); (c) a subgoal graph with R=1.5; (d) a subgoal graph with R=3.
Figure 8.
Examples of using LRTA* to search paths, with or without guidance of a highlevel path provided by DMbased subgoal graph. (a) A DM with safety radius R=1.5 constructed on a concave blocked region; (b) The highlevel path connecting the start and the goal cells can be searched from the DMbased 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.
Figure 9.
The grid maps used to compare performance of different algorithms. (a) Full reconstruction; (b) 75% changed; (c) 50% changed; (d) 25% changed. Within these figures, the obstacles marked in black and blue are static, while the obstacles marked in yellow randomly change their positions or shapes.
Figure 10.
The average computation time provided by the four algorithms for updating DMs. The algorithms prefixed with "*" are dynamic algorithms and only update the affected areas.
Figure 12.
Performance comparison between agent X and Y. (a) The resulting path of agent X who simply uses LRTA* to engage a global search in a DM. As the light green trajectory shows, repeated heuristic corrections and meaningless movements caused by local minima (such as the regions denoted by identifiers A, B, and C) seriously reduce the rationality of the resulting trajectory when it is compared with the high level path searched by A* form the DMbased subgoal graph (denoted as the red trajectory); (b) The resulting path of agent Y who adopts LRTA* to refine the highlevel path searched from the DMbased subgoal graph. Due to the sparse and hdirect reachable feature, Agent Y prefers to expand cells between consecutive subgoals and thus can efficiently avoid local minima. Therefore, the resulting path (shown as light green trajectory in (b)) is, to a large extent, close to the optimal path denoted as the red trajectory.
initialize()
 setObstacleCell (Cell o)
 removeObstacleCell (Cell o)

update()
 lower (cell s)
 raise (cell o)

findSuccessors(cell s)

ConstrucDMBasedSubGraph(safetyRadius R)

GetDirectHReachable(subgoals V, safetyRadius R, cell s
 SafeClearance(safetyRadius R, subgoals V, cell s, direction d)

FindHighLevelPath(subgoals V, edges E, safetyRadius R, startCell s, endCell s’)

ConnectToGraph(subgoals V, edges E, safetyRadius R, cells s)

LRTA*(start s, end s’, safety radius R)

Algorithm  Full Reconstruction  75% Dynamic  50% Dynamic  25% Dynamic  

μ  $\mathit{\sigma}$  μ  $\mathit{\sigma}$  μ  $\mathit{\sigma}$  μ  $\mathit{\sigma}$  
Brushfire  6.34  0.2533  6.53  0.1824  6.48  0.2316  6.32  0.2339 
Dynamic Brushfire *  8.61  0.1753  6.19  0.2018  3.48  0.1899  1.39  0.2037 
BL *  7.22  0.2451  5.24  0.2342  2.93  0.2171  1.12  0.1746 
CODB *  4.26  0.2124  3.25  0.1937  1.67  0.2248  0.73  0.2102 
* Dynamic algorithms that only repair the affected portions.
Table 8.
Comparison of the maximal size of the priority queue OPEN (Only dynamic algorithms are compared).
Algorithm  Full Reconstruction  75% Dynamic  50% Dynamic  25% Dynamic  

μ  $\mathit{\sigma}$  μ  $\mathit{\sigma}$  μ  $\mathit{\sigma}$  μ  $\mathit{\sigma}$  
Dynamic Brushfire  27,550  149  24,899  110  16,765  124  13,184  96 
BL  12,523  78  10,268  106  8024  95  5327  77 
CODB  9048  90  7611  121  6027  102  4116  88 
© 2019 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).