A Hybrid A* Path Planning Method Based on a Guiding Corridor

Abstract

A Hybrid A* path planning method based on a guiding corridor. It relates to the technical field of autonomous driving path planning, specifically to the technical field of global path planning based on Voronoi guiding and safety corridors. Based on the Voronoi guiding method and the safety corridor method, the present invention improves the search process of the Hybrid A* algorithm, enhancing the computational efficiency of the algorithm and the safety of the generated path. The method includes the following steps: constructing a Voronoi path to obtain a set of Voronoi path points; processing the set of Voronoi path points to obtain a set of guiding points; calculating the boundary points of the safety corridor based on the set of Voronoi path points to obtain the safety corridor; preprocessing the safety corridor and dividing the preprocessed safety corridor into an orange corridor and a red corridor according to the distribution density of the guiding points; constructing and optimizing a Hybrid A* model: and outputting the final path according to the optimized Hybrid A* model.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous driving path planning, and particularly to a HybridA* path planning technology based on a guiding corridor. Background Art

[0002] As one of the core technologies of autonomous navigation, path planning technology is directly related to the safety, efficiency and comfort of vehicle driving. It is divided into global path planning technology and local path planning technology. Among them, global path planning provides a basic route for local path planning. An effective global path planning algorithm needs to comprehensively consider various factors such as the motion characteristics of the vehicle, the road environment and the distribution of obstacles, to ensure that the generated path not only conforms to the kinematic constraints of the vehicle, but also can effectively avoid all obstacles, enabling the vehicle to drive safely and efficiently.

[0003] HybridA* is a heuristic path search algorithm, which is an improvement and development of the A* algorithm. The path it plans is more continuous and smooth, and is very suitable for autonomous driving vehicles. However, in practical applications, when dealing with complex environments, this algorithm still has problems such as a large search space and low computational efficiency. Moreover, the path planned by this algorithm often has the problem that the planned path is too close to obstacles in terms of safety, resulting in potential collision risks during the actual execution process.

[0004] In the search process of the traditional HybridA* algorithm, when the distance from the starting point to the end point is too far or there are too many obstacles, in order to balance the path quality and weight, the heuristic information weight is usually not set too high. This leads to a large number of invalid searches when the algorithm searches for the target point due to the lack of sufficient guiding information.

[0005] The traditional target point arrival condition requires that the newly generated node must exactly match the preset target point in terms of coordinates and heading angle in these three dimensions. This results in a large amount of search required by the system when the node expands to the terminal, thereby increasing the number of node expansions. Summary of the Invention

[0006] Aiming at the defects of the traditional HybridA* algorithm, such as a large search space, low computational efficiency, and insufficient safety of the planned path, resulting in collision risks during the execution of the path, the present invention proposes a HybridA* path planning method based on a guiding corridor. Based on the HybridA* path planning algorithm, and on the basis of the Voronoi guiding method and the safety corridor method proposed by the present invention, the search process of the HybridA* algorithm is improved, thereby enhancing the computational efficiency of the algorithm and the safety of the generated path.

[0007] The method includes the following steps:

[0008] S1. Construct a Voronoi path and perform smoothing to obtain a set of Voronoi path points;

[0009] S2. Successively sample and filter the set of Voronoi path points to obtain a set of guiding points;

[0010] S3. Calculate the boundary points of the safety corridor based on the set of Voronoi path points to obtain the safety corridor;

[0011] S31. Preprocess the safety corridor and divide the preprocessed safety corridor into an orange corridor and a red corridor according to the distribution density of the guiding points;

[0012] S4. Construct a HybridA* model and optimize the HybridA* model:

[0013] S41. Change the target search method and expand the target range;

[0014] S5. Output the final path according to the optimized HybridA* model:

[0015] S51. Set the set of guiding points as the set of stage target points of the optimized HybridA* model and start the search;

[0016] S52. Judge the position of the newly generated nodes during the search process:

[0017] When the position of the newly generated node is in the orange corridor, set the node expansion step size of the optimized HybridA* model to A;

[0018] When the position of the newly generated node is in the red corridor, set the node expansion step size of the optimized HybridA* model to B;

