# Path Planning of Underground Robots via Improved A* and Dynamic Window Approach
## 2 Improved A* Algorithm
### Improvement of Heuristic Function
#### 2.3.4 Diagonal Distance  
Diagonal distance is a heuristic function that combines Manhattan distance and Chebyshev distance. It considers the distance of moving in eight directions. It is suitable for the path search problem that allows horizontal, vertical, and diagonal movement in the grid. The calculation formula is  
\[h(n)=\max(\left|x_{2}-x_{1}\right|,\left|y_{2}-y_{1}\right|)+(\sqrt{2}-2) \times\min(\left|x_{2}-x_{1}\right|,\left|y_{2}-y_{1}\right|) \tag{5}\]  
The heuristic function of the A* algorithm is adjusted to the four different heuristic functions mentioned above, and tested in the roadway grid map environment shown in Figure 4. The comparison of the results is shown in Table 1.  
It can be seen from the data in the table that when the Manhattan distance is selected as the heuristic function, the calculation time is the shortest, the search nodes and the extended nodes are the least, and the number of inflection points is the least. Therefore, the Manhattan distance is selected as the heuristic function. At the same time, considering the diversity of the width of the underground roadway, in order to ensure the safety of the path planning, the ideal path should be set in the central area of the roadway. However, the traditional heuristic function often tends to the edge of the roadway when planning the path, which undoubtedly increases the potential collision risk. Therefore, it is expected that the path planning algorithm can be more inclined to the area with a larger roadway width or the center of the roadway when selecting the path, \begin{table}
\begin{tabular}{c c c c c} \hline \hline  & **Calculation Time** & **Search Nodes** & **Expanded Nodes** &
\begin{tabular}{c} **Number of** \\ **Inflection Points** \\ \end{tabular} \\ \hline Euclidean distance & 0.025 & 1036 & 999 & 88 \\ Manhattan distance & 0.0215 & 741 & 580 & 57 \\ Chebyshev distance & 0.023 & 1051 & 1018 & 74 \\ Diagonal distance & 0.0455 & 4520 & 4215 & 66 \\ \hline \hline \end{tabular}
\end{table} 
Table 1: Parameters comparison. so as to effectively reduce the possibility of collisions and improve the overall safety and reliability of the path. We expect that by optimizing the heuristic function, the performance of the A* algorithm in roadway path planning can be significantly improved. The expected effect is shown in Figure 6.  
As shown in the diagram, the orange route represents the planned path diagram after anti-collision processing using the original Manhattan distance heuristic function. Obviously, the path tends to be close to the edge of the roadway. In contrast, the effect we expect to achieve is shown in the green route in the figure. The route is kept in the center of the roadway to ensure that the global path is basically always located in the middle of the roadway during the travel process. Therefore, in order to achieve the desired path planning effect, the roadway width is integrated into the heuristic function as a key factor. The premise of this scheme is that the width information of the roadway where the current node is located can be accurately obtained from the grid map. The process of calculating roadway width information is shown in Figure 7.  
If the channel width remembered is \(d\), then the improved heuristic function is  
\[h=h_{\text{distance}}\times(1+\beta\times\frac{1}{d+1}) \tag{6}\]  
Figure 6: The expected schematic diagram. Figure 7: Channel width calculation flow chart. \(h_{\text{distance}}\) is the Euclidean distance and \(\beta\) is the weight coefficient. The larger \(\beta\) will increase the proportion of the roadway width in the heuristic function, which makes the path search algorithm more inclined to select those areas with more spacious space to avoid falling into narrow or limited space.