3D UAV Route Planning Method Based on Piecewise Random and Angle Random Algorithms

Abstract

The present invention discloses a three-dimensional route planning method for unmanned aerial vehicles based on a segmented random and angle random algorithm, including the steps of: S1. equally dividing the route of the unmanned aerial vehicle to obtain the coordinates of the segmented points on the horizontal projection plane after equal division; S2. initializing the coordinates of the constructed route points on the route, as well as the number and coordinates of the filled route points to determine the initial route; S3. determining whether there are route points in the obstacle area in the initial route; S4. determining the moving route points to form an obstacle avoidance route; S5. calculating the fitness value of the obstacle avoidance route and assigning a penalty coefficient to the solutions that do not meet the constraint requirements; S6. performing population update using a non-associated iterative update method and outputting the optimal solution R of the population opt ; S7. smoothing the optimal solution R output by the iteration opt and outputting the best solution R best ; By processing the route for obstacle avoidance, smoothing, etc., this method can improve the quality of the route planning of the unmanned aerial vehicle, ensure the safety and efficiency of the unmanned aerial vehicle in performing tasks, and has the characteristics of high accuracy and safety in route planning.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicle route planning, and particularly to a three-dimensional route planning method for unmanned aerial vehicles based on a segmented random and angle random algorithm. Background Art

[0002] In modern industrial and agricultural activities, as well as in the military field, unmanned aerial vehicles (UAVs) play an important role, and the research on UAVs has received increasing attention; the route planning of UAVs is one of the important directions in the research field of UAVs, which refers to finding an optimal or feasible route from the starting position to the target position in a given planning space and meeting the given constraint conditions and performance indicators, so as to meet the requirements for the UAV to safely and timely reach the target area and complete the specified tasks as much as possible; with the rapid development of technologies such as wireless communication, intelligent control, and sensor detection, the ability of UAVs to fly autonomously according to the planned route is constantly enhanced, while the functional requirement for remote control flight by ground personnel is gradually reduced, which further highlights the irreplaceability of UAV route planning. UAV route planning has received more and more attention from researchers;

[0003] UAV route planning is an NP (non-deterministic polynomial) problem, and direct solution often easily leads to combinatorial explosion; therefore, in order to accelerate the planning process, researchers have carried out in-depth research on single UAV route planning and achieved a large number of results. Traditional numerical methods such as the optimal control method, artificial potential field method, heuristic A* search method, and dynamic programming method have been widely applied to route planning; with the development of intelligent algorithms, especially the rapid development of swarm intelligence algorithms, particle swarm optimization (PSO), ant colony optimization (ACO), genetic algorithm (GA), simulated annealing (SA), etc. have also been widely applied to route planning and achieved better planning results compared with traditional methods. However, in the planning of three-dimensional UAV routes, there are still many problems to be solved, such as

[0004] In the literature "Huang C, Fei J, Deng W. A novel route planning method of fixed-wing unmanned aerial vehicle based on improved QPSO[J]. IEEE Access, 2020, 8: 65071-65084", a quantum PSO (QPSO) algorithm was proposed for the three-dimensional route planning of unmanned aerial vehicles. However, the obstacle avoidance ability of this algorithm is limited in complex obstacle environments, and the smoothness of route planning is poor.

[0005] In the literature "Zhang Defeng. MATLAB R2020a Intelligent Algorithms and Case Analyses[M]. Beijing: Publishing House of Electronics Industry, 2021: 164-172", a three-dimensional route planning method based on the ACO algorithm was proposed. This method uses the grid division of the terrain as the route planning unit. However, this algorithm is sensitive to the positions of the start point (StP) and the end point (EnP) of the route, and the smoothness of the route and altitude planning is poor.

[0006] In fact, when planning the flight space in the face of complex terrains and obstacle areas, the current swarm intelligence-based route planning generally has difficulty achieving better results. Because most of these algorithms achieve route planning by continuously adjusting the position coordinates of way points (WP) in space, there are prone to problems such as the back-and-forth fluctuations of the planned way points, uneven distances between way points, and inevitably poor route smoothness and insufficient obstacle avoidance ability. Some algorithms use the grid method to evenly divide the flight space into grids and progress sequentially along a certain coordinate axis, and plan in the plane perpendicular to this axis. Since it can only move forward in one direction along the fixed coordinate axis, when planning the route in some complex flight spaces, there are usually situations where it moves in both the positive and negative directions of the coordinate axis. At this time, the grid method obviously cannot adapt to the route planning of such complex flight spaces. On the other hand, single route planning is difficult to meet the backup and concealment requirements of specific tasks. Therefore, the research on multi-route planning has more application value.

[0007] Therefore, there is an urgent need to design a new three-dimensional route planning method for unmanned aerial vehicles to solve the key problems existing in the above-mentioned prior art, such as uneven distances between way points, poor smoothness, insufficient obstacle avoidance ability, sensitivity to the positions of the start point and the end point of the route, and single route planning. Summary of the Invention

[0008] In view of the above problems, the present invention aims to provide a three-dimensional route planning method for unmanned aerial vehicles (UAVs) based on a segmented random and angular random algorithm. This method randomly divides the line connecting the start and end points of the route into equal segments, determines a constructed route point on the vertical line passing through the segmented points at a random angle, inserts and fills route points between the constructed route points, and then connects all the route points in sequence to form a route. At the same time, the route is processed for obstacle avoidance, smoothing, etc., and multiple routes are planned to overcome the above problems, improve the quality of UAV route planning, ensure the safety and efficiency of UAV mission execution, and have the characteristics of high accuracy and safety in route planning.

[0009] To achieve the above object, the technical solution adopted by the present invention is as follows:

[0010] A three-dimensional route planning method for unmanned aerial vehicles based on a segmented random and angular random algorithm, comprising the steps

[0011] S1. Divide the line connecting the start and end points of the UAV route into equal segments by using a random equal division method to obtain the coordinates of the segmented points of the line connecting the start and end points on the horizontal projection plane after equal division;

[0012] S2. After obtaining the coordinates of the segmented points, use the angular random method to initialize the coordinates of the constructed route points on the UAV route, as well as the number and coordinates of the filled route points, and determine the initial route;

[0013] S3. Determine whether there are route points in the obstacle area in the initial route obtained in step S2. If so, go to step S4; if not, set the obstacle avoidance route = IR and go to step S7;

[0014] S4. Determine the moving route points according to the obstacle avoidance method of the SRAR algorithm to move the initial route points out of the obstacle area and form an obstacle avoidance route OAR;

[0015] S5. Calculate the fitness values of all the obstacle avoidance routes OAR formed in each iteration, and assign a penalty coefficient to the solutions that do not meet the constraint requirements;

[0016] S6. Use the non-associated update iteration method to update the population until the optimal solution R of the population is output at the end of the iteration opt ;

[0017] S7. Smooth the optimal solution R output at the end of the iteration opt and finally output the best solution R of the UAV route planning best , completing a single route planning.

[0018] Preferably, the process of dividing the line connecting the start and end points of the UAV route into equal segments and obtaining the coordinates of the segmented points of the line connecting the start and end points on the horizontal projection plane in step S1 includes

[0019] S101. Let the angle between the x-axis and the line connecting the start and end points of the UAV flight path be α. If it is equally divided into S segments, each segment has a length of l, generating S - 1 segmentation points, and there are S - 1 lines passing through the segmentation points and perpendicular to the line connecting the start and end points;

[0020] S102. Let the set of segmentation points be [StP, SeP1, …, SeP i , …, SeP S-1 , EnP], which has a total of S + 1 points, including the start point and the end point. According to trigonometric functions, the coordinates of the segmentation points on the horizontal projection plane can be calculated.

[0021] Preferably, the determination process of the initial flight path described in step S2 includes

[0022] S201. In the set of segmentation points [StP, SeP1, …, SeP i , …, SeP S-1 , EnP] obtained in step S1, except for the two points SeP S-1 , EnP in the set of segmentation points, taking the points in the set of segmentation points as rotation points in turn, using the line connecting the start and end points as the starting axis, randomly generating an angle A clockwise or counterclockwise, and forming a constructed flight path point from the rotation point along the connection point of the angle A and the perpendicular line. Then, S - 1 constructed flight path points can be formed in turn;

