A trajectory planning method based on dynamic selection under different situations

Abstract

This invention discloses a trajectory planning method based on dynamic selection of different situations. It loads a digital elevation map (DEM) of a selected area, records the matrix of the DEM with m rows and n columns, and reads relevant parameters of the DEM. The DEM is then converted to grayscale to obtain a grayscale map (map1). A threat area is manually designated to form a threat map (map2). A starting point Ps and an ending point Pe are selected on the DEM for trajectory planning. Based on the parent node Open.k in the Open table, the parent node of each node is found starting from Pe until the starting point Ps is found, generating the final trajectory. By optimizing the data structure and corresponding the parameters of the Open table with the map parameters, the computational load of the trajectory planning algorithm is reduced. Furthermore, the increase in overall computational load after adding other situational weights is minimal, facilitating airborne implementation and solving the technical problem of excessively long computation time in existing trajectory planning methods.

Description

Technical Field

[0001] This invention belongs to the field of aircraft and relates to a trajectory planning method, specifically a trajectory planning method based on dynamic selection under different situations. Background Technology

[0002] With the development of modern technology, especially aircraft technology, relying solely on manual operation to pilot aircraft can no longer meet people's needs, leading to the emergence of aircraft trajectory planning technology. Aircraft trajectory planning involves comprehensively considering factors such as arrival time, fuel consumption, threats, and flight area to plan the optimal or satisfactory flight trajectory for an aircraft, ensuring the successful completion of flight missions. Global trajectory planning methods utilize computer technology to analyze and process digital maps to find the optimal trajectory suitable for aircraft flight, including A... Algorithms, such as Dijkstra's algorithm, are used to plan and navigate along a global path. During flight, factors such as flight time, threat sources, flight area, and surrounding environment must be considered. Furthermore, the planned path must be optimal, with minimal time consumption and a short flight path. Currently, there are many path planning and navigation algorithms available, such as A... The algorithm is one of the most representative heuristic algorithms. However, traditional A... The algorithm has a large computational load, and its computational load increases exponentially with the increase of map accuracy. It can only obtain the shortest route distance, which cannot meet the actual needs of different mission situations such as route stability, aircraft flight area (mountain peak / valley), and threat avoidance area. Therefore, a route planning method based on dynamic selection of different situations is proposed. Summary of the Invention

[0003] To address the shortcomings of existing technologies, the present invention aims to provide a trajectory planning method based on dynamic selection under different situations, thereby solving the technical problems of excessively long calculation time and the inability to reasonably adjust the calculated trajectory according to the mission situation in existing trajectory planning methods.

[0004] To solve the above-mentioned technical problems, the present invention adopts the following technical solution:

[0005] A trajectory planning method based on dynamic selection under different situations specifically includes the following steps:

[0006] Step 1: Load the digital elevation map of the selected area. Record digital elevation maps The matrix has m rows and n columns, and a digital elevation map is read. Relevant parameters;

[0007] The relevant parameters include distance weights. Height weight (HW), slope weight (SW), and threat weight (TW);

[0008] Step 2, digital elevation map The resulting grayscale image is obtained by performing grayscale conversion. Threat zones are manually designated to create a threat map. ;

[0009] Step 3, in the digital elevation map Select the starting point Ps and the ending point Pe for the trajectory planning;

[0010] The coordinates of the starting point on the digital elevation map are: The coordinates of the endpoint on the digital elevation map are: ;

[0011] Step 4: Create the Open table and initialize its parameters;

[0012] The parameters in the Open table include , and ;

[0013] initialization , Let each matrix be m x n, and each element in the matrix be an infinite value; initialize... Given an m x n matrix, where each element has a value of 0;

[0014] Step 5, Set the current point Coordinates are Assign the coordinates of the starting point Ps to the current point. .

[0015] Set current point The cost g is 0, that is .

[0016] Calculate the current point according to formula (1) Estimated cost to reach the destination Pe The calculation formula is as follows:

[0017]

[0018] The cost f of the current point is calculated according to formula (2):

[0019]

[0020] Step 6: Determine the point with the minimum cost f in the Open table and set it as the current point. and the current point The cost g is given , will the current point The cost f is changed to a null value;

[0021] in:

[0022] Let g represent the cost from the starting point to the current point;

[0023] Step 7, determine the current point Is it the endpoint? and This indicates the current point If the destination is found, proceed to step nine; otherwise, proceed to step eight.

[0024] Step 8: Calculate the current point The calculation results are assigned to the Open table, and the values ​​of the eight points in the eight-neighborhood are recorded. This is the parent node of these eight points. Current point. After the value calculation of the eight neighboring points is completed, return to step six and repeat steps six to eight until the track from the starting point to the end point is found.

[0025] Step 9, based on the parent node in the Open table Starting from Pe, find the parent node of each node until the starting point Ps is found, and generate the final track.

[0026] This invention also includes the following technical features:

[0027] In step three, in the threat map map2, the value of points within the threat zone is 1, and the value of points outside the threat zone is 0.

[0028] In step eight, the eight neighborhoods of the current point are processed and calculated to obtain the area surrounding the current point. The value is 8 points. This includes the following steps.

[0029] Step 8.1, calculate the coordinates of point p in one loop:

[0030]

[0031]

[0032]

[0033]

[0034]

[0035] in:

[0036] The coordinates of point p are ;

[0037] This represents the number of iterations in step eight, and also represents the eight neighborhoods, i.e. ;

[0038] Step 8.2, calculate the cost value, using the following formula:

[0039] The current point is calculated according to formula 5. The cost g to reach a point p in the eight neighborhood is denoted as g2;

[0040]

[0041]

[0042] in:

[0043] For point Time The distance cost;

[0044] The point is calculated according to formula 7. The cost g;

[0045]

[0046] The point is calculated according to formula 8. The cost f;

[0047]

[0048] Step 8.3, determine the point Check if the cost f in the Open table is infinite. If not, the point is in the Open table, and proceed to step 8.4; if it is, the point is not in the Open table, and proceed to step 8.5.

[0049] Step 8.4, compare the cost g of point p with... The size, if If the condition is met, proceed to step 8.5; otherwise, proceed to step 8.6.

[0050] Step 8.5: Assign the cost f and cost g of point p to the corresponding positions in the Open table, and record point p0 as the parent node of point p, then proceed to step 8.6.

[0051]

[0052]

[0053] Step 8.6, make Return to step 8.1 and repeat step 8 until all eight neighboring points of the current point p0 have been calculated.

[0054] Compared with the prior art, the beneficial technical effects of this invention are:

[0055] (I) In this invention, by optimizing the data structure and corresponding the parameters of the Open table with the map parameters, the computational load of the trajectory planning algorithm is reduced. At the same time, the increase in the overall computational load of the algorithm after adding other situational weights is very small, which facilitates airborne implementation and solves the technical problem of excessively long computation time in existing trajectory planning methods.

[0056] (II) In this invention, different situations such as distance, altitude, slope, and threat are incorporated into the calculation of trajectory planning costs, which enables the planned trajectory to meet the requirements of different situations and solves the problem that the trajectory calculated by the existing trajectory planning method cannot be reasonably adjusted according to the mission situation.

[0057] (III) In this invention, digital elevation maps and slope maps are used as the basis for the trajectory planning algorithm. The calculated trajectory can meet the requirements of different terrains and there is no situation where there is no solution. Attached Figure Description

[0058] Figure 1 This is a top view of mode 1 in the embodiment;

[0059] Figure 2 This is the height diagram of mode 1 in the embodiment;

[0060] Figure 3 This is a top view of mode 2 in the embodiment;

[0061] Figure 4 This is the height diagram of mode 2 in the embodiment;

[0062] Figure 5 This is a top view of mode 3 in the embodiment;

[0063] Figure 6 This is the height diagram of mode 3 in the embodiment;

[0064] Figure 7 This is a top view of mode 4 in the embodiment;

[0065] Figure 8 This is the height diagram of mode 4 in the embodiment.

[0066] The specific content of the present invention will be further explained in detail below with reference to the embodiments. Detailed Implementation

[0067] The three cost values ​​in this method are cost g, cost f, and cost h. Cost g is the cost value from the starting point to the current point; cost h is the estimated cost value from the current point to the end point; and cost f is the total cost value at the current point, which is the sum of cost g and cost h.

