A Pathfinding Algorithm for Large-Scale Complex Terrain Environments in the Field

Abstract

Pathfinding for autonomous vehicles in large-scale complex terrain environments is difficult when aiming to balance efficiency and quality. To solve the problem, this paper proposes Hierarchical Path-Finding A* based on Multi-Scale Rectangle, called RHA*, which achieves efficient pathfinding and high path quality for large-scale unequal-weighted maps. Firstly, the original map grid cells were aggregated into fixed-size clusters. Then, an abstract map was constructed by aggregating equal-weighted clusters into rectangular regions of different sizes and calculating the nodes and edges of the regions in advance. Finally, real-time pathfinding was performed based on the abstract map. The experiment showed that the computation time of real-time pathfinding was reduced by 96.64% compared to A* and 20.38% compared to HPA*. The total cost of the generated path deviated no more than 0.05% compared to A*. The deviation value is reduced by 99.2% compared to HPA*. The generated path can be used for autonomous vehicle traveling in off-road environments.

1. Introduction

1.1. Background and Significance

In recent years, with the rapid development of mobile robots, artificial intelligence, and other technologies, autonomous vehicles have been gradually applied in military and civil fields such as off-road material transportation, fire support, geological research and investigation, etc. Before carrying out these tasks, it is necessary to provide an optimal path for vehicles and guide them to travel autonomously. The off-road environment presents numerous challenges. The mission area is large, with diverse geological conditions and complex terrain. However, there is a lack of road network data with complete topology. Therefore, it is necessary to develop an algorithm that can quickly generate an optimal path with high access reliability and a short travel time. This has great value for the autonomous vehicle’s rapid access in the off-road environment without a road network.

1.2. Literature Review

The ability of an autonomous vehicle in the off-road environment is affected by its own off-road performance and the geographic environment, so it is necessary to comprehensively consider the influence of these two factors when modeling the accessibility of the off-road environment. Wang et al. developed a three-dimensional-like map to represent the off-road environment. This environment model contains not only contour information of obstacles, but also the height information [1]. The vehicle’s off-road performance is taken into account when identifying obstacles. Silver et al. studied the cost assessment of unstructured models for complex off-road environments, and provided an environmental cost function for the pathfinding of autonomous vehicles [2]. Bagnell et al. proposed to build an accessibility map based on data from LADAR and satellites, and to generate the travel cost of two-dimensional grids using three-dimensional voxel-related features [3]. Rankin et al. constructed a 3D stereo model of the small-scale off-road environment using on-board sensors, which can be used for small area pathfinding [4]. Huertas et al. proposed a stereo-based algorithm for tree traversability analysis that considers vehicle speed, mainly for accessibility assessment in wooded environments [5]. Carsten et al. used voxels to construct a geographic graphics environment and provided a three-dimensional capacity map for local area pathfinding [6,7,8]. Regardless of what method is used, it is necessary to comprehensively consider the impact of various environmental factors on vehicle driving, so as to construct a field accessibility map. This forms the basis for pathfinding in off-road environments.

Pathfinding algorithms can mainly be divided into two categories: sampling algorithms and graph search algorithms.

Sampling algorithms approximate the state space by randomly sampling in the space and then finding paths based on these sampled points. Kavraki et al. first proposed the Probabilistic Road Maps (PRM) algorithm [9], which demonstrated a method for finding high-dimensional spatial paths by randomly sampling and forming a roadmap. LaValle proposed the Rapidly-exploring Random Trees (RRT) algorithm [10], which is a method that constructs a search tree based on random sampling to explore the configuration space and find paths. Karaman et al. proposed the RRT* [11] algorithm based on RRT, which introduces a specific cost function to minimize the connection cost of a new node to its parent node, ensuring a near-optimal path, but it is computationally intensive and inefficient in generating paths. Jordan et al. proposed the Bidirectional Rapidly-Exploring Random Trees (B-RRT*) algorithm [12], which adopts a bidirectional expansion strategy from the start and end points to improve convergence speed. Esposito et al. designed an optimized probabilistic roadmap processing algorithm [13] to simplify the computation required to deal with the number of convex units and nodes in free space, which further improves efficiency. Sampling-based pathfinding algorithms are simple in principle, easy to implement, and do not need to accurately model the obstacles in the environment to achieve pathfinding, but in the off-road large-scale complex terrain environment, the number of sampling points is large, the computational efficiency is low, and it is easy to lead to a planning path which has a large deviation from the actual optimal path.

