A path planning method for fixed scenes

Abstract

A path planning method for a fixed scene, comprising: obtaining a map of the fixed scene, constructing a KD tree based on obstacles in the map, and planning a moving path of a mobile object through a path search algorithm; calculating the shortest distance between a current path node and an obstacle based on the KD tree as a risk distance, and judging the risk level of the mobile object according to the risk distance; expanding the path node according to the risk level of the mobile object; when expanding to a path node that can directly connect the end point using an RS curve, regularizing the angle information of the path node; and smoothing the RS curve. The present invention provides a path planning method for a fixed scene, which adjusts a hybrid A* algorithm based on the distance between a mobile object and an obstacle, improves the efficiency of path planning, and has a higher quality of the planned mobile path.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and in particular to a path planning method for a fixed scene. Background Art

[0002] As autonomous driving technology matures, the application scenarios of autonomous driving are becoming more and more extensive, including grain depots, airports, and docks. In such closed factories, obstacles such as warehouses are generally regular rectangular in shape, and there are no roundabouts in the accessible road network. In addition, the work of intelligent logistics vehicles in closed factories usually involves multi-vehicle collaboration. In the global path planning algorithm of multiple intelligent mobile platforms, it is more necessary to coordinate the paths of each other to avoid disorderly and random intersections between global paths. In such scenarios, the use of autonomous driving technology is of great significance to achieve standardized management of vehicle driving, reduce the chaos of fleet driving, reduce the occurrence of congestion, improve the work efficiency of the factory, reduce the workload of personnel, and improve the intelligence level of the factory. At present, most intelligent logistics vehicles use the A* algorithm for path planning. The planned path has problems such as not meeting the vehicle kinematics, the path is close to obstacles, and the path does not consider the vehicle orientation.

[0003] The hybrid A* algorithm was proposed by Dmitri Dolgov, Sebastian Thrun, Michael Montemerlo and others at Stanford in 2010 to solve the path planning problem in the process of parallel parking and reversing into a garage. At the same time, the hybrid A* algorithm is a combination of the A* algorithm and vehicle kinematics to solve the problem that the A* algorithm does not meet the vehicle kinematics. Compared with the A* algorithm, the hybrid A* algorithm not only meets the vehicle kinematics, but also adds the vehicle head direction, steering cost and reversing cost, so that the path generated by the hybrid A* algorithm is more in line with the vehicle driving requirements than the path generated by the A* algorithm. At present, the hybrid A* algorithm is often used in global path planning in autonomous driving technology. The algorithm includes the following steps: (1) obtain the vehicle starting position and direction and set the end position and direction; (2) calculate and select the minimum cost node as the parent node; (3) use the RS curve to connect the parent node and the end point; (4) if there is no obstacle on the curve, use the RS curve to connect the parent node and the end point; if there is an obstacle on the curve, return to (2) to continue searching. The path generated by the hybrid A* algorithm conforms to the vehicle kinematics, but still does not take into account the safety and stability of vehicle driving. Therefore, the current hybrid A* algorithm still has the following problems: (1) The cost function of the hybrid A* algorithm does not take into account the distance between the current node and the obstacle, resulting in some paths being too close to the obstacle; (2) The hybrid A* algorithm uses a fixed curvature and step-size expansion search, which may cause the vehicle to frequently turn in open areas and cause the search time to be too long when the algorithm is applied to a large map; (3) The hybrid A* algorithm uses a fixed curvature RS curve when generating the RS curve, resulting in the generated RS curve being unfavorable for vehicle driving. When multiple intelligent mobile platforms use the hybrid A* algorithm to plan global paths, they randomly cross each other in a disorderly manner, resulting in chaotic convoy driving, increased collision risks, and increased probability of congestion.