[0068] It should be noted that, unless otherwise specified, all components in this invention are those known in the art.

[0069] The following are specific embodiments of the present invention. It should be noted that the present invention is not limited to the following specific embodiments. All equivalent modifications made based on the technical solutions of this application fall within the protection scope of the present invention.

[0070] This invention provides a trajectory planning method based on dynamic selection under different situations, specifically including the following steps:

[0071] Step 1: Load the digital elevation map of the selected area. Record digital elevation maps The matrix has m rows and n columns, and a digital elevation map is read. Relevant parameters;

[0072] The relevant parameters include distance weights. Height weight (HW), slope weight (SW), and threat weight (TW);

[0073] Step 2, digital elevation map The resulting grayscale image is obtained by performing grayscale conversion. Threat zones are manually designated to create a threat map. ;

[0074] Step 3, in the digital elevation map Select the starting point Ps and the ending point Pe for the trajectory planning;

[0075] The coordinates of the starting point on the digital elevation map are: The coordinates of the endpoint on the digital elevation map are: ;

[0076] Step 4: Create the Open table and initialize its parameters;

[0077] The parameters in the Open table include , and ;

[0078] initialization , Let each matrix be m x n, and each element in the matrix be an infinite value; initialize... Given an m x n matrix, where each element has a value of 0;

[0079] Step 5, Set the current point Coordinates are Assign the coordinates of the starting point Ps to the current point. ;

[0080] Set current point The cost g is 0, that is ;

[0081] Calculate the current point according to formula (1) Estimated cost to reach the destination Pe The calculation formula is as follows:

[0082]

[0083] The cost f of the current point is calculated according to formula (2):

[0084]

[0085] Step 6: Determine the point with the minimum cost f in the Open table and set it as the current point. and the current point The cost g is given , will the current point The cost f is changed to a null value;

[0086] in:

[0087] Let g represent the cost from the starting point to the current point;

[0088] Step 7, determine the current point Is it the endpoint? and This indicates the current point If the destination is found, proceed to step nine; otherwise, proceed to step eight.

[0089] Step 8: Calculate the current point The calculation results are assigned to the Open table, and the values ​​of the eight points in the eight-neighborhood are recorded. The parent node of these eight points, the current point After the value calculation of the eight neighboring points is completed, return to step six and repeat steps six to eight until the track from the starting point to the end point is found.

[0090] Step 9, based on the parent node in the Open table Starting from Pe, find the parent node of each node until the starting point Ps is found, and generate the final track.

[0091] In the above technical solution, distance weight The sum of the weight coefficients for altitude (HW), slope (SW), and threat (TW) is 1. To find the shortest flight path, increase the proportion of distance weight; to make the aircraft fly in valleys, increase the proportion of altitude weight; to make the flight path smoother, increase the proportion of slope weight; and to improve flight safety and completely avoid threat areas, increase the proportion of threat weight. The default weight values ​​for all four are 0.25. You can adjust the proportion of a weight by increasing one weight value while correspondingly decreasing the others.

[0092] By optimizing the data structure and correlating the parameters of the Open table with map parameters, the computational load of the trajectory planning algorithm is reduced. Simultaneously, the increase in overall computational load after adding other situational weights is minimal, facilitating airborne implementation and resolving the technical problem of excessively long computation times in existing trajectory planning methods. Incorporating different situations such as distance, altitude, slope, and threat into the trajectory planning cost calculation ensures that the planned trajectory meets the requirements of various situations, addressing the issue that existing trajectory planning methods cannot reasonably adjust trajectories based on mission situation. Using digital elevation maps and slope maps as the basis for the trajectory planning algorithm, the calculated trajectory can meet different terrain requirements, eliminating unsolvable cases.

[0093] Specifically, in step three, in the threat map map2, the value of points within the threat zone is 1, and the value of points outside the threat zone is 0.

[0094] Specifically, in step eight, the eight neighborhoods of the current point are processed and calculated to obtain the area surrounding the current point. The 8-point cost value specifically includes the following steps:

[0095] Step 8.1, calculate the coordinates of point p in one loop:

[0096]

[0097]

[0098]

[0099]

[0100]

[0101] in:

[0102] The coordinates of point p are ;

[0103] This represents the number of iterations in step eight, which is equivalent to representing eight neighborhoods. ;

[0104] Step 8.2, calculate the cost value, using the following formula:

[0105] The current point is calculated according to formula 5. The cost g to reach a point p in the eight neighborhood is denoted as g2;

[0106]

[0107]

[0108] in:

[0109] For point Time The distance cost;

[0110] The point is calculated according to formula 7. The cost g;

[0111]

[0112] The point is calculated according to formula 8. The cost f;

[0113]

[0114] Step 8.3, determine the point Check if the cost f in the Open table is infinite. If not, the point is in the Open table, and proceed to step 8.4; if it is, the point is not in the Open table, and proceed to step 8.5.

[0115] Step 8.4, compare the cost g of point p with... The size, if If the condition is met, proceed to step 8.5; otherwise, proceed to step 8.6.

[0116] Step 8.5: Assign the cost f and cost g of point p to the corresponding positions in the Open table, and record point p0 as the parent node of point p, then proceed to step 8.6.

[0117]

[0118]

[0119] Step 8.6, make Return to step 8.1 and repeat step 8 until all eight neighboring points of the current point p0 have been calculated.

[0120] Example 1:

[0121] Using the above technical solution, a flight path planning test was conducted in a certain area. The map parameters are shown in Table 1. In the map, dark areas represent areas with steep slopes, and light areas represent areas with gentle slopes.

[0122] Table 1 Map information for a certain region

[0123]

[0124] Modality 1: When the pilot needs to reach the target area in the shortest time, requiring the shortest planned flight path, the distance weight is set to 1, and all other weights are set to 0. The calculated flight path is as follows: Figure 1 As shown, the height map is as follows Figure 2 As shown.

[0125] Mode 2: When the pilot needs to perform low-altitude penetration, i.e., fly close to the ground, and the planned flight path is required to pass through a valley area, then the distance weight is set to 0.5 and the altitude weight is set to 0.5. The calculated flight path is as follows: Figure 3 As shown, the height map is as follows Figure 4 As shown.

[0126] Mode 3: During a pilot's low-altitude penetration maneuver, flight stability and safety are required, meaning a more stable and gentler trajectory is needed. Therefore, a distance weight of 0.5, a bank angle weight of 0.25, and an altitude weight of 0.25 are set. The calculated trajectory is as follows: Figure 5 As shown, the height map is as follows Figure 6 As shown.

[0127] Modal 4: When a pilot detects enemy radar in a valley area during a low-altitude penetration maneuver, the planned flight path must avoid the radar detection zone. Therefore, after adding a radar detection threat zone to the map, set the weights as follows: distance 0.25, slope 0.25, altitude 0.25, and threat zone 0.25. The calculated flight path is as follows: Figure 7 As shown, the height map is as follows Figure 8 As shown.

[0128] The relevant parameters for the planned trajectory are shown in Table 2. It can be seen that for a trajectory length of about 14,000m, the global planning time is less than 0.1 seconds, which is very fast and can meet the planning time requirements in most cases. The planning time is the shortest when only distance weight is used. As other weights are added, the planning time increases, but the increase is small. The planning time can still meet the requirements after adding different weights.

[0129] Table 2 Map information for a certain region

[0130]

[0131] For mode 1, since only distance weights are included, the path planning algorithm will choose the shortest path, but this path will climb high peaks with very large gradient changes.

[0132] For Mode 2, an altitude weight is added to the distance weight. The trajectory planning algorithm will calculate the shorter trajectory with a lower altitude according to the weight of distance and terrain altitude. The planned trajectory will prioritize valley areas with lower terrain altitude.

[0133] For Mode 3, a slope weight is added to the distance and altitude weights, and the trajectory planning algorithm will prioritize areas with relatively smooth terrain in the valley.

[0134] For mode 4, after adding a threat zone to the graph, the trajectory planning algorithm will prioritize areas with relatively smooth terrain in the valley while avoiding the threat zone.

Claims

1. A trajectory planning method based on dynamic selection under different situations, characterized in that, Specifically, the following steps are included: Step 1: Load the digital elevation map (DEM) of the selected area, record the number of rows (m) and columns (n) of the DEM matrix, and read the relevant parameters of the DEM. The relevant parameters include distance weight (DW), height weight (HW), slope weight (SW), and threat weight (TW); Step 2: Perform grayscale processing on the digital elevation map to obtain grayscale map1; manually designate threat areas to form threat map2; Step 3: Select the starting point Ps and ending point Pe for the flight path planning on the digital elevation map. The coordinates of the starting point on the digital elevation map are (x... s ,y s The coordinates of the endpoint on the digital elevation map are (x...). e ,y e ); Step 4: Create the Open table and initialize its parameters; The parameters in the Open table include Open.f, Open.g, and Open.k; Initialize Open.g and Open.f as m x n matrices, with each element having a value of infinity; initialize Open.k as an m x n matrix, with each element having a value of 0. Step 5: Set the coordinates of the current point p0 to (c1, c2), and assign the coordinates of the starting point Ps to the current point p0; Set the cost g of the current point p0 to 0, that is, Open.g(c1,c2)=0; The estimated cost h from the current point p0 to the destination Pe is calculated according to formula (1). The calculation formula is as follows: The cost f of the current point is calculated according to formula (2): Open.f(c1,c2)=Open.g(c1,c2)+h (2) Step 6: Determine the point with the minimum cost f in the Open table, set it as the current point p0, assign the cost g of the current point p0 to g1, and change the cost f of the current point p0 to null. in: g1 represents the cost g from the starting point to the current point; Step 7: Determine if the current point (c1, c2) is the termination point. If c1 = x e And c2 = y e If the current point (c1, c2) is the endpoint, the track has been found, and step nine is executed; otherwise, step eight is executed. Step 8: Calculate the cost value of the eight neighboring points of the current point p0, and assign the calculation result to the Open table. At the same time, record point p0 as the parent node of these eight points. After the cost value of the eight neighboring points of the current point p0 is calculated, return to step 6 and repeat steps 6 to 8 until the track from the starting point to the ending point is found. Step 9: Based on the parent node Open.k in the Open table, find the parent node of each node starting from Pe until the starting point Ps is found, and generate the final track.

2. The trajectory planning method based on dynamic selection of different situations as described in claim 1, characterized in that, In step three, in the threat map map2, the value of points within the threat zone is 1, and the value of points outside the threat zone is 0.

3. The trajectory planning method based on dynamic selection of different situations as described in claim 1, characterized in that, In step eight, the eight neighborhoods of the current point are processed and calculated to obtain the cost value of the eight points surrounding the current point p0. This specifically includes the following steps: Step 8.1, calculate the coordinates of point p in one loop: a=c1+K1(i) (3)b=c2+K2(i) (4) K1＝[-1,0,1,0,-1,1,1,-1] K2＝[0,-1,0,1,1,-1,1,-1] K3＝[1,1,1,1,1.414,1.414,1.414,1.414] in: The coordinates of point p are (a, b); i represents the number of iterations in step eight, and also represents the eight neighborhoods, i.e., i = 1 to 8; Step 8.2, calculate the cost value, using the following formula: The cost g from the current point p0 to a point p in the eight neighborhood is calculated according to Formula 5 and denoted as g2. g2 = d * DW + map(a, b) * HW + map1(a, b) * SW + map2(a, b) * TW (5) d = K3(i) (6) Wherein: d is the distance cost from point p0 to point p; The cost g of point p is calculated according to formula 7; g = g1 + g2 (7) The cost f of point p is calculated according to formula 8; Step 8.3, determine whether the cost f of point p in the Open list is infinite. If not, it means that the point is in the Open list, and execute step 8.4; if so, it means that the point is not in the Open list, and execute step 8.5; Step 8.4, compare the cost g of point p with Open.g(a, b). If g < Open.g(a, b), execute step 8.5, otherwise execute step 8.6; Step 8.5, assign the cost f and cost g of point p to the corresponding positions in the Open list, and record point p0 as the parent node of point p, and enter step 8.6; Open.f(a, b) = f (9) Open.g(a, b) = g (10) Step 8.6, make i = i + 1, return to step 8.1, and loop to execute step eight until the eight-neighborhood points of the current point p0 are all calculated.

Images