[0019] When the position of the newly generated node is outside the orange corridor and the red corridor, determine that the newly generated node is an invalid node;

[0020] S53. When there are no stage target points to be searched, the search ends and the final path is output.

[0021] Furthermore, the construction of the Voronoi path is specifically as follows: construct a Voronoi diagram based on the obstacle distribution of the grid map, and on the basis of the Voronoi diagram, use the A* algorithm to construct a Voronoi path between the starting point and the ending point with the shortest distance as the constraint;

[0022] The smoothing process includes: smoothing of the starting segment, the ending segment, and the middle segment;

[0023] The smoothing of the starting segment and the ending segment is specifically as follows: Calculate the angles between the lines connecting the starting point and the ending point to the nearest Voronoi point and its three adjacent Voronoi points respectively and the corresponding Voronoi edges, and then select the largest angle among the angles as the smoothing angle to replace the sharp corners at the starting point and the ending point. Herein, the Voronoi edge represents the line connecting two adjacent Voronoi points.

[0024] The smoothing of the middle segment is specifically as follows: For the middle segment other than the starting point and the ending point, use the path smoothing algorithm based on the inscribed circle to calculate the smooth path, and replace the broken line path of the middle segment with the smooth path.

[0025] Furthermore, the sequential sampling and filtering process of the Voronoi path point set is specifically as follows: First, extract samples from the Voronoi path point set at a sampling rate of 6:1 to generate a sparse Voronoi point sequence, and then apply a distance threshold filter to the Voronoi point sequence to retain the Voronoi points whose distance to the nearest obstacle is greater than 6.5 meters, generating a set of guiding points.

[0026] Furthermore, the safety corridor boundary points include the left boundary points and the right boundary points ;

[0027] The is calculated by the formula: wherein, represents the th Voronoi point, represents the width of the safety corridor, represents the unit normal vector of the th Voronoi point in the left unit vector;

[0028] The is calculated by the formula: wherein, represents the unit normal vector of the th Voronoi point in the right unit vector;

[0029] Connect the left boundary points and the right boundary points on the same side respectively to obtain the safety corridor.

[0030] Furthermore, the preprocessing is specifically as follows: Check for self-crossing problems in the corridor boundary, and replace the relevant Voronoi points with self-crossing problems with the intersection points, where the intersection points represent the intersection points of two Voronoi edges.

[0031] Furthermore, the division of the preprocessed safety corridor into an orange corridor and a red corridor according to the distribution density of the guiding points is specifically as follows: Calculate the distance from the current guiding point to the next guiding point. If is greater than the distance threshold Divide the safety corridor between the current guiding point and the next guiding point into an orange corridor, and vice versa, into a red corridor.

[0032] Furthermore, the change of the target search method to expand the target range is specifically: expand the target point to a target area, and expand the target heading angle to a target sector area.

[0033] Furthermore, the target area is specifically: a circular area centered on the target point with a radius of 1 meter;

[0034] The target sector area is specifically: centered on the target heading angle, expanding in the upper and lower directions

[0035] Furthermore, A is greater than B.

[0036] The beneficial effects of the method of the present invention are as follows:

[0037] (1) In the present invention, guiding points are selected on the Voronoi path. These guiding points not only provide clear guidance for the search process as stage target points, reducing some ineffective expansions of the algorithm, but also because the guiding points are selected on the Voronoi path, they are themselves located in a safe position far from obstacles. As a result, the path search process also tends to be carried out in a safe area, indirectly increasing the safety of the generated path.

[0038] (2) A large number of ineffective expansions exist in the search process of the traditional HybridA* algorithm, and many of the generated node positions are too close to obstacles, which makes the vehicle prone to approaching obstacles when executing the path, thus increasing the risk of collision. The present invention effectively restricts the expansion range of nodes by introducing an innovative safety corridor, ensuring that it is always carried out within a safe interval. Therefore, a certain safety distance is maintained between the generated path and the obstacles, effectively avoiding the risk of collision.

