A global path planning method for unmanned vehicle considering vehicle rollover stability

Abstract

The application provides a kind of unmanned vehicle global path planning method considering vehicle rollover stability, belong to unmanned vehicle path planning technical field, method includes: according to open source data obtains original map, establishes fitness map by extracting topographic information of original map by fusing various environmental information;According to the fitness map, obtain the subsampling factor, get the sub-fitness map through the subsampling factor;Global path planning is carried out to the sub-fitness map using improved A* algorithm, and the global path is mapped to the original map;Introduce vehicle basic constraint by using the method of quadratic optimization, complete the optimization of global path.Compared with the prior art, the vehicle rollover stability considered in the application can plan a more flat and safer road compared to the traditional three-dimensional A* algorithm.

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 rollover stability. BACKGROUND

[0002] With the development of automatic driving technology, the global path planning problem of vehicles in complex environments becomes particularly important. In the field of autonomous vehicles, there are five major modules in total: perception, positioning, planning, decision-making, and control. As a part of planning, path planning plays a connecting role and provides a safe and efficient navigation solution for unmanned vehicles. In the field of unmanned vehicles, global path planning is based on maps and positioning systems to determine the overall route from the starting point to the ending point, providing a basic path for subsequent local path planning and ensuring 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 the planning efficiency of global path planning for intelligent unmanned vehicles. The constraints of vehicle dynamics, especially the rollover stability constraints of vehicles, have a significant impact on the stability and passability of vehicles in the planning quality of paths in unstructured environments. Unreasonable path planning may cause safety accidents such as vehicle rollover and overturning. The traditional three-dimensional A* algorithm ignores the rollover stability constraints of vehicles, and unmanned vehicles are prone to rollover accidents on the planned road. Therefore, it is quite challenging to find a method that ensures the safety of global paths while improving the efficiency of global path planning. SUMMARY

[0004] To solve the above problems, the application provides a global path planning method for unmanned vehicles considering vehicle rollover stability.

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

[0006] S1, obtaining an original map according to open source data, and obtaining an original fitness map SYMAP by extracting topographic information of the original map;

[0007] The extraction method includes: for a digital elevation map, using a differential-based method to obtain an elevation change rate, and obtaining topographic information through the elevation change rate;

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

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

[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 SYMAP is obtained in the following manner: different weight coefficients are assigned to each terrain information, and the sum is obtained as a value; wherein the value is the fitness value M SY of the unmanned vehicle passing through the terrain recorded by each grid of the SYMAP, and the specific value is calculated by the following formula:

[0012]

[0013] In the formula, α1 is the weight coefficient of the ground slope information M S , α2 is the weight coefficient of the ground roughness information M R , and α3 is the weight coefficient of the ground height difference information M H .

[0014] S2, obtaining a down-sampling factor s k from the original fitness map SYMAP, and obtaining a sub-fitness map through the down-sampling factor;

[0015] The down-sampling factor is obtained in the following manner: the original fitness map SYMAP is a map with a size of M×N, and the size of the original fitness map SYMAP is consistent with that of the original map; the down-sampling factor s k is determined by the prime factors of the maximum value of M and N, and a number is randomly selected from the prime factors of the maximum value to obtain the down-sampling factor; the down-sampling factor needs to ensure that the number of prime factors is greater than or equal to 2, if the number of prime factors is less than 2, the operation is stopped; or when the size of the sub-fitness map is less than or equal to 10, the operation is also stopped, that is, even if the number of prime factors is greater than or equal to 2, the operation is also stopped;

[0016] The size of each layer of the sub-fitness map after down-sampling is calculated by the following formula:

[0017]

[0018] In the formula, s k represents the down-sampling factor, wherein k represents the kth layer.

[0019] The kth layer of the sub-fitness map SYMAPk The generation should comply with the following expression:

[0020]

[0021] In the formula, (i, j) and (x, y) represent the coordinates in the sub-fitness map and the original fitness map respectively; min(ixs k , M) and min(jxs k , N) represent the upper limits of the boundaries of x and y respectively, used to control the boundaries to 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 square matrix with a size equal to s k ; and the sorting is completed according to the size of the map.

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

[0023] S3.1, to introduce the vehicle rollover stability constraint, a vehicle dynamics model is established to obtain the limit speed of the vehicle on the slope;

[0024] The vehicle rollover stability constraint is the vehicle dynamics constraint. The vehicle will roll over if the speed is too fast on the slope. The dynamics model is established to obtain the limit speed of the vehicle. If the speed does not exceed the limit speed, the constraint is met.

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

[0026]

[0027] In the formula, B represents the wheel track of the vehicle; θ y represents the slope angle; g represents the acceleration of gravity; R min represents the minimum turning radius of the vehicle; when the vehicle is driving on the slope, the speed does not exceed the limit speed, and the constraint is met.

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

[0029] The vehicle global path is a complete path from the start point to the end point, which is connected by one-step planning of the algorithm in the map at the node n-1 and its neighborhood node n; the neighborhood node n is obtained from the current node n-1, whether the node n is recorded in the CloseList list is judged, if it has been recorded, the process is skipped; if it has not been recorded, the total cost of the node n-1 to the node n is calculated by the improved A* algorithm, the vehicle dynamics model is introduced, and the specific calculation formula is calculated by the following formula:

[0030] S3.2.1, when the node n is not recorded in the CloseList list, the actual cost g * (n) of the improved A* algorithm of the node n and the estimated cost h * (n) of the improved A* algorithm of the node n are calculated respectively; 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] In the formula, M S (n) represents the slope information of the node n; M SY (n) represents the fitness value of the node n; G represents the end point; g R (n) is the value of the vehicle rollover stability constraint, which is defined by the following formula whether the constraint is established:

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

[0035] In the formula, v is the speed information of the current position; D is the direction information when the unmanned vehicle is planned; when v≤v max , the operation is ended, otherwise the actual cost g * (n) is output;

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

[0037]

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

[0039] S3.2.2, add the actual cost g * (n) and the estimated cost h * (n) to obtain the total cost f * (n) of the improved A* algorithm, expressed as follows:

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

[0041] S3.2.3, obtain the total cost f * (n) and check whether the node n is recorded in the OpenList list, if recorded, it means that the actual cost of the node n has been calculated before, and it is necessary to judge whether the actual cost g * (n) calculated this time is higher than the actual cost calculated before, if higher, the previous value is retained, and the single-step planning this time is completed; if not higher, the covering operation is performed, the actual cost calculated this time is covered to the actual cost calculated before, the parent node of the node n is updated to n-1, and the single-step planning this time is completed; otherwise, if the node n is not recorded in the OpenList list, the values of the respective costs are directly recorded, the parent node of the node n is recorded as n-1, and the single-step planning this time is completed.

[0042] The single-step planning from the starting point to the ending point is repeatedly performed, and the global path planning of the improved A* algorithm is completed;

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

[0044] The mapping manner is that: two lists RoadList and MapList are established for the map planned by the improved A* algorithm on the low resolution, which respectively store the path information and the environment information, the RoadList is used to store the position information of the path in the grid map, the MapList stores the grid which is not passed through by the path, and the fitness value in the grid is adjusted to ∞, which is recorded as an obstacle; the positions in the table are mapped to the high resolution map according to the map scale; one grid in the low resolution map is composed of grids in the high resolution map at the same position, after the information of the two lists is mapped to the grid map with higher resolution, the fitness values of all the grids at the same position in the high resolution map remain unchanged for the path information, and the fitness values of all the grids at the same position are replaced by ∞ for the environment information; wherein, the low resolution map is obtained by two maps through the downsampling factor, the map with lower resolution is the low resolution map; the map scale is obtained according to the downsampling factor, and is defined as the map scale;

[0045] S3.2 and S3.3 are repeatedly executed, and the global path planning from the lowest resolution sub-fitness map to the original fitness map is gradually completed, and one adaptive resolution global path planning is completed, and one global path is obtained.

[0046] S4, a method of quadratic optimization is used to introduce vehicle basic constraints, and optimization of the global path is completed.

[0047] The vehicle basic 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; and the energy consumption constraint of the vehicle to control the length of the optimized path to avoid excessive consumption under the condition of smoothness.

[0048] The optimization steps are as follows:

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

[0050] S4.2, B-spline curve is used to complete the optimization of the global path, and the turning position in the curve is recorded.

[0051] S4.3, the curvature of the recorded position is checked, if the minimum turning radius constraint of the vehicle is found, S4.1~S4.2 is re-executed, when the curvature of all turning positions meets the minimum turning radius constraint of the vehicle, the optimization is stopped, and the final result, i.e. the optimized global path, is obtained.

[0052] The beneficial effects of the present application are:

[0053] The present application introduces the vehicle dynamics constraint condition into the improved A* algorithm, establishes a vehicle dynamics model, introduces the vehicle rollover stability constraint, improves the safety and feasibility of path planning, and then uses an adaptive resolution planning method to improve the planning efficiency of the improved A* algorithm, so that the safety and planning efficiency of the path are considered, and a more reliable and efficient solution is provided for the path planning of unmanned vehicles. BRIEF DESCRIPTION OF DRAWINGS

[0054] Figure 1 The method flowchart of the present application is shown in the figure;

[0055] Figure 2 The fitness map is shown in the figure;

[0056] Figure 3 The relationship between the original fitness map and the sub-fitness map is shown in the figure;

[0057] Figure 4 The vehicle dynamics modeling is shown in the figure;

[0058] Figure 5 The improved A* algorithm based on the single-step planning flowchart of the fitness map is shown in the figure;

[0059] Figure 6 A low-resolution map planning backward high-resolution map information mapping schematic diagram;

[0060] Figure 7 A secondary optimization flow chart;

[0061] Figure 8 A traditional three-dimensional A* algorithm and improved A* algorithm LTR comparison chart;

[0062] Figure 9 The planning method of the application changes with the relationship diagram of different sizes of maps. DETAILED DESCRIPTION

[0063] The application will be further described in detail below with specific embodiments.

[0064] As Figure 1 shown, a global path planning method for unmanned vehicles considering vehicle rollover stability, comprising the following steps:

[0065] S1, obtaining an original map according to open source data, and obtaining an original fitness map SYMAP by extracting terrain information of the original map;

[0066] The extraction method includes: for a digital elevation map, using a differential-based method to obtain an elevation change rate, and obtaining terrain information from the elevation change rate;

[0067] In this embodiment, the open source data used comes from ASTER Global Digital Elevation Model V003, and the part with a starting point of (24.28°N, 111.28°E) and an ending point of (24.53°N, 111.53°E) is selected as the original map of this embodiment, denoted as Map Figure 1 , and the size of the obtained prior map is 900x900; the extraction method is: using a differential-based method to obtain an elevation change rate, and obtaining terrain information from the elevation change rate; as Figure 2 shown;

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

[0069] The terrain information is obtained in the following way: the ratio of the ground surface area to the projection area is obtained from the elevation change rate, and the ratio of the ground surface area to the projection 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 elevation difference information.

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

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

[0072]

[0073] In the formula, α1 is the weight coefficient of the ground slope information M S ; α2 is the weight coefficient of the ground roughness information M R ; and α3 is the weight coefficient of the ground elevation difference information M H .

[0074] S2, obtaining a down-sampling factor s k from the original fitness map SYMAP, to obtain a sub-fitness map;

[0075] The way to obtain the down-sampling factor is that the original fitness map SYMAP is a map with a size of M×N, and the size of the original fitness map SYMAP is consistent with that of the original map; the down-sampling factor s k is determined by the prime factors of the maximum value of the two values of M and N, and a number is randomly selected from the prime factors of the maximum value to obtain the down-sampling factor; the down-sampling factor needs to ensure that the number of prime factors is greater than or equal to 2, and if the number of prime factors is less than 2, the operation is stopped; or when the size of the sub-fitness map is less than or equal to 10, the operation is also stopped, that is, even if the number of prime factors is greater than or equal to 2, the operation is also stopped;

[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 down-sampling factor s1 is obtained, and a sub-fitness map SYMAP1 of the original fitness map SYMAP is obtained through the down-sampling 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 down-sampling factor s2 is obtained, and a sub-fitness map SYMAP2 of the original fitness map SYMAP is obtained through the down-sampling factor s2; when the sub-fitness map does not satisfy the condition 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, even if the number of prime factors is greater than or equal to 2, the operation is also stopped;

[0077] The size of each layer of the sub-fitness map after down-sampling is calculated by the following formula:

[0078]

[0079] In the formula, sk where k represents the kth layer, and in the present embodiment, k = 4;

[0080] A part of the sub-fitness map is created as shown in Figure 3 The same color grids in SYMAP1 and SYMAP2 are generated from the same color grids of the original fitness map SYMAP. From the original fitness map SYMAP to the kth layer sub-fitness map SYMAP k The generation should comply with the following expression:

[0081]

[0082] where (i, j) and (x, y) represent the coordinates in the sub-fitness map and the original fitness map, respectively; min(i x s k , M) and min(j x s k , N) represent the upper limits of the boundaries of x and y, respectively, for controlling the boundaries to ensure that the integrated extraction does not exceed the range of the original fitness map SYMAP; w is a weighting matrix, which is a square matrix with a size equal to s k According to the map size, the sorting is completed;

[0083] In the present embodiment, the prior map size is 900 x 900, so M = 900 and N = 900, and a 900 x 900 original fitness map can be obtained. A set of down-sampling factors: [s1 = 5, s2 = 3, s3 = 2, s4 = 3] are obtained from the original fitness map, and four sub-fitness maps with sizes of [SYAMP1: 180 x 180, SYMAP2: 60 x 60, SYMAP3: 30 x 30, SYMAP4: 10 x 10] are obtained.

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

[0085] S3.1, to introduce the vehicle rollover stability constraint, a vehicle dynamics model is established to obtain the limit speed of the vehicle on the slope;

[0086] The vehicle rollover stability constraint is the vehicle dynamics constraint. The vehicle will roll over if the speed is too fast on the slope. The limit speed of the vehicle is obtained by establishing a dynamics model. If the speed does not exceed the limit speed, the constraint is met.

[0087] The specific model is shown in Figure 4 , where m is the vehicle mass, a y is the lateral acceleration, g is the gravitational acceleration, f y is the friction force generated by the slope, and θ yis the slope angle, h is the vehicle mass center height, F zi is the normal support force of the ground to the inner wheel, B is the vehicle wheel base; the roll moment balance equation is established by the dynamics model, and the vehicle dynamics model input U = [θ y , h, B, R min ] is obtained, and the limit speed of the vehicle on the slope is constructed as:

[0088]

[0089] In the formula, B represents the vehicle wheel base; θ y represents the slope angle; g represents the acceleration of gravity; R min represents the minimum turning radius of the vehicle; when the vehicle is running on the slope, the vehicle speed does not exceed the limit speed, which meets the constraint;

[0090] S3.2, the improved A* algorithm is used for global path planning of the sub fitness map;

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

[0092] From node n-1, node n is obtained, and it is judged 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 calculation from node n-1 to node n is completed by the improved A* algorithm;

[0093] The vehicle dynamics model is introduced, and the improved A* algorithm is improved as shown in Figure 5 , and the steps are as follows:

[0094] S3.2.1, when node n is not recorded in the CloseList list, the actual cost g * (n) of the improved A* algorithm of node n and the estimated cost h * (n) of the improved A* algorithm of node n are calculated respectively; 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 of 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 the constraint is defined as follows:

[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 End the operation when , otherwise output the actual cost g * (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 estimated cost h * (n) Add up 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. Obtain 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) Is it higher than the actual cost calculated previously? If so, keep the previous value and complete the current single-step planning. If not, 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 the current single-step planning. Otherwise, if node n is not recorded in the OpenList, directly record the cost values, record the parent node of node n as n-1, and complete the current single-step planning.

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

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

[0108] The mapping manner is that: the map planned by the improved A* algorithm at the low resolution is used to establish two lists RoadList and MapList, which respectively store path information and environment information, RoadList is used to store the position information (the grid where the circle exists) of the path in the grid map, MapList stores the grid where the path does not pass (the grid where the circle does not exist), and the fitness value in the grid is adjusted to be infinite, which is recorded as an obstacle; the positions in the table are mapped to the high-resolution map according to the map scale; one grid in the low-resolution map is composed of grids in the high-resolution map at the same position, after the information of the two lists is mapped to the grid map with higher resolution, the fitness values of all grids at the same position in the high-resolution map remain unchanged, and the environment information replaces the fitness values of all grids at the same position with infinite; wherein, the low-resolution map is obtained by two maps through a down-sampling factor, and the map with lower resolution is the low-resolution map; the map scale is obtained according to the down-sampling factor, which is defined as the map scale;

[0109] As shown in FIG. 3, the blue grids correspond to different fitness values, the darker the color, the higher the fitness value corresponding to the grid, the red circle represents the planned path, the yellow circle represents the starting point of the path, the green circle represents the ending point of the path, and the purple grid represents the obstacle; Figure 6 S3.2 and S3.3 are repeatedly performed, and the global path planning from the lowest-resolution sub-fitness map to the original fitness map is gradually completed, and thus one global path planning at an adaptive resolution is completed, and one global path is obtained;

[0110] In this embodiment, after the global path planning of the improved A* algorithm is completed on the 10x10 sub-fitness map SYMAP4, the information needs to be mapped to the next level 30x30 sub-fitness map SYMAP3. According to the size comparison of the two maps, the scale is 3; after the mapping is completed, S3.2 is repeatedly performed on the sub-fitness map SYMAP3, and the global path planning of the improved A* algorithm is completed once, and S3.3 is repeatedly performed to complete the information mapping of SYMAP2; until the global path planning of the improved A* algorithm on the original fitness map SYMAP is completed, since there is no fitness map, the mapping cannot be performed, and the operation is stopped, and one global path planning at an adaptive resolution is completed, and one global path is obtained.

[0111] ​

[0112] S4, such as Figure 7 As shown in the figure, the quadratic optimization method is used to introduce basic vehicle constraints to optimize the global path.

[0113] Basic vehicle constraints include: a minimum turning radius constraint to increase path smoothness; a vehicle shape constraint to prevent contact with obstacles; and a vehicle energy consumption constraint 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. Use the B-spline curve to optimize the global path and record the turns in the curve;

[0117] S4.3. Check the curvature of the recorded points. If it is found that it does not meet the vehicle's minimum turning radius constraint, repeat S4.1 to S4.2. When the curvature of all turns meets the vehicle's minimum turning radius constraint, 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 range 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 security of the improved A* algorithm compared with the traditional three-dimensional A* algorithm. Figure 8 Showing local Figure 1 The lateral load transfer ratio (LTR) values ​​of the two algorithms in mountainous terrain are compared. This means that the LTR is 1 when all tires on one side of the vehicle are off the ground, reaching the rollover threshold. 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 flatter and safer roads compared to the traditional three-dimensional A* algorithm because it takes into account the vehicle's rollover stability.

[0123] The position of the starting point is still set in the database (24.28 °N, 111.28 °E), and the end points are set as (24.61 °N, 111.61 °E), (24.69 °N, 111.69 °E), and (24.83 °N, 111.83 °E) respectively, so that three maps with sizes of 1200x1200, 1500x1500, and 2000x2000 are obtained for simulation. The improvement of the planning efficiency of the method under different sizes is verified. Figure 9 The relationship between the global path planning efficiency of the present application and the map size is shown. The adaptive resolution planning method proposed in the present application can greatly improve the planning efficiency.

[0124] Finally, it should be noted that the above examples are only used to illustrate the technical solutions of the present application and do not limit the scope of protection of the present application. Although the present application has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solutions of the present application can be modified or replaced equivalently without departing from the essence and scope of the technical solutions of the present application.

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. The steps are as follows: S3.

1. To introduce vehicle rollover stability constraints, establish a vehicle dynamics model to obtain the vehicle's maximum speed on a slope. The vehicle rollover stability constraint is the vehicle dynamics constraint. If the vehicle is traveling too fast on a slope, it will roll over. A dynamics model is established to obtain the vehicle's maximum speed. If the vehicle 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 vehicle wheelbase; θ y represents the slope angle; g represents the acceleration due to gravity; R min Indicates 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 the constraint is defined as follows: g R (n)=[v(n),θ y (n),D(n)]≤g Rmax (n)=[v max ,θ y (n),D(n)] Where v(n) is the vehicle speed information of the current position of node n; θ y (n) represents the slope angle of node n; D(n) is the direction information of the unmanned vehicle planning at node n; when v≤v max End the operation when , otherwise output the actual cost g * (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; i n Represents the horizontal coordinate of node n in the sub-fitness map; j n Represents the vertical coordinate of node n in the sub-fitness map; S3.2.2、The actual cost g * (n) and estimated cost h * (n) Add up 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. Obtain 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, directly record the cost values, record the parent node of node n as n-1, and complete the single-step planning; S3.3, mapping the global path to the original map to obtain a global path; S4. Use the secondary optimization method to introduce basic vehicle constraints and complete the optimization of the global path.

2. The global path planning method for 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 as follows: 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 adopts 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 together 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 of α2 is the ground roughness information M R The weight coefficient; α3 is the ground height difference information M H The weight coefficient of .

3. The global path planning method for an unmanned vehicle considering vehicle rollover stability according to claim 1, characterized in that: The downsampling factor s k The way to get it 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; 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 will be stopped; or when the size of the sub-fitness map is less than or equal to 10, the operation will be stopped even if the number of prime factors is greater than or equal to 2.

4. The global path planning method for an unmanned vehicle considering vehicle rollover stability according to claim 1, characterized in that: The quadratic optimization method 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. Use the B-spline curve to optimize the global path and record the turns in the curve; S4.

3. Check the curvature of the recorded points. 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