Graph search algorithms are based on graphical methods to carry out pathfinding, and the classic algorithms include Dijkstra [14], A* [15], Jump Point Search (JPS) [16], JPS+ [17], Hierarchical A* [18] and Hierarchical Path-Finding A* (HPA*) [19].

The Dijkstra algorithm adopts a breadth-first search strategy, searching for the shortest paths between two points in the weighted graph. The A* algorithm is a heuristic search algorithm, and the optimal pathfinding time can be accelerated by reasonably choosing the heuristic function. The JPS algorithm reduces unnecessary node search by finding jump points and substantially improves the pathfinding speed. The JPS+ algorithm proposed by Steve Rabin further improved the pathfinding efficiency by preprocessing and calculating the key jump points in advance. The Dijkstra and A* algorithms ensure that the optimal path is found, but the search efficiency decreases dramatically as the map range expands. JPS is advantageous for its runtime efficiency and reduced memory usage compared to A*, but it is complex to implement and less suitable for non-uniform cost grids. JPS+ builds on JPS’s strengths by further reducing runtime complexity through preprocessing, but it incurs higher preprocessing costs and memory usage and is even more complex to implement. However, both methods are not suitable for off-road environments with complex terrain.

In order to improve the efficiency of pathfinding for large-scale, unequal-weighted maps, Holte et al. proposed the Hierarchical A* method, which abstracts the map into clusters of the same size. The midpoints in these clusters are directly connected to form an abstract map. A quadtree-based decomposition [20] of the original map into accessible and obstacle grids of varying sizes was proposed by Samet. This method significantly reduces the number of abstract map nodes and improves the pathfinding efficiency, but the generated paths are of low quality. In order to improve the accuracy of the generated paths, Yahja et al. adopted a framed-quadtree instead of the standard quadtree [21]. This method expands the grid boundaries with cells of the highest resolution, allowing the boundaries to be traversed in any angular direction, which significantly improves the path quality. The Hierarchical A* and quadtree algorithms significantly improve the efficiency of large-scale map search but result in paths of poorer quality. Framed-quadtree improves the quality of paths by expanding the boundary points, but the significant increase in the number of nodes reduces the pathfinding efficiency.

The HPA* algorithm was proposed by Botea et al. [19]. This method aggregates the original map grids to form multiple layers of abstract maps. Each layer of the abstract maps includes multiple fixed-size clusters, which are connected to each other by symmetric nodes. The optimal paths between the boundary nodes of the clusters are precomputed and cached in a data table. Real-time pathfinding is completed based on the abstract map, and the generated paths within the clusters are refined by checking the table. The HPA* algorithm realizes a significant increase in real-time pathfinding efficiency by aggregating the original maps into several clusters of the same size and precomputing the paths between the nodes at the boundaries of the clusters, solving the problem that algorithms such as A* are unable to adapt to the pathfinding of large unequal-weighted maps. However, in the off-road large-scale complex terrain environment, there are still two problems:

• The total cost of generating paths is higher than the optimal paths computed by A*, and it cannot be directly applied to vehicles traveling in off-road environments.
• The terrain in off-road environments has continuity, which means that there are many continuous equal-weighted accessible areas (e.g., grassland). HPA* uses fixed-size clusters to construct the abstract map, which splits these areas into multiple clusters, generating a large number of unnecessary nodes and reducing the pathfinding efficiency in these areas.

1.3. Research Objective