[0039] (3) According to the distribution density of the guiding points, the present invention divides the corridor into an orange corridor and a red corridor. In the area with a lower distribution density, it indicates that this section of the path maintains a relatively long distance from the surrounding obstacles, and is marked as the empty corridor part in orange. On this section of the path, the search step size is set to a larger value to meet the need for rapid expansion. While in the area with a higher distribution density, it means that this section of the path is adjacent to the surrounding obstacles, and is marked as the narrow corridor part in red. On this section of the path, the search step size is correspondingly adjusted to a smaller value to ensure the safety and high quality of the path. This dynamic step size strategy enables the algorithm to adaptively adjust the search step size according to the environment, making the search process of the algorithm more flexible and better able to adapt to different environments.

[0040] (4) In the present invention, the original "reach the target point" is changed to "reach the target area", and at the same time, "reach the target heading angle" is adjusted to "reach the target sector area". This improvement not only significantly alleviates the problem of "difficult to reach the end", improves the overall search efficiency, but also combines a safety corridor, so it can improve the path search efficiency and safety of autonomous vehicles in complex environments. Description of the Drawings

[0041] Figure 1 It is a flowchart of the method described in the embodiment of the present invention;

[0042] Figure 2 It is a schematic diagram of the smoothing preprocessing method described in the embodiment of the present invention. (a) represents the smoothing processing method for the starting segment and the ending segment, and (b) represents the smoothing processing method for the middle segment;

[0043] Figure 3 It is a schematic diagram of the smoothing preprocessing result described in the embodiment of the present invention. (a) represents the Voronoi path before smoothing preprocessing, and (b) represents the Voronoi path after smoothing preprocessing;

[0044] Figure 4 It is a schematic diagram of the target area and the target sector area described in the embodiment of the present invention;

[0045] Figure 5 It is a schematic diagram of the safety corridor described in the embodiment of the present invention. (a) represents the schematic diagram of the safety corridor boundary, (b) represents the schematic diagram of the safety corridor boundary after preprocessing, (c) represents the schematic diagram of generating the safety corridor according to the corridor boundary line, and (d) represents the schematic diagram of the red safety corridor and the orange safety corridor;

[0046] Figure 6 It is a schematic diagram of the forward expansion process of applying the Voronoi guidance strategy and the safety corridor strategy to the HybridA* algorithm described in the embodiment of the present invention. Specific Embodiments

[0047] Next, the technical solutions of the present invention will be clearly and completely described in conjunction with the accompanying drawings. Obviously, the described embodiments are some, but not all, of the embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0048] This embodiment provides a HybridA* path planning method based on a guidance corridor. The method includes the following steps:

[0049] S1. Construct a Voronoi path and perform smoothing processing to obtain a set of Voronoi path points , , represents the total number of Voronoi points.

[0050] The relevant operations in step S1 are introduced with specific examples:

[0051] Construct a Voronoi diagram based on the obstacle distribution of the grid map, and on the basis of the Voronoi diagram, use the A* algorithm to construct a Voronoi path between the starting point and the ending point with the shortest distance as the constraint, which serves as the basis for subsequent staged searches; since each edge of the Voronoi diagram maintains a certain distance from the surrounding obstacles, the generated Voronoi path also has good safety.

[0052] The A* algorithm is a heuristic search algorithm widely used in path planning problems, and its core idea is to efficiently find the optimal path by evaluating the comprehensive priority of nodes.

[0053] As the basis for subsequent guiding points and safety corridors, the Voronoi path needs to smooth the sharp corners on the path, detect all the sharp corners in the Voronoi path, and smooth this part to obtain a smoother Voronoi path: the Voronoi path point set.

[0054] The smoothing process includes: smoothing of the starting section, ending section, and middle section;

[0055] The smoothing of the starting section and ending section is specifically as follows: As Figure 2 shown in (a), calculate the angles between the lines connecting the starting point (start) and the ending point (goal) to the nearest Voronoi point and its three adjacent Voronoi points and the corresponding Voronoi edges , , and , and then respectively select the largest angle among the angles as the smoothing angle to replace the sharp corners at the starting point and the ending point, , where the Voronoi edge represents the line connecting two adjacent Voronoi points; in this embodiment .

[0056] The smoothing of the middle section is specifically as follows: For the middle section except the starting point and the ending point, use the path smoothing algorithm based on the inscribed circle to calculate the smooth path, and replace the broken line path in the middle section with the smooth path.

