Hierarchical Path-Planning for Mobile Robots Using a Skeletonization-Informed Rapidly Exploring Random Tree*

: An efﬁcient, hierarchical, two-dimensional (2D) path-planning method for large complex environments is presented in this paper. For mobile robots moving in 2D environments, conventional path-planning algorithms employ single-layered maps; the proposed approach engages in hierarchical inter- and intra-regional searches. A navigable graph of an environment is constructed using segmented local grid maps and safe junction nodes. An inter-regional path is obtained using the navigable graph and a graph-search algorithm. A skeletonization-informed rapidly exploring random tree* (SIRRT*) efﬁciently computes converged intra-regional paths for each map segment. The sampling process of the proposed hierarchical path-planning algorithm is locally conducted only in the start and goal regions, whereas the conventional path-planning should process the sampling over the entire environment. The entire path from the start position to the goal position can be achieved more quickly and more robustly using the hierarchical approach than the conventional single-layered method. The performance of the hierarchical path-planning is analyzed using a publicly available benchmark environment.


Introduction
Path-planning for mobile robots involves identifying an efficient path from a start position to a goal position on a given environmental map using online sensor data. The path-planning algorithms are divided into those facilitating local and global planning. Local path-planning ("obstacle avoidance") prevents collisions, even when unexpected (unregistered) obstacles are encountered [1,2]. Global path-planning generates waypoints that connect the start to the goal using the given map [3,4]. In practice, local and global plannings are integrated as the robot proceeds via various waypoints to a goal, while the local planner calculates the actions to arrive at each waypoint using current sensor data. Therefore, the efficient waypoints in safe regions that can constitute the shortest path are essential for higher level applications.
Most global planners initially extract safe/navigable regions from a global map and then find an appropriate path between the safe regions. Safe regions can be represented as empty/unoccupied cells of a grid map or as the nodes of a graph. Grid map-based planning is applicable in two-dimensional (2D) environments because it is possible to directly recognize safe regions and their relationships when planning possible paths between them via regularly decomposed grid cell representation [5,6]. However, to solve scalability issues in high-dimensional applications with limited computational power and time, the grid-based planners require additional effort like the cost approximation [7]. hierarchical, path-planning algorithm seeking to optimize paths by placing bridge nodes on the potential field of the grid map [23]. A three-level hierarchical structure exploiting particle swarm optimization was suggested to yield feasible, safe, and optimal paths [24]. However, its hierarchy means the algorithmic structure is composed of map decomposition and path optimization; the method does not reveal the hierarchical geometric structure of the environment. A hierarchical roadmap (HRM), similar to what we develop here, can be incrementally constructed via normalized graph-cut segmentation of a local sonar grid map [25].
Here, a two-layered, hierarchical path-planning method is developed. The combined method of the previous work, the SIRRT*, and the well-known image segmentation method, the watershed segmentation, is presented. A global path, thus a sequence of path regions from a start region to a goal region, is firstly obtained through the segmented grid map. Within each path region, the SIRRT* is applied to compute a local path between two junction nodes. An entire path from the start to the goal is generated by combining these local paths. Figure 1 illustrates the proposed hierarchical path-planning method. The local grid maps (A, B, and C) and the junction nodes (J1, J2) between adjacent local maps constitute a graph. The local paths within local grid maps, defined via intra-regional searching, are combined using the inter-regional path. The proposed approach can be applied to most mobile robots operated in 2D environments, including wheeled mobile robots and legged mobile robots. The proposed hierarchical planning is a kind of divide-and-conquer method, and it suggests the required sequence and its corresponding components from environmental segmentation to database construction. The contribution of this paper lies in the overall sequence for hierarchical path-planning. Although divide-and-conquer or local-metric/global-topological approaches have already been discussed in other research areas, we need to consider additional components to apply them to practical path-planning problems. Especially, a navigable graph structure, which is a database of local paths, is constructed by the SIRRT*. The previous works, for SIRRT* planning, show robust performance for multiple running applications, and it is efficient for constructing the navigable graph structure. The remainder of the paper is organized as follows. In Section 2, a navigable graph of an environment is constructed; we segment the grid map and extract junction nodes between two adjacent segments. In Section 3, the hierarchical path-planning algorithm using the navigable graph is developed; we employ the Dijkstra graph-search for inter-regional path-planning and the SIRRT* intra-regional path-planning to this end. Section 4 describes the experimental results using a well-known (benchmark) environment to verify the efficacy of the proposed hierarchical approach. Section 5 contains conclusions and future directions.