[0004] For example, Chinese patent document CN113359757A discloses an unmanned vehicle path planning and trajectory tracking method based on an improved hybrid A* algorithm. This patent mainly improves the local path planning of unmanned vehicles, uses an adaptive model prediction algorithm for trajectory tracking, and sets an error threshold within a period of time to limit the trajectory error to a fixed range, thereby reducing the trajectory tracking error. However, the global path planning of this patent does not take into account the distance between the path and the obstacle, the changing search step size and curvature, and the rule that the vehicle's driving route should remain parallel to the road.

[0005] In addition, Chinese patent document CN112606830A discloses a two-stage autonomous parking path planning method based on a hybrid A* algorithm. The patent divides the parking path into two sections. The first section is the path from the vehicle entering the parking lot to the vehicle driving to the minimum parking distance point, and the second section is the path from the minimum parking distance point to the parking end point. By combining the characteristics of the two parts of the path, different heuristic functions are used for path planning for the two parts, thereby improving the efficiency of path planning. However, the invention does not consider the distance to obstacles in the first section of the path search process; the invention does not consider the changing search step size and curvature in the second section of the search process. Summary of the invention

[0006] In order to address the deficiencies in the prior art, the present invention provides a path planning method for fixed scenes, which adjusts the hybrid A* algorithm based on the distance between the moving object and the obstacle, thereby improving the efficiency of path planning and achieving higher quality of the planned moving path.

[0007] In order to achieve the above object, the specific scheme adopted by the present invention is: a path planning method for a fixed scene, comprising:

[0008] Get a map of a fixed scene, build a KD tree based on the obstacles in the map, and plan the moving path of the mobile object through a path search algorithm;

[0009] The shortest distance between the current path node and the obstacle is calculated based on the KD tree and recorded as the risk distance, and the risk level of the moving object is determined based on the risk distance;

[0010] Expand path nodes according to the risk level of the moving object;

[0011] When it is expanded to a path node that can directly connect the end point using the RS curve, the angle information of the path node is regularized; and the RS curve is smoothed.

[0012] As a further optimization of the above path planning method for fixed scenes: the path search algorithm adopts the hybrid A* algorithm;

[0013] After calculating the risk distance, add the risk distance to the cost function of the hybrid A* algorithm.

[0014] As a further optimization of the above-mentioned path planning method for fixed scenes: when judging the risk level of a mobile object according to the risk distance, multiple extended areas with different areas are determined based on the maximum self-size data of the mobile object, different extended areas correspond to different risk levels, and the risk level of the mobile object is determined based on the fitting relationship between the risk distance and the extended area.

[0015] As a further optimization of the above-mentioned path planning method for fixed scenes: when the risk level reaches the first limit, the step size of the hybrid A* algorithm is adjusted to the preset minimum value and the curvature is adjusted to the preset maximum value. When the risk level reaches the second limit, the step size and curvature of the hybrid A* algorithm are dynamically adjusted based on the risk distance.

[0016] As a further optimization of the above path planning method for fixed scenarios: the method for dynamically adjusting the step size of the hybrid A* algorithm based on the risk distance is:

[0017]

[0018] Where D(x) is the risk distance, max_l and min_l represent the preset maximum step length and minimum step length respectively, and ε is the maximum size data of the moving object;

[0019] The method for dynamically adjusting the curvature of the hybrid A* algorithm based on risk distance is:

[0020]

[0021] Among them, max_c and min_c represent the preset maximum curvature and minimum curvature respectively.

[0022] As a further optimization of the above-mentioned path planning method for fixed scenes: when expanding to the path node that can directly connect the end point using the RS curve, the angle information of the path node is regularized to An integer multiple of .

[0023] As a further optimization of the above-mentioned path planning method for fixed scenes: the method for smoothing the RS curve includes:

[0024] The path node where the angle jump occurs is determined as the angle jump point;

[0025] The shortest distance between the angle jump point and the obstacle is calculated and recorded as the corrected distance;

[0026] Calculate the length correction data and curvature correction data of the RS curve based on the corrected distance;

[0027] The RS curve is corrected according to the length correction data and the curvature correction data.

[0028] As a further optimization of the above path planning method for fixed scenes: the method for determining the angle jump point is:

[0029] Traverse the angle of each path node on the moving path;

[0030] Calculate the difference between the angle of a path node and the angle of the previous path node;

[0031] When the difference is greater than a preset threshold, it is determined that the angle of the path node has jumped, and the path node is recorded as an angle jump point.

[0032] Beneficial effects: The present invention can speed up the search speed by changing the search step size of the hybrid A* algorithm, realize the expansion of large step size and small curvature in open areas, and the expansion of short step size and large curvature in narrow areas, thereby avoiding frequent turning of mobile objects and improving the efficiency of path planning; by adding distance constraints from obstacles, the generated moving path is kept at a certain distance from obstacles, thereby improving the safety of mobile objects; by optimizing the generation of RS curves in the hybrid A* algorithm and correcting the RS curves, the generated path is kept parallel to the road network of fixed scenes as much as possible, thereby improving the quality of the path, and then improving the safety and stability of mobile objects. The present invention can be applied to the automatic driving of vehicles in fixed scenes such as grain depots, factory areas, airports and docks, providing a safe, stable and highly executable path for automatic driving vehicles, improving the driving stability of automatic driving vehicles, greatly reducing the occurrence of dangerous situations during driving, and improving the working efficiency of vehicles. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] Figure 1 is a flow chart of the present invention;

[0034] Figure 2 It is a schematic diagram of the distribution form of the extended area;

[0035] Figure 3 This is a schematic diagram of the effect of smoothing the RS curve;

[0036] Figure 4 It is a comparison diagram between the method of the present invention and the existing hybrid A* algorithm. DETAILED DESCRIPTION

[0037] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0038] See also Figure 1 ,A path planning method for a fixed scene includes the following steps.

[0039] First, a map of a fixed scene is obtained, a KD tree is constructed based on obstacles in the map, and a moving path of the moving object is planned by a path search algorithm. In this embodiment, the path search algorithm adopts a hybrid A* algorithm.

[0040] Secondly, the shortest distance between the current path node and the obstacle is calculated based on the KD tree and recorded as the risk distance, and the risk level of the moving object is determined according to the risk distance. Specifically, the KDTree.query() function is used to calculate the shortest distance between the current search node and the obstacle KD tree, which belongs to the prior art in this field and will not be repeated here.

[0041] See also Figure 2 When judging the risk level of a moving object according to the risk distance, multiple extension areas of different areas are determined based on the maximum self-size data of the moving object. Different extension areas correspond to different risk levels, and the risk level of the moving object is determined based on the fitting relationship between the risk distance and the extension area. In fixed scenes such as grain depots, factory areas, airports, and docks, the moving objects are mainly vehicles. When the vehicle is unloaded or the size of the load does not exceed the size of the vehicle itself, the maximum self-size data is the diagonal length of the vehicle. When the size of the vehicle load exceeds the size of the vehicle itself, the maximum self-size data can be the maximum length of the load. The maximum self-size data needs to be determined according to actual conditions, which will not be described in detail here. After determining the maximum self-size data, the extension area can be determined based on the maximum self-size data. For example, a certain multiple of the maximum self-size data can be used as the diameter to construct a circular area as the extension area. The number of extended areas directly corresponds to the level of risk. For example, when three extended areas are determined, the risk level can be divided into three levels. The corresponding three extended areas can be defined as collision area, danger area and safety area, respectively. Specifically, during the movement of the moving object, when the obstacle enters the collision area, it will collide with the moving object. When the obstacle enters the danger area, it may collide with the moving object. When the obstacle only enters the safety area, it will not collide with the moving object.

[0042] After determining the degree of risk, the hybrid A* algorithm needs to be adjusted to avoid interference between the planned moving path and obstacles. The specific adjustment method is as follows.