In order to balance the efficiency of real-time pathfinding and the quality of generated paths, this paper proposes a Hierarchical Path-Finding A* based on Multi-Scale Rectangle (RHA*), which improves the HPA* abstract map construction method and aggregates the original grid cells to form rectangular regions of varying sizes. This fully aggregates the equal-weighted areas of the off-road environment, greatly reducing the total cost of generated paths and narrowing the deviation from the optimal path. By optimizing the real-time search algorithm and node selection strategy, the real-time pathfinding efficiency is improved. The RHA* algorithm ensures high performance of the Hierarchical Path-Finding algorithm while significantly improving the quality of the generated paths.

2. Materials and Methods

2.1. RHA* Algorithm Architecture Design

Using the RHA* algorithm to carry out pathfinding in the off-road environment includes two steps: offline preprocessing and real-time pathfinding. The specific process is shown in Figure 1.

• In the offline preprocessing stage, the original map is abstracted hierarchically using multi-scale rectangular regions. The number of nodes and edges is reduced by optimizing node selection. This creates an abstract map of nodes, edges, and travel costs for real-time pathfinding.
• In the real-time pathfinding stage, different algorithms are selected to compute the optimal abstract paths based on the given start and end locations, and then the generated paths are refined by looking up the table.

2.2. Offline Preprocessing

The original map needs offline preprocessing to generate the abstract map, which includes three steps: regular clusters construction, multi-scale rectangular regions construction, and edges calculation. In the following, a 40 × 40 example map is used to detail the entire process of offline preprocessing.

2.2.1. Regular Clusters Construction

Firstly, starting from the upper left corner of the map, n × n (this paper takes 10 × 10) neighboring grid cells are aggregated to form a cluster. When the number of original grid cells at the edge of the map is less than n × n, the vacant grid cells are automatically filled using a certain number of inaccessible grids. The example map containing 40 × 40 grid cells is decomposed into 4 × 4 clusters. All types of grid cells are involved in the aggregation process. Figure 2 illustrates the aggregation process. Each cluster has a type value that determines whether the adjacent clusters can be further aggregated. The type value is determined by the travel costs of all grid cells within the cluster. When all internal grid cells are inaccessible or there are n (n ≥ 2) types of travel costs, the cluster type is set to a fixed value and no further aggregation is required. When all internal grid cells have the same travel cost and are accessible, different type values are set according to the travel cost. See Appendix A for details on cluster definition.

Next, the nodes on the cluster boundaries need to be found. A cluster is like a separate room, and the nodes are like doors to the next room. The number and location of the nodes are determined by the channel on the boundary of the neighboring clusters. A channel is defined as the longest accessible segment on the boundary of two neighboring clusters. For each channel, 1–n (this paper takes 3) nodes are determined based on the channel width. If the value of n is too large, the number of nodes will increase significantly and reduce the search efficiency. The gray grids in Figure 3 represent the nodes on the cluster boundary. See Appendix B for details on nodes definition.

Finally, the edges connecting the nodes need to be defined. Two symmetric nodes on the boundary of two adjacent clusters are directly connected to form an inter-edge. The shortest path formed by nodes inside a cluster is called an intra-edge. The gray curves in Figure 3 denote intra-edges, and the blue straight lines denote inter-edges.

2.2.2. Multi-Scale Rectangular Regions Construction

In order to aggregate equal-weighted areas, it is necessary to further aggregate clusters to form multi-scale rectangular regions. For clusters that are inaccessible or contain multiple travel costs, no further aggregation is required. A region containing only these clusters is directly generated, which is called a barrier region or mixed region. Therefore, the equal-weighted regions must be focused on. An equal-weighted region is a collection of clusters {C₁, C₂…C_n}, which needs to satisfy the following conditions:

• All the clusters have the same type:

C₁.type = C₂.type = … = C_n.type

(1)

• One region contains at least one cluster, i.e., n ≥ 1.
• The interior of a region does not contain any inaccessible area.
• The region is a rectangle.