[0057] Using the path smoothing algorithm based on the inscribed circle to calculate the smooth path is specifically as follows:

[0058] First, determine the positions of the corner points that need to be smoothed: Traverse all the Voronoi points on the Voronoi path, and filter out those Voronoi points with degrees greater than or equal to 3 and mark them as first-level key points. The degree represents the number of connecting edges of a Voronoi point. Due to the properties of the Voronoi diagram, the positions where these key points are located are most likely to have curvature mutations and result in path non-smoothness. Therefore, these types of Voronoi points are marked as first-level key points. For each Voronoi point marked as a first-level key point , according to the formula , further analyze the angles formed with the adjacent nodes before and after and . Among them, represents the angle between the line connecting the first-level key point and other Voronoi points and the corresponding Voronoi edge, and are the direction vectors pointing from to and respectively. When is less than 100 degrees, it indicates that there is a large curvature change here and smoothing treatment is required. The Voronoi points that meet the conditions are marked as second-level key points and used as the objects for subsequent calculations.

[0059] Next, calculate the inscribed circle smoothing segment: The schematic diagram of the arc calculation is as shown in Figure 2 (b). For each marked as a second-level key point, first calculate the position of the inscribed circle center. From the formula: , calculate the unit vector of the angle bisector of and . Among them, represents the unit vector of the angle bisector; then, according to the sine theorem and the predefined inscribed circle radius , the offset of the center relative to can be calculated. From the formula: , the coordinates of the center can be calculated. Then, calculate the required smoothing arc segment. Denote the vertical vector of as , 's vertical vector is denoted as . The positions of the two tangent points of the inscribed circle and the path and are obtained from the formulas: and respectively. Among them passes through the center and the two tangent points and , and a circular arc connecting these two tangent points can be generated as a new part of the smooth path. The points on the circular arc are obtained from the standard parametric equation for calculating the circular arc trajectory: Definition, where is a variable between the starting angle and the ending angle , depending specifically on and the position relative to , and represents the standard parametric equation of the arc trajectory. ([[]] ) represents the coordinates of. Finally, the original polyline segment from through to is replaced by the newly generated arc segment. This step ensures the overall coherence and smoothness of the path while maintaining the safety and feasibility of the original path. The comparison of the Vino path before and after smoothing is shown by Figure 3 : (a) represents the Vino path before smoothing, and (b) represents the Vino path after smoothing.

[0060] S2. Sequentially sample and filter the Vino path point set to obtain a set of guiding points.

[0061] Introduce the relevant operations of step S2 with a specific example:

[0062] The Vino path is actually composed of a series of Vino points. All the generated Vino points are selected using a two-stage screening strategy of sampling first and then filtering to pick out the required key guiding points to serve as the stage target points for subsequent path search.

[0063] The sequential sampling and filtering of the Vino path point set are specifically as follows: First, samples are extracted from the Vino path point set at a sampling rate of 6:1 to generate a sparsified Vino point sequence. Then, a distance threshold filter is applied to the Vino point sequence, and the Vino points with a distance greater than 6.5 meters from the nearest obstacle (in a relatively complex or narrow section) are retained as the final guiding points to generate a set of guiding points. The set of guiding points is set as the stage target point set of the HybridA* algorithm.

[0064] S3. Calculate the boundary points of the safety corridor based on the Vino path point set to obtain the safety corridor;

[0065] S31. Preprocess the safety corridor and divide the preprocessed safety corridor into an orange corridor and a red corridor according to the distribution density of the guiding points;

[0066] Introduce the relevant operations of step S3 with a specific example:

[0067] The boundary points of the safety corridor include the left boundary point and the right boundary point ;

[0068] As shown Figure 6 for each Voronoi point , generate the direction vectors and formed by its two adjacent points , and then calculate the unit vector of the direction vector, where represents the modulus length of . Denote the unit normal vector of the path direction unit vector as ， . Expand along the normal direction on both sides of the path point according to the set safety corridor width and to obtain the left and right boundary points , . After calculating all the safety corridor boundary points for , connect the left boundary points and the right boundary points on the same side respectively to obtain the safety corridor, as shown in Figure 5 (a).

