## 3 Hierarchical Path-Planning
(**b**) Local nodes (blue dots) extracted by the Harris corner detector; the red dot is the start position; the green dot is the goal. (**c**) The initial tree (blue line) and the initial path (yellow line). (**d**) The final tree (blue line) and the optimized path (red line). 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\to 10\to 9\to 6\to 7\). The cyan squares are the selected junction nodes, or \(Node_{\text{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. Figure 5: The steps of hierarchical path-planning. Figure 6: Hierarchical path-planning results: (**a**,**c**,**e**) The global paths from starts (red dots) to goals (green dots); the cyan squares are the selected junction nodes. (**b**,**d**,**f**) The cost matrix for the chosen junction nodes, the start, and the goal. ```
1:the inter-regional path from the start region, \(seg_{S}\), to the goal region, \(seg_{G}\), \(Seg=\left\{seg_{k}\right|\)\(1\leq k\leq K\), \(seg_{1}=seg_{S}\), \(seg_{K}=seg_{G}\), junction nodes \(Node_{JC}=\left\{node_{i,j}^{n}|1\leq n\leq N\right\}\), where \(N\) is the number of junction nodes, \(node_{i,j}^{n}\) is the junction node between \(seg_{i}\) and \(seg_{j}\), and the SIRRT* paths among the junction nodes \(Path=\left\{path_{p,q}^{seg_{i}}\right|\)\(p,q\in Node_{JC}\), \(seg_{i}\in Seg\right\}\)
2:the final path \(Path_{SG}\) that is a sequence of waypoints from the start to goal positions
3:functionHierarchical SIRRT*(\(Seg\), \(Node_{JC}\), \(Path\))
4:Set \(Node_{\text{path}}=\emptyset\)
5:\(\triangleright\) Selecting the junction nodes (\(Node_{\text{path}}\)) according to the inter-regional path
6:for\(seg_{k}\in Seg\), \(1\leq k\leq K-1\)do
7: Find all \(node_{k,k+1}^{n}\in Node_{JC}\). \(Node_{\text{path}}\gets Node_{\text{path}}\cup\left\{node_{k,k+1}^{n}\right\}\)
8:endfor
9:\(\triangleright\) Computing the intra-regional path (\(Path_{S}\)) from the start position to the junction nodes
10: Set \(Path_{S}=\emptyset\). 11:for all\(node_{1,k}^{n}\in Node_{\text{path}}\)do
12: Compute the SIRRT* path \(path_{S,n}^{seg_{1}}\) from the start position to \(node_{1,k}^{n}\) of \(seg_{1}=seg_{S}\). 13:\(Path_{S}\gets Path_{S}\cup\left\{path_{S,n}^{seg_{1}}\right\}\)\(\triangleright\)\(n\in Node_{\text{path}}\)
14:endfor\(\triangleright\) Computing the intra-regional path (\(Path_{G}\)) from the goal position to the junction nodes
15: Set \(Path_{G}=\emptyset\). 16:for all\(node_{k,K}^{n}\in Node_{\text{path}}\)do
17: Compute SIRRT* path \(path_{n,G}^{seg_{K}}\) from the goal position to \(node_{k,K}^{n}\) of \(seg_{K}=seg_{G}\). 18:\(Path_{G}\gets Path_{G}\cup\left\{path_{n,G}^{seg_{K}}\right\}\)\(\triangleright\)\(n\in Node_{\text{path}}\)
19:endfor\(\triangleright\) Calculating the cost matrix among the selected junction nodes and start/goal positions
20: Set a zero-matrix \(Cost_{JC}=\left\{cost_{i,j}\right|\)\(1\leq i,j\leq M+2\)}
21:for\(seg_{k}\in Seg\), \(2\leq k\leq K-1\)do
22: Find all pairs of \(node_{k-1,k}^{i}\), \(node_{k,k+1}^{j}\in Node_{path}\). \(\triangleright\)\(Node_{path}\subset Node_{JC}\)
23:\(cost_{i,j}\) and \(cost_{j,i}\leftarrow\) the cost of \(path_{i,j}^{seg_{k}}\in Path\)
24:endfor
25:for all\(path_{S,n}^{seg_{1}

## 4 Experimental Results

The proposed hierarchical path-planning method was analyzed to verify its stable and efficient performance.