Navigable Graph of an Environment
This section shows how to construct a navigable graph of an environment. The grid map image is firstly segmented using a watershed segmentation algorithm. Then, junction nodes lying on edge grid cells separating adjacent segments are defined.

Grid Map Segmentation
Many studies have explored environmental segmentation, and most have sought to construct topological maps by extracting the nodes and edges of empty areas defined using various metrics by reference to a generalized Voronoi graph (GVG) [26][27][28]. Using the positions of nodes, the environment can be easily segmented so that each grid cell is assigned to the nearest node; cells assigned to the same node are in the same segment. Other studies have focused more on dividing a grid map [29]. RoomSegis a watershed, grid map segmentation method used to guide autonomous cleaning robots in large environments and is a practical end-to-end implementation even if the grid map is noisy [30]. It is possible to divide a consecutive, local sonar grid map into two regions via cell decomposition and the normalized graph cut method and incrementally segment the environment to construct a connected graph [31,32].
A watershed algorithm that is easily implemented and that yields reliable segmentation results is applied to the grid map segmentation in this paper. We engage in hierarchical path-planning using a navigable graph of segmented local grid maps; we do not engage in grid map segmentation per se. The watershed transform detects catchment basins in a grayscale image by considering the image as a topographic relief; the height of each pixel is proportional to its gray level [33]. As the grid map is considered a black (occupied)-and-white (empty) binary image, it must be converted to impart various intensities to empty cells. A distance transform operator is used to calculate the distance to the closest boundary (an occupied cell) from each empty cell. Figure 2a shows a grid map image of a public benchmark environment [34], and the grid map image is obtained by constructing a grid map using laser measurements and converting it to a binary image. Figure 2b shows the normalized distance transform image computed using the Euclidean distance metric.    Use of the basic watershed segmentation algorithm can cause over-segmentation. Figure 2c shows the segmentation result of the distance transform image of Figure 2b, computed using the basic watershed algorithm. To overcome this problem, a marker-controlled, watershed segmentation algorithm has been developed [33,35]. This uses markers to indicate initial segment information, thus the number and approximate positions of objects or foregrounds that can be definitely distinguished from backgrounds. Markers are computed using morphological operations such as erosion and dilation. Prior to the watershed application to the grid map, the definite background pixels (occupied cells) are easily recognized from the grid map image; this is not the case for other gray-scale images. When path-planning, we are interested in safe navigable areas (empty cells) and calculate the distance transforms for all such cells. The pixels lying further from occupied cells are considered as definitive foreground segments. The empty cells that exceed the threshold value of the normalized distance transform serve as marker regions. Markers are labeled using the connected components labeling (CCL) algorithm (which assigns unique labels to all pixels in each connected segment). Figure 2d shows the labeled markers; these are combined with the empty cell information of Figure 2e. The marker-controlled watershed algorithm gradually grows regions and decides the segment to which each non-labeled empty cell belongs. Marker-controlled segmentation is shown in Figure 3a.