[0069] Since the order of the safety corridor boundary points generated by the Voronoi points at the sharp corner path may be disordered, resulting in self-crossing of the corridor boundary line, traverse the corridor boundary line to identify the crossing segments, check for self-crossing problems of the corridor boundary, and replace the relevant Voronoi points with self-crossing problems with the intersection points, where the intersection points represent the intersection points of two Voronoi edges, and reconnect to form a smooth corridor boundary line, as shown in Figure 5 (b).

[0070] Then generate the safety corridor according to the corridor boundary line, as shown in Figure 5 (c).

[0071] According to the distribution density of the guiding points, divide the preprocessed safety corridor into an orange corridor and a red corridor. Specifically: calculate the distance from the current guiding point to the next guiding point. If is greater than the distance threshold , divide the safety corridor between the current guiding point and the next guiding point into an orange corridor; otherwise, divide it into a red corridor, as shown in Figure 5 (d). The part circled by the frame line is the orange corridor, and the part not circled by the frame line is the red corridor

[0072] Calculate the distance from each guiding point to the next guiding point. If is greater than the predefined distance threshold , it indicates that the guiding points are sparsely distributed here, indirectly indicating that there are fewer or farther obstacles around. This part of the corridor is divided into orange. On the contrary, if the predefined distance threshold is not exceeded , it means that the guiding points are densely distributed here, indirectly indicating that there are more or closer obstacles around. This part of the corridor is divided into red. In this embodiment, the predefined distance threshold is set to 13.

[0073] S4. Construct a HybridA* model and optimize the HybridA* model:

[0074] S41. Change the target search method and expand the target range.

[0075] The relevant operations in step S4 are introduced with specific examples:

[0076] As Figure 4 shown, changing the target search method and expanding the target range specifically means: expanding the target point into a target area and expanding the target heading angle into a target sector area.

[0077] Define the tangent direction of the Voronoi path where the guiding point (stage target point) is located as the heading angle of this point, and construct the three-dimensional attributes of each guiding point accordingly ( ), , represents the total number of guiding points, and respectively represent the rd guiding point in the axis and axis directions, represents the course angle of the th guiding point.

[0078] The specific target area is: set a circular area with a radius of 1 meter centered at ( ) for each guiding point;

[0079] The specific target sector area is: a sector angle range centered at and extending up and down.

[0080] S5. Output the final path according to the optimized HybridA* model:

[0081] S51. Set the guiding point set as the stage target point set of the optimized HybridA* model and start the search;

[0082] S52. Judge the position of the newly generated nodes during the search process:

[0083] When the position of the newly generated node is in the orange corridor, set the node expansion step size of the optimized HybridA* model to A;

[0084] When the position of the newly generated node is in the red corridor, set the node expansion step size of the optimized HybridA* model to B;

[0085] When the position of the newly generated node is outside the orange corridor and the red corridor, determine that the newly generated node is an invalid node;

[0086] S53. When there is no stage target point to be searched, the search ends and the final path is output.

[0087] The relevant operations of step S5 are introduced with a specific example:

[0088] Apply the Vino guiding strategy and the safety corridor strategy to the forward expansion process of the HybridA* algorithm as Figure 1 shown. Starting from the starting point, judge the color of the corridor where the current node is located, then expand the next set of nodes according to the corresponding step size, calculate the costs of all nodes and select the node with the minimum cost as the new node, and continuously expand to the next target point until the final target point is reached, and output the final path.

[0089] Limit the search process of the HybridA algorithm within the safety corridor, that is, determine whether the position of the node generated by each expansion is within the safety corridor. If it is not determined to be an invalid node, it is not added to the node set for subsequent expansion. At the same time, determine the color of the corridor where the node position is located in real time. If it is orange, set the expansion step size of the algorithm to the larger value A. If it is red, set the expansion step size of the algorithm to the smaller value B, so that the expansion process can flexibly adapt to the environment and achieve a balance between the path quality and the search speed. In this embodiment , .

Claims

