# Speed Optimization for Incremental Updating of Grid-Based Distance Maps
## 4 The Methodology
### Algorithm Pseudocode
#### 4.2.4 Find Paths in DM-Based Subgoal Graphs  
The algorithms shown in Table 5 are proposed by [PERSON] 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^{\prime}\) to the underlying subgoal graph, engage an A* search to find a shortest collision-free path between \(s\) and \(s^{\prime}\) 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^{\prime}\) (line 77 and line 78), and then add new edges between them into the graph (line 85)). According to [PERSON] 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^{\prime}\) 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^{\prime})\) as the immediate goal for the next move (line 89). During the learning part, the numeric value \(h(s^{\prime},s^{\prime})\) is updated to approach the \begin{table}
\begin{tabular}{|l|l|} \hline
**GetDirectHReachable(subgoals \(V\), safetyRadius \(R\), cell \(s\)** & **SafeClearance(safetyRadius \(R\), subgoals \(V\), cell \(s\), direction \(d\))** \\
55. \(S\leftarrow\varnothing\) & 71. \(i\gets 0\) \\
56. **for each** _directions_\(d\)**do** & 72. **while** _true_ **do** \\
57. \(s^{*}\leftarrow(s\)+SafeClearance(\(R\),\(V\),\(s\),\(d\))\(\bowd\)) & 73. **if**\(\
eg(\)C(R,\(s\)+\(i\),\(i\)) = 0\(\land\) C(R,\(s\)+\(i\),\(i\)+\(d\))=0) **then** \\
58. **if** \(s^{\prime}\in V\)**then** \(S\gets\bigcup\bigcup\{s\}\) & 74. _return_\(i\) \\
59. **for each** _diagonal directions_\(d\)**do** & 75. \(i\gets i\)+\(1\) \\
60. **for each** _cardinal directions_\(c\) associated with \(d\)**do** & 76. **if** \((s\)+\(i\)) \(\in V\) **then** _return_\(i\) \\
61. \(max\leftarrow\) SafeClearance(\(R\),\(V\),\(s\),\(c\)) & \\
62. _diag_\(\leftarrow\) SafeClearance(\(R\),\(V\),\(s\),\(d\)) & \\
63. **if** \((s\)+(\(i\),\(m\))\(\times\))\(\in V\)**then** _max_ \(\leftarrow\) _max_-1_ \\
64. **if** \((s\)+(\(i\),\(m\))\(\bowd\))\(\in V\)**then** _diag_ \(\leftarrow\) _diag_ -1_ \\
65. **for each \(i\) from \(1\) _to_ _diag_ \\
66. \(j\leftarrow\) SafeClearance(\(R\),\(V\),\(s\)+\(i\),\(c\)) & \\
67. **if** \(j\)\(\leq\)_max_ _and_ \((s\)+\(i\),\(d\)\(c)) \(\in V\)**then** \\
68. \(S\leftarrow\bigcup\{s\)+\(i\)+\(d\)\(c\)\(\}\) & \\
69. \(j\gets j\)-1 & \\
70. **if** \(j\)\(<\)_max_ **then** _max_ \(\leftarrow\) \(j\) \\ \hline \end{tabular}
\end{table} 
Table 4: The pseudocode of determining the direct-h-reachable area of a given unblocked cell s.