[0043] When the risk level reaches the first limit, the step size of the hybrid A* algorithm is adjusted to the preset minimum value and the curvature is adjusted to the preset maximum value, and the first limit corresponds to the collision area; when the risk level reaches the second limit, the step size and curvature of the hybrid A* algorithm are dynamically adjusted based on the risk distance, and the second limit corresponds to the danger zone; when the risk level reaches the third limit, the step size of the hybrid A* algorithm is adjusted to the preset maximum value and the curvature is adjusted to the preset minimum value, and the third limit corresponds to the safe area.

[0044] Furthermore, the method for dynamically adjusting the step size of the hybrid A* algorithm based on the risk distance is:

[0045]

[0046] Where D(x) is the risk distance, max_l and min_l represent the preset maximum step length and minimum step length respectively, and ε is the maximum size data of the moving object;

[0047] The method for dynamically adjusting the curvature of the hybrid A* algorithm based on risk distance is:

[0048]

[0049] Among them, max_c and min_c represent the preset maximum curvature and minimum curvature respectively.

[0050] Through the above process, in the process of expanding the path nodes, the step size and curvature of the hybrid A* algorithm of the present invention will be continuously adjusted as the risk distance changes, which can ensure the safety of the obtained moving path and improve the search speed of the hybrid A* algorithm and the quality of the moving path generated in the open area.

[0051] Thirdly, the path nodes are expanded according to the risk level of the moving object. In the process of expanding the path nodes, the risk distance is added to the cost function of the hybrid A* algorithm, so that the path generated by the hybrid A* algorithm can keep a certain distance from obstacles as much as possible, further reducing the possibility of collision between the moving object and the obstacle.

[0052] Afterwards, when the moving path is extended to a path node that can be directly connected to the end point using the RS curve, the angle information of the path node is regularized. Specifically, the regularization method is to regularize the angle information of the path node to Since the traversable road network in a fixed scene is usually set as a horizontal and vertical route, the corrected RS curve can remain parallel to the road network, thereby improving the quality of the RS curve.

[0053] Since the starting point of the RS curve is forcibly regularized, there will be an angle jump between the modified RS curve and the path extended by the hybrid A* algorithm, which may interfere with the movement of the moving object. Therefore, the modified RS curve needs to be further processed. Specifically, the RS curve needs to be smoothed. The method for smoothing the RS curve includes the following steps.

[0054] The path node where the angle jumps is determined to be an angle jump point. The method for determining the angle jump point is as follows.

[0055] First, traverse the angle of each path node on the moving path.

[0056] Second, the difference between the angle of the path node and the angle of the previous path node is calculated.

[0057] Third, when the difference is greater than a preset threshold, it is determined that the angle of the path node has changed, and the path node is recorded as an angle change point. The threshold can be determined according to actual conditions. In this embodiment, the threshold is set to 15°, that is, when the difference between the two is greater than 15°, the current path node can be determined to be the angle change point of this moving path.

[0058] After the angle jump point is determined, the shortest distance between the angle jump point and the obstacle is calculated and recorded as the corrected distance. In the present invention, the Kd_Tree module of the KD tree algorithm is used to calculate the shortest distance between the angle jump point and the surrounding obstacles and record it as the corrected distance turn_dis.

[0059] The length correction data and curvature correction data of the RS curve are calculated based on the corrected distance turn_dis. The calculation method of the curvature correction data is consistent with the method of dynamically adjusting the curvature of the hybrid A* algorithm based on the risk distance, which will not be repeated here. The specific calculation method of the length correction data is: take the angle jump point as the midpoint, take the number of nodes 2*turn_dis forward as the starting point of the RS curve, and take the number of nodes 2*turn_dis backward as the end point of the RS curve.

[0060] Finally, the RS curve is corrected according to the length correction data and the curvature correction data. After the length correction data is determined, the starting point and end point of the RS curve are determined, and then combined with the curvature correction data, the RS curve can be smoothed and corrected.

[0061] See also Figure 3 Because the RS curve only considers the vehicle kinematic model and ignores the surrounding obstacles, the RS curve may be too close to the obstacle or even pass through the obstacle. Therefore, the present invention mainly corrects the length and curvature of the RS curve. When the angle jump point is close to the obstacle, the generated RS curve is shorter and has a larger curvature, which ensures that the vehicle kinematic model is stable while ensuring that it does not contact the obstacle. When the angle jump point is far from the obstacle, the generated RS curve is longer and has a smaller curvature, which improves the quality of the moving path.

