Unmanned vehicle global path planning method considering vehicle rollover stability

Abstract

The invention provides an unmanned vehicle global path planning method considering vehicle rollover stability, and belongs to the technical field of unmanned vehicle path planning, and the method comprises the steps: obtaining an original map according to open source data, and building a fitness map fusing all environment information through extracting the topographic information of the original map; obtaining a downsampling factor according to the fitness map, and obtaining a sub-fitness map through the downsampling factor; performing global path planning on the sub-fitness map by using an improved A * algorithm, and mapping a global path to an original map; and vehicle basic constraints are introduced by adopting a quadratic optimization method, and optimization of a global path is completed. Compared with the prior art, the vehicle rollover stability is considered, and compared with a traditional three-dimensional A * algorithm, a smoother and safer road can be planned.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned vehicle path planning, and in particular relates to an unmanned vehicle global path planning method taking vehicle rollover stability into consideration. Background Art

[0002] With the development of autonomous driving technology, the global path planning problem of vehicles in complex environments has become particularly important. In the field of autonomous driving vehicles, it can be summarized into five major modules: perception, positioning, planning, decision-making and control. As a part of planning, path planning plays a connecting role and provides safe and efficient navigation solutions for unmanned vehicles. In the field of unmanned vehicles, global path planning is to determine the overall route from the starting point to the end point based on the map and positioning system, provide a basic path for subsequent local path planning, and ensure that unmanned vehicles can complete tasks efficiently and safely. Existing technologies include: traditional three-dimensional A* algorithm, which can efficiently plan a path in a grid map.

[0003] However, there is an inevitable conflict between the number of constraints and planning efficiency in the global path planning of intelligent unmanned vehicles. In addition, the vehicle's dynamic constraints, especially the vehicle's rollover stability constraints, will have a significant impact on the stability and passability of the vehicle in an unstructured environment. Improper path planning may lead to safety accidents such as vehicle rollover and overturning. The traditional three-dimensional A* algorithm ignores the problem of vehicle rollover stability constraints, and unmanned vehicles are prone to rollover and other accidents on the planned roads. Therefore, it is quite challenging to find a method to ensure the safety of the global path while improving the efficiency of global path planning. Summary of the invention

[0004] To solve the above problems, the present invention provides a global path planning method for an unmanned vehicle taking into account the rollover stability of the vehicle.

[0005] To implement the above technical solution, the specific steps are as follows:

[0006] S1. Obtain the original map based on open source data, and obtain the original fitness map SYMAP by extracting the terrain information of the original map;

[0007] The extraction methods include: for the digital elevation map, using a difference-based method to obtain the elevation change rate, and obtaining terrain information through the elevation change rate;

[0008] Specifically, the third-order inverse distance square weighted difference method in the difference method is used to calculate the elevation change rate;

[0009] The terrain information is obtained as follows: the ratio of the ground surface area to the projected area is obtained from the elevation change rate, and the ratio of the ground surface area to the projected area is the ground roughness information; the ratio between the elevation change rates is the ground slope information; the tangent value of the ground slope information multiplied by the distance is the ground height difference information.

[0010] The terrain information of the original map includes: ground slope information M S , ground roughness information M R and ground height difference information M H ;

[0011] The way to obtain SYMAP is to assign different weight coefficients to each terrain information and add them up to get the value; the value is the fitness value M of the unmanned vehicle passing through the terrain recorded by each grid of SYMAP SY , the specific value is calculated by the following formula:

[0012]

[0013] Where α1 is the ground slope information M S The weight coefficient; α2 is the surface roughness information M R The weight coefficient; α3 is the ground height difference information M H The weight coefficient of .

[0014] S2. Get the downsampling factor s based on the original fitness map SYMAP k , the sub-fitness map is obtained by downsampling factor;