Junction Node Extraction
The watershed segmentation algorithm produces several grid map segments with different labels, separated by edges/boundaries one pixel in width. To construct a navigable graph using the segments, the junction nodes between adjacent segments must be defined. In the proposed path-planning method, these nodes serve as sub-goals/waypoints, allowing movement to the next path regions, defined as local regions through which the robot must pass.
Algorithm 1 shows how to extract junction nodes, Node, and how to construct the adjacency matrix, Adj, of the grid map segments. Algorithm 1 requires two inputs, gridmap dist and gridmap WS . gridmap dist is the normalized distance transform of the grid map, and gridmap WS is the watershed segmentation result where each pixel value shows the label identification of the corresponding segment. The algorithm first inspects all edges of cells, grid edge , in the watershed segmentation result and combines the edge cells that share neighbors into a matricized form, Adj cells , wherein each component is a group of cells lying between the same adjacent segments (Lines 4-10). Adj is the adjacency matrix among grid map segments. If there are M segments, the size of the symmetric matrix Adj is M. If there are any edge cells between two segments i and j, adj i,j and adj j,i are one (Line 8). Each component adj cells i,j of Adj cells stores all edge cells between two segments i and j (Line 7).
For each group of edge cells, we inspect and obtain the connected edge segments using the CCL (Line 13), which can deal with the situation in which two adjacent grid map segments can be connected via several separate edge areas. The junction node should be the safest grid cell; thus, we select the cell exhibiting the maximum, normalized distance transform value with respect to one of the connected edge segments (Line 16). Finally, we obtain the junction nodes and the adjacency matrix of the grid map segments (Line 20).

Algorithm 1 Junction node extraction.
Require: gridmap dist , gridmap ws Ensure: junction nodes Node = node n i,j | 1 ≤ n ≤ N, N is the number of junction nodes. , adjacency matrix among grid map segments Adj Grouping edge cells that have the two same neighboring segments and calculating the adjacency 4: for all grid edge ∈ gridmap ws do 5: Inspect eight neighbor cells around grid edge . 6: if there are two different labeled neighbors, and their labels are i and j then 7: adj cells for all non-empty adj cells The safest cell with the maximum distance to obstacles is selected as a junction node. 16: with the maximum gridmap dist . 17: node n i,j ← grid d max 18: end for 19: end for 20: return Node, Adj 21: end function Figure 3 shows the graph structure of the segmented grid map. In Figure 3a, the yellow dots are the junction nodes, and the numbers identify the segments. Figure 3b is the corresponding adjacency matrix.
In Figure 3a, Segments "6" and "7" are structurally connected in two separate places. Two segments in Figure 3c feature two edges and two junction nodes.