Starting with the upper left cluster of the map, determine whether each cluster needs to be aggregated. Clusters containing obstacles or multiple travel costs are not aggregated. For clusters that need to be aggregated, this paper adopts a simple and efficient maximum rectangle search method. First, starting from the current cluster to be aggregated (called currentCluster), find the clusters that have the same type value and are continuous. Then, the same search is performed on each subsequent row until the type of the starting cluster differs from currentCluster. The starting cluster for each row search is located directly below the currentCluster. Finally, based on the clusters obtained from the search, find the largest rectangle that can contain the currentCluster. In Figure 4, the gray cluster is the first cluster that needs to be aggregated. Blue clusters are the search start points for each row. Black arrows indicate the direction of the search. The red wireframe is the largest rectangle that can be found, containing a region. See Appendix C for details on region definition and maximum rectangle search method.

An equal-weighted region aggregated from multiple clusters contains many unnecessary nodes that need to be optimized, including the following two situations:

• For nodes that are completely inside the region, just discard them.
• For nodes on the region boundary, they need to be processed according to the number and type of adjacent regions. The basic principle is to discard one of the two adjacent nodes on the region boundary as much as possible while maintaining the complete connectivity of the abstract map. See Appendix D for detailed processing steps.

After optimization, the number of nodes in equal-weighted regions is significantly reduced, but the map connectivity and integrity remain unchanged. In Figure 5, the red nodes need to be discarded, and the gray ones need to be retained.

2.2.3. Edges Calculation

After completing the construction of regions, edges need to be constructed, including both inter- and intra-edges. The inter-edges are connected by neighboring adjacent nodes, and no additional computation is required. For the unequal-weighted region, which contains only one cluster with a small number of original grid cells, the A* algorithm can be directly used to quickly calculate the optimal path between two nodes. The travel cost of each grid cell that the path passes through is summed to obtain the total cost between the two nodes.

For equal-weighted regions, the number of original grid cells, nodes, and edges contained will be large. According to the Euclidean geometric axioms, the straight-line distance between two points in a rectangular region is the shortest. Therefore, computing edges within the equal-weighted region becomes straightforward. Instead of finding the optimal path based on the original grid cells, the total cost of the edges is computed directly based on the coordinates of the two nodes. Given any two different nodes node1 (x1, y1), node2 (x2, y2) on the boundary of the equal-weighted region, as well as the travel cost on the unit grid g, the travel cost between the nodes is obtained by the following equation:

$$\mathit{cost}=\sqrt{{(x1-x2)}^2+{(y1-y2)}^2}\times g$$

(2)

Figure 6 shows the nodes and edges included in the example map after constructing regions.

After preprocessing, the cost of each edge in the abstract map and its corresponding path on the original map are calculated in advance. The calculation results need to be stored in the local data table and will be used in real-time pathfinding.

2.3. Real-Time Pathfinding

The offline preprocessing obtains an abstract map containing nodes and edges, and the real-time pathfinding is performed based on this abstract map, which mainly consists of two steps: abstract path calculation and path refinement.

2.3.1. Abstract Path Calculation

According to the different positions of the start point S and the end point E added to the abstract map, the calculation process is divided into the following two cases:

• Points S and E lie in the same equal-weighted region. The equal-weighted region is a rectangular equal-weighted accessible area, so a straight line between S and E is the optimal path. The total cost between S and E is calculated using Equation (2). This situation is more likely to occur when the region’s size is larger.
• Points S and E are not in the same region. S and E need to be added to the abstract map first. For S within the unequally weighted region, use the A* algorithm to compute the edges from S to each node at the boundary of the region and add them to the abstract map. For S within an equal-weighted region, the edges from S to each node are calculated based on the straight-line distance from S to the boundary node and added to the abstract map. The same process is performed for E. Then, based on the complete abstract map, the A* algorithm is used to search for paths starting from S. The search process is performed by using the formula to select the next search node:

f(n) = g(n) + h(n)

(3)

where:

f(n) is the integrated travel cost to the current node to be detected, and the one with the smallest cost is preferred as the next search node; g(n) is the travel cost from the start point along the generated path to the current node to be detected; and h(n) is the valuation function, which is calculated using Euclidean distance.

$$h(n)=\sqrt{{(x_n-x_{n-1})}^2+{(y_n-y_{n-1})}^2}$$

