Unmanned vehicle global path planning method considering vehicle ground clearance constraint

Abstract

The invention discloses an unmanned vehicle global path planning method considering vehicle ground clearance constraint, and belongs to the technical field of unmanned vehicle path planning. The method comprises the following steps: obtaining an original digital elevation map according to open source data, extracting topographic information of the original digital elevation map, and constructing a planning map; performing global path planning on the planning map by using an improved A * algorithm, and mapping a global path to an original digital elevation map; and vehicle basic constraints are introduced by adopting a quadratic optimization method, and optimization of a global path is completed. According to the method, generation of an unsafe path is cut off from the source by considering the ground clearance, so that the driving robustness and reliability of the unmanned vehicle in a complex terrain are fundamentally guaranteed, the risk of'underpinning 'is fundamentally avoided, and the path safety is remarkably improved. The method effectively deals with the uncertainty of the terrain and disturbances such as attitude change of the vehicle in the unstructured terrain, and shows extremely strong engineering robustness.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of unmanned vehicle path planning, and particularly relates to a global path planning method for unmanned vehicles considering vehicle ground clearance constraints. BACKGROUND

[0002] With the expansion of automatic driving technology to unstructured environments such as the wild and mining areas, global path planning of vehicles in complex terrain is crucial. As a core part of the automatic driving system, global path planning is responsible for generating a macro safe route from the starting point to the ending point based on map information, and it plays a connecting role, providing a safe and efficient navigation solution for unmanned vehicles and providing a basis for subsequent local planning.

[0003] Existing traditional path planning algorithms (such as A* algorithm) are mostly based on structured road design, and the premise is that the road surface is flat and the vehicle passability is guaranteed. However, in unstructured terrain, the ground is uneven, and there are obstacles such as bumps and potholes. These algorithms generally ignore the vehicle chassis ground clearance constraints and only pursue geometric accessibility. This easily leads to a serious engineering problem: when the vehicle drives to the uneven area according to the planned path, it is easy to "bottom out", that is, the chassis interferes with the ground and cannot pass. At this time, forced emergency re-planning or manual takeover will significantly increase the task time cost and uncertainty, and even cause the vehicle to be stranded or damaged. Therefore, in the global path planning stage, how to efficiently integrate the chassis ground clearance constraints and increase the necessary robustness of the vehicle in unstructured terrain is quite challenging to generate a safe path with robustness from the source and avoid the risk of "bottoming out". SUMMARY

[0004] To solve the above technical problems, a global path planning method for unmanned vehicles considering vehicle ground clearance constraints is disclosed.

[0005] To achieve the above technical scheme, the steps include: S1, obtaining an original digital elevation (DEM) map according to open source data, and constructing a planning map by extracting terrain information of the original digital elevation model map; The extraction method of the terrain information of the original map is: for the digital elevation map, the elevation change rate is obtained by using the difference-based method, and the terrain information is obtained by the elevation change rate; Further, the third-order inverse distance square difference method in the difference method is used to calculate the elevation change rate; The terrain information obtained by the elevation change rate includes: the ground elevation coefficient of variation , ground slope information , ground roughness information , and ground elevation difference information The method for obtaining terrain information through the rate of change of elevation is as follows: The ratio of ground surface area to projected area is obtained from the elevation change rate. The ratio of ground surface area to projected area is the ground roughness information. The ratio between the rates of change of elevation is the ground slope information, and the tangent of the ground slope information multiplied by the distance is the ground elevation difference information; The elevation variation coefficient is represented by the ratio of the standard deviation of the ground elevation in the analysis window to the average elevation. The standard deviation of the ground elevation is obtained by subtracting the sum of squares of the total average elevation from each elevation value in the analysis window, and then dividing by the square root of the number of elevation values ​​in the analysis window. The planning map includes three sub-maps; The planning map is obtained by assigning different weighting coefficients to each piece of terrain information and then summing them to obtain the value. The amount of terrain information used in different levels of planning maps varies, with the elevation variation coefficient serving as a specific reference. The formula is defined as follows: In the formula, This represents the first planning map; This represents the second planning map; This represents the third planning map; Indicated in the first planning map Medium surface roughness information Weighting coefficients; Represents the second planning map Medium surface roughness information Weighting coefficients; Represents the second planning map Ground slope information Weighting coefficients; Represents the third planning map Medium surface roughness information Weighting coefficients; Represents the third planning map Ground slope information Weighting coefficients; Represents the third planning map Ground elevation difference information Weighting coefficients; Elevation variation coefficient The weighting coefficients.

[0006] S2. Use the improved A* algorithm to perform global path planning on the planning map and map the global path to the original digital elevation map; The steps include: S2.1 To introduce vehicle ground clearance constraints, a three-dimensional vehicle chassis model is established; The setup method is as follows: Input the overall coordinate point set of the vehicle chassis model. ,in, This indicates the height of the center point of the vehicle's four wheels relative to the ground; Indicates the height of the vehicle's center point relative to the ground; This represents the mapped coordinates of the vehicle chassis center point and the center points of the four wheels. The vehicle chassis is constructed using quadratic surface fitting, as shown in the following expression: In the formula, Indicates by The set of coordinate points for the chassis model constructed using quadratic fitting; This represents a cell array that stores the set of coordinate points of a chassis model linearly constructed from the center point of the vehicle chassis and the center points of the four wheels. and The set of horizontal and vertical coordinate points representing the chassis model linearly constructed from the center point of the vehicle chassis and the center points of the four wheels; Indicates the first weighting coefficient; This represents the second weighting coefficient; This represents the third weighting coefficient; This represents the fourth weighting coefficient; This represents the fifth weighting coefficient; This represents the sixth weighting coefficient; S2.2. Use the improved A* algorithm to perform global path planning on the planning map; The vehicle's global path is a complete path from the starting point to the destination, which is determined by the algorithm at nodes on the map. and its neighboring nodes Single-step planning connects the components; consisting of the current node. Get its neighboring nodes Determine the node If a node is already recorded in the CloseList, skip this step; otherwise, use the improved A* algorithm to complete the node process. To the node To calculate the total cost, a vehicle chassis model is introduced, and the improved A* algorithm steps are as follows: S2.2.1, by node Get Node Determine the node Check if the node is recorded in the CloseList. If it is, skip this step; otherwise, read the node. The corresponding elevation variation coefficient value is selected from the three planning maps based on the defined coefficient range. To the node Map; Based on the defined range of coefficient values, the selection method from the three planning maps is as follows: When node The range of elevation variation coefficient is At that time, choose to be In the node Get Node Single-step planning; When node The range of elevation variation coefficient is At that time, choose to be In the node Get Node Single-step planning; When node The range of elevation variation coefficient is At that time, choose to be In the node Get Node Single-step planning; S2.2.2 Introduce vehicle ground clearance constraints and calculate the nodes respectively. The actual cost of the improved A* algorithm and nodes The estimated cost of the improved A* algorithm ; Improved A* algorithm, such as Figure 4 As shown, node The actual cost of the improved A* algorithm The expression is as follows: In the formula, Represents a node The actual cost of the improved A* algorithm; Indicates the distance from the starting point to the node. The cumulative actual value of the transaction; Represents a node Move to node The single-step value; Represents a node To the node Improved three-dimensional Euclidean distance; Represents a node The terrain cost function is expressed as follows: In the formula, express Weighting coefficients; Represents a node The cost value is determined by the ground clearance. express Weighting coefficients; Represents a node The value of slope; and satisfy The constraints are such that, because this invention considers the "bottoming out" problem in path planning, the cost value obtained from ground clearance determination is given greater weight, i.e. , ; The cost is obtained by a determination method based on Euler angles, the specific process of which is as follows: Based on the underlying search logic of the improved A* algorithm, the nodes can be determined. To the node It can obtain the value of the yaw angle; According to the node and This allows us to obtain the coordinates of the vehicle's four tires on the planned map. Based on these coordinates, we extract the corresponding elevation values. Using the four wheel elevation values ​​and their corresponding coordinates, we establish a virtual plane. We then use this plane to extract the vehicle's position at each node. The values ​​of roll and pitch; By identifying the two points with the largest elevation difference among the four elevation points where the tire is located, a strong constraint matrix is ​​set to ensure that the virtual plane must pass through these two points. The final virtual plane is then solved by constructing an augmented matrix. The formula for the strong constraint matrix is: In the formula, C represents the constraint matrix; d c Represents the constraint vector; and These represent the maximum values ​​among the four elevation points. x coordinates and y coordinate; and These represent the minimum values ​​among the four elevation points. x coordinates and y coordinate; and These represent the maximum elevation value and the minimum elevation value among the four tire contact points, respectively. The augmented matrix is ​​constructed using a strong constraint matrix, and the final virtual plane is solved by solving a system of linear equations, as shown in the following expression: In the formula, Represents an augmented matrix; This represents a 4×3 matrix, where 4 corresponds to the four tire points, used to store the coordinates of the four wheels; This represents a 3×3 matrix, which is the canonical equation representation of the standard least squares problem; For a size of The zero matrix; This represents a matrix that stores the elevation values ​​of four wheels; Represents the right-hand term of the equation; Represent the vector to be determined; The Euler angles (roll and pitch) of the virtual plane are solved using a method based on local coordinate system projection and rotation matrix decomposition. First, a normal vector is established... Using the base plane A as a reference, the x-axis direction of plane A is then projected onto the virtual plane to obtain the x-axis direction of the virtual plane. A complete local coordinate system (x-axis, y-axis, z-axis) is constructed for the two planes. The rotation moment is obtained through the transformation relationship between the two local coordinate systems. Finally, the pitch and roll angles of the virtual plane are decomposed from the rotation matrix. The rotation matrix can be established using yaw, roll, and pitch angles. The formula for establishing the rotation matrix is: In the formula, pass Complete the overall coordinate point set of the vehicle chassis model The specific formula for attitude change is as follows: Get the driverless car at the node Specific posture at time ; Based on the comparison between the chassis elevation value and the map elevation value, the following is obtained: The value of the product is defined by the following formula. Specific values: In the formula, Indicates at node The elevation of the point with the largest elevation value in the chassis model point set under the vehicle's posture; This indicates the ground elevation on the map corresponding to the lowest point of the chassis; and These represent nodes respectively. At that time, the elevation data of the chassis point set and the map elevation data corresponding to the chassis point set; Represents a node The slope cost is calculated using the following formula: In the formula, and Represents a node and nodes Height data; Represents a node To the node The improved three-dimensional Euclidean distance is given by the following formula: In the formula, and Representing nodes respectively Row and column indices in a map network coordinate system, i.e., planar coordinates; and Representing nodes respectively Row and column indices in a map network coordinate system, i.e., planar coordinates; Represents distance weight, taking ; S2.2.3, the actual cost and estimated costs Adding them together gives the total cost of the improved A* algorithm. The expression is as follows: ; S2.2.4 Obtain the total cost Then, check the nodes. Whether it is recorded in the OpenList; if recorded, it indicates the node. The actual cost has been calculated previously; it is necessary to determine the actual cost in this calculation. If the actual cost is higher than the previously calculated actual cost, retain the previous value and complete this single-step planning. If it is lower, perform an overwrite operation, replacing the previously calculated actual cost with the current actual cost, and update the node. The parent node is Complete this single-step planning; Conversely, if the node If a node is not recorded in the OpenList, then its value is recorded directly. The parent node is Complete this single-step planning; By repeatedly performing single-step planning from the starting point to the end point, a global path planning of the improved A* algorithm is completed. S2.3. Stitch and map the global paths in the planning map to the original digital elevation model (DEM) map to obtain a global path; Because it is a node To the node Perform single-step planning to ensure nodes Switch anytime, always with the node To ensure proper path planning, all paths are located on the same planning map. Once the entire planning process from the starting point to the destination is complete, all coordinates traversed are recorded, and the path is re-planned on the original digital elevation map, thus completing the path mapping from the planning map to the original digital elevation map.

[0007] S3. A secondary optimization method is used to introduce basic vehicle constraints and optimize the global path. Basic vehicle constraints include: Minimum turning radius constraints for vehicles increase path smoothness; The overall shape of the vehicle is constrained to prevent contact with obstacles; The energy consumption of the vehicle is constrained, and the optimized path length is controlled to avoid excessive consumption while ensuring smoothness. The optimization steps are as follows: S3.1 Record the turning position of the vehicle in the global path; S3.2. Use B-spline curves to optimize the global path and record the turning points in the curve; S3.3 Check the curvature at the recorded point. If it is found that it does not meet the minimum turning radius constraint of the vehicle, repeat S3.1~S3.2. When the curvature at all turns meets the minimum turning radius constraint of the vehicle, stop the optimization and obtain the final result, which is the optimized global path.

[0008] Beneficial effects of the present invention By taking ground clearance into account, this invention eliminates the generation of unsafe paths at the source, fundamentally ensuring the reliability of unmanned vehicles in complex terrain, fundamentally avoiding the risk of "bottoming out," and significantly improving path safety.

[0009] Compared to the traditional A* algorithm, which can only find geometrically reachable paths, the path planned by this invention fully considers terrain undulations and vehicle attitude, naturally tending to select areas with flatter terrain and greater ground clearance. This not only significantly improves the robustness of autonomous vehicles in driving paths in complex and uncertain terrains, but also reduces vehicle wear and energy consumption.

[0010] This invention improves the A* algorithm, ensuring both safety and robustness. By introducing an elevation variation coefficient to dynamically switch the planning map, it significantly enhances the algorithm's robustness to terrain undulations and uncertainties. Furthermore, it utilizes weighted 3D distances for search, ensuring planning efficiency and enabling the rapid generation of safe and relatively efficient global paths in complex global environments. Attached Figure Description

[0011] Figure 1 This is a flowchart illustrating the overall process of the method of the present invention. Figure 2 This is a schematic diagram of the planning map; Figure 3 This is a diagram showing the relationship between the chassis model, the ground, and the vehicle chassis. Figure 3 (a) A schematic diagram showing the vehicle chassis and the ground without bottoming out; Figure 3 (b) A schematic diagram showing the undercarriage model of the vehicle chassis and the ground; Figure 3 (c) A schematic diagram showing the non-bottomed curved surface of the vehicle chassis and the ground; Figure 3 (d) shows a schematic diagram of the undercarriage surface of the vehicle and the ground. Figure 4 To improve the A* algorithm in single-step planning flowchart for planning maps; Figure 5 Create a schematic diagram for the virtual plane; Figure 6 A schematic diagram illustrating the path stitching and mapping from the planned map to the original digital elevation map; Figure 7 For secondary optimization of the flowchart; Figure 8 This is a comparison chart of the performance of the traditional 3D A* algorithm and the improved A* algorithm. Figure 8 (a) shows a schematic diagram of the traditional 3D A* algorithm path planning; Figure 8 (b) shows a schematic diagram of the improved 3D A* algorithm path planning. Detailed Implementation

[0012] The present invention will be further described in detail below with reference to specific embodiments.

[0013] like Figure 1 As shown, a global path planning method for autonomous vehicles considering ground clearance constraints includes the following steps: S1. Obtain the original digital elevation model (DEM) map based on open source data, extract the terrain information from the original digital elevation model map, and construct a planning map. This invention uses open-source data to build a hilly and mountainous terrain dataset with a resolution of 320×320, which is referred to as the original digital elevation map. The method for extracting terrain information from the original map is as follows: for digital elevation maps, the elevation change rate is obtained using a difference-based method, and terrain information is obtained through the elevation change rate. Furthermore, the third-order inverse distance squared weight difference method in the difference method is used to calculate the elevation change rate; Topographic information obtained from elevation change rate includes: ground elevation variation coefficient. Ground slope information Surface roughness information Ground elevation information ; The method for obtaining terrain information through the rate of change of elevation is as follows: The ratio of ground surface area to projected area is obtained from the elevation change rate. The ratio of ground surface area to projected area is the ground roughness information. The ratio between the rates of change of elevation is the ground slope information, and the tangent of the ground slope information multiplied by the distance is the ground elevation difference information; The elevation variation coefficient is represented by the ratio of the standard deviation of the ground elevation in the analysis window to the average elevation. The standard deviation of the ground elevation is obtained by subtracting the sum of squares of the total average elevation from each elevation value in the analysis window, and then dividing by the square root of the number of elevation values ​​in the analysis window. The planning map includes three sub-maps; The planning map is obtained by assigning different weighting coefficients to each piece of terrain information and then summing them to obtain the value. The amount of terrain information used in different levels of planning maps varies, with the elevation variation coefficient serving as a specific reference. The formula is defined as follows: In the formula, This represents the first planning map; This represents the second planning map; This represents the third planning map; Indicated in the first planning map Medium surface roughness information Weighting coefficients; Represents the second planning map Medium surface roughness information Weighting coefficients; Represents the second planning map Ground slope information Weighting coefficients; Represents the third planning map Medium surface roughness information Weighting coefficients; Represents the third planning map Ground slope information Weighting coefficients; Represents the third planning map Ground elevation difference information Weighting coefficients; Elevation variation coefficient The weighting coefficient; in this embodiment, using As an example, such as Figure 2 As shown; In this invention, based on the original digital elevation map with a size of 320×320, three planning maps with a size of 320×320 can be obtained, and a set of corresponding weight coefficients are obtained according to the number of terrain factors. The specific values ​​are as follows: .

[0014] S2. Use the improved A* algorithm to perform global path planning on the planning map and map the global path to the original digital elevation map; The steps include: S2.1 To introduce vehicle ground clearance constraints, a three-dimensional vehicle chassis model is established; The setup method is as follows: Input the overall coordinate point set of the vehicle chassis model. ,in, This indicates the height of the center point of the vehicle's four wheels relative to the ground; Indicates the height of the vehicle's center point relative to the ground; This represents the mapped coordinates of the vehicle chassis center point and the center points of the four wheels. The vehicle chassis is constructed using quadratic surface fitting, as shown in the following expression: In the formula, Indicates by The set of coordinate points for the chassis model constructed using quadratic fitting; This represents a cell array that stores the set of coordinate points of a chassis model linearly constructed from the center point of the vehicle chassis and the center points of the four wheels. and The set of horizontal and vertical coordinate points representing the chassis model linearly constructed from the center point of the vehicle chassis and the center points of the four wheels; Indicates the first weighting coefficient; This represents the second weighting coefficient; This represents the third weighting coefficient; This represents the fourth weighting coefficient; This represents the fifth weighting coefficient; This represents the sixth weighting coefficient; This represents the total coordinate point set of the vehicle chassis model, used to establish the three-dimensional coordinates of the virtual vehicle chassis, and subsequently used to determine whether the vehicle chassis has "bottomed out" of the ground. The relationship between the chassis model, the actual vehicle chassis, and the ground is as follows: Figure 3 middle, Figure 3 (a) Figure 3 (b) Figure 3 (c) and Figure 3 As shown in (d), the black line represents the ground, and the blue dashed line represents the chassis surface, i.e., the relationship diagram ( Figure 3 (c) and Figure 3 (d) The blue-yellow curved surface below; the red dots at the four corners of the chassis surface represent tires, and the highest red dot in the middle represents the center point of the actual chassis. When the ground is not higher than the chassis model, it means that there will be no safety issues such as "bottoming out" between the ground and the chassis; when the ground is higher than the chassis model, that is, when the ground intersects with the chassis model, the ground is judged to be a dangerous ground, and there will be a problem of "bottoming out" between the ground and the chassis; In this embodiment, the size of the vehicle model used is... ,set up , , ; This was accomplished using linear interpolation, with the linear interpolation parameter set to 30, resulting in a set of... A binary cell array, storing the chassis model's... Coordinate point set and coordinate point set; It is a group A 3-cell array, storing the chassis model's... coordinate point set coordinate point set and Z coordinate point set; S2.2. Use the improved A* algorithm to perform global path planning on the planning map; In this embodiment, at the resolution Plan on the map, setting the starting point and the end point as follows: and The vehicle's global path is a complete path from the starting point to the destination, consisting of nodes. and nodes The single-step planning connection components; The vehicle's global path is a complete path from the starting point to the destination, which is determined by the algorithm at nodes on the map. and its neighboring nodes Single-step planning connects the components; consisting of the current node. Get its neighboring nodes Determine the node If a node is already recorded in the CloseList, skip this step; otherwise, use the improved A* algorithm to complete the node process. To the node To calculate the total cost, a vehicle chassis model is introduced, and the improved A* algorithm steps are as follows: S2.2.1, by node Get Node Determine the node Check if the node is recorded in the CloseList. If it is, skip this step; otherwise, read the node. The corresponding elevation variation coefficient value is selected from the three planning maps based on the defined coefficient range. To the node Map; Based on the defined range of coefficient values, the selection method from the three planning maps is as follows: When node The range of elevation variation coefficient is At that time, choose to be In the node Get Node Single-step planning; When node The range of elevation variation coefficient is At that time, choose to be In the node Get Node Single-step planning; When node The range of elevation variation coefficient is At that time, choose to be In the node Get Node Single-step planning; S2.2.2 Introduce vehicle ground clearance constraints and calculate the nodes respectively. The actual cost of the improved A* algorithm and nodes The estimated cost of the improved A* algorithm ; Improved A* algorithm, such as Figure 4 As shown, node The actual cost of the improved A* algorithm The expression is as follows: In the formula, Represents a node The actual cost of the improved A* algorithm; Indicates the distance from the starting point to the node. The cumulative actual value of the transaction; Represents a node Move to node The single-step value; Represents a node To the node Improved three-dimensional Euclidean distance; Represents a node The terrain cost function is expressed as follows: In the formula, express Weighting coefficients; Represents a node The cost value is determined by the ground clearance. express Weighting coefficients; Represents a node The value of slope; and satisfy The constraints are such that, because this invention considers the "bottoming out" problem in path planning, the cost value obtained from ground clearance determination is given greater weight, i.e. , ; The cost is obtained by a determination method based on Euler angles, the specific process of which is as follows: Based on the underlying search logic of the improved A* algorithm, the nodes can be determined. To the node It can obtain the value of the yaw angle; According to the node and This allows us to obtain the coordinates of the vehicle's four tires on the planned map. Based on these coordinates, we extract the corresponding elevation values. Using the four wheel elevation values ​​and their corresponding coordinates, we establish a virtual plane. We then use this plane to extract the vehicle's position at each node. The values ​​of roll and pitch; like Figure 5 As shown, the establishment of the virtual plane can be approximated as a constrained least squares problem. The specific process is as follows: Find the two points with the largest height difference among the four elevation points of the tire (the four red points in the diagram). Set a strong constraint matrix to ensure that the virtual plane must pass through these two points (the two points marked in green among the four red points). The final virtual plane is solved by constructing an augmented matrix. The formula for the strong constraint matrix is: In the formula, C represents the constraint matrix; d c Represents the constraint vector; and These represent the maximum values ​​among the four elevation points. x coordinates and y coordinate; and These represent the minimum values ​​among the four elevation points. x coordinates and y coordinate; and These represent the maximum elevation value and the minimum elevation value among the four tire contact points, respectively. The augmented matrix is ​​constructed using a strong constraint matrix, and the final virtual plane is solved by solving a system of linear equations, as shown in the following expression: In the formula, Represents an augmented matrix; This represents a 4×3 matrix, where 4 corresponds to the four tire points, used to store the coordinates of the four wheels; This represents a 3×3 matrix, which is the canonical equation representation of the standard least squares problem; For a size of The zero matrix; This represents a matrix that stores the elevation values ​​of four wheels; Represents the right-hand term of the equation; Represent the vector to be determined; The Euler angles (roll and pitch) of the virtual plane are solved using a method based on local coordinate system projection and rotation matrix decomposition. First, a normal vector is established... Using the base plane A as a reference, the x-axis direction of plane A is then projected onto the virtual plane to obtain the x-axis direction of the virtual plane. A complete local coordinate system (x-axis, y-axis, z-axis) is constructed for the two planes. The rotation moment is obtained through the transformation relationship between the two local coordinate systems. Finally, the pitch and roll angles of the virtual plane are decomposed from the rotation matrix. The rotation matrix can be established using yaw, roll, and pitch angles. The formula for establishing the rotation matrix is: In the formula, pass Complete the overall coordinate point set of the vehicle chassis model The specific formula for attitude change is as follows: Get the driverless car at the node Specific posture at time ; Based on the comparison between the chassis elevation value and the map elevation value, the following is obtained: The value of the product is defined by the following formula. Specific values: In the formula, Indicates at node The elevation of the point with the largest elevation value in the chassis model point set under the vehicle's posture; This indicates the ground elevation on the map corresponding to the lowest point of the chassis; and These represent nodes respectively. At that time, the elevation data of the chassis point set and the map elevation data corresponding to the chassis point set; Represents a node The slope cost is calculated using the following formula: In the formula, and Represents a node and nodes Height data; Represents a node To the node The improved three-dimensional Euclidean distance is given by the following formula: In the formula, and Representing nodes respectively Row and column indices in a map network coordinate system, i.e., planar coordinates; and Representing nodes respectively Row and column indices in a map network coordinate system, i.e., planar coordinates; Represents distance weights, set ; Represents the square of the elevation difference; and Represents a node and nodes Height data; S2.2.3, the actual cost and estimated costs Adding them together gives the total cost of the improved A* algorithm. The expression is as follows: ; S2.2.4 Obtain the total cost Then, check the nodes. Whether it is recorded in the OpenList; if recorded, it indicates the node. The actual cost has been calculated previously; it is necessary to determine the actual cost in this calculation. If the actual cost is higher than the previously calculated actual cost, retain the previous value and complete this single-step planning. If it is lower, perform an overwrite operation, replacing the previously calculated actual cost with the current actual cost, and update the node. The parent node is Complete this single-step planning; Conversely, if the node If a node is not recorded in the OpenList, then its value is recorded directly. The parent node is Complete this single-step planning; By repeatedly performing single-step planning from the starting point to the end point, a global path planning of the improved A* algorithm is completed. S2.3. Stitch and map the global paths in the planning map to the original digital elevation model (DEM) map to obtain a global path; Because it is a node To the node Perform single-step planning to ensure nodes Switch anytime, always with the node Both should be on the same planning map to ensure proper path planning. Once completed from the starting point... To the finish line During the entire planning process, all coordinates along the route are recorded and replanned in the original digital elevation map to complete the path mapping from the planned map to the original digital elevation map; like Figure 6 As shown, the green dot represents the starting point, and the yellow dot represents the ending point. The map colors are simply to make the differences between different maps more obvious. Selecting to start from the planning map... The planning process begins, and during this process, the planning map will be continuously updated. and Switching between nodes and ensuring node integrity. and nodes In single-step planning, the paths exist within the same planning map (MAP) until the destination is reached. In mapping, the paths from the two planning maps are joined together and finally mapped onto the original digital elevation map.

[0015] S3, such as Figure 7 As shown, a quadratic optimization method is used to introduce basic vehicle constraints and optimize the global path. Basic vehicle constraints include: Minimum turning radius constraints for vehicles increase path smoothness; The overall shape of the vehicle is constrained to prevent contact with obstacles; The energy consumption of the vehicle is constrained, and the optimized path length is controlled to avoid excessive consumption while ensuring smoothness. The optimization steps are as follows: S3.1 Record the turning position of the vehicle in the global path; S3.2. Use B-spline curves to optimize the global path and record the turning points in the curve; S3.3 Check the curvature at the recorded point. If it is found that it does not meet the minimum turning radius constraint of the vehicle, repeat S3.1~S3.2. When the curvature at all turns meets the minimum turning radius constraint of the vehicle, stop the optimization and obtain the final result, which is the optimized global path. To verify the feasibility of this invention, a comparison was made between the improved 3D A* algorithm and the traditional 3D A* algorithm, in order to verify the improvement in path safety of the improved A* algorithm compared to the traditional 3D A* algorithm.

[0016] Figure 8 The results of the two algorithms on the self-built dataset are shown, and the planned paths are compared with the terrain roughness as the main safety indicator.

[0017] The simulation results are shown in Table 1. Table 1: Algorithm Comparison Results By analyzing Table 1 and Figure 8 middle Figure 8 (a) and Figure 8 (b) It can be seen that the traditional A* algorithm still tends to traverse areas with large terrain undulations. The improved A* algorithm, due to the consideration of vehicle ground clearance constraints, has higher robustness in unstructured terrain compared to the traditional 3D A* algorithm, and can plan a flatter and safer road.

[0018] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit the scope of protection of the present invention. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the essence and scope of the technical solutions of the present invention.

Claims

1. A global path planning method for unmanned vehicles considering ground clearance constraints, characterized in that, Includes the following steps: S1. Obtain the original digital elevation map based on open source data, and construct a planning map by extracting the terrain information from the original digital elevation model map. The method for extracting terrain information from the original map is as follows: for digital elevation maps, the elevation change rate is obtained using a difference-based method, and terrain information is obtained through the elevation change rate; wherein, the difference method is the third-order inverse distance squared weight difference method. The terrain information obtained through the elevation change rate includes: ground elevation variation coefficient, ground slope information, ground roughness information, and ground elevation difference information; The planning map includes three sub-maps; S2. Use the improved A* algorithm to perform global path planning on the planning map and map the global path to the original digital elevation map; When using the improved A* algorithm to perform global path planning on the planning map, vehicle ground clearance is introduced as a constraint. S3. A secondary optimization method is used to introduce basic vehicle constraints and optimize the global path.

2. The global path planning method for unmanned vehicles considering ground clearance constraints according to claim 1, characterized in that: The method for obtaining terrain information through the rate of elevation change is as follows: The ratio of ground surface area to projected area is obtained from the elevation change rate. The ratio of ground surface area to projected area is the ground roughness information. The ratio between the rates of change of elevation is the ground slope information, and the tangent of the ground slope information multiplied by the distance is the ground elevation difference information; The coefficient of variation of ground elevation is represented by the ratio of the standard deviation of ground elevation in the analysis window to the average elevation. The standard deviation of ground elevation is obtained by subtracting the sum of squares of the total average elevation from each elevation value in the analysis window, and then dividing by the square root of the number of elevation values ​​in the analysis window.

3. The global path planning method for unmanned vehicles considering ground clearance constraints according to claim 1, characterized in that: The method for constructing the planning map is as follows: assign different weight coefficients to each piece of terrain information, and sum them to obtain a value. The formula is defined as follows: In the formula, This represents the first planning map; This represents the second planning map; This represents the third planning map; Indicated in the first planning map Medium surface roughness information Weighting coefficients; Represents the second planning map Medium surface roughness information Weighting coefficients; Represents the second planning map Ground slope information Weighting coefficients; Represents the third planning map Medium surface roughness information Weighting coefficients; Represents the third planning map Ground slope information Weighting coefficients; Represents the third planning map Ground elevation difference information Weighting coefficients; Elevation variation coefficient The weighting coefficients.

4. The global path planning method for unmanned vehicles considering ground clearance constraints according to claim 1, characterized in that: The steps in S2 include: S2.1 To introduce vehicle ground clearance constraints, a three-dimensional vehicle chassis model is established; The setup method is as follows: Input the overall coordinate point set of the vehicle chassis model. ,in, This indicates the height of the center point of the vehicle's four wheels relative to the ground; Indicates the height of the vehicle's center point relative to the ground; This represents the mapped coordinates of the vehicle chassis center point and the center points of the four wheels. The vehicle chassis is constructed using quadratic surface fitting, as shown in the following expression: In the formula, Indicates by The set of coordinate points for the chassis model constructed using quadratic fitting; This represents a cell array that stores the set of coordinate points of a chassis model linearly constructed from the center point of the vehicle chassis and the center points of the four wheels. and The set of horizontal and vertical coordinate points representing the chassis model linearly constructed from the center point of the vehicle chassis and the center points of the four wheels; Indicates the first weighting coefficient; This represents the second weighting coefficient; This represents the third weighting coefficient; This represents the fourth weighting coefficient; This represents the fifth weighting coefficient; This represents the sixth weighting coefficient; S2.

2. Use the improved A* algorithm to perform global path planning on the planning map; S2.

3. Stitch together the global paths in the planning map and map them onto the original digital elevation model (DEM) map to obtain a global path.

5. The global path planning method for unmanned vehicles considering ground clearance constraints according to claim 4, characterized in that: The steps for performing global path planning on the planning map using the improved A* algorithm include: S2.2.1, The nodes are completed using the improved A* algorithm. To the node Calculate the total cost and determine the node. Check if the node is recorded in the CloseList. If it is, skip this step; otherwise, read the node. The corresponding elevation variation coefficient value is selected from the three planning maps based on the defined coefficient range. To the node Map; Based on the defined range of coefficient values, the selection method from the three planning maps is as follows: When node The range of elevation variation coefficient is At that time, choose to be In the node Get Node Single-step planning; When node The range of elevation variation coefficient is At that time, choose to be In the node Get Node Single-step planning; When node The range of elevation variation coefficient is At that time, choose to be In the node Get Node Single-step planning; S2.2.2 Introduce vehicle ground clearance constraints and calculate the nodes respectively. The actual cost of the improved A* algorithm and nodes The estimated cost of the improved A* algorithm ; node The actual cost of the improved A* algorithm The expression is as follows: In the formula, Represents a node The actual cost of the improved A* algorithm; Indicates the distance from the starting point to the node. The cumulative actual value of the transaction; Represents a node Move to node The single-step value; Represents a node To the node Improved three-dimensional Euclidean distance; Represents a node The terrain cost function is expressed as follows: In the formula, express Weighting coefficients; Represents a node The cost value is determined by the ground clearance. express Weighting coefficients; Represents a node The value of slope; and satisfy The constraints, because the path planning considers the "bottom-line" problem, therefore assign a greater weight to the cost obtained from the ground clearance determination, i.e. , ; S2.2.3, the actual cost and estimated costs Adding them together gives the total cost of the improved A* algorithm. The expression is as follows: ； S2.2.4 Obtain the total cost Then, check the nodes. Whether it is recorded in the OpenList; if recorded, it indicates the node. The actual cost has been calculated previously; it is necessary to determine the actual cost in this calculation. If the actual cost is higher than the previously calculated actual cost, retain the previous value and complete this single-step planning. If it is lower, perform an overwrite operation, replacing the previously calculated actual cost with the current actual cost, and update the node. The parent node is Complete this single-step planning; Conversely, if the node If a node is not recorded in the OpenList, then the value of each node is recorded directly. The parent node is Complete this single-step planning; By repeatedly performing single-step planning from the starting point to the end point, a global path planning of the improved A* algorithm is completed.

6. The global path planning method for unmanned vehicles considering ground clearance constraints according to claim 5, characterized in that: The node The value obtained by judging ground clearance The expression is as follows: In the formula, Indicates at node The elevation of the point with the largest elevation value in the chassis model point set under the vehicle's posture; This indicates the ground elevation on the map corresponding to the lowest point of the chassis; and These represent nodes respectively. At that time, the elevation data of the chassis point set and the map elevation data corresponding to the chassis point set.

7. The global path planning method for unmanned vehicles considering ground clearance constraints according to claim 5, characterized in that: The node The value of slope The expression is as follows: In the formula, and Represents a node and nodes Height data, Represents a node To the node Improved three-dimensional Euclidean distance.

8. The global path planning method for unmanned vehicles considering ground clearance constraints according to claim 5, characterized in that: The node To the node Improved three-dimensional Euclidean distance The expression is as follows: In the formula, and Representing nodes respectively Row and column indices in a map network coordinate system, i.e., planar coordinates; and Representing nodes respectively Row and column indices in a map network coordinate system, i.e., planar coordinates; Represents distance weights, set ; Represents the square of the elevation difference; and Represents a node and nodes The height data.

9. The global path planning method for unmanned vehicles considering ground clearance constraints according to claim 1, characterized in that: The method of secondary optimization introduces basic vehicle constraints, including: minimum turning radius constraint to increase path smoothness; overall shape constraint to prevent contact with obstacles; and energy consumption constraint to control the optimized path length while ensuring smoothness and avoiding excessive energy consumption.

10. The global path planning method for unmanned vehicles considering ground clearance constraints according to claim 1, characterized in that: The steps for optimizing the global path include: S3.1 Record the turning position of the vehicle in the global path; S3.

2. Use B-spline curves to optimize the global path and record the turning points in the curve; S3.3 Check the curvature at the recorded point. If it is found that it does not meet the minimum turning radius constraint of the vehicle, repeat S3.1~S3.

2. When the curvature at all turns meets the minimum turning radius constraint of the vehicle, stop the optimization and obtain the final result, which is the optimized global path.

Images