Hierarchical Path-Planning
In this section, the hierarchical path-planning method using the graph-search algorithm and the SIRRT* is developed. Previously, a navigable graph of the environment consisting of local grid map segments, the adjacency matrix, and the junction nodes between all adjacent segments was constructed. We now use the SIRRT* to compute intra-regional paths among the junction nodes of each grid map segment. Figure 4 shows the SIRRT* result for Segment "9". The initial solution path is computed using the Harris corner nodes of the skeletonized grid map image and Prim's algorithm (Figure 4a-c). The final path is obtained via informed sampling optimization (Figure 4d). Figure 5 shows the overall procedure of the proposed method. The first layer in path-planning is an inter-regional search using the navigable graph structure of the segmented grid maps; we find a start region and a goal region (A). Then, the inter-regional path from the start region to the goal region is computed using the adjacency matrix and the graph-search algorithm (B). The Dijkstra graph-search algorithm is employed to this end; all adjacent segment pairs are equally weighted during the inter-regional search. When the inter-regional path is defined, the junction nodes are selected (C) to yield the SIRRT* paths among those nodes for each path region. At this time, several junction nodes for two adjacent segments can be selected because obstacles may lie between the segments. Thus, two adjacent segments can be connected at several nodes. To ensure that the junction node sequence from the start to the goal is efficient, we construct a cost matrix. The SIRRT* yields the intra-regional paths from the start to the junction nodes of the start region and from the goal to the junction nodes of the goal region (D). The cost matrix is calculated by the costs of the SIRRT* paths (E). By using the cost matrix and the Dijkstra graph-search algorithm, the node sequence from the start position to the goal position can be computed (F). The graph-search algorithm selects the cheapest junction nodes. Finally, a global path is achieved by combining the intra-regional paths with reference to the node sequence (G).    Algorithm 2 is the pseudocode of the hierarchical path-planning, and it describes the procedure after obtaining the inter-regional path from the start region, seg S , to the goal region, seg G . Seg is the sequence of the path regions according to the inter-regional path, and K is the number of the path regions. Algorithm 2 also requires the navigable graph structure including the junction nodes, Node JC , and the intra-regional paths, Path, of all segments. The junction nodes, Node path , are selected according to the inter-regional path, Seg, in Lines 2-5. The intra-regional paths, Path S , from the start position to the junction nodes of the start region are computed by the SIRRT* (Lines 6-10). The intra-regional paths, Path G , from the goal position to the junction nodes of the goal region are also computed by the SIRRT* (Lines 11-15). The cost matrix, Cost JC , among the selected junction nodes, the start position, and the goal position is obtained in Lines 16-26. If M is the number of the selected junction nodes, Node path , the size of the symmetric cost matrix is M + 2. The cost values between the start/goal to the selected junction nodes should be added to the cost matrix. Using the cost matrix, the Dijkstra graph-search algorithm computes the node sequence, Node SG , from the start to the goal. Here, the indices of the start and the goal in the cost matrix are M + 1 and M + 2, respectively (Line 27). By connecting the intra-regional paths in series, we obtain the final path, Path SG (Line 28).  Figure 6a,c,e shows the hierarchical path-planning results derived using three different starts and goals, and Figure 6b,d,f is the corresponding cost matrices for the selected junction nodes along the inter-regional paths. As shown in Figure 6a, the inter-regional path is 16 → 10 → 9 → 6 → 7. The cyan squares are the selected junction nodes, or Node path of the path regions in Algorithm 2. As Segment "7" is connected to Segment "6" through two junction nodes, both nodes were selected when computing the cost matrix. The second-to-last entries of the cost matrix are the SIRRT* distances from the start to the junction nodes of the start region and the last entries the SIRRT* distances from the goal to the junction nodes of the goal region. Such path-planning chooses the most efficient junction nodes when computing a global path using the cost matrix and the graph-search algorithm.

Algorithm 2 Hierarchical path-planning.
Require: the inter-regional path from the start region, seg S , to the goal region, seg G , Seg = {seg k | 1 ≤ k ≤ K, seg 1 = seg S , seg K = seg G }, junction nodes Node JC = node n i,j |1 ≤ n ≤ N , where N is the number of junction nodes, node n i,j is the junction node between seg i and seg j , and the SIRRT* paths among the junction nodes Path = path seg i p,q | p, q ∈ Node JC , seg i ∈ Seg Ensure: the final path Path SG that is a sequence of waypoints from the start to goal positions 1: function HIERARCHICAL SIRRT*(Seg, Node JC , Path) 2: Set Node path = ∅ Selecting the junction nodes (Node path ) according to the inter-regional path 3: for Find all node n k,k+1 ∈ Node JC . Node path ← Node path ∪ node n k,k+1 5:

end for
Computing the intra-regional path (Path S ) from the start position to the junction nodes 6: Set Path S = ∅.

7:
for all node n 1,k ∈ Node path do 8: Compute the SIRRT* path path seg 1 S,n from the start position to node n 1,k of seg 1 = seg S .

end for
Computing the intra-regional path (Path G ) from the goal position to the junction nodes 11: Set Path G = ∅.

12:
for all node n k,K ∈ Node path do 13: Compute SIRRT* path path seg K n,G from the goal position to node n k,K of seg K = seg G . 14: Path G ← Path G ∪ path seg K n,G n ∈ Node path 15:

end for
Calculating the cost matrix among the selected junction nodes and start/goal positions 16: Set a zero-matrix Cost JC = cost i,j | 1 ≤ i, j ≤ M + 2 17: for seg k ∈ Seg, 2 ≤ k ≤ K − 1 do 18: Find all pairs of node i k−1,k , node j k,k+1 ∈ Node path . Node path ⊂ Node JC