1. A HybridA* path planning method based on a guiding corridor, characterized in that, The method includes the following steps: S1. Construct a Voronoi path and perform smoothing processing to obtain a set of Voronoi path points; S2. Sequentially sample and filter the set of Voronoi path points to obtain a set of guiding points; S3. Calculate the boundary points of the safety corridor based on the set of Voronoi path points to obtain the safety corridor; S31. Preprocess the safety corridor and divide the preprocessed safety corridor into an orange corridor and a red corridor according to the distribution density of the guiding points; S4. Construct a HybridA* model and optimize the HybridA* model: S41. Change the target search method and expand the target range; S5. Output the final path according to the optimized HybridA* model: S51. Set the set of guiding points as the set of stage target points of the optimized HybridA* model and start the search; S52. Judge the position of the newly generated nodes during the search process: When the position of the newly generated node is in the orange corridor, set the node expansion step size of the optimized HybridA* model to A; When the position of the newly generated node is in the red corridor, set the node expansion step size of the optimized HybridA* model to B; When the position of the newly generated node is outside the orange corridor and the red corridor, determine that the newly generated node is an invalid node; S53. When there are no stage target points to be searched, the search ends and the final path is output.

2. The HybridA* path planning method based on a guiding corridor according to claim 1, wherein, The specific construction of the Voronoi path is as follows: Based on the obstacle distribution of the grid map, construct a Voronoi diagram, and on the basis of the Voronoi diagram, use the A* algorithm to construct a Voronoi path between the starting point and the ending point with the shortest distance as the constraint; The smoothing processing includes: smoothing processing of the starting section, the ending section, and the middle section; The smoothing processing of the starting section and the ending section is specifically as follows: Calculate the angles between the lines connecting the starting point and the ending point to the nearest Voronoi point and its three adjacent Voronoi points and the corresponding Voronoi edges respectively, and then select the largest angle among the angles respectively as the smoothing angle to replace the sharp corners at the starting point and the ending point, where the Voronoi edge represents the line connecting two adjacent Voronoi points; The smoothing processing of the middle section is specifically as follows: For the middle section except the starting point and the ending point, use a path smoothing algorithm based on the inscribed circle to calculate the smooth path and replace the broken line path of the middle section with the smooth path.

3. The HybridA* path planning method based on a guiding corridor according to claim 2, wherein The sequential sampling and filtering processing of the set of Voronoi path points is specifically as follows: First, extract samples from the set of Voronoi path points at a sampling rate of 6:1 to generate a sparse Voronoi point sequence, and then apply a distance threshold filter to the Voronoi point sequence to retain the Voronoi points whose distance to the nearest obstacle is greater than 6.5 meters to generate a set of guiding points.

4. A Hybrid A* path planning method based on a guiding corridor according to claim 3, characterized in that The safety corridor boundary points include the left boundary point and the right boundary point ; The said is calculated by the formula: wherein represents the th Voronoi point, represents the safety corridor width, represents the unit normal vector of the th Voronoi point in the left unit vector; The said is calculated by the formula: wherein represents the unit normal vector of the th Veronese point on the right unit vector; Connect the left boundary points on the same side and the right boundary points respectively to obtain a safety corridor.

5. A HybridA* path planning method based on a guiding corridor according to claim 4, wherein, The preprocessing is specifically as follows: Check for self-crossing problems occurring at the corridor boundary and replace the relevant Voronoi points with self-crossing problems with the intersection points, where the intersection point represents the intersection of two Voronoi edges.

6. The HybridA* path planning method based on a guiding corridor according to claim 5, wherein, Specifically, according to the distribution density of the guiding points, the pre-processed safety corridor is divided into an orange corridor and a red corridor as follows: calculate the distance from the current guiding point to the next guiding point , if is greater than the distance threshold , divide the safety corridor between the current guiding point and the next guiding point into an orange corridor; otherwise, divide it into a red corridor.

7. A Hybrid A* path planning method based on a guiding corridor according to claim 6, characterized in that, The change of the target search method and the expansion of the target range are specifically as follows: Expand the target point to a target area and expand the target heading angle to a target sector area.

8. A Hybrid A* path planning method based on a guiding corridor according to claim 7, characterized in that, The target area is specifically as follows: A circular area centered on the target point with a radius of 1 meter; The specific target sector area is: centered on the target heading angle, expanding in both the upper and lower directions of the sector area.

9. A Hybrid A* path planning method based on a guiding corridor according to claim 8, characterized in that, A is greater than B.

Images