(4)

where:

x_n, y_n are the coordinates of the current node, x_n−1, y_n−1 are the coordinates of the previous node.

2.3.2. Path Refinement

Path refinement requires transforming the abstract path into a detailed path based on the original grid cells. The original grid paths corresponding to each unequal-weighted region intra-edge on the abstract path can be retrieved by querying the locally cached data table from Section 2.2.3. Edges within the equal-weighted region contain only two nodes, the start point and the end point, and do not need to be refined. The entire path refinement process can be completed at once, or it can be gradually refined in segments during the vehicle’s traveling process as required.

3. Experiment and Results

3.1. Experiment Setup

Benchmark map sets provided by the Mobile AI Lab at the University of Alberta are widely used for testing pathfinding algorithms. This paper selected one of the StarCraft maps named Archipelago as a test, which contains 512 × 512 raw grid cells with three terrains having different travel costs. The experiment uses the standard test set that comes with the map for real-time pathfinding testing. This set includes 2311 test cases, each containing a start point and an end point, and all the test cases are evenly distributed across different areas of the map, which helps to improve the accuracy of the test results. Figure 7 shows the test map.

The tests were conducted on a 12th Gen Intel(R) Core(TM) i5-1240P 1.70 GHz CPU with 16 GB of RAM. The application was run under a Windows 11 operating system environment.

3.2. Experiment Steps

The experiment consists of two steps: offline preprocessing and real-time pathfinding.

3.2.1. Offline Preprocessing

First, the cluster size in the test program is set to 10 × 10. Then the experimental map data is read for offline preprocessing, which includes regular clusters construction, multi-scale rectangular regions construction, and edges calculation.

After completing the offline preprocessing, an abstract map G = (V, E, C) is formed, where V is the set of all nodes containing a total of 7842 nodes, E is the set of all edges containing a total of 78,994 edges, and C is the set of corresponding travel cost of edges.

3.2.2. Real-Time Pathfinding

The test program reads the standard test set of the experimental map and executes 2311 test cases based on G. Before performing each real-time pathfinding process, the program automatically removes the last addition of the start and end points and their associated edges in G. This ensures the independence and accuracy of each real-time pathfinding test result.

3.3. Result Analysis

Firstly, the real-time pathfinding efficiency of the algorithms is compared. The computation time includes the time for inserting the start and end points into the abstract map, calculating the optimal abstract path, and refining the abstract path. Figure 8 shows a comparison of the average time consumed by the three algorithms: A*, HPA* and RHA*. A* consumes an average of 111.03 ms, HPA* consumes an average of 3.73 ms, and RHA* consumes an average of 2.97 ms. This demonstrates that the improved algorithm proposed in this paper exhibits excellent real-time pathfinding efficiency. The computation time was reduced by 96.64% compared to A* and 20.38% compared to HPA*.

In terms of the comparison of the quality of the generated paths, it is first defined as:

$$e=\frac{il-ol}{ol}\times 100\%$$

(5)

to indicate the deviation value between the total cost of the generated paths and the total cost of the optimal paths computed by A*. Where il represents the total cost of the generated path using HPA* or RHA*, and ol represents the total cost of the optimal path using A*. This paper performed a total of 2311 real-time pathfinding tests, each with a different start point and end point, evenly distributed across different areas of the map.

Table 1. Comparison of A*, HPA* and RHA* path quality.

	A*	HPA*	RHA*
Total cost of paths	399.5	415.94	399.63
Deviation e	-	4.12%	0.033%

presents the average travel cost of paths generated by HPA*, RHA* and A*. It can be seen that the average deviation between the total cost of paths generated by RHA* and the optimal paths generated by A* is only 0.033%, which is significantly better than 4.12% of HPA*.

From Figure 8 and Table 1, it can be seen that the RHA* algorithm achieves both real-time pathfinding efficiency and generated path quality. While ensuring high efficiency of real-time pathfinding, it also significantly reduces the deviation between the total cost of the generated path and the optimal path generated by A*.

