Hybrid A-Star Path Planning Method Based on Hierarchical Clustering and Trichotomy

Abstract

Aiming to improve on the poor smoothness and longer paths generated by the traditional Hybrid A-star algorithm in unstructured environments with multiple obstacles, especially in confined areas for autonomous vehicles, a Hybrid A-star path planning method based on hierarchical clustering and trichotomy is proposed. This method first utilizes the Prewitt compass gradient operator (Prewitt operator) to identify obstacle boundaries and discretize boundaries. Then, it employs a single linkage hierarchical clustering algorithm to cluster obstacles based on boundaries. Subsequently, the clustered points are enveloped using a convex hull algorithm, considering collision safety for vehicle expansion. This fundamentally addresses the ineffective expansion issue of the traditional Hybrid A-star algorithm in U-shaped obstacle clusters. Finally, the expansion strategy of Hybrid A-star algorithm nodes is improved based on the trichotomy method. Simulation results demonstrate that the improved algorithm can search for a shorter and smoother path without significantly increasing the computational time.

1. Introduction

In recent years, the rapid development of autonomous driving technology has greatly improved traffic safety and efficiency. Autonomous driving technology mainly includes positioning navigation, environmental perception, decision-making planning, and tracking control [1]. Decision-making planning includes behavioral decision-making, global path planning, and local path planning. Global path planning is the process of calculating a safe and efficient driving path from a given starting point to a destination [2]. Many scholars have conducted research on global path planning for autonomous vehicles [3]. Common methods of global path planning include graph-based algorithms [4,5,6], probability-sampling-based algorithms [7,8,9], and intelligent-based algorithms [10,11,12]. The Hybrid A-star algorithm belongs to graph-based algorithms, and it is an improvement and development of the A-star algorithm. It overcomes the disadvantage of the A-star algorithm that the expanded nodes must fall on the grid center and considers vehicle kinematic constraints [13]. The planned path is continuous and smooth, making it highly suitable for autonomous vehicles.

Many scholars have conducted extensive research to address the incompleteness and suboptimality of the Hybrid A-star algorithm. Xu et al. [14] proposed an improved A-star algorithm and map local scaling technique to solve the problem of non-optimal and inaccurate paths in the path planning process. Compared with traditional methods, their proposed method reduces the time by 23% and the path length by 21%. Zhao et al. [15] proposed a hierarchical motion planning method based on improved hybrid A-star algorithm and time-elastic band algorithm. By modifying the collision detection process and introducing the map guide cost, the search efficiency is accelerated. The improved hybrid A-star algorithm can quickly respond to environmental changes. The proposed algorithm can effectively solve the problem of vehicle lateral motion planning in conventional scenarios.

Sheng et al. [16] proposed a multi-stage Hybrid A-star algorithm to address narrow passage problems. The Hybrid A-star algorithm generates a coarse path, which is then converted into a rough trajectory with an additional time-optimal speed profile and serves as the initial guess input for numerical optimization layers. The multi-stage Hybrid A-star algorithm significantly outperforms existing planners. Dang et al. [17] addressed the issue of collisions at corners due to system errors by proposing an improved Reeds–Shepp method to enhance the safety of the Hybrid A-star algorithm. Fine-tuning of primitive actions during the forward search stage leads to smoother paths, reducing collision risk costs and the number of turning points by around 20% in the improved approach. Jiao et al. [18] proposed an improved Hybrid A-star algorithm by enhancing the heuristic function and designing obstacle penalty functions to proactively avoid obstacles, preventing the algorithm from becoming stuck in local optima, especially in U-shaped obstacles. The improved Hybrid A-star path planning algorithm exhibits high planning efficiency, safety performance, and path smoothness. Deng et al. [19] integrated CCRS curve filtering with mining area scenarios to enhance the applicability of the Hybrid A-star algorithm. Segmented spline curves based on quadratic programming are utilized for path smoothing. The proposed method significantly improves the applicability of the traditional Hybrid A-star algorithm in searching paths and achieves a 10-fold increase in smoothing efficiency compared to traditional discrete point smoothing methods.

Cui et al. [20] proposed an improved Hybrid A-star path planning algorithm tailored to tight parking scenarios, efficiently generating safe parking paths by swapping the starting and target points of the path. The proposed reverse Hybrid A-star algorithm reduces the expansion of invalid nodes during a path search, enhancing the real-time performance of path planning. Meng et al. [21] introduced an enhanced Hybrid A-star algorithm with safety-enhanced and efficiency-enhanced designs, integrating Voronoi diagrams into the path search stage to further consider path safety. The efficiency-enhanced design proposes a multi-stage dynamic optimization strategy. The paths generated by the improved algorithm are safer and significantly improve the search efficiency. Zhong et al. [22] addressed efficiency and safety concerns in path planning by proposing an enhanced Hybrid A-star algorithm. They introduced a distance penalty function into the heuristic function, constructing safe corridors to guide mobile robots away from obstacles. The improved Hybrid A-star algorithm enhances the path planning efficiency, resulting in safer planned paths. Liu et al. [23] proposed an optimization solution to address the slow search time of the Hybrid A-star algorithm by integrating a bidirectional search algorithm and the Hybrid A-star algorithm. They optimized the bidirectional search algorithm using grid methods and improved the node search process of the Hybrid A-star algorithm. The integrated algorithm can shorten the search time. Zhou et al. [24] optimized the Hybrid A-star algorithm based on the minimum jerk to address the issue of discomfort. They conducted algorithmic research on grid maps and proposed a “corridor” automatic adjustment strategy to prevent trajectory collisions caused by optimization. The optimized trajectory has a smaller curvature.