[0015] The downsampling factor is obtained as follows: the original fitness map SYMAP is a map of size M×N, and the size of the original fitness map SYMAP is consistent with that of the original map; the downsampling factor s k Determined by the prime factor of the maximum value of the two values ​​of M and N, and a number is randomly selected from the prime factor of the maximum value to obtain the downsampling factor; the downsampling factor must ensure that the number of prime factors is greater than or equal to 2, if it does not meet the number of prime factors greater than or equal to 2, the operation is stopped; or when the size of the sub-fitness map is less than or equal to 10, the operation is stopped even if the number of prime factors is greater than or equal to 2;

[0016] The size of the downsampled sub-fitness map at each layer is calculated as follows:

[0017]

[0018] In the formula, s k represents the downsampling factor, where k represents the kth layer;

[0019] From the original fitness map SYMAP to the k-th sub-fitness map SYMAPk The generated expression should conform to the following:

[0020]

[0021] Where (i, j) and (x, y) represent the coordinates in the sub-fitness map and the original fitness map respectively; min(i×s k ,M) and min(j×s k , N) represent the upper limits of x and y respectively, which are used to control the boundaries and ensure that the integrated extraction of information does not exceed the range of the original fitness map SYMAP; w is a weighting matrix, which is a size equal to s k Array; sorting is done according to map size.

[0022] S3. Use the improved A* algorithm to perform global path planning on the sub-fitness map and map the global path to the original map. The steps are as follows:

[0023] S3.1. In order to introduce the vehicle rollover stability constraint, a vehicle dynamics model is established to obtain the vehicle's maximum speed on the slope;

[0024] The vehicle rollover stability constraint is the vehicle dynamics constraint. If the vehicle speed is too fast on a slope, it will roll over. The dynamics model is established to obtain the vehicle's maximum speed. If the vehicle speed does not exceed this maximum speed, it meets the constraint.

[0025] The roll moment balance formula is established by the dynamic model, and the vehicle dynamic model input U=[θ y ,h,B,R min ], the maximum speed of the vehicle on the slope is constructed as:

[0026]

[0027] Where B represents the wheelbase of the vehicle; θ y represents the slope angle; g represents the acceleration due to gravity; R min Indicates the minimum turning radius of the vehicle; when the vehicle is driving on a slope, the constraint is met if the speed does not exceed the limit speed;

[0028] S3.2, use the improved A* algorithm to perform global path planning on the sub-fitness map;

[0029] The global path of the vehicle is a complete path from the starting point to the end point, which is composed of the algorithm's single-step planning connection between node n-1 and its neighboring node n in the map; the neighboring node n is obtained from the current node n-1, and it is determined whether node n is recorded in the CloseList list. If it has been recorded, the process is skipped; if it has not been recorded, the total cost from node n-1 to node n is calculated by the improved A* algorithm, and the vehicle dynamics model is introduced. The specific calculation formula is calculated by the following formula: Introducing the vehicle dynamics model, the steps of the improved A* algorithm are as follows:

[0030] S3.2.1. When node n is not recorded in the CloseList list, calculate the actual cost g of the improved A* algorithm for node n * (n) and the estimated cost h of the improved A* algorithm for node n * (n); the expressions are as follows:

[0031] g*(n)=g*(n-1)+(L(n-1,n)(1+M SY (n))+g R (n))

[0032] h*(n)=L(n,G)(1+0.5M S (n))

[0033] Where M S (n) represents the slope information of node n; M SY (n) represents the fitness value of node n; G represents the end point; g R (n) is the cost value of the vehicle rollover stability constraint, and whether the constraint is established is defined by the following formula:

[0034] g R (n)=[v(n),θ y (n),D(n)]≤g Rmax (n)=[v max ,θ y (n),D(n)]

[0035] Where v is the speed information of the current position; D is the direction information of the unmanned vehicle during planning; when v≤v max The operation ends when , otherwise the actual cost g is output * (n);

[0036] L(n-1,n) is an improved three-dimensional Euclidean distance, and the formula is as follows:

[0037]

[0038] Where L(n-1,n) represents the improved Euclidean distance from node n-1 to n; DEM nand DEM n-1 Represents the height data of node n and node n-1; μ is the distance weight;

[0039] S3.2.2. The actual cost g * (n) and the estimated cost h * (n) Add to get the total cost f of the improved A* algorithm * (n), the expression is as follows:

[0040] f*(n)=g*(n)+h*(n);

[0041] S3.2.3. Get the total cost f * (n), check whether node n is recorded in the OpenList list. If it is recorded, it means that the actual cost of node n has been calculated before, and it is necessary to determine the actual cost g of this calculation. * (n) Whether it is higher than the actual cost calculated before. If it is higher than the actual cost calculated before, keep the previous value and complete the single-step planning. If it is not higher, perform an overwriting operation, overwrite the actual cost calculated before with the actual cost calculated this time, update the parent node of node n to n-1, and complete the single-step planning. Otherwise, if node n is not recorded in the OpenList list, directly record the cost values, record the parent node of node n as n-1, and complete the single-step planning.

[0042] In this way, single-step planning from the starting point to the end point is continuously performed to complete the global path planning of the improved A* algorithm;

[0043] S3.3, mapping the global path to the original map to obtain a global path;

[0044] The mapping method is as follows: two lists RoadList and MapList are established on the map planned by the improved A* algorithm at low resolution to store path information and environmental information respectively. RoadList is used to store the location information of the path in the grid map, and MapList stores the grids that the path has not passed through, and adjusts the fitness value in the grid to ∞, which is recorded as an obstacle; the position in the table is mapped to the high-resolution map according to the map scale; a grid in the low-resolution map is composed of grids at the same position in the high-resolution map. After mapping the information of the two lists to the grid map with higher resolution, the fitness values ​​of all grids at the same position in the high-resolution map of the path information remain unchanged, and the environmental information replaces the fitness values ​​of all grids at the same position with ∞; among them, the low-resolution map is the map with lower resolution after two maps are obtained by downsampling factor; according to the map scale, the scale is obtained according to the downsampling factor, which is defined as the map scale;

[0045] Repeat S3.2 and S3.3 continuously to gradually complete the global path planning from the sub-fitness map with the lowest resolution to the original fitness map. At this point, an adaptive resolution global path planning is completed and a global path is obtained.

[0046] S4. Use the secondary optimization method to introduce basic vehicle constraints and complete the optimization of the global path;

[0047] The basic vehicle constraints include: the minimum turning radius constraint of the vehicle to increase the smoothness of the path; the overall shape constraint of the vehicle to prevent contact with obstacles; the energy consumption constraint of the vehicle to control the length of the optimized path while ensuring smoothness to avoid excessive consumption;

[0048] The optimization steps are as follows:

[0049] S4.1. Record the turning position of the vehicle in the global path;

[0050] S4.2, using a B-spline curve to optimize the global path, and recording the turning points in the curve;

[0051] S4.3. Check the curvature of the recorded location. If it is found that it does not meet the minimum turning radius constraint of the vehicle, repeat S4.1 to S4.2. When the curvature of 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.

[0052] Beneficial effects of the present invention:

[0053] The present invention introduces vehicle dynamics constraints into the improved A* algorithm, establishes a vehicle dynamics model, introduces the rollover stability constraints of the vehicle, and improves the safety and feasibility of path planning. Then, an adaptive resolution planning method is used to improve the planning efficiency of the improved A* algorithm, so that path safety and planning efficiency are taken into account, providing a more reliable and efficient solution for the path planning of unmanned vehicles. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0055] Figure 2 It is a schematic diagram of the fitness map;

[0056] Figure 3 Schematic diagram of the relationship between the original fitness map and the sub-fitness map;

[0057] Figure 4 Modeling vehicle dynamics;

[0058] Figure 5 To improve the A* algorithm in the single-step planning flowchart based on the fitness map;

[0059] Figure 6 Schematic diagram of mapping high-resolution map information after low-resolution map planning;

[0060] Figure 7 It is the secondary optimization flow chart;

[0061] Figure 8 This is a LTR comparison chart of the traditional 3D A* algorithm and the improved A* algorithm;

[0062] Fig. 9 This is a diagram showing how the planning method of the present invention changes with maps of different scales. DETAILED DESCRIPTION

[0063] The present invention is further described in detail below in conjunction with specific embodiments.

[0064] like Figure 1 As shown, a global path planning method for an unmanned vehicle considering the rollover stability of the vehicle comprises the following steps:

[0065] S1. Obtain the original map based on open source data, and obtain the original fitness map SYMAP by extracting the terrain information of the original map;

[0066] The extraction methods include: for the digital elevation map, using a difference-based method to obtain the elevation change rate, and obtaining terrain information through the elevation change rate;

[0067] In this embodiment, the open source data used comes from the ASTER global digital elevation model V003, and the starting point (24.28°N, 111.28°E) and the end point (24.53°N, 111.53°E) are selected as the original map of this embodiment, denoted as map Figure 1 , the size of the prior map obtained is 900×900; the extraction method is: using a difference-based method to obtain the elevation change rate, and obtaining terrain information through the elevation change rate; Figure 2 As shown;

[0068] Specifically, the third-order inverse distance square weighted difference method in the difference method is used to calculate the elevation change rate;

[0069] The terrain information is obtained as follows: the ratio of the ground surface area to the projected area is obtained from the elevation change rate, and the ratio of the ground surface area to the projected area is the ground roughness information; the ratio between the elevation change rates is the ground slope information; the tangent value of the ground slope information multiplied by the distance is the ground height difference information.

[0070] The terrain information of the original map includes: ground slope information M S , ground roughness information M R and ground height difference information MH ;

[0071] The way to obtain SYMAP is to assign different weight coefficients to each terrain information and add them up to get the value; the value is the fitness value M of the unmanned vehicle passing through the terrain recorded by each grid of SYMAP SY , the specific value is calculated by the following formula:

[0072]

[0073] Where α1 is the ground slope information M S The weight coefficient; α2 is the surface roughness information M R The weight coefficient; α3 is the ground height difference information M H The weight coefficient of .

[0074] S2. Get the downsampling factor s based on the original fitness map SYMAP k , the sub-fitness map is obtained by downsampling factor;

[0075] The downsampling factor is obtained as follows: the original fitness map SYMAP is a map of size M×N, and the size of the original fitness map SYMAP is consistent with that of the original map; the downsampling factor s k Determined by the prime factor of the maximum value of the two values ​​of M and N, and a number is randomly selected from the prime factor of the maximum value to obtain the downsampling factor; the downsampling factor must ensure that the number of prime factors is greater than or equal to 2, if it does not meet the number of prime factors greater than or equal to 2, the operation is stopped; or when the size of the sub-fitness map is less than or equal to 10, the operation is stopped even if the number of prime factors is greater than or equal to 2;

[0076] Further, when the number of prime factors of the maximum value of the two values ​​of M and N of the original fitness map SYMAP is greater than or equal to 2, a downsampling factor s1 is obtained, and a sub-fitness map SYMAP1 of the original fitness map SYMAP is obtained by the downsampling factor s1; if the number of prime factors of the maximum value of the two values ​​of M1 and N1 of SYMAP1 is greater than or equal to 2, a downsampling factor s2 is obtained, and a sub-fitness map SYMAP2 of the original fitness map SYMAP is obtained by the downsampling factor s2; when the sub-fitness map does not satisfy that the number of prime factors is greater than or equal to 2, the operation is stopped; or when the size of the sub-fitness map is less than or equal to 10, the operation is stopped even if the number of prime factors is greater than or equal to 2;

[0077] The size of the downsampled sub-fitness map at each layer is calculated as follows:

[0078]

[0079] In the formula, sk represents the downsampling factor, where k represents the kth layer. In this embodiment, k=4;

[0080] A portion of the sub-fitness map is created as Figure 3 As shown, the grids of the same color in SYMAP1 and SYMAP2 are generated by the grids of the same color in the original fitness map SYMAP. k The generated expression should conform to the following:

[0081]

[0082] Where (i, j) and (x, y) represent the coordinates in the sub-fitness map and the original fitness map respectively; min(i×s k ,M) and min(j×s k , N) represent the upper limits of x and y respectively, which are used to control the boundaries and ensure that the integrated extraction of information does not exceed the range of the original fitness map SYMAP; w is a weighting matrix, which is a size equal to s k Array; sort by map size;

[0083] In this embodiment, based on the prior map size of 900×900, M=900, N=900 can be obtained, and an original fitness map of size 900×900 can be obtained. A set of downsampling factors: [s1=5, s2=3, s3=2, s4=3] and four sub-fitness maps of size 1×1802×603×304:10×10] are obtained from the original fitness map.

[0084] S3. Use the improved A* algorithm to perform global path planning on the sub-fitness map and map the global path to the original map. The steps are as follows:

[0085] S3.1. In order to introduce the vehicle rollover stability constraint, a vehicle dynamics model is established to obtain the vehicle's maximum speed on the slope;

[0086] The vehicle rollover stability constraint is the vehicle dynamics constraint. If the vehicle speed is too fast on a slope, it will roll over. The dynamics model is established to obtain the vehicle's maximum speed. If the vehicle speed does not exceed this maximum speed, it meets the constraint.

[0087] Specific models such as Figure 4 As shown, where m is the vehicle mass, a y is the lateral acceleration, g is the acceleration due to gravity, f y is the friction force generated by the slope, θ y is the slope inclination, h is the height of the vehicle's center of mass, F ziis the normal support force of the ground on the inner wheel, B is the wheelbase of the vehicle; the roll moment balance formula is established by the dynamic model, and the vehicle dynamic model input U=[θ y ,h,B,R min ], the maximum speed of the vehicle on the slope is constructed as:

[0088]

[0089] Where B represents the wheelbase of the vehicle; θ y represents the slope angle; g represents the acceleration due to gravity; R min Indicates the minimum turning radius of the vehicle; when the vehicle is driving on a slope, the constraint is met if the speed does not exceed the limit speed;

[0090] S3.2, use the improved A* algorithm to perform global path planning on the sub-fitness map;

[0091] In this embodiment, planning is performed on a map with the lowest resolution of 10×10, and the start and end points are set to [1,1] and [10,10] respectively. The vehicle global path is a complete path from the start point to the end point, which is composed of a single-step planning connection between node n-1 and node n.

[0092] Get node n from node n-1, and determine whether node n is recorded in the CloseList list. If it is recorded, skip this process; if it is not recorded, use the improved A* algorithm to complete the calculation of the total cost from node n-1 to node n;

[0093] Introducing the vehicle dynamics model, improving the A* algorithm such as Figure 5 As shown, the steps are as follows:

[0094] S3.2.1. When node n is not recorded in the CloseList list, calculate the actual cost g of the improved A* algorithm for node n * (n) and the estimated cost h of the improved A* algorithm for node n * (n); the expressions are as follows:

[0095] g*(n)=g*(n-1)+(L(n-1,n)(1+M SY (n))+g R (n))

[0096] h*(n)=L(n,G)(1+0.5M S (n))

[0097] In the formula, g * (n-1) represents the actual cost of the improved A* algorithm for the neighborhood node n-1; M S (n) represents the slope information of node n; M SY(n) represents the fitness value of node n; G represents the end point; g R (n) is the cost value of the vehicle rollover stability constraint, and whether the constraint is established is defined by the following formula:

[0098] g R (n)=[v(n),θ y (n),D(n)]≤g Rmax (n)=[v max ,θ y (n),D(n)]

[0099] Where v is the speed information of the current position; D is the direction information of the unmanned vehicle during planning; when v≤v max The operation ends when , otherwise the actual cost g is output * (n);

[0100] L(n-1,n) is an improved three-dimensional Euclidean distance, and the formula is as follows:

[0101]

[0102] Where L(n-1,n) represents the improved Euclidean distance from node n-1 to n; DEM n and DEM n-1 represents the height data of node n and node n-1; μ is the distance weight, which is selected as 15 in this embodiment;

[0103] S3.2.2. The actual cost g * (n) and the estimated cost h * (n) Add to get the total cost f of the improved A* algorithm * (n), the expression is as follows:

[0104] f*(n)=g*(n)+h*(n);

[0105] S3.2.3. Get the total cost f * (n), check whether node n is recorded in the OpenList list. If it is recorded, it means that the actual cost of node n has been calculated before, and it is necessary to determine the actual cost g of this calculation. * (n) Whether it is higher than the actual cost calculated before. If it is higher than the actual cost calculated before, keep the previous value and complete the single-step planning. If it is not higher, perform an overwriting operation, overwrite the actual cost calculated before with the actual cost calculated this time, update the parent node of node n to n-1, and complete the single-step planning. Otherwise, if node n is not recorded in the OpenList list, directly record the cost values, record the parent node of node n as n-1, and complete the single-step planning.

[0106] In this way, single-step planning is performed repeatedly from the starting point [1,1] to the end point [10,10] to complete the global path planning of the improved A* algorithm;

[0107] S3.3, mapping the global path to the original map to obtain a global path;

[0108] The mapping method is as follows: two lists RoadList and MapList are established on the map planned by the improved A* algorithm at low resolution to store path information and environmental information respectively. RoadList is used to store the location information of the path in the grid map (grids with dots), and MapList stores grids that the path has not passed through (grids without dots), and the fitness value in the grid is adjusted to ∞, which is recorded as an obstacle; the position in the table is mapped to the high-resolution map according to the map scale; a grid in the low-resolution map is composed of grids at the same position in the high-resolution map. After mapping the information of the two lists to the grid map with higher resolution, the fitness values ​​of all grids at the same position in the high-resolution map of the path information remain unchanged, and the environmental information replaces the fitness values ​​of all grids at the same position with ∞; among them, the low-resolution map is the low-resolution map after two maps are obtained by the downsampling factor; the map scale is the scale obtained according to the downsampling factor, which is defined as the map scale;

[0109] like Figure 6 As shown, the blue grid corresponds to different fitness values. The darker the color, the higher the fitness value corresponding to the grid. The red dots represent the planned path, the yellow dots represent the starting point of the path, the green dots represent the end point of the path, and the purple grid represents obstacles.

[0110] Repeat S3.2 and S3.3 continuously to gradually complete the global path planning from the sub-fitness map with the lowest resolution to the original fitness map. Thus, an adaptive resolution global path planning is completed and a global path is obtained.

[0111] In this embodiment, after the global path planning of the improved A* algorithm is completed in the 10×10 sub-fitness map SYMAP4, the information needs to be mapped to the next-level 30×30 sub-fitness map SYMAP3. According to the size comparison of the two maps, the ratio is 3; after the mapping is completed, S3.2 is repeated again in the sub-fitness map SYMAP3 to complete the global path planning of the improved A* algorithm, and S3.3 is repeated to complete the information mapping of SYMAP2; until the global path planning of the improved A* algorithm in the original fitness map SYMAP is completed, since there is no fitness map, it cannot be mapped, the operation is stopped, and the global path planning of the adaptive resolution is completed to obtain a global path.

[0112] S4, such as Figure 7 As shown in the figure, the basic constraints of the vehicle are introduced by the secondary optimization method to complete the optimization of the global path;

[0113] The basic vehicle constraints include: the minimum turning radius constraint of the vehicle to increase the smoothness of the path; the overall shape constraint of the vehicle to prevent contact with obstacles; the energy consumption constraint of the vehicle to control the length of the optimized path while ensuring smoothness to avoid excessive consumption;

[0114] The optimization steps are as follows:

[0115] S4.1. Record the turning position of the vehicle in the global path;

[0116] S4.2, using a B-spline curve to optimize the global path, and recording the turning points in the curve;

[0117] S4.3. Check the curvature of the recorded location. If it is found that it does not meet the minimum turning radius constraint of the vehicle, repeat S4.1 to S4.2. When the curvature of 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.

[0118] To prove the credibility of this embodiment, the ASTER global digital elevation model V003 is still selected as the data source, and the area within the longitude and latitude of 24°N~25°N and 111°E~112°E is selected as the database for simulation analysis in Matlab.

[0119] First, a comparison is made between the improved A* algorithm and the traditional three-dimensional A* algorithm to verify the improvement of path safety of the improved A* algorithm compared with the traditional three-dimensional A* algorithm. Figure 8 Shown in the local Figure 1 The lateral load transfer ratio (LTR) of the two algorithms in mountainous terrain is compared, which means that when all tires on one side of the vehicle are off the ground, the LTR is 1, and the rollover threshold is reached at this time. The simulation results are shown in Table 1.

[0120] Table 1 Algorithm comparison results

[0121]

[0122] By analyzing Table 1 and Figure 8 It can be seen that the path planned by the traditional three-dimensional A* algorithm is very prone to rollover in mountainous terrain. The improved A* algorithm can plan a flatter and safer road compared to the traditional three-dimensional A* algorithm because it takes into account the vehicle's rollover stability.

[0123] The starting point is still set in the database (24.28°N, 111.28°E), and the end points are set to (24.61°N, 111.61°E), (24.69°N, 111.69°E), and (24.83°N, 111.83°E). Three maps with sizes of 1200×1200, 1500×1500, and 2000×2000 can be obtained for simulation. This verifies the improvement of planning efficiency of this method at different scales. Fig. 9 The present invention demonstrates the relationship between the global path planning efficiency under maps of different scales. The adaptive resolution planning method proposed in the present invention can greatly improve the planning efficiency.

[0124] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit the scope of protection of the present invention. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solution of the present invention can be modified or replaced by equivalents without departing from the essence and scope of the technical solution of the present invention.

Claims

1. A global path planning method for an unmanned vehicle considering vehicle rollover stability, characterized in that: The following steps are involved: S1. Obtain the original map based on open source data, and obtain the original fitness map SYMAP by extracting the terrain information of the original map; S2. Get the downsampling factor s based on the original fitness map SYMAP k , the sub-fitness map is obtained by downsampling factor; S3, use the improved A* algorithm to perform global path planning on the sub-fitness map, and map the global path to the original map; S4. Use the secondary optimization method to introduce basic vehicle constraints and complete the optimization of the global path.

2. The method for global path planning of an unmanned vehicle considering vehicle rollover stability according to claim 1, characterized in that: The method of extracting the terrain information of the original map is: for the digital elevation map, a difference-based method is used to obtain the elevation change rate, and the terrain information is obtained through the elevation change rate; wherein the difference method uses a third-order inverse distance square weighted difference method; The terrain information of the original map includes: ground slope information M S , ground roughness information M R and ground height difference information M H ; The way to obtain SYMAP is to assign different weight coefficients to each piece of terrain information and add them up to get the value. The specific value is calculated by the following formula: Where α1 is the ground slope information M S The weight coefficient; α2 is the surface roughness information M R The weight coefficient; α3 is the ground height difference information M H The weight coefficient of .

3. The method for global path planning of an unmanned vehicle considering vehicle rollover stability according to claim 1, characterized in that: The downsampling factor s k The way to obtain is: the original fitness map SYMAP is a map of size M×N, and the size of the original fitness map SYMAP is consistent with the size of the original map; the downsampling factor s k The downsampling factor is determined by the prime factor of the maximum value of M and N, and a number is randomly selected from the prime factor of the maximum value to obtain the downsampling factor; The downsampling factor must ensure that the number of prime factors is greater than or equal to 2. If the number of prime factors is not greater than or equal to 2, the operation is stopped; or when the size of the sub-fitness map is less than or equal to 10, the operation is stopped, even if the number of prime factors is greater than or equal to 2.

4. The method for global path planning of an unmanned vehicle considering vehicle rollover stability according to claim 1, characterized in that: The steps of using the improved A* algorithm to perform global path planning on the sub-fitness map and mapping the global path to the original map are as follows: S3.

1. In order to introduce the vehicle rollover stability constraint, a vehicle dynamics model is established to obtain the vehicle's maximum speed on the slope; The vehicle rollover stability constraint is the vehicle dynamics constraint. If the vehicle speed is too fast on a slope, it will roll over. The dynamics model is established to obtain the vehicle's maximum speed. If the vehicle speed does not exceed this maximum speed, it meets the constraint. Vehicle dynamics model input U = [θ y ,h,B,R min ], the maximum speed of the vehicle on the slope is constructed as: Where B represents the wheelbase of the vehicle; θ y represents the slope angle; g represents the acceleration due to gravity; R min represents the minimum turning radius of the vehicle; h is the height of the vehicle's center of mass; B is the vehicle's wheelbase; S3.2, use the improved A* algorithm to perform global path planning on the sub-fitness map; The steps to improve the A* algorithm are as follows: S3.2.

1. When node n is not recorded in the CloseList list, calculate the actual cost g of the improved A* algorithm for node n * (n) and the estimated cost h of the improved A* algorithm for node n * (n); the expressions are as follows: g*(n)=g*(n-1)+(L(n-1,n)(1+M SY (n))+g R (n)) h*(n)=L(n,G)(1+0.5M S (n)) Where M S (n) represents the slope information of node n; M SY (n) represents the fitness value of node n; G represents the end point; g R (n) is the cost value of the vehicle rollover stability constraint, and whether the constraint is established is defined by the following formula: g R (n)=[v(n),θ y (n),D(n)]≤g Rmax (n)=[v max ,θ y (n),D(n)] Where v is the speed information of the current position; D is the direction information of the unmanned vehicle during planning; when v≤v max The operation ends when , otherwise the actual cost g is output * (n); L(n-1,n) is an improved three-dimensional Euclidean distance, and the formula is as follows: Where L(n-1,n) represents the improved Euclidean distance from node n-1 to n; DEM n and DEM n-1 Represents the height data of node n and node n-1; μ is the distance weight; S3.2.

2. The actual cost g * (n) and the estimated cost h * (n) Add to get the total cost f of the improved A* algorithm * (n), the expression is as follows: f*(n)=g*(n)+h*(n); S3.2.

3. Get the total cost f * (n), check whether node n is recorded in the OpenList list. If it is recorded, it means that the actual cost of node n has been calculated before, and it is necessary to determine the actual cost g of this calculation. * (n) Whether it is higher than the actual cost calculated previously. If it is higher than the actual cost calculated previously, the previous value is retained and the single-step planning is completed; If it is not higher, then perform an overwriting operation, overwrite the actual cost calculated previously with the actual cost calculated this time, update the parent node of node n to n-1, and complete this single-step planning; On the contrary, if node n is not recorded in the OpenList list, directly record the cost values, record the parent node of node n as n-1, and complete this single-step planning; S3.

3. Map the global path to the original map to obtain a global path.

5. The method for global path planning of an unmanned vehicle considering vehicle rollover stability according to claim 1, characterized in that: The method of secondary optimization is used to introduce basic vehicle constraints to optimize the global path; The basic constraints of the vehicle include: the minimum turning radius constraint of the vehicle; the overall shape constraint of the vehicle; the energy consumption constraint of the vehicle; The optimization steps are as follows: S4.

1. Record the turning position of the vehicle in the global path; S4.2, using a B-spline curve to optimize the global path, and recording the turning points in the curve; S4.

3. Check the curvature of the recorded location. If it is found that it does not meet the minimum turning radius constraint of the vehicle, repeat S4.1 to S4.

2. When the curvature of all turns meets the minimum turning radius constraint of the vehicle, stop the optimization.

Images