In the experimental test set, the start and end points are different for each test. However, in actual applications, there may be multiple searches for driving paths from different start points to the same end point. In this case, the process of inserting the end point into the abstract map only needs to be calculated once, and the result can be cached and reused. This can further improve the efficiency of real-time pathfinding.

4. Discussion

4.1. Optimization of Real-Time Pathfinding Efficiency

The HPA* algorithm uses fixed-size clusters to construct an abstract map. In a large-scale off-road environment, this will split continuous equal-weighted areas (such as large grassland, flat bare land, etc.) into multiple clusters, generating a large number of unnecessary nodes. The RHA* algorithm proposed in this paper uses regions of different sizes to construct an abstract map. Fixed-size equal-weighted accessible clusters are further aggregated to form larger rectangular regions. Nodes inside these regions are removed directly, while boundary nodes are optimized as described in Section 2.2.2. This significantly reduces the number of nodes in the abstract map.

Before processing, the experimental map has 262,144 nodes. After HPA* preprocessing, it has 12,088 nodes, and after RHA* preprocessing, it has 7842 nodes. This shows that the number of nodes in the RHA* algorithm is reduced by 35.13% compared to HPA* and by 97.01% compared to the original map. The shortest paths between nodes on the region boundaries are computed offline in advance and stored locally. No further computation for regions is required in the real-time pathfinding stage. The reduction in the number of abstract map nodes and the precomputation of the shortest path within the regions enhance the real-time pathfinding efficiency of RHA*.

4.2. Improvement in the Quality of Generated Paths

In wide off-road environments, the quality of the generated paths is equally important. As shown in Figure 8 and Table 1, the paths generated by RHA* exhibit significantly higher quality compared to those generated by HPA*. Such differences are mainly reflected in the offline map preprocessing stage. HPA* decomposes the original map into multiple clusters of a fixed size. Consequently, each equally-weighted accessible cluster generates at least one node on each boundary. When performing pathfinding in large areas of equal weight, the generated path will consist of nodes at the boundaries of multiple clusters. Such a path is often a fold line.

In contrast, the RHA* algorithm maximally aggregates adjacent equal-weighted accessible clusters during preprocessing to form equal-weighted regions. Nodes within these regions are discarded, and the shortest path between nodes on the region boundary is a straight line. When the start point, end point and access conditions are the consistent, the travel cost of a straight-line path will be significantly lower than that of a fold-line path. Thus, when a generated path traverses both non-equal-weighted and equal-weighted areas, the segment passing through the equal-weighted area consistently improves in quality. This fundamental difference explains why RHA* produces higher-quality paths compared to HPA*.

5. Conclusions

In this paper, the RHA* algorithm is proposed for the off-road environment pathfinding problem of autonomous vehicles. The algorithm adopts an abstract map construction method based on multi-scale rectangular regions. First, the original map grid cells are aggregated to form multiple fixed-size clusters. Then, unlike HPA*, the equal-weighted clusters are further aggregated to form rectangular regions of varying sizes. The shortest paths between nodes at the boundaries of the regions are computed offline in advance, and the results are stored locally. Finally, real-time pathfinding based on the abstract map is performed to obtain the abstract paths. The abstract paths are refined into final paths based on the original map grid by querying the local data table. The algorithm achieves complete aggregation of equal-weighted areas in the field, enhancing real-time pathfinding efficiency. Simultaneously, it effectively addresses the challenge where the total travel cost of paths generated based on the abstraction map significantly exceeds the optimal paths computed by A*.

The experimental benchmark map used in this paper is provided by the Mobile AI Laboratory at the University of Alberta. The experiment shows that the RHA* algorithm proposed in this paper not only ensures high efficiency in real-time pathfinding, but also significantly reduces the deviation between the total cost of the generated path and the optimal path generated by A*. The generated path can be directly used for unmanned vehicles to travel in the off-road environment, solving the problem of real-time pathfinding in large-scale complex terrain environments in the field. The RHA* algorithm is designed for raster maps. When applied to other types of maps, further improvements to the algorithm may be necessary.