This paper addresses the issues of poor smoothness and longer paths in Hybrid A-star algorithm navigating through small-scale, multi-obstacle, and unstructured scenarios. A solution is proposed using a Hybrid A-star algorithm based on single linkage hierarchical clustering and trichotomy searching, fundamentally resolving the ineffective expansion problem near U-shaped obstacle clusters and improving the node expansion approach of the Hybrid A-star algorithm. The enhanced algorithm is capable of searching for a shorter and smoother path without significantly increasing the computational complexity.

The main contributions of this paper are as follows:

(1) An obstacle hierarchical clustering method is proposed, which uses the Prewitt operator to identify an obstacle boundary and clusters the obstacles based on the single linkage hierarchical clustering algorithm after discretization of a boundary, fundamentally solving the problem of the ineffective expansion of the Hybrid A-star algorithm in U-shaped obstacle groups.

(2) An obstacle expansion method considering vehicle collision safety is proposed. The point set after obstacle clustering is enveloped by a convex hull algorithm, and the convex hull expansion is processed considering vehicle collision safety. The function of expansion is to ensure vehicle safety.

(3) A path planning method of the Hybrid A-star algorithm based on the expansion of the trichotomy is proposed to solve the problem of poor smoothness and a long path planned.

The remaining sections of the article are organized as follows: Section 2 introduces the single linkage hierarchical clustering algorithm. Section 3 presents the Hybrid A-star algorithm and trichotomy search. Section 4 describes the simulation results and discussions. Section 5 concludes the paper.

2. Single Linkage Hierarchical Clustering Algorithm

2.1. Scene Map Preprocessing

The research was conducted based on a known map since the algorithm proposed is a global path planning algorithm, and the existing research on global path planning algorithms was based on known maps; even in different known maps, the algorithm can run without limitations. In a known map, the distribution of obstacles, as well as the shape, size, and position of obstacles, are known in advance. Hierarchical clustering calculates the distance between data points of each category and all data points to determine their similarity. The smaller the distance, the higher the similarity, and the two data points closest to each other are combined to generate a clustering tree. To generate data points, it is necessary to use the Prewitt operator to identify the boundaries of obstacles, and only after the identified obstacles are discretized can data points be obtained. The principle of the Prewitt operator is to realize edge detection by using the difference generated by the gray value of pixels in a specific region. Although the map is known, there is no guarantee that the map is grayscale. During processing, it needs to be processed into a grayscale map to facilitate the work of the Prewitt operator. Firstly, the scene map is converted into a grayscale image. Then, the Prewitt operator is applied for edge detection of obstacles. The Prewitt operator is a type of differential operator used for image edge detection, which relies on the difference in pixel grayscale values within a specific region for edge detection [25]. It is suitable for identifying images with high noise and grayscale gradients, as it is a first-order differential operator [26]. The Prewitt operator employs a 3 × 3 template and utilizes the grayscale differences between the pixel’s upper, lower, left, and right neighboring points for edge detection. Therefore, its edge detection results are more pronounced in both the horizontal and vertical directions compared to the Robert operator. The Prewitt operator’s mathematical expressions are as follows:

$$G\left(x\right)=\left[f\left(x-1,y+1\right)+f\left(x-1,y\right)+f\left(x-1,y-1\right)\right]-[f\left(x+1,y+1\right)+f\left(x+1,y\right)+f\left(x+1,y-1\right)]$$

(1)

$$G\left(y\right)=\left[f\left(x-1,y+1\right)+f\left(x,y+1\right)+f\left(x+1,y+1\right)\right]-[f\left(x-1,y-1\right)+f\left(x,y-1\right)+f\left(x+1,y-1\right)]$$

(2)

$$P\left(x,y\right)=\mathrm{m}\mathrm{a}\mathrm{x}\left\{\left|G\left(x\right)\right|,\left|G\left(y\right)\right|\right\}$$

(3)

where $G\left(x\right)$ is the calculation result of the gray difference in the horizontal direction; $G\left(y\right)$ is the calculation result of the gray difference in vertical direction; $P\left(x,y\right)$ is the detection result of the Prewitt operator; $f\left(x-1,y+1\right)$ is the pixel value corresponding to the upper left corner of the template; $f\left(x-1,y\right)$ is the pixel value corresponding to the left middle position in the template; $f\left(x-1,y-1\right)$ is the pixel value corresponding to the lower left corner of the template; $f\left(x+1,y+1\right)$ is the pixel value corresponding to the upper right corner of the template; $f\left(x+1,y\right)$ is the pixel value corresponding to the left and right middle positions in the template; $f\left(x+1,y-1\right)$ is the pixel value corresponding to the lower right corner of the template; $f\left(x,y+1\right)$ is the pixel value corresponding to the top position in the middle of the template; and $f\left(x,y-1\right)$ is the pixel value corresponding to the bottom of the middle of the template.