19:
cost i,j and cost j,i ← the cost of path seg k i,j ∈ Path 20: end for 21: for all path seg 1 S,n ∈ Path S do 22: cost n,M+1 and cost M+1,n ← the cost of path seg 1 S,n 23: end for 24: for all path seg K n,G ∈ Path G do 25: cost n,M+2 and cost M+2,n ← the cost of path seg K n,G 26: end for 27: A node sequence node S to node G , Node SG ← Dijkstra_Graph_Search(Cost JC , M + 1, M + 2) 28: Obtain the final path, Path SG , by combining the intra-regional paths according to Node SG . 29: return Path SG 30: end function

Experimental Results
The proposed hierarchical path-planning method was analyzed to verify its stable and efficient performance. All experiments were performed on an Intel Core i7-9700K (eight cores of 3.6 GHz) with 32 GB of memory in the same benchmark environment; several rooms were connected by corridors, as in the real world.
The SIRRT* was compared with the IRRT* to verify that the SIRRT* is more efficient in constructing the proposed navigable graph structure that needs multiple intra-regional planning runs among junction nodes. The stability of the SIRRT* in terms of computational time and the final costs is shown in Table 1. The start and goal positions are given in Figure 6a. The IRRT* and SIRRT* performed 1000, 2000, and 3000 informed sampling optimizations after obtaining an initial solution. For each optimization number, one-hundred experiments were performed. In the SIRRT*, the initial solution is deterministically computed by skeletonization employing the Harris corner detector; random sampling is not in play. However, the IRRT* algorithm uses RRT* sampling to achieve an initial solution throughout the entire environment. The total number of IRRT* iterations is the sum of the RRT* samplings required until an initial solution is obtained and the number of informed samplings required for optimization. As the IRRT* performs the RRT* sampling to obtain an initial solution, the computational time is negatively affected. If initial solution computation is slow, the number of RRT* samplings increases, as does the computational time. The mean IRRT* computational times were 8.01, 8.74, and 7.46 s. However, the differences were not significant; the SDs were very large (5.86, 6.59, and 7.39 s). Thus, the IRRT* performed very differently during each trial, although the start and goal positions were the same. By contrast, the SIRRT* computational times were faster and more stable (means 0.51, 0.83, and 1.19 s) than those of the IRRT*. The SDs were relatively small at 0.08, 0.11, and 0.12 s. The SIRRT* generated initial solutions faster than did the IRRT*, and the total computational time for the final solution was lower and more robust than that of the IRRT*. The final costs of the SIRRT* were relatively larger than those of the IRRT*. However, the IRRT* SDs were larger than those of the SIRRT*. Sampling-based algorithms such as the RRT variants are more likely to achieve an optimal solution as the number of samples increases. The larger number of the total IRRT* iterations reduced the mean final cost, but at the expense of large SDs. Constructing the navigable graph structure requires conducting the local planning algorithm multiple times to obtain the intra-regional paths among junction nodes. Therefore, the SIRRT* that shows more consistent results (low SDs) is efficient for the hierarchical planning approach.
The hierarchical path-planning method consists of two phases: we construct a navigable graph of the environment featuring grid map segmentation and the SIRRT*-mediated path computation among the junction nodes of each segment, then we engage in inter-and intra-regional path-planning using the graph. Table 2 shows the computational times required for the construction of navigable environmental graphs using different numbers of the SIRRT* optimization iterations. The number of such iterations is the number of ellipsoidal-informed samplings performed after obtaining an initial solution via skeletonization. For each fixed iteration number of the SIRRT* optimization, thirty experiments were performed. The environment was divided into 20 local segments, and twenty-eight junction nodes were registered. As all intra-regional paths were defined using the SIRRT*, the SIRRT* algorithm totally ran 117 times for each experiment. Therefore, the computational times in Table 2 reflect 117 SIRRT* path-plannings. The SIRRT* algorithm exploits the deterministic feature of skeletonization and the random nature of the RRT*, thus affording more stable performance than the other RRT* variants. Even after many trials, the standard deviations (SDs) were small; it is appropriate to construct the navigable graph via multiple computations of local paths in each grid map segment.  Table 3 shows the hierarchical path-planning results including the inter-and intra-regional path-planning. The starts and goals are those of Table 1. The pre-constructed navigable graph featured 3000 SIRRT* iterations; we performed 100 runs for each iteration number of the SIRRT* optimization. The iteration number of the SIRRT* optimization reveals intra-regional paths from the start and goal positions to the junction nodes. Computationally, global path calculation time increased as the number of iterations increased. When sampling-based planners were used, the time required and the sample number increased as the search area increased. In the experiments of Table 1, we searched the entire environment; optimization required many samples. However, hierarchical intra-regional searches of start and goal positions were performed on environmental segments. Intra-regional searches do not require large numbers of samples. After only 100 iterations, the converged final path was apparent. Cost was reduced if the navigable graph featured over 3000 iterations. Moreover, the proposed hierarchical planning based on the SIRRT* yielded smaller SDs. Min. 1420.00 1420.00 1420.00 1420.00 1420.00 1420.00 1420.00 Max. 1431.00 1420.00 1420.00 1420.00 1420.00 1420.00 1420.00 Figure 7 shows the comparison of the IRRT*, the SIRRT*, and hierarchical path-planning (the H-SIRRT*). Each graph component presents the mean, minimum, and maximum values for 100 runs. As mentioned earlier, because the results of the IRRT* have large differences for each run, it cannot be suitable for practical real-time applications. Moreover, even with the low number of optimization iterations after obtaining the initial solution, the IRRT* required too many iterations to achieve the initial solution, making the total computational time high. As shown in Figure 6a, the start region and the goal region have two junction nodes. The hierarchical path-planning includes the inter-regional graph-search and four intra-regional path-plannings using the SIRRT*. Therefore, the computational time of the hierarchical planning is higher than that of the SIRRT*. The sampling areas of the hierarchical planning's intra-regional planning are restricted as the small parts of the entire environment. In contrast, the sampling area of the SIRRT* is the whole environment. The hierarchical planning can achieve the lower cost solution using only a lower number of optimization iterations.