[0062] The present invention can speed up the search speed by changing the search step size of the hybrid A* algorithm, realize the expansion of large step size and small curvature in open areas, and the expansion of short step size and large curvature in narrow areas, so as to avoid frequent turning of mobile objects and improve the efficiency of path planning; by adding the distance constraint with obstacles, the generated mobile path is kept at a certain distance from the obstacles, and the safety of mobile objects is improved; by optimizing the generation of RS curve in the hybrid A* algorithm, the RS curve is corrected, so that the generated path is kept parallel to the road network of the fixed scene as much as possible, so as to improve the quality of the path, and then improve the safety and stability of mobile objects. The present invention can be applied to the automatic driving of vehicles in fixed scenes such as grain depots, factory areas, airports and docks, and provide a safe, stable and executable path for automatic driving vehicles, improve the driving stability of automatic driving vehicles, greatly reduce the occurrence of dangerous situations during driving, and improve the working efficiency of vehicles.

[0063] Finally, in a specific model, the present invention is compared with the existing hybrid A* algorithm, and the results are as follows: Figure 4 As shown, it can be seen that the search time of the present invention is effectively reduced, and the planned moving path is smoother.

[0064] The above description of the disclosed embodiments enables one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A path planning method for a fixed scene, characterized in that: include: Obtain a map of a fixed scene, build a KD tree based on the obstacles in the map, and plan the movement path of the mobile object through a path search algorithm; The path search algorithm uses a hybrid A* algorithm; The shortest distance between the current path node and the obstacle is calculated based on the KD tree and recorded as the risk distance, and the risk level of the moving object is determined based on the risk distance; After calculating the risk distance, add the risk distance to the cost function of the hybrid A* algorithm; When judging the risk level of a moving object based on the risk distance, multiple extended areas with different areas are determined based on the maximum self-size data of the moving object. Different extended areas correspond to different risk levels, and the risk level of the moving object is determined based on the fitting relationship between the risk distance and the extended areas. Expand path nodes based on the risk level of the moving object; when the risk level reaches a first limit, adjust the step size of the hybrid A* algorithm to a preset minimum value and the curvature to a preset maximum value; when the risk level reaches a second limit, dynamically adjust the step size and curvature of the hybrid A* algorithm based on the risk distance; The method for dynamically adjusting the step size of the hybrid A* algorithm based on risk distance is: Where D(x) is the risk distance, max_l and min_l represent the preset maximum step size and minimum step size respectively, and ε is the maximum size data of the moving object; The method for dynamically adjusting the curvature of the hybrid A* algorithm based on risk distance is: Among them, max_c and min_c represent the preset maximum curvature and minimum curvature respectively; When the path node is expanded to one that can be directly connected to the end point using the RS curve, the angle information of the path node is regularized; Smooth the RS curve.

2. A path planning method for a fixed scene according to claim 1, characterized in that: When the path node is expanded to be able to directly connect the end point using the RS curve, the angle information of the path node is regularized to An integer multiple of .

3. A path planning method for a fixed scene according to claim 1, characterized in that: Methods for smoothing the RS curve include: The path node where the angle jump occurs is determined to be the angle jump point; The shortest distance between the angle jump point and the obstacle is calculated and recorded as the corrected distance; Calculate the length correction data and curvature correction data of the RS curve based on the corrected distance; The RS curve is corrected according to the length correction data and the curvature correction data.

4. A path planning method for a fixed scene according to claim 3, characterized in that: The method to determine the angle jump point is: Traverse the angle of each path node on the moving path; Calculate the difference between the angle of a path node and the angle of the previous path node; When the difference is greater than a preset threshold, it is determined that the angle of the path node has jumped, and the path node is recorded as an angle jump point.

Images