Figure 1 provides calculation templates for the horizontal and vertical directions of the Prewitt operator. The value in each cell in the template represents the pixel value corresponding to that position. The Prewitt operator extracts the edge by calculating the gray difference in the horizontal direction and vertical direction. Figure 1a is the horizontal direction of the calculation template: $G\left(x\right)$, Figure 1b is the vertical direction of the calculation template: $G\left(y\right)$. After edge detection is completed, the map undergoes binarization, where the values of obstacle positions are set to 1, and the remaining positions are set to 0. Finally, the boundaries of the obstacles are discretized into several points, preparing for obstacle clustering.

2.2. Single Linkage Hierarchical Clustering Algorithm

Clustering is the process of dividing a dataset into different classes according to a specific criterion, such as distance, aiming to minimize the intra-class differences of data objects within the same class while maximizing the inter-class differences of data objects not in the same class. In this way, the obstacles are partitioned to achieve the purpose of hierarchical clustering. Suppose there is currently a partition $P_K=(C_1,C_2,\cdots ,C_K)$, dividing the sample population into K classes. The total intra-class difference of this partition can be represented by Equation (4).

$$\mathrm{W}\left(P_K\right)=\sum _{k=1}^{K}Int\left(C_k\right)$$

(4)

where $Int\left(C_k\right)$ represents the within-cluster variance of class $C_k$, and $Int\left(C_k\right)$ is also the sum of the Euclidean distances between all the data objects in the class. Commonly used within-cluster variances include variance, the maximum distance between any two points within the class, the longest edge in the minimum spanning tree, etc. The smaller the value of $\mathrm{W}\left(P_K\right)$, the more homogeneous the partition of class K tends to be.

Assuming there are N samples in the dataset to be clustered, hierarchical clustering initially forms N classes, with each sample forming its own class, resulting in N initial classes. By computing the inter-cluster differences between any two classes, the method iteratively merges the two classes with the smallest inter-cluster difference. The two classes are designated as any two of the K classes for example calculation, that is, all the classes between the K classes should be calculated, reducing the number of classes from K + 1 to K. In other words, from $P_{K+1}$, two classes, A and B, are merged to obtain a new partition, $P_K$, while satisfying Equation (5):

$$arg\underset{A,\mathit{B}\in P_{K+1}}{\min }W\left(P_K\right)$$

(5)

where $P_K=\frac{P_{K+1}}{\left\{A,B\right\}}\cup \{A\cap B\}$ represents the new partition obtained by merging A and B; and $W\left(P_K\right)=W\left(P_{K+1}\right)-Int\left(A\right)-Int\left(B\right)+Int\left(A\cup B\right)$ represents the total intra-class difference of the new partition. Equation (5) is equivalent to Equation (6):

$$\underset{A,\mathit{B}\in P_{K+1}}{\min }Int(A\cup B)-Int\left(A\right)-Int\left(B\right)$$

(6)

Equation (7), denoting the inter-class difference between two classes A and B as $D_{A,B}$, is defined as follows:

$$D_{A,B}=Int(A\cup B)-Int\left(A\right)-Int\left(B\right)$$

(7)

As deduced above, the inter-class difference between two classes is the increment of the total intra-class difference after merging the two classes. The smaller the inter-class difference, the greater the similarity between the two classes.

The essence of hierarchical clustering is to find the two classes with the smallest inter-class difference in each iteration, minimizing the increment of the total intra-class difference after merging [27]. Common computational methods for hierarchical clustering include the centroid method, Ward’s minimum variance method, single linkage method, complete linkage method, and group average method [28]. Single linkage clustering, also known as nearest neighbor clustering, is selected in this paper. It defines the inter-class difference between two classes as the shortest distance between any two points in different classes calculated by Equation (8).

$$D_{A,B}^{sin}=\underset{x_i\in A,x_j\in B}{\min }d({\overline{x}}_A,{\overline{x}}_B)$$

(8)

Single linkage clustering imposes no constraints on the shape of clusters and exhibits good scalability, capable of identifying irregular and elongated clusters. After merging two classes A and B to form a new class M, it is necessary to recalculate the inter-class difference between the new class M and any other class C. To simplify this calculation using previous inter-class difference information, the following update Equation (9) can be utilized:

$$D_{M,C}^{sin}=\min (D_{A,C}^{sin},D_{B,C}^{sin})$$

(9)

Figure 2 illustrates the process of preprocessing obstacles, where Figure 2a shows the original map and Figure 2b depicts the map after identifying obstacle boundaries using the Prewitt operator and discretizing these boundaries. Figure 2c shows a visualization of the obstacle clustering dendrogram, and Figure 2d represents the map after hierarchically clustering the obstacles, then expanding by 3 m in all directions and rasterizing. The purpose of the expansion is to ensure vehicle safety, so that even if the planned path is very close to the obstacles clustered on the map, collisions will not occur. Compared with a potential field collision avoidance scheme and limit ellipsis collision avoidance scheme, the collision scheme proposed retains a larger safety distance, so it is safer, and there is no need to calculate the equation, while the calculation complexity is lower. In this paper, twice the width of the vehicle is selected, and the length after integer operation is considered the reserved safety distance. Generally, the width of a vehicle is about 1.6 m, so the safety distance is 3 m, which does not depend on the size and speed of the map. In addition, this paper engages in global path planning, and speed is not considered.