Conclusions
A hierarchical path-planning method using a navigable graph structure of the environment evaluated via the SIRRT* was proposed in this paper. Conventional global path-planning algorithms find optimal paths by processing entire single-layer environments. In the proposed approach, paths are identified on the intrinsic hierarchical structure; inter-and intra-regional searches are performed. The navigable, environmental graph structure using local map segments and safe junction nodes is defined in the hierarchical path-planning. The inter-regional paths from start regions to goal regions are revealed when navigable graph structures are explored using the Dijkstra graph-search algorithm; the intra-regional paths are computed using the SIRRT*. The hierarchical approach assured stable and efficient performance. The SIRRT* was compared with the IRRT*; the SIRRT* was optimal for the proposed hierarchical path-planning because the SDs of the computational times were lower than those of the IRRT*. Experimentally, hierarchical path-planning using the SIRRT* (over various iterations) afforded efficient, final path convergences (from the start to the end) using few iterations. The proposed method will aid stable and efficient path-planning in large environments.
The proposed method assumes a general global planning situation when a robot finds a global path from the start to the goal before it starts to navigate. Although we can use local planners in dynamic environments, global planners also need to cope with dynamic situations in real time. The hierarchical approach can efficiently handle such dynamic situations because it can re-plan the path by performing the intra-regional planning in the changed area and connecting the new intra-regional path with the inter-regional path. Detecting dynamic obstacles and integrating them into the navigation framework in highly dynamic environments need to be investigated in the future. In addition, applying the proposed approach to non-grid-based maps can be valuable for a wide range of practical applications.