[0023] S202. The start and end points and each constructed flight path point form a set of constructed flight path points [StP, CWP1, …, CWP i , …, CWP S-1 , EnP]. Connecting the points in the set of constructed flight path points initializes a flight path on the horizontal projection plane; evenly filling points between the points in the set of constructed flight path points to form filled flight path points, and then determining the coordinates of the constructed flight path points;

[0024] S203. Using the start and end points, constructed flight path points, and filled flight path points obtained in step S202 to jointly form a complete initial flight path;

[0025] S204. The initial solution randomly generated by the UAV flight path after equal division on the horizontal projection plane is

[0026] S ij = [S, A1, …, A i , …, A S-1 ,

[0027] where S ij represents the jth feasible solution of the ith time, S represents the number of segments, and A i is the angle of the ith flight path point;

[0028] S205. Let the length of the line connecting the start and end points of the route be \(L\), and the length of each segment be \(l = L / S\). The coordinates of the start point are \(StP=(St x ,St y ), and the coordinates of the end point are \(EnP=(En x ,En y ). The coordinates of the \(i\)-th segmentation point \(SeP i =(Se ix ,Se iy ) are as follows:

[0029]

[0030] S206. The coordinates of the \(i\)-th constructed route point \(CWP i =(CW ix ,CW iy ) are obtained by the following formula:

[0031]

[0032] In formula (2), \(i = 1,\ldots,S - 1\). If \(i - 1 = 0\), then \(Se (i-1)x＝ St x , Se (i-1)y＝ St y ; sign(·) is the sign function. When \(En x -St x \geq0\), \(sign(En x -St x ) = 1; otherwise, \(sign(En x -St x ) = -1;

[0033] S207. After the coordinates of the constructed route points are determined, fill in the route points evenly between adjacent points in the set of constructed route segment points according to the method of formula (1); assume that the total number of route points is \(D\), including the start point and the end point. Then the number of filled route points is \(D - S - 1\), and there are \(S\) segments. The number of filled route points \(D i for each segment is as follows:

[0034]

[0035] In formula (3), \(i = 1,\ldots,S - 1\). If \(i - 1 = 0\), then \(Se (i-1)x＝ St x , Se (i-1)y＝ St y , and round() is the rounding function;

[0036] S208. By randomly generating S parameters and using equations (1)-(3), determine the coordinates of the constructed waypoints, the number of waypoints to be filled, and the coordinates, form a complete route, and determine the initial route IR.

[0037] Preferably, the calculation process of the obstacle avoidance route OAR described in step S4 includes

[0038] S401. Assume that WP1 to WP4 is a route passing through OA, WP2 and WP3 are within OA, and the route must avoid OA. Take the line connecting WP1 to WP4 as the horizontal axis and WP1 as the origin to form 3 vectors According to the vector operation rules, The modulus of is the distance from MWP1 to CP;

[0039] S402. Take WP1 as the rotation point and the line connecting WP1 to WP4 as the starting axis, calculate the angle α between the line connecting WP1 to WP4 and the line connecting WP1 to CP, and at the same time calculate the distance between WP1 and WP2 as l m , the distance between WP1 and CP is l c ; obtain The modulus of is l c , the complex angle is α, The modulus of is l m / cos(γ), the complex angle is γ;

[0040] S403. Use equation (4) to find γ:

[0041]

[0042] Among them, in equation (4), ε represents the safety margin;

[0043] After equation (4) is sorted out:

[0044]

[0045] From equation (5), the unique solution of γ can be obtained, and γ is the smallest angle that makes equation (5) hold;

[0046] When the value of γ is determined, the coordinates of MWP1 can be obtained according to formula (2); similarly, the coordinates of MWP2 can be obtained; form a route that can completely avoid the obstacle area.

[0047] Preferably, when the route passes through WP1 to WP4 in sequence and then to WP7 in sequence:

[0048] S404. Connect WP1 and WP7, and then equally divide the connecting line segment according to the original number of flight segments; finally, construct the obstacle avoidance flight path from WP1 to MWP1, …, MWP5, WP7 in sequence according to the method described in steps S401 - S403.

[0049] Preferably, the process of calculating the fitness values of all obstacle avoidance flight paths OAR formed in each iteration and assigning penalty coefficients to the solutions that do not meet the constraint requirements in step S5 includes

[0050] S501. Design an optimization model considering constraint conditions:

[0051]

[0052] Among them, ψ represents the constraint of the horizontal turning angle, θ represents the constraint of the pitch angle, l Li represents the constraint of the flight segment length, z i represents the constraint of the waypoint height, n(i) represents the number of elements in the i - th moving waypoint set, that is, the number of waypoints in the i - th obstacle area;

[0053] S502. The fitness function fit(·) is expressed as:

[0054]

[0055] Among them, S = [S, A1, …, A i , …, A S-1 , R represents the set of planned waypoint coordinates, S→R represents the set of coordinates of each waypoint in the obstacle avoidance flight path determined by S, n st represents the number of unsatisfied constraint conditions, P st represents the penalty coefficient when any constraint condition is not satisfied, P st takes a relatively large number. When all constraint conditions are met, the fitness value is equal to the flight distance.

[0056] Preferably, the process of using the non - associated update iteration method to update the population until the optimal solution R of the population is output at the end of the iteration in step S6 opt includes

[0057] Let the maximum number of iterations be Iter max , the number of feasible solutions be Ns, S opt represents the optimal solution, S ij is the j - th feasible solution in the i - th time, then i ≤ Iter max , j ≤ Ns. If min(fit(S ij →R ij )) ≤ fit(S opt →R opt ), then let Sopt = S ij , R opt = R ij , until the iteration termination condition is satisfied, and output the optimal solution R of the population opt .

[0058] Preferably, the process of smoothing the optimal solution R output at the end of iteration in step S7 opt includes

[0059] S701. Perform position smoothing on the optimized solution;

[0060] S702. Perform ultra-high processing on the optimized solution after position smoothing to obtain the best solution R for the UAV route planning after smoothing processing best .

[0061] Preferably, the planning method further includes step S8. Multi-route planning

[0062] S801. Adjust steps S1 - S2, and initialize the coordinates of the constructed waypoints according to multi-route planning methods such as specified waypoints on the way, specified course and distance, specified waypoints and angles, and mixed methods, to obtain the initialized coordinates of the constructed waypoints;

[0063] S802. After obtaining the initialized coordinates of the constructed waypoints, perform iterative optimization according to steps S3 - S7, and finally output the best multi-route planning after smoothing.

[0064] The beneficial effects of the present invention are: The present invention discloses a UAV three-dimensional route planning method based on a segmented random and angle random algorithm. Compared with the prior art, the improvements of the present invention are as follows:

[0065] The present invention proposes a UAV three-dimensional route planning method based on a segmented random and angle random algorithm. When this method is used:

[0066] (1) Randomly segment the connection line between the head and the end, determine the constructed waypoints through random angles, and then determine the initial route by connecting the constructed waypoints and filling waypoints; (2) Specify the angle of the directed line segment, design a circular obstacle area avoidance method, a route smoothing method, and a multi-route planning method to improve the quality of route planning, meet the requirements of route planning diversity, and make the SRAR algorithm have the advantages of few optimization parameters and low computational cost; (3) Propose a non-associated iterative update method for solving variable-length solutions to update the variable-length solution population, effectively solve problems such as restrictions on the types of head and end positions, uneven spacing of waypoints, poor route smoothness, weak ability to handle ultra-high obstacles in obstacle avoidance, and single route planning in route planning, improve the quality of route planning under complex multi-constraint conditions, meet the requirements of route planning diversity, and have the advantages of high route planning accuracy and safety. Description of the Drawings

[0067] Figure 1 This is the algorithm flowchart of the UAV three-dimensional route planning method based on the segmented random and angle random algorithm of the present invention.

[0068] Figure 2 This is the schematic diagram of the route initialization of the 5SRAR algorithm of the present invention.

[0069] Figure 3 This is the position relationship diagram of the 4 directed line segments of the present invention.

[0070] Figure 4 This is the schematic diagram of the obstacle avoidance principle of the SRAR algorithm of the present invention.

[0071] Figure 5 This is the schematic diagram of the special obstacle avoidance situation of the present invention.

[0072] Figure 6 This is the schematic diagram of the position smoothing of the present invention.

[0073] Figure 7 This is the flowchart of the position smoothing of the present invention.

[0074] Figure 8 This is the schematic diagram of the ultra-high treatment of the present invention.

[0075] Figure 9 This is the demonstration diagram of the multi-route specification method of the present invention.

[0076] Figure 10 This is the complex 3D topographic map of Embodiment 2 of the present invention

[0077] Figure 11 This is the verification diagram of different start and end types in Embodiment 2 of the present invention

[0078] Figure 12 This is the verification diagram of the ultra-high recognition in Embodiment 2 of the present invention.

[0079] Figure 13 This is the verification diagram of the route smoothing effect in Embodiment 2 of the present invention.

[0080] Figure 14 This is the limitation diagram of the obstacle avoidance ability of the algorithm in Embodiment 2 of the present invention.

[0081] Figure 15 This is the verification diagram of the multi-route planning performance in Embodiment 2 of the present invention.

[0082] Figure 16 This is the comparison diagram of the route 1 planning of 4 algorithms in Embodiment 3 of the present invention.

[0083] Figure 17 This is the comparison diagram of the route 2 planning of 4 algorithms in Embodiment 3 of the present invention.

[0084] Figure 18 This is the comparison chart of the route planning of 34 algorithms in Embodiment 3 of the present invention for Route 3.

[0085] Figure 19 This is the comparison chart of the route planning of 34 algorithms in Embodiment 3 of the present invention for Route 4.

[0086] Wherein: in Figure 6 among, Figure 6 (a) represents the schematic diagram of obstacle avoidance before position smoothing, Figure 6 (b) represents the schematic diagram of obstacle avoidance after position smoothing;

[0087] In Figure 10 among, Figure 10 (a) represents the 3D topographic map, Figure 10 (b) represents the projection map of the topographic map on the horizontal plane;

[0088] In Figure 12 among, Figure 12 (a) represents the route planning map of the SRAR algorithm for the 3D topographic map, Figure 12 (b) represents the route planning map of the SRAR algorithm for the horizontal projection. Specific implementation manner

[0089] In order to enable those of ordinary skill in the art to better understand the technical solution of the present invention, the technical solution of the present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0090] Embodiment 1: Referring to a Figures 1 - 19 shown three-dimensional route planning method for unmanned aerial vehicles based on the segmented random and angle random (SRAR) algorithm, including the steps

[0091] S1. Use the method of random equal division to equally divide the connection line between the start and end points of the UAV route, and obtain the coordinates of the segmented points (segment point) of the connection line between the start and end points after equal division on the horizontal projection plane

[0092] S101. As Figure 2 shown, let the angle between the x-axis and the connection line between the start and end points of the UAV route be α. If it is equally divided into 3 segments, then the number of segments S = 3, the length of each segment is l, S - 1 segmented points are generated, and there are S - 1 straight lines passing through the segmented points and perpendicular to the connection line between the start and end points;

[0093] S102. Let the segmented point set (segment-point set, SPS) be [StP, SeP1,…, SeP i ,…, SeP S-1 , EnP], with a total of S + 1 points, including the start point and the end point. The coordinates of the segmented points on the horizontal projection plane can be calculated based on trigonometric functions;

[0094] S2. After obtaining the coordinates of the segmentation points, use a random angle method to initialize the coordinates of the constructed way points (CWP) on the UAV flight path, as well as the number and coordinates of the filled way points (FWP), and finally determine the initial route (IR).

[0095] S201. As Figure 2 shown, in the set of segmentation points [StP, SeP1, …, SeP i , …, SeP S-1 , EnP] obtained through step S1, except for the two points SeP S-1 and EnP in the set of segmentation points, successively use the points in the set of segmentation points as rotation points, and use the connection line between the start and end points as the starting axis to randomly generate a clockwise or counterclockwise angle A. From the rotation point along the connection point of angle A and the perpendicular line, a constructed way point (CWP) is formed, and then S - 1 constructed way points can be successively formed;

[0096] S202. The start and end points and each constructed way point form a constructed way - point set (CWPS): [StP, CWP1, …, CWP i , …, CWP S-1 , EnP]. Connect the points in the constructed way - point set, then initialize a flight path on the horizontal projection plane; to ensure the implementation of the subsequent obstacle avoidance algorithm, evenly fill points between the points in the constructed way - point set to form filled way points (FWP), and then determine the coordinates of the constructed way points;

[0097] S203. Use the start and end points, constructed way points, and filled way points obtained through step S202 to jointly form a complete initial route (IR). At the same time, since the filled flight segments are evenly filled, their positions will not be randomly adjusted again, so that the distance between each way point can always be kept uniform;

[0098] S204. Therefore, the initial solution randomly generated by the UAV flight path after equal division on the horizontal projection plane is

[0099] S ij = [S, A1, …, A i , …, A S-1 ,

[0100] where S ijDenote the \(j\)-th feasible solution at the \(i\)-th time, \(S\) represents the number of segments, and \(A\) i is the angle of the \(i\)-th waypoint;

[0101] Among them, the solution method for the angle \(A\) of the waypoint is:

[0102] Since in route planning, the route is directed, in order to ensure that any starting endpoint on the horizontal projection plane can reach any ending endpoint through any path, this embodiment clarifies the calculation method of the included angle between any two directed line segments;

[0103] Among the two directed line segments, one is defined as the starting axis (SA), and the other is the rotation axis (RA). The position of the rotation axis is obtained by rotating clockwise or counterclockwise with the starting endpoint of the starting axis as the rotation point and the starting axis as the initial position; there are 4 kinds of azimuth relationships of a directed line segment in the horizontal coordinate plane, that is, it can exist in four quadrants respectively, so there are 16 kinds of azimuth relationships of two directed line segments in the horizontal coordinate plane; Figure 3 Show the possible quadrant distributions of the rotation axis when the starting axis is in the first quadrant, where \(\alpha\) S is the included angle between the \(x\)-axis and the starting axis, \(\alpha\) R is the included angle between the \(x\)-axis and the rotation axis, and \(\beta\) is the included angle from the starting axis to the rotation axis;

[0104] Table 1 shows the calculation formula of the included angle \(\beta\) between two directed line segments when the starting axis and the rotation axis are in different quadrants;

[0105] Table 1: Table of calculation formulas for the included angle of directed line segments

[0106]

[0107] S205. Let the length of the line connecting the start and end of the route be \(L\), the length of each segment be \(l = L / S\), the coordinates of the starting point be \(StP=(St x , St y ), and the coordinates of the ending point be \(EnP=(En x , En y ), then the coordinates \(SeP\) i =(Se ix , Se iy ) of the \(i\)-th segment point can be expressed as:

[0108]

[0109] Actually, the coordinates of the segment points obtained by formula (1) are obtained by evenly filling between the start and end points;

[0110] S206. In Figure 2 the coordinates \(CWP\) of the \(i\)-th constructed waypointi =(CW ix , CW iy ) can be calculated by the following formula:

[0111]

[0112] In formula (2), i = 1, …, S - 1. If i - 1 = 0, then Se (i-1)x＝ St x , Se (i-1)y＝ St y ; sign(·) is the sign function. When En x - St x ≥ 0, sign(En x - St x ) = 1; otherwise, sign(En x - St x ) = -1. The role of sign(En x - St x ) is to ensure that when calculating the coordinates of the constructed waypoints, formula (2) is applicable to the start and end position types in all four quadrants;

[0113] S207. After determining the coordinates of the constructed waypoints, uniformly insert and fill waypoints between adjacent points in the set of constructed segment points according to the method of formula (1); to make the number of waypoints searched in the iterative process consistent, assume that the total number of waypoints is D, including the start point and the end point. Then the number of filled waypoints is D - S - 1, and there are S segments. The number of filled waypoints in each segment D i is:

[0114]

[0115] Similarly, in formula (3), i = 1, …, S - 1. If i - 1 = 0, then Se (i-1)x＝ St x , Se (i-1)y＝ St y . round() is the rounding function; by randomly generating S parameters and using formulas (1)-(3), the determination of the coordinates of the constructed waypoints, the number and coordinates of the filled waypoints is completed, forming a complete route, that is, determining the initial route IR. This method can fully meet the application requirements of the start and end position types in all four quadrants;

[0116] S3. Determine whether there is a waypoint WP in the obstacle area (OA) in the initial route obtained in step S2. If so, go to step S4; if not, let the obstacle avoidance route (OAR) = IR and go to step S7

[0117] The obstacle area described in this embodiment refers to the area that poses a threat to flight safety, including areas such as shooting-down threat, ultra-high threat, meteorological threat, and no-fly zone. The obstacle area is equivalently treated as a circular area to facilitate determining whether a waypoint is within the obstacle area by comparing the distance from the waypoint to the center point (CP).

[0118] S4. Determine the moving waypoint (MWP) according to the obstacle avoidance method of the SRAR algorithm to move the initial waypoint out of the obstacle area and form the obstacle avoidance route OAR

[0119] The obstacle avoidance principle of the SRAR algorithm described in this embodiment is as Figure 4 shown;

[0120] Case 1: WP1 to WP4 is a route passing through OA, and WP2 and WP3 are within OA. Therefore, the route planning must avoid this OA. The specific method is as follows: draw perpendicular lines through the waypoints WP2 and WP3 within OA. With WP1 as the rotation point and the line segment from WP1 to WP4 as the starting axis, rotate counterclockwise by γ. The moving waypoint (MWP) MWP1 outside OA obtained by the intersection of the rotation line and the perpendicular line passing through WP2; similarly, the MWP2 outside OA for WP3 can be obtained. Connect WP1, MWP1, MWP2, and WP4 to make the initial route become the obstacle avoidance route (OAR), completely avoiding OA;

[0121] To avoid the obstacle area, the key is to determine the rotation angle γ, which is solved by means of the vector method; as Figure 4 shown, perform coordinate transformation. Take the connection line from WP1 to WP4 as the horizontal axis and WP1 as the origin point, then three vectors can be formed, which are respectively According to the vector operation rules, And the modulus of is the distance from MWP1 to CP;

[0122] S402. With WP1 as the rotation point and the connection line from WP1 to WP4 as the starting axis, the included angle α between the connection line from WP1 to WP4 and the connection line from WP1 to CP can be calculated, and the distance between WP1 and WP2 is calculated as l m , the distance between WP1 and CP is l c ; Therefore, the modulus of is l c , the complex angle is α, the modulus of is l m / cos(γ), and the complex angle is γ;

[0123] S403. Then γ can be obtained by using Equation (4), and Equation (4) is expressed as follows:

[0124]

[0125] Among them, in Equation (4), ε represents the safety margin, mainly to balance the influence of the arc obstacle area on the MPW and ensure that the route planning can completely avoid the obstacle area.

[0126] After arranging Equation (4), we can get:

[0127]

[0128] Equation (5) is actually to solve a quadratic equation of one variable. According to the geometric relationship, the plus and minus signs of α and γ are just opposite. Based on this, the unique solution of γ can be obtained, and γ is the minimum angle that makes Equation (5) hold. After the value of γ is determined, the coordinates of MWP1 can be calculated according to Formula (2). Similarly, the coordinates of MWP2 can be calculated. The newly formed route can completely avoid the obstacle area.

[0129] Case 2: As Figure 5 shown, the route passes through WP1 to WP4 in sequence, and then to WP7 in sequence. Obviously, the planned route passes through the obstacle area and is not a straight line. At this time:

[0130] S404. It is necessary to connect WP1 and WP7, and then equally divide the connecting line segment according to the original number of route segments. Figure 4 It is equally divided into 6 parts. Then, according to the method shown in steps S401 - S403, the obstacle avoidance route OAR from WP1 to MWP1,..., MWP5, WP7 in sequence can be constructed.

[0131] S5. Calculate the fitness values of all the obstacle avoidance routes OAR formed in each iteration, and assign a penalty coefficient to the solutions that do not meet the constraint requirements.

[0132] S501. Design a fitness function fit(·). The fitness function fit(·) includes the optimization objective and the constraint conditions, and is a standard for quantitatively evaluating the quality of solutions. As a heuristic random search algorithm, the design of the SRAR algorithm's fit(·) mainly considers the flight performance characteristics of the UAV and the threat of the obstacle area, etc. A penalty coefficient is used to assign values to the solutions that violate the constraints, so as to exclude the solutions that violate the constraint conditions. In this embodiment, the fixed-wing UAV is mainly studied. Compared with the rotary-wing UAV, the fixed-wing UAV has higher requirements for flight performance and more complex constraint conditions. The main constraint conditions are:

[0133] (1) Constraint of the horizontal turning angle

[0134] The horizontal turning angle ψ restricts the flight of the UAV along a certain flight segment. When turning to the next flight segment, the change in the heading angle can only be limited within a certain range to meet the flight performance requirements. A flight segment refers to the line segment between two adjacent waypoints and is a directed line segment. The horizontal turning angle ψ is calculated according to the method shown in Table 1 for the directed line segment between WP i and WP i+1 and the directed line segment between WP i+1 and WP i+2 . Then this constraint can be expressed as: |ψ| ≤ ψ max ;

[0135] (2) Constraint on the pitch angle

[0136] The pitch angle θ restricts the maximum angle of climb or descent of the planned flight path in the height direction, and this constraint is determined by the performance of the UAV. Let the positions of adjacent WPs i and WP i+1 be (x i, y i , z i ) and (x i+1, y i+1 , z i+1 ), then |θ| ≤ θ max , and this constraint can be expressed as:

[0137]

[0138] (3) Constraint on the flight segment length

[0139] The flight segment length l Li reflects the shortest distance that the UAV must maintain a straight flight before starting to change its flight state. This situation not only depends on the flight performance of the UAV but is usually also associated with navigation requirements to meet the navigation state requirements. This constraint can be expressed as l Li ≤ l Lmin , then the flight segment constraint between WP i and WP i+1 can be expressed as:

[0140]

[0141] (4) Constraint on the waypoint height

[0142] The waypoint height z i restricts the flight height range of the UAV on the planned flight path. Different types of flight missions have different height constraint ranges. Let Z i be the terrain height corresponding to the horizontal coordinate of the i-th waypoint, the maximum flight height be Z max , the optimal flight height be Z best , and the flight height and the terrain height should not be lower than the relative height Z rel; To meet the needs of the unmanned aerial vehicle (UAV) to perform tasks, the planned altitude of each waypoint is as follows:

[0143]

[0144] Equation (8) better simulates the whole process of the UAV taking off from the ground and landing. The constraint on the waypoint altitude is: z i ≤Z max ;

[0145] (5) Constraint of the obstacle area

[0146] When the route planning crosses the obstacle area, let n(i) represent the number of elements in the i-th moving waypoint set, that is, the number of waypoints in the i-th obstacle area, and n is the number of obstacle areas crossed during the initial planning. This constraint is to ensure that the UAV completely avoids the obstacle area and ensure flight safety, which can be expressed as:

[0147] To better reflect the planning effect of the SRAR algorithm, this embodiment adopts the optimization goal of minimizing the planned route range. Then the whole optimization model is expressed as follows:

[0148]

[0149] S502. The fitness function fit(·) can be expressed as:

[0150]

[0151] Where: S = [S, A1, …, A i , …, A S-1 , R represents the set of planned waypoint coordinates, S→R represents the set of coordinates of each waypoint in the obstacle-avoiding route determined by S, n st represents the number of unsatisfied constraint conditions, and P st represents the penalty coefficient when any constraint condition is not satisfied. Since the optimization problem is a minimization problem, P st usually takes a relatively large number. In this embodiment, P st = 1000. When all constraint conditions are satisfied, the fitness value is equal to the route range;

[0152] S6. Use the non-associated update iteration method to update the population until the optimal solution R of the population is output at the end of the iteration. opt

[0153] The SRAR algorithm is a heuristic random search algorithm. Therefore, the optimization process also needs to be continuously iterated and updated to achieve; Since the solution vector of SRAR optimization is a variable-length solution, S = [S, A1, …, A i , …, A S-1, the number of elements in S depends on S; while traditional swarm intelligence algorithms are all based on the iterative process of fixed-length solutions; therefore, to solve the problem of variable-length solutions and facilitate the planning of multiple air routes, this embodiment proposes a non-associated iterative update method; by non-associated, it means that when solving for S in the i-th iteration i , it does not depend on the S obtained in the (i - 1)-th iteration i-1 , extreme value and other calculation results; the S in the i-th iteration i is only randomly generated; this non-associated iterative method is suitable for the SRAR algorithm and can well handle the problem of variable-length solutions;

[0154] Let the maximum number of iterations be Iter max , the number of feasible solutions be Ns, S opt represents the optimal solution, S ij is the j-th feasible solution in the i-th iteration, then i ≤ Iter max , j ≤ Ns, if min(fit(S ij → R ij )) ≤ fit(S opt → R opt ), then let S opt = S ij , R opt = R ij , until the iteration termination condition is met, and output the optimal solution R opt of the population;

[0155] S7. Smooth the optimal solution R opt output at the end of the iteration, including position smoothing and over-height handling, and finally output the best solution R best for the UAV route planning, completing a single route planning

[0156] Although the route planning of the SRAR algorithm is carried out in a straight line, the planned route (optimal solution R opt ) still needs to be smoothed to further optimize the route and finally obtain the optimal route to ensure the quality of the route planning; Figure 6 shows the significance of route smoothing; as Figure 6 shown in (a), according to the SRAR algorithm, a route is planned, and the distances between the route points are uniform, completely avoiding the obstacle area, but this route still needs to be smoothed. The principle of smoothing is to represent the route with a straight line as much as possible, reduce the existence of corners, and shorten the flight distance as much as possible; Figure 6 The dashed line in (b) is the route before smoothing, and the thick solid line is the optimal route after smoothing; obviously, the route after smoothing is more efficient, concise, and has a shorter flight distance than before smoothing;

[0157] The smoothing method of the SRAR algorithm is an operation performed on the optimized solution after the optimization ends, including two steps: position smoothing and over height disposal;

[0158] S701. Perform position smoothing on the optimized solution

[0159] As Figure 6 shown, when planning the route from StP to EnP, assuming that a total of n OAs are avoided in sequence, corresponding moving waypoints will inevitably be formed. Let the moving waypoint formed by avoiding the i-th OA be the i-th moving waypoint set (MWP set, MWPS) [MWP i1 , MWP i2 , …, MWP in(i) , and n(i) represents the number of elements in the i-th moving waypoint set; Figure 7 shows the process of position smoothing;

[0160] The steps of position smoothing are as follows:

[0161] S7011. Assign a nearest segment point (NSP) in the constructed waypoint set to each MWP i1 and MWP in(i) , denoted as NSP i1 and NSP in(i) respectively; in the i-th MWPS, if n(i) is even, select two moving waypoints numbered n(i) / 2 and (n(i)+1) / 2 as the optimized connection points (OCP) [OCP i1 , OCP i2 ; if n(i) is odd, select the moving waypoint numbered (n(i)+1) / 2 as the optimized connection point [OCP i1 , OCP i2 , at this time, OCP i1 = OCP i2 ; at the same time, insert the OCPs into the constructed waypoint set in order;

[0162] S7012. For the first MWPS, re-fill the waypoints evenly between OCP 11 and StP, and determine whether there are newly generated waypoints within the OA; if not, use the newly filled waypoints evenly between OCP 11 and StP to replace the original waypoints between OCP 11 and StP; if so, use the waypoints between OCP 11 and NSP i1Replace the original OCP with a new set of evenly spaced waypoints 11 to NSP i1 The waypoints in between;

[0163] S7013. For the nth MWPS, between the OCP n2 and EnP, re-fill the waypoints evenly and check if any newly generated waypoints are within OA; if not, use the new evenly filled waypoints between the OCP n2 and EnP to replace the original OCP n2 and EnP waypoints; if so, use the new evenly filled waypoints between the OCP n2 and NSP nn(i) to replace the original OCP n2 and NSP nn(i) waypoints;

[0164] S7014. For the 2nd to the (n - 1)th MWPS, let j = 2, …, n - 2, and successively re-fill the waypoints evenly between the OCP j2 and OCP (j+1)1 and check if any newly generated waypoints are within OA; if not, use the new evenly filled waypoints between the OCP j2 and OCP (j+1)1 to replace the original OCP j2 and OCP (j+1)1 waypoints; if so, find the MWPS between the OCP j2 and OCP (j+1)1 and embed and execute steps S5011 - S5014 until the newly evenly filled waypoints between the OCP j2 and OCP (j+1)1 no longer pass through OA;

[0165] S702. Perform over-height handling on the optimized solution after position smoothing

[0166] Smoothing the route actually means re-filling the waypoints evenly between each optimized connection point. However, for 3D route planning, there is a possibility that the newly evenly filled waypoints do not meet the height constraints, resulting in over-high waypoints (OHWP). Therefore, these waypoints need to be processed to meet the height constraints; Figure 8 shows the principle of over-height handling;

[0167] In essence, Figure 8 not only shows the principle of over-height handling but also shows the process of position smoothing. The thin dotted line is the route output after optimization, from StP to EnP. The route with the thick dotted line in the middle is the route after position smoothing, but there are over-height waypoints in the middle, so over-height handling is required;

[0168] The specific disposal steps are as follows:

[0169] S7021. Detect whether the smoothed waypoints do not meet the altitude constraint conditions; if so, use the original waypoints corresponding to the replacement as an MWPS, and assume that the MWPS has n elements;

[0170] S7022. Determine the NSP and OCP of the MWPS; determine NSP1 and NSP in the constructed waypoint set n ; use MWP1 and MWP in the MWPS n as OCP1 and OCP2;

[0171] S7023. Re-uniformly fill waypoints between NSP1 and MWP1, NSP2 and MWP2, and determine whether there are newly generated waypoints within the OA; if not, use the newly uniformly filled waypoints between NSP1 and MWP1, NSP2 and MWP2 to replace the original waypoints between NSP1 and MWP1, NSP2 and MWP2; if so, calculate all MWPSs, including the MWPSs generated due to excessive height, use NSP1 as the StP and NSP2 as the EnP, and perform position smoothing according to the Figure 7 method shown; at this time, the obtained route will necessarily meet the requirements of altitude constraints, and the optimal solution R of the UAV route planning after smoothing processing is obtained best ;

[0172] As Figure 8 shown, to meet the altitude constraint conditions, the finally output route after position smoothing and excessive height disposal is the thick solid line route from StP to EnP.

[0173] Complete a single route planning process according to the above steps S1 - S7.

[0174] Preferably, the method described in this embodiment further includes the step

[0175] S8. Multi-route planning

[0176] On the basis of obtaining the optimal route planning of the UAV single route, perform UAV multi-route planning. During the UAV multi-route planning process:

[0177] S801. It is necessary to adjust steps S1 - S2, and initialize the coordinates of the constructed waypoints (CWP) according to multi-route planning methods such as specifying waypoints on the route, specifying heading and distance, specifying waypoints and angles, and hybrid methods, to obtain the initialized coordinates of the constructed waypoints;

[0178] After obtaining the initialized constructed waypoint coordinates, perform iterative optimization according to steps S3 - S7, and finally output the optimal multi - route plan after smoothing;

[0179] Among them, the multi - route planning methods such as specifying waypoints during the journey, specifying heading and distance, specifying waypoints and angles, and the hybrid method respectively include:

[0180] 1. Route planning for specifying waypoints during the journey

[0181] Set a certain point as the midway target point (MTP), and its coordinates are MTP = [MTP x , MTP y . The planned route must pass through this point; as Figure 9 shown, assume that the line connecting StP and EnP is randomly divided into S segments. The distance l from StP to MTP can be calculated, and taking StP as the rotation point, with the line connecting StP and EnP as the starting axis, the included angle A1 as shown in Figure 8 is calculated according to the method in Table 1. Therefore, the information of the first segment is determined, that is, taking this MTP as the first CWP1; the remaining S - 1 segments are randomly determined for constructed waypoints and filled waypoints according to the aforementioned method; then it can be ensured that the planned route starts from StP, passes through MTP, and finally reaches EnP;

[0182] 2. Route planning for specifying heading and distance

[0183] This method is essentially the same as the method of specifying waypoints during the journey. As Figure 9 shown, the method of specifying waypoints during the journey is to first determine the MTP coordinates, and then determine A1 and l; while this method is to first determine [A1, l], that is, the heading and distance, and then determine the first CWP1; the remaining S - 1 segments are determined for waypoints and filled waypoints according to the aforementioned method;

[0184] 3. Route planning for specifying waypoints and angles

[0185] As Figure 9 shown, assume the total number of waypoints is D, and randomly select the i - th waypoint as the selected waypoint (SWP); specify StP as the rotation point, with the line connecting StP and EnP as the starting axis, rotate to the line connecting StP and SWP, and calculate the included angle δ as shown in Figure 9 according to the method in Table 1. At this time, find the shortest planned route that is not less than δ. Therefore, this method requires specifying [SWP, δ];

[0186] 4. Route planning for the hybrid method

[0187] The hybrid method combines the specified intermediate target points, waypoints, and angular points. First, the intermediate target point MTP is specified, and then [SWP, δ] is determined. It is stipulated that MTP is the rotation point, the line connecting MTP to EnP is the starting axis, and it rotates to the line connecting MTP to SWP, and the generated included angle δ is calculated to find the shortest planned route not less than δ.

[0188] Embodiment 2: Different from Embodiment 1, in this embodiment, experiments are carried out to verify the feasibility of the UAV three-dimensional route planning method based on the segmented random and angular random algorithm described in Embodiment 1, including the steps:

[0189] S9. To verify the UAV route planning performance of the SRAR algorithm, a complex topographic map of 100km×100km will be generated to verify the algorithm's handling of the start and end types, ultra-high recognition, route optimization, obstacle avoidance in complex obstacle areas, multi-route planning, optimization performance, etc. The relationship between the three-dimensional coordinates (x Figure 3 , y i , Z i ) is represented according to the following formula: i ) is represented according to the following formula:

[0190]

[0191] where: a, b, c, d, e are all 1×50 coefficient vectors, and their values are:

[0192]

[0193] Then the topographic map generated by Equation (12) is as Figure 10 shown, where Figure 10 (a) is a three-dimensional topographic map, Figure 10 (b) is the horizontal projection of the topographic map; Figure 10 (b) The red circles represent obstacle areas. There are a total of 24 obstacle areas. The density of the obstacle areas is large and they intersect with each other, forming a complex obstacle area distribution, which greatly increases the difficulty of route planning; at the same time, the 24 obstacle areas far exceed the number of obstacle areas set in other literatures [14,17,20] , and the obstacle areas can also have a coupling effect on the algorithm, which can effectively test the route planning quality of the algorithm;

[0194] S901. Performance verification of planning for different start and end types

[0195] According to the plane rectangular coordinate system, it can be seen that there are a total of 4 types of start and end points; find 4 pairs of start and end points on the topographic map, which are: StP1 = [50, 50], EnP1 = [6, 9]; StP2 = [50, 50], EnP2 = [10, 88]; StP3 = [50, 50], EnP3 = [96, 89]; StP4 = [50, 50], EnP4 = [98, 4], representing 4 types of start and end points; perform route planning according to the SRAR algorithm, set 2 ≤ S ≤ 4, A ∈ [-40°, 40°], Iter max = 50, Ns = 50, D = 40, ψ max = 80°, θ max = 25°, ε = 0.5km, l Lmin = 1km, Z max = 2.8km, Z best = 1km, Z rel = 0.5km,, then the 4 route plans are as Figure 11 shown; for the convenience of display and the planned height meets the constraint requirements, Figure 11 the terrain information is omitted and only the planned route is displayed on the 2D plane;

[0196] Figure 11 The 4 routes planned by the SRAR algorithm are shown. Under the condition of meeting the constraint conditions, the obstacle area is successfully avoided and the destination is reached, indicating that the SRAR algorithm can handle any type of start and end position type and can plan routes from any position to any position, which meets the actual needs of route planning; therefore, the SRAR algorithm enhances the practicability of route planning;

[0197] S902. Performance verification of ultra-high recognition

[0198] To verify the ability of the SRAR algorithm to recognize ultra-high during route planning and avoid the situation of ultra-high planning, Figure 10 (b) Remove the obstacle areas of the two highest peaks, and according to the algorithm settings in step S901, when Z max are 2.8km and 3.8km respectively, verify the ultra-high recognition performance of the planned routes between StP = [32, 95] and EnP = [84, 69], and between StP = [6, 9] and EnP = [70, 47]; as Figure 12 shown, where Figure 12 (a) Intuitively shows the route planning effect of the SRAR algorithm from the three-dimensional topographic map, Figure 12 (b) Shows the ultra-high recognition performance of the algorithm from the horizontal projection;

[0199] As Figure 12As shown in the figure, under the altitude limit of up to 2.8 km, the maximum altitudes of Route 1 and Route 3 are 2.789 km and 2.792 km respectively, and the voyage distances are 62.09 km and 77.50 km respectively; while under the altitude limit of up to 3.8 km for Route 2 and Route 4, the maximum altitudes of the routes are 3.408 km and 3.057 km respectively, and the voyage distances are 59.78 km and 74.75 km respectively. Obviously, under different altitude constraints, the algorithm will automatically select appropriate routes to meet the altitude constraint requirements; therefore, the SRAR algorithm can effectively avoid the route exceeding the height limit;

[0200] S903. Verification of the Route Smoothing Effect

[0201] In this embodiment, it is proposed to smooth the output route after optimization to improve the quality of route planning. To verify the route smoothing effect of the SRAR algorithm, according to the algorithm settings in step S901, the planned route between StP = [44, 2] and EnP = [63, 49] is selected to verify the route optimization ability; as Figure 13 shown in the figure, Route 1 is the route directly output after optimization without any smoothing treatment, with a voyage distance of 56.97 km. Route 1 has the largest voyage distance and is not very smooth; Route 2 is the route output after position smoothing and height handling of Route 1, with a voyage distance of 55.50 km. Route 2 is smoother than Route 1; while Route 3 is the route output after only position smoothing of Route 1, with a voyage distance of 54.89 km. Route 3 does not perform height handling. As Figure 13 shown in the figure, the route points between the two route points exceeding the height limit also exceed the height limit. Although the voyage distance is the shortest, it does not meet the altitude constraint conditions;

[0202] Figure 13 This well proves the necessity of smoothing the output route after optimization. However, when performing smoothing treatment, it is necessary to consider both the position and altitude constraint conditions and perform position smoothing and altitude handling to obtain a higher-quality planned route.

[0203] S904. Verification of the Obstacle Avoidance Ability and Optimization Performance in Complex Obstacle Areas

[0204] Generally speaking, a complex obstacle area means that the obstacle areas are close to each other and arranged in a crossed manner; this poses a great challenge to the route specification algorithm. At the same time, to verify the optimization performance of the non-associated iterative method proposed by the SRAR algorithm, under the route planning conditions of a complex obstacle area, the SRAR algorithm is continuously run 50 times, and the minimum value (min), mean value (mean), maximum value (max), and standard deviation (StD) of the fitness value are respectively taken to measure the optimization performance of the algorithm. According to the algorithm settings in step S901, route 1 is planned from StP = [53, 3] to EnP = [32, 95], route 2 is planned from StP = [44, 87] to EnP = [18, 11], route 3 is planned from StP = [78, 86] to EnP = [85, 5], and route 4 is planned from StP = [44, 2] to EnP = [67, 87], and the planning is continuously carried out 50 times. There are obstacle areas that cross and are closely arranged among these 4 routes. Figure 14 Select the route with the minimum fitness value among the 50 route plans. All 4 routes avoid the complex obstacle area, indicating that the SRAR algorithm has good obstacle avoidance ability, can ensure the safety of route planning, and ensure that the UAV successfully completes the task.

[0205] The statistics of the 50 calculations of the 4 planned routes are shown in Table 2, and Table 2 also gives the solutions corresponding to the minimum and maximum voyage. Table 2 shows that when planning in the face of a complex obstacle area, the routes planned by the SRAR algorithm can meet all constraints, the range change is very small, and the output routes have strong robustness and stability. Therefore, the algorithm has excellent optimization performance and can meet the requirements of complex route planning.

[0206] Table 2: Verification Table for the Optimization Ability of the SRAR Algorithm

[0207]

[0208]

[0209] S905. Verification of the performance of multi-route planning

[0210] To meet the need of route planning under different conditions, the SRAR algorithm has the ability of multi-route planning. To verify the multi-route planning ability of the SRAR algorithm, according to the algorithm setting of step S901, a route is planned between StP = [7, 40] and EnP = [95, 40]. Route 1 is planned according to the specified intermediate target point method, and the coordinates of the intermediate target point are MTP = [18, 77], and the voyage is 131.2868 km; Routes 2 and 3 are planned according to the specified heading and distance method, and the headings and distances are [60°, 25] and [-40°, 15] respectively, and the voyages are 104.6126 km and 95.3105 km respectively; Routes 4 and 5 are planned according to the specified waypoint and angle method, and the waypoint SWP = 21 is selected, and the angles are γ≥20° and γ≤-20° respectively, and the voyages are 99.6968 km and 100.8934 km respectively; Routes 6 and 7 are planned in a mixed way, the coordinates of the intermediate target point are MTP = [38, 5], the waypoint SWP = 25 is selected, and the angles are γ≥5° and γ≤-5° respectively, and the voyages are 117.6766 km and 118.8198 km respectively; Figure 15 It shows 7 routes planned by 4 multi-route methods, and all 7 routes meet the constraint requirements; Obviously, in multi-route planning, the shortest voyage is achieved on the premise of meeting the route planning method, thereby increasing the diversity and backup of route planning, meeting the planning requirements under specific mission conditions, and multi-route planning has an important practical role in actual route planning.

[0211] Embodiment 3: Different from the above embodiment, to verify the superiority of the UAV three-dimensional route planning method based on the segmented random and angle random algorithm described in this embodiment, in fact, the UAV three-dimensional route planning method based on the segmented random and angle random algorithm in this embodiment is compared with the performance of other route planning algorithms

[0212] To verify the route planning performance of the SRAR algorithm proposed in Embodiment 1 and prove the rationality, practicability, and scientificity of the SRAR algorithm design, the SRAR algorithm is compared with other route planning methods based on swarm intelligence. It is respectively compared with the route planning method based on the QPSO algorithm, the route planning method based on the GA algorithm, and the route specification method based on the ACO algorithm. Both the method of continuously calculating 50 times is used to compare the optimization performance of the algorithms, and the route planning performance in response to different obstacle areas is also compared. Four algorithms will plan four routes. Route 1 is planned between StP = [2, 40] and EnP = [57, 30], Route 2 is planned between StP = [63, 98] and EnP = [98, 4], Route 3 is planned between StP = [34, 34] and EnP = [45, 76], and Route 4 is planned between StP = [32, 95] and EnP = [53, 3]. These four routes have their own characteristics. Route 1 is a simple route and no obstacle areas need to be avoided on the route. Route 2 is an ordinary route with a longer voyage and it is easy to avoid obstacle areas on the route. Route 3 and Route 4 belong to complex routes and have higher requirements for obstacle avoidance ability. The difference between them lies in the length of the voyage.

[0213] Each constraint condition for route planning is as shown in step S7 of Embodiment 1; the parameter settings of the SRAR algorithm are as shown in step S901 of Embodiment 2; the Iter of the QPSO algorithm, AC algorithm, and GA algorithm max = 100, Ns = 200; essentially, the above three algorithms optimize the position coordinates of each route point. The number of route points of the QPSO and GA algorithms is fixed, set D = 40, while the ACO algorithm uses the grid method, and the number of route points depends on the equal division quantity of the fixed coordinate axes. The comparison of the route planning quality of each algorithm is shown in Table 3. Table 3 includes the average time consumption (elapse time, ET) per calculation under the same operating conditions for each algorithm, with the unit of seconds (s); the number of parameters to be optimized (number of parameters, NP); the minimum value (min), mean value (mean), maximum value (max), and standard deviation (standard deviation, StD) of the voyage calculated 50 times; the number of times of violating the constraint conditions (constraint violations, CV) in 50 calculations; the number of times of violating each constraint condition is statistically counted and represented by a 1×6-dimensional vector CV. The elements in CV correspond to the number of times of violating the 3.1 economy constraint condition in sequence; as shown in

[00060] in Table 3, it means violating the 4th constraint condition 6 times, that is, violating the altitude constraint.

[0214] Table 3: Comparison of Route Planning Performance of Four Algorithms

[0215]

[0216] Table 3 shows that: in different route planning scenarios, in the face of multiple constraints, the SRAR algorithm has the least time consumption, the fewest optimization parameters, the most stable performance, the strongest robustness, and the highest route planning quality, which strongly proves the reasonable and effective design of the SRAR algorithm; due to factors such as their own principles, the other three algorithms are difficult to handle complex route planning problems with multiple constraints; Figures 13 - 19 It respectively shows the comparison of the routes with the shortest voyage obtained by different algorithms when planning 4 routes, visually showing the route planning quality of the SRAR algorithm; the reasons for the low planning quality of the other three algorithms are: in addition to the optimization performance of the algorithms themselves, mainly when adjusting the coordinate positions of waypoints to achieve complex and multi-constraint route planning, it is very difficult to ensure that the coordinates between all waypoints obtain appropriate positions that are coordinated and consistent; while the SRAR algorithm cleverly determines the coordinates by selecting angles, thus avoiding the common problems caused by directly adjusting the waypoint coordinates and ensuring the quality of route regulations.

[0217] Combined with the results of the above-mentioned Embodiment 2 and Embodiment 3, it can be seen that the UAV three-dimensional route planning method based on the segmented random and angle random algorithm described in Embodiment 1 of the present invention can be used to plan the three-dimensional routes of UAVs in complex spaces with multiple constraints, effectively solving problems such as restrictions on the types of start and end positions, uneven spacing of waypoints, poor route smoothness, weak ability to handle obstacle avoidance with excessive height, and single route planning in route planning, improving the route planning quality under complex multi-constraint conditions, and meeting the requirements of diverse route planning.

[0218] The above shows and describes the basic principles, main features and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited by the above embodiments. What is described in the above embodiments and the specification only illustrates the principles of the present invention. Without departing from the spirit and scope of the present invention, the present invention will have various changes and improvements, and these changes and improvements all fall within the scope of the present invention claimed. The scope of protection claimed by the present invention is defined by the appended claims and their equivalents.

Claims

1. A three-dimensional route planning method for unmanned aerial vehicles based on a segmented random and angle random algorithm, characterized in that: Including steps S1. Use the method of random equal division to equally divide the connection line between the start and end points of the UAV route, and obtain the coordinates of the segmented points of the connection line between the start and end points on the horizontal projection plane after equal division; S2. After obtaining the coordinates of the segmented points, use the method of random angles to initialize the coordinates of the constructed route points on the UAV route, as well as the number and coordinates of the filling route points, and determine the initial route; S3. Determine whether there are route points in the obstacle area in the initial route obtained in step S2. If so, transfer to step S4; if not, let the obstacle avoidance route = IR, and transfer to step S7; S4. Determine the moving route points according to the obstacle avoidance method of the SRAR algorithm, so that the initial route points move out of the obstacle area to form the obstacle avoidance route OAR; S5. Calculate the fitness values of all the obstacle avoidance routes OAR formed in each iteration, and assign a penalty coefficient to the solutions that do not meet the constraint requirements; The process of calculating the fitness values of all the obstacle avoidance routes OAR formed in each iteration in step S5 and assigning a penalty coefficient to the solutions that do not meet the constraint requirements includes S501. Design an optimization model considering the constraint conditions: Among them, ψ represents the constraint of the horizontal turning angle, θ represents the constraint of the pitch angle, l Li represents the constraint of the flight segment length, z i represents the constraint of the waypoint altitude, n(i) represents the number of elements in the i-th moving waypoint set, that is, the number of waypoints in the i-th obstacle area, Z max is the maximum flight altitude, l Lmin is the minimum flight segment length; θ max is the maximum pitch angle; ψ max is the maximum horizontal turning angle, D is the total number of waypoints; S502. The fitness function fit(·) is expressed as: Among them, S = [S, A1, …, A i , …, A S-1 , R represents the set of planned waypoint coordinates, S→R represents the set of coordinates of each waypoint in the obstacle avoidance route determined by S, n st represents the number of unsatisfied constraint conditions, P st represents the penalty coefficient when any constraint condition is not satisfied, P st takes a larger number. When all constraint conditions are satisfied, the fitness value is equal to the voyage distance; S6. Update the population using a non-associated update iteration method until the optimal solution R of the population is output at the end of the iteration opt ; S7. Smooth the optimal solution R output at the end of the iteration, and finally output the best solution R for the UAV route planning opt to complete a single route planning. best ​ 2. The UAV three-dimensional route planning method based on the segmented random and angular random algorithms according to claim 1, wherein: The process of equally dividing the connection line between the start and end points of the UAV route in step S1 and obtaining the coordinates of the segmented points of the connection line between the start and end points on the horizontal projection plane after equal division includes S101. Let the angle between the x-axis and the connection line between the start and end points of the UAV route be α. If it is equally divided into S segments, each segment has a length of l, S - 1 segmented points are generated, and S - 1 straight lines pass through the segmented points and are perpendicular to the connection line between the start and end points; S102. Let the set of segmentation points be [StP, SeP1, …, SeP i , …, SeP S-1 , EnP], with a total of S + 1 points, including the start point and the end point. The coordinates of the segmentation points on the horizontal projection plane can be calculated based on trigonometric functions.

3. The UAV three-dimensional route planning method based on the segmented random and angle random algorithms according to claim 2, characterized in that: The process of determining the initial route in step S2 includes The set of segmentation points [StP, SeP1, …, SeP i , …, SeP S-1 , EnP] obtained in step S1, except for the two points SeP S-1 , EnP in the set of segmentation points, taking the points in the set of segmentation points as rotation points in sequence, taking the connection line of the head and tail as the starting axis, randomly generating an angle A clockwise or counterclockwise, and forming a constructed waypoint from the rotation point along the connection point of the angle A and the perpendicular line, then S - 1 constructed waypoints can be formed in sequence; S202. The start and end points and each constructed waypoint form a set of constructed waypoints [StP, CWP1, …, CWP i , …, CWP S-1 , EnP]. Connecting each point in the set of constructed waypoints initializes a flight path on the horizontal projection plane; uniformly filling points between each point in the set of constructed waypoints to form filled waypoints, and then determining the coordinates of the constructed waypoints; S203. Use the start and end points, constructed route points, and filling route points obtained in step S202 to jointly form a complete initial route; S204. The initial solution randomly generated on the horizontal projection plane of the UAV route after equal division is S ij = [S, A1, …, A i , …, A S-1 , Among them, S ij represents the j-th feasible solution at the i-th time, S represents the number of segments, and A i is the angle of the i-th waypoint; S205. Let the length of the line connecting the start and end points of the route be L, and the length of each segment be l = L / S. The coordinates of the start point are StP = (St x , St y ), and the coordinates of the end point are EnP = (En x , En y ). The coordinates of the i-th segmentation point SeP i = (Se ix , Se iy ) are as follows: S206. Coordinates CWP of the i-th constructed waypoint i =(CW ix , CW iy ) is calculated by the following formula: In formula (2), i = 1, …, S - 1. If i - 1 = 0, then Se (i-1)x＝ St x , Se (i-1)y＝ St y ; sign(·) is the sign function. When En x -St x ≥0, sign(En x -St x ) = 1; otherwise, sign(En x -St x ) = -1; After determining the coordinates of the constructed waypoints, uniformly insert and fill waypoints between adjacent points in the set of constructed segment points according to the method of formula (1); assuming that the total number of waypoints is D, including the start point and the end point, the number of filled waypoints is D - S - 1, and there are S segments in total, and the number of filled waypoints in each segment is D i is as follows: In formula (3), i = 1, …, S - 1. If i - 1 = 0, then Se (i-1)x＝ St x , Se (i-1)y＝ St y , and round() is a rounding function; S208. By randomly generating S parameters and using equations (1)-(3), complete the determination of the coordinates of the constructed route points, the number and coordinates of the filling route points, form a complete route, and determine the initial route IR.

4. The method for three-dimensional route planning of an unmanned aerial vehicle based on a segmented random and angular random algorithm according to claim 3, wherein: The calculation process of the obstacle avoidance route OAR in step S4 includes S401. Let WP1 to WP4 be a flight path passing through OA. WP2 and WP3 are within OA. The flight path must avoid OA. Using the line connecting WP1 to WP4 as the horizontal axis and WP1 as the origin, three vectors are formed. According to the vector operation rules, The modulus of is the distance from MWP1 to CP; S402. With WP1 as the rotation point and the line connecting WP1 to WP4 as the starting axis, calculate the angle α between the line connecting WP1 to WP4 and the line connecting WP1 to CP. At the same time, calculate the distance l between WP1 and WP2 m , and the distance l between WP1 and CP c ; obtain with a modulus of l c , a complex angle of α, with a modulus of l m / cos(γ), and a complex angle of γ; S403. Use equation (4) to find γ: Among them, in equation (4), ε represents the safety margin; After equation (4) is sorted out: From equation (5), the unique solution of γ can be obtained, and γ is the smallest angle that makes equation (5) hold. r represents the equivalent circular radius of the obstacle area; After the value of γ is determined, the coordinates of MWP1 can be obtained according to formula (2); similarly, the coordinates of MWP2 can be obtained; form a route that completely avoids the obstacle area.

5. The method for three-dimensional route planning of an unmanned aerial vehicle based on the segmented random and angular random algorithms according to claim 4, characterized in that: When the route passes through WP1 in sequence to WP4, and then to WP7 in sequence: S404. Connect WP1 and WP7, and then equally divide the connection line according to the original number of route segments; finally, according to the methods described in steps S401 - S403, construct an obstacle avoidance route from WP1 in sequence to MWP1,..., MWP5, WP7.

6. The method for three-dimensional route planning of an unmanned aerial vehicle based on a segmented random and angle random algorithm according to claim 1, wherein: The process of performing population update using the non-associated update iteration method as described in step S6 until the optimal solution R of the population is output at the end of iteration opt includes Let the maximum number of iterations be Iter max , the number of feasible solutions be Ns, and S opt represent the optimal solution, and S ij be the j-th feasible solution at the i-th iteration, then i ≤ Iter max , j ≤ Ns, if min(fit(S ij → R ij )) ≤ fit(S opt → R opt ), then let S opt = S ij , and R opt = R ij , until the iteration termination condition is satisfied, and output the optimal solution R opt .

7. The method for three-dimensional route planning of an unmanned aerial vehicle based on a segmented random and angle random algorithm according to claim 1, characterized in that: The process of smoothing the optimal solution R output at the end of iteration in step S7 opt The process of smoothing includes S701. Smooth the position of the optimized solution; S702. Perform ultra-high processing on the optimized solution after position smoothing to obtain the optimal solution R for the UAV route planning after smoothing processing best .

8. The method for three-dimensional route planning of an unmanned aerial vehicle based on the segmented random and angular random algorithms according to claim 1, wherein: The planning method also includes steps S8. Multi-route planning Adjust steps S1 - S2, and initialize the coordinates of the constructed waypoints according to the multi - route planning method of the specified target point in the figure, the specified course and distance, the specified waypoint and angle, and the hybrid method to obtain the initialized coordinates of the constructed waypoints; After obtaining the initialized coordinates of the constructed waypoints, perform iterative optimization according to steps S3 - S7, and finally output the optimal multi - route plan after smoothing.

Images