3. Hybrid A-Star Algorithm and Trichotomy Search

3.1. Hybrid A-Star Algorithm

The A-star algorithm, as a hybrid of Dijkstra and BFS, differs from these two algorithms by employing an evaluation function, which is the core of the algorithm. The evaluation function takes the form of Equation (10):

$$f\left(n\right)=g\left(n\right)+h\left(n\right)$$

(10)

where $f\left(n\right)$ represents the overall priority of a node. When selecting a node, this overall priority, also known as the evaluation function, is considered; $g\left(n\right)$ represents the cumulative cost from the starting point to the current node, also known as the cumulative cost function; and $h\left(n\right)$ represents the estimated cost from the current node to the destination point, also referred to as the heuristic cost function. $g\left(n\right)$ and $h\left(n\right)$ are typically calculated using either the Manhattan distance or Euclidean distance. To calculate these two values, $g\left(n\right)$ and $h\left(n\right)$ must use the same unit of measurement. In this paper, the Euclidean distance is chosen. The Euclidean distance is one of the most common distance measurement methods. In two-dimensional space, it measures the shortest linear distance between two points. Choosing the Euclidean distance can more accurately calculate the cumulative cost function and the heuristic cost function, and it can guide the algorithm nodes to expand towards a shorter path direction.

A-star algorithm controls the points in the map by establishing an open list and a closed list. First, the starting point is added to the open list. Then, the following process is repeated:

• Traverse the open list to find the node with the minimum $f\left(n\right)$ value, which becomes the node to be processed. Remove it from the open list to prevent node reuse;
• Add the node to be processed to the closed list;
• For the nodes adjacent to the current node, the following procedures are applied: If a node is unreachable or already on the closed list, it is ignored. If a node is not on the open list, it is added to the open list. The current node is then set as its parent node, and the $f\left(n\right)$, $g\left(n\right)$, and $h\left(n\right)$ values of the current node are recorded. If a node is already on the open list, the algorithm checks if the path (i.e., from the current node to the adjacent node) is better using the $g\left(n\right)$ value as a reference. A smaller $g\left(n\right)$ value indicates a better path. If the new path is better, the parent node is set as the current node, and the $g\left(n\right)$ value and $f\left(n\right)$ value are recalculated. If the open list is sorted based on $f\left(n\right)$ values, reordering is necessary after the change;
• When the destination point has been added to the open list, indicating that either the path has been found or the search for the endpoint has failed, and the open list is empty, indicating that there is no path, the loop ends and the search stops;
• The Hybrid A-star algorithm and the A-star algorithm both belong to graph-based search algorithms, with the Hybrid A-star algorithm being an improvement on the A-star algorithm. While the Hybrid A-star algorithm and the A-star algorithm share a similar process, the Hybrid A-star algorithm does not limit expansion to the center of the grid. Instead, a set of step sizes, a turning radius, and a discrete number of front wheel steering angles form the basis for expanding nodes. This allows child nodes to be located at any position within the grid rather than just at the grid center. Since the Hybrid A-star algorithm considers vehicle kinematic constraints during state node expansion, the resulting path is executable by the vehicle. However, the Hybrid A-star algorithm sacrifices completeness and optimality [29]. The node expansion methods of the Hybrid A-star algorithm and the A-star algorithm are illustrated in Figure 3.

3.2. Design of the Evaluation Function

The evaluation function of the Hybrid A-star algorithm consists of the cumulative cost function $g\left(n\right)$ and the heuristic cost function $h\left(n\right),$ as depicted in Equation (10). For the cumulative cost function $g\left(n\right)$, aiming to minimize the path length, a cumulative cost function for the path is defined. Meanwhile, to ensure path smoothness and minimize steering maneuvers, a penalty is imposed on vehicle steering within the cumulative cost function. The specific cumulative cost function is shown in Equations (11)–(13):

$$g\left(n_1\right)=S$$

(11)

$$g\left(n_2\right)=\sum _0^i\left|{\varnothing }_i-{\varnothing }_p\right|$$

(12)

$$g\left(n_i\right)=g\left(n\right)+\alpha g\left(n_1\right)+\beta g\left(n_2\right)$$

(13)

where $S$ represents the set expansion step length; $g\left(n_1\right)$ considering the cumulative length of the path to ensure it is as short as possible; $g\left(n_2\right)$ incorporates a penalty for the change in heading angle between consecutive states to steer the vehicle smoothly; ${\varnothing }_i$ denotes the heading angle of the new expanded node; ${\varnothing }_p$ represents the heading angle of the parent node; $g\left(n_i\right)$ represents the cumulative cost for the child node; $g\left(n\right)$ represents the cumulative cost for the parent node; and $\alpha$ and $\beta$ are the weighting coefficients for each term in the cost function.

The heuristic function $h\left(n\right)$ estimates the cost from the current point to the goal point. In the A-star algorithm, if the heuristic cost function provides a closer estimate of the actual cost of the remaining path, it can significantly improve the search efficiency of the algorithm. A similar property exists in the Hybrid A-star algorithm. Therefore, to bring the heuristic function value closer to the actual value, the shortest collision-free distance from the endpoint to each grid point planned by the A-star algorithm is used as the heuristic function value.

3.3. Trichotomy Node Expansion Method

The Hybrid A-star algorithm typically expands three new nodes for path planning in each iteration. Increasing the number of nodes expanded per iteration can effectively enhance the smoothness of path planning and reduce the path length. However, uniformly distributing node expansions consume significant computational resources, leading to a decrease in planning efficiency. Considering both node expansion and planning efficiency, this paper proposes an improvement to the node expansion method of the Hybrid A-star algorithm, introducing a trichotomy expansion method (Tri-Hybrid A-star). The resulting Tri-Hybrid A-star algorithm generates shorter and smoother paths. The node expansion method of the Hybrid A-star algorithm is shown in Figure 4a, which can only be expanded by three nodes: maximum left turn, straight, and maximum right turn. Based on the node expansion method of the Hybrid A-star algorithm, the node with the smallest evaluation function value in the maximum left turn, straight, and maximum right turn and its adjacent nodes are selected for the trichotomy expansion. When the set number of iterations is satisfied, the trichotomy expansion ends. The spatial position relationship diagram of the trichotomy expansion method is shown in Figure 4b.

By comparing the evaluation functions of the three new nodes expanded by the Hybrid A-star algorithm, the node corresponding to the minimum evaluation function and its neighboring nodes are selected for trichotomy expansion. Initially, the maximum heading angle ${\theta }_{max}$ and minimum heading angle ${\theta }_{min}$ of the subnodes within the trichotomy region are used as the left and right boundaries for expansion. This yields the steering angle of the vehicle’s front wheels corresponding to the parent node, as obtained by Equation (14):

$${\delta }_i=\frac{{\theta }_{max}-{\theta }_{min}}{3}\times i$$

(14)

where ${\delta }_i$ represents the front wheel steering angle corresponding to the parent node, with positive values to the left of the heading axis of the parent node and negative values to the right. $i$ represents the index corresponding to the expansion point, with values ranging from 1 to 3.

Based on the front wheel steering angle obtained from Equation (14), the turning radius corresponding to each expansion point can be calculated according to spatial positional relationships in Equation (15):

$$R_i=\frac{S}{2\sin \frac{{\delta }_i}{2}}$$

(15)

where $S$ represents the set expansion step length; and $R_i$ denotes the turning radius corresponding to each expansion point, with positive values to the left of the heading axis of the parent node and negative values to the right.

Based on the turning radius corresponding to each expansion point, the coordinates of the centers of curvature corresponding to the turning radius can be determined according to Equations (16) and (17):

$$X_{R_i}=X_p-R_i\sin \theta$$

(16)

$$Y_{R_i}=Y_p+R_i\cos \theta$$

(17)

where $X_p$ represents the parent node’s x-coordinate; $Y_p$ represents the parent node’s y-coordinate; $X_{R_i}$ denotes the x-coordinate of the center; $Y_{R_i}$ denotes the y-coordinate of the center; and $\theta$ represents the heading angle of the parent node.

To obtain the coordinates corresponding to the expansion node, it is necessary to introduce the intermediate variable (Equation (18)), which is an angle.

$${\gamma }_i={\delta }_i+{\tan }^{-1}\frac{Y_{R_i}-Y_p}{X_{R_i}-X_p}$$

(18)

After introducing the intermediate variable, the corresponding transverse and longitudinal coordinates and the corresponding heading angle of the extended node can be calculated according to Equations (19) and (20).

$$X_i=X_{R_i}+\left|R_i\right|\cos {\gamma }_i$$

(19)

$$Y_i=Y_{R_i}+\left|R_i\right|\sin {\gamma }_i$$

(20)

$${\theta }_i=\theta +{\delta }_i$$

(21)

Combining the vehicle’s single-track kinematic model [30] with Equations (14)–(21) allows for the calculation of coordinates and heading angles for all expandable nodes within the trichotomy region. The single-track kinematic model constrains the vehicle’s minimum turning radius, thereby imposing constraints on the curvature of the planned path to ensure smooth vehicle passage. After reaching the specified number of iterations, the trichotomy expansion method selects the node with the minimum evaluation function value for planning execution. The pseudocode for the Hybrid A-star algorithm improved by trichotomy expansion (Algorithm 1) is as follows:

Algorithm 1. Hybrid A-star algorithm improved by trichotomy expansion.
1:	$\mathrm{Input}:N_{\mathit{start}},N_{\mathit{goal}},S,D_{\mathit{safe}},\alpha$$,\beta$, Iterations.
2:	Initialize: Nstart, f(n).
3:	$\mathrm{While} N_{\mathit{new}}\ne$ Ngoal
4:	Delete minf(n) in open list as Nparent.
5:	for i = 0: 3
6:	if no collision
7:	Create Ntemporary, calculate g(n) with Equation (13), update f(n), Mark as 1.
8:	end
9:	end
10:	if [1,1,1] & Nmiddle is minf(n)
11:	Expansion areas are Aleft and Aright.
12:	if Aleft and Aright are no collision
13:	Minf(n) side to update Nnew.
14:	elseif Aright is no collision, then: Right side to update Nnew.
15:	elseif Aleft is no collision, then: Left side to update Nnew.
16:	end
17:	elseif [1,1,1] & Nleft is minf(n) or [1,1,0], then: Expansion area is Aleft.
18:	elseif [1,1,1] & Nright is minf(n) or [0,1,1], then: Expansion area is Aright.
19:	end
20:	Find a path, add Nnew to open list.
21:	Get information from minf(n).
22:	if Nnew − Ngoal ≤ 2 × Dsafe, then: End loop.
23:	end
24:	Backtracking the expanded nodes.
25:	Cubic B-spline interpolation fitting path.

4. Simulation Results and Discussion

To validate the research conducted, the Hybrid A-star algorithm was implemented using MATLAB software (version number 2020a). The simulation used a laptop computer, with the specific configuration including the following: the CPU is an Intel Core i7-8750H, the RAM is 16 GB, and the graphics card is an NVIDIA GEFORCE GTX 1080. The map used was a small area with multiple obstacles in an unstructured scene measuring 150 m × 200 m, with a grid map discretization size of 1 m. First, simulations were performed using the Hybrid A-star algorithm and the Hierarchical Clustering-based Hybrid A-star algorithm (HC-Hybrid A-star algorithm) for two different setups. The measurement method of the path length used was to discretize the path obtained from the backtracking node, and the discretized distance was 1 m. By summing the discretized Euclidean distance between the points, the path length could be obtained.

As shown in Figure 5a,c, during the node expansion process of the Hybrid A-star algorithm, even when using the shortest collision-free distance from the endpoint to each grid point planned by the A-star algorithm as the heuristic value, it can still be erroneously directed towards a cluster of U-shaped obstacles, resulting in a significant number of ineffective node expansions and thus reducing planning efficiency. As shown in Figure 5b,d, the HC-Hybrid A-star algorithm fundamentally addresses the issue of ineffective expansions into U-shaped obstacle clusters encountered by the Hybrid A-star algorithm.

The computational times, expanded node counts, and planned path lengths of the two algorithms under different setups are shown in

Table 1. Data of two algorithms in different setups.

Indexes	Setups	Hybrid A-Star	HC-Hybrid A-Star	Improvement
Computational time (s)	Setup 1	18.738	13.973	25.43%
	Setup 2	322.768	15.194	95.29%
Expanded node count	Setup 1	130	49	62.31%
	Setup 2	2686	49	98.18%
Planned path length (m)	Setup 1	189.739	190.313	−0.30%
	Setup 2	197.554	198.256	−0.36%

. In setup 1, the computational time of the Hybrid A-star algorithm is 18.738 s, while the computational time of the HC-Hybrid A-star algorithm is 13.973 s. In setup 2, the computational time of the Hybrid A-star algorithm is 322.768 s, while the computational time of the HC-Hybrid A-star algorithm is 15.194 s. In setup 1, the expanded node count of the Hybrid A-star algorithm is 130, while the expanded node count of the HC-Hybrid A-star algorithm is 49. In setup 2, the expanded node count of the Hybrid A-star algorithm is 2686, while the expanded node count of the HC-Hybrid A-star algorithm is 49. In setup 1, the planned path length of the Hybrid A-star algorithm is 189.739 m, while the planned path length of the HC-Hybrid A-star algorithm is 190.313 m. In setup 2, the planned path length of the Hybrid A-star algorithm is 197.554 m, while the planned path length of the HC-Hybrid A-star algorithm is 198.256 m. In the two different setups, the computational time of the HC-Hybrid A-star algorithm is reduced by 25.43% and 95.29%, respectively, compared to the Hybrid A-star algorithm. The expanded node count is reduced by 62.31% and 98.18%, respectively. Although the HC-Hybrid A-star algorithm shows significant improvements in the computational time and expanded node count, there is only a slight change in the planned path length. This is because the HC-Hybrid A-star algorithm only clusters obstacles, fundamentally addressing the issue of ineffective expansions within U-shaped obstacle clusters, without modifying the structure of the Hybrid A-star algorithm.

The Hybrid A-star algorithm typically expands three nodes per iteration during usage. Many scholars, considering path optimality and completeness, have increased the number of nodes expanded per iteration to five in the Hybrid A-star algorithm, resulting in lower computational planning efficiency. The improved Hybrid A-star algorithm using trichotomy and dichotomy methods expands four nodes per iteration, leading to increased computational complexity compared to the Hybrid A-star algorithm that expands three nodes per iteration. To investigate the improvement of the Tri-HC-Hybrid A-star algorithm over the Hybrid A-star algorithm that expands three nodes per iteration, simulations were conducted on different maps.

In Figure 6, the path planning maps for different algorithms are shown under two setups for map 1. For setup 1, the accumulated heading angle changes for the HC-Hybrid A-star, Dic-HC-Hybrid A-star, and Tri-HC-Hybrid A-star algorithms are 6.785 radians, 8.779 radians, and 3.208 radians, respectively. The percentage increase in accumulated heading angle changes for the Dic-HC-Hybrid A-star algorithm and Tri-HC-Hybrid A-star algorithm compared to the HC-Hybrid A-star algorithm are −29.39% and 52.72%, respectively. The number of expanded nodes for the HC-Hybrid A-star algorithm, Dic-HC-Hybrid A-star algorithm, and Tri-HC-Hybrid A-star algorithm are 49, 66, and 65, respectively. The percentage increase in expanded nodes for the Dic-HC-Hybrid A-star algorithm and Tri-HC-Hybrid A-star algorithm compared to the HC-Hybrid A-star algorithm are 34.69% and 32.65%, respectively. For setup 2, the accumulated heading angle changes for the HC-Hybrid A-star algorithm, Dic-HC-Hybrid A-star algorithm, and Tri-HC-Hybrid A-star algorithm are 8.756 radians, 11.471 radians, and 8.036 radians, respectively. The percentage increase in accumulated heading angle changes for the Dic-HC-Hybrid A-star algorithm and Tri-HC-Hybrid A-star algorithm compared to the HC-Hybrid A-star algorithm are −31% and 8.22%, respectively. The number of expanded nodes for the HC-Hybrid A-star algorithm, Dic-HC-Hybrid A-star algorithm, and Tri-HC-Hybrid A-star algorithm are 49, 64, and 72, respectively. The percentage increase in expanded nodes for the Dic-HC-Hybrid A-star algorithm and Tri-HC-Hybrid A-star algorithm compared to the HC-Hybrid A-star algorithm are 30.61% and 46.94%, respectively.

The path planning for different algorithms on map 2 is illustrated in Figure 7. The cumulative changes in heading angle for the HC-Hybrid A-star, Dic-HC-Hybrid A-star, and Tri-HC-Hybrid A-star algorithms are 8.785 radians, 6.813 radians, and 4.242 radians, respectively. The cumulative changes in heading angle for the Dic-HC-Hybrid A-star algorithm and Tri-HC-Hybrid A-star algorithm are increased by 22.45% and 51.71%, respectively, compared to the HC-Hybrid A-star algorithm. The numbers of expanded nodes for the HC-Hybrid A-star algorithm, Dic-HC-Hybrid A-star algorithm, and Tri-HC-Hybrid A-star algorithm are 45, 58, and 67, respectively. The numbers of expanded nodes for the Dic-HC-Hybrid A-star algorithm and Tri-HC-Hybrid A-star algorithm are increased by 28.89% and 48.89%, respectively, compared to the HC-Hybrid A-star algorithm.

The path planning for different algorithms on map 3 is depicted in Figure 8. The cumulative changes in heading angle for the HC-Hybrid A-star algorithm, Dic-HC-Hybrid A-star algorithm, and Tri-HC-Hybrid A-star algorithm are 17.146 radians, 14.128 radians, and 8.475 radians, respectively. The cumulative changes in heading angle for the Dic-HC-Hybrid A-star algorithm and Tri-HC-Hybrid A-star algorithm are increased by 17.60% and 50.57%, respectively, compared to the HC-Hybrid A-star algorithm. The numbers of expanded nodes for the HC-Hybrid A-star algorithm, Dic-HC-Hybrid A-star algorithm, and Tri-HC-Hybrid A-star algorithm are 90, 99, and 99, respectively. Both the Dic-HC-Hybrid A-star algorithm and Tri-HC-Hybrid A-star algorithm have an increased number of expanded nodes by 10% compared to the HC-Hybrid A-star algorithm.

Figure 9 shows the statistical cumulative change in heading angle and the number of expanded nodes.

Table 2. Planned path lengths under different maps.

Indexes	Maps	HC-Hybrid A-Star	Dic-HC-Hybrid A-Star	Tri-HC-Hybrid A-Star
Planned path length (m)	Map 1:Setup 1	190.313	195.583	185.114
	Map 1:Setup 2	198.256	203.883	196.475
	Map 2	151.388	150.452	148.295
	Map 3	275.262	260.032	250.029

provides a statistical analysis of the planned path lengths under different algorithms. For setup 1 of map 1, the Dic-HC-Hybrid A-star algorithm and Tri-HC-Hybrid A-star algorithm exhibit reductions of -2.77% and 2.73%, respectively, in planned path length compared to the HC-Hybrid A-star algorithm. For setup 2, these algorithms demonstrate reductions of −2.84% and 0.90%, respectively. Under map 2, the Dic-HC-Hybrid A-star algorithm and Tri-HC-Hybrid A-star algorithm show reductions of 0.62% and 2.04%, respectively. Similarly, under map 3, these algorithms exhibit reductions of 5.53% and 9.17%, respectively, compared to the HC-Hybrid A-star algorithm.

Comparing the cumulative change in heading angle, number of expanded nodes, and planned path length under different algorithms across two states produced the following insights: The Dic-HC-Hybrid A-star algorithm performs poorer than the HC-Hybrid A-star algorithm in both states of map 1, with all three indexes showing suboptimal results. However, its performance improves compared to the HC-Hybrid A-star algorithm in maps 2 and 3, although the improvement in all three indexes is not significant. The Tri-HC-Hybrid A-star algorithm consistently outperforms both the HC-Hybrid A-star and Dic-HC-Hybrid A-star algorithms in terms of cumulative change in heading angle and planned path length across all states. This confirms that the proposed Tri-HC-Hybrid A-star algorithm can generate shorter and smoother paths.

In order to prove the effectiveness of the proposed algorithm, the RRT algorithm was used to carry out global path planning for the two setups of map 1. In Figure 10, a node expansion diagram, original path diagram, stretched path diagram, and smoothed path diagram of the RRT algorithm under two setups are shown.

Table 3. Comparison data between RRT algorithm and Tri-HC-Hybrid A-star algorithm.

Indexes	Setups	RRT	Tri-HC-Hybrid A-Star	Improvement
Sum of heading angle changes (rad)	Setup 1	6.914	3.208	53.60%
	Setup 2	4.126	8.036	−94.76%
Expanded node count	Setup 1	757	65	91.41%
	Setup 2	860	72	91.63%
Planned path length (m)	Setup 1	243.792	185.114	24.07%
	Setup 2	267.416	196.475	26.53%

presents data for the RRT algorithm and the Tri-HC-Hybrid A-star algorithm. Under setup 1, the RRT algorithm expands 757 nodes, resulting in a smoothed path length of 243.792 m and a cumulative change in heading angles of 6.914 radians. For the same setup, the Tri-HC-Hybrid A-star algorithm expands 65 nodes, resulting in a smoothed path length of 185.114 m and a cumulative change in heading angles of 3.208 radians. The Tri-HC-Hybrid A-star algorithm reduces the number of node expansions by 91.41% compared to the RRT algorithm, decreases the path length by 24.07%, and reduces the cumulative change in heading angles by 53.60%. Under setup 2, the RRT algorithm expands 860 nodes, resulting in a smoothed path length of 267.416 m and a cumulative change in heading angles of 4.126 radians. Conversely, the Tri-HC-Hybrid A-star algorithm expands 72 nodes, resulting in a smoothed path length of 196.475 m and a cumulative change in heading angles of 8.036 radians. The Tri-HC-Hybrid A-star algorithm decreases the number of node expansions by 91.63% compared to the RRT algorithm, and decreases the path length by 26.53%, but increases the cumulative change in heading angles by 94.76%.

Through comparison, it is evident that the Tri-HC-Hybrid A-star algorithm generates shorter paths with fewer node expansions compared to the RRT algorithm. Therefore, the proposed Tri-HC-Hybrid A-star algorithm is effective.

To verify the practicability of the algorithm in different scenarios, the algorithm was tested on different test maps. Figure 11 shows the paths planned by the HC-Hybrid A-star algorithm, Dic-HC-Hybrid A-star algorithm, and Tri-HC-Hybrid A-star algorithm on the four different test maps. The sums of heading angle changes, the numbers of nodes expanded, and the planned path lengths of different test maps are sorted in

Table 4. Different test maps’ data comparison.

Indexes	Test Maps	HC-Hybrid A-Star	Dic-HC-Hybrid A-Star	Tri-HC-Hybrid A-Star
Sum of heading angle changes (rad)	Test map 1	11.756	12.231	4.775
	Test map 2	7.215	6.215	4.265
	Test map 3	11.258	11.263	8.381
	Test map 4	5.434	6.141	3.840
Expanded node count	Test map 1	57	94	76
	Test map 2	64	87	87
	Test map 3	69	91	87
	Test map 4	59	74	74
Planned path length (m)	Test map 1	227.429	226.361	217.189
	Test map 2	228.090	225.976	225.080
	Test map 3	262.570	260.749	254.267
	Test map 4	207.299	205.664	205.138

. It can be seen from the table that the planned path of the Tri-HC-Hybrid A-star algorithm is the shortest and smoothest among different test maps.

5. Conclusions

This paper addresses the issues of poor smoothness and longer paths in the traditional Hybrid A-star algorithm for the path planning of autonomous vehicles in small-area, multi-obstacle unstructured scenarios. It proposes a Hybrid A-star path planning method based on a single linkage hierarchical clustering algorithm and trichotomy expansion. To fundamentally solve the problem of invalid expansion near U-shaped obstacle clusters in the Hybrid A-star algorithm, obstacles are hierarchically clustered, and the convex hull algorithm is used for envelope construction. A comparison of simulation results between the Hybrid A-star algorithm and the HC-Hybrid A-star algorithm shows that, after clustering obstacles, the algorithm successfully navigates around U-shaped obstacle clusters, effectively reducing the number of expansion nodes while improving the planning efficiency.

In order to enhance path smoothness and shorten the path length, the expansion strategy of nodes in the HC-Hybrid A-star algorithm is improved based on trichotomy expansion. A comparison between the proposed algorithm, the HC-Hybrid A-star algorithm, the Dic-HC-Hybrid A-star algorithm, and the RRT algorithm using simulated data on different maps reveals that the proposed Tri-HC-Hybrid A-star algorithm can generate shorter and smoother paths.