A Fusion Optimization Method for Multi-UAV Cooperative Trajectory Planning

Abstract

The present invention provides a fusion optimization method for multi-UAV cooperative path planning, constructs a UAV motion model, initializes the information of our own UAVs and established targets, obtains the state of the UAVs themselves and the relative state between the UAVs and the targets, forms the total state, and inputs the total state into an optimization algorithm model that integrates the optimized A* algorithm, model predictive control algorithm, and Standoff algorithm to complete the multi-UAV formation planning. By leveraging the advantages of different intelligent algorithms, the present invention formulates fusion algorithm rules to form a fusion algorithm with more excellent performance, making up for the functional shortcomings of single algorithms; the optimized fusion algorithm is applied to multi-UAV cooperative path planning in a three-dimensional complex environment, enabling real-time collision avoidance and formation control for the UAV swarm path planning, maximizing the detection coverage range of the UAVs, and obtaining a safer and more feasible path planning.

Description

Technical Field

[0001] The present invention relates to the field of autonomous control of unmanned aerial vehicles, and particularly to a method for multi-unmanned aerial vehicle trajectory planning based on the fusion of an optimized A* algorithm, a model predictive control algorithm, and a Standoff algorithm. Background Art

[0002] With the increasingly complex task environment and increasing task requirements, it is difficult for a single unmanned aerial vehicle to complete a specific flight task. The intelligent cooperative control of multi-unmanned aerial vehicles has gradually become the research focus in the field of unmanned intelligence. Among them, trajectory planning, as a key technology for studying the cooperative control of multi-unmanned aerial vehicles, especially the research on multi-unmanned aerial vehicle cooperative trajectory planning in complex environments, has extremely important significance.

[0003] In recent years, relevant researchers have achieved some research results in multi-unmanned aerial vehicle cooperative trajectory planning. For example, algorithms such as the A* algorithm, the artificial potential field method, the bionic algorithm, and the control algorithm have been applied to unmanned aerial vehicle trajectory planning. As the core algorithm for offline unmanned aerial vehicle trajectory planning, the A* algorithm has good global planning ability, but it is only applicable to path search with known obstacles, cannot avoid sudden obstacles in real time, and the application scenarios of the algorithm are mostly two-dimensional environments; when the artificial potential field method is applied to unmanned aerial vehicle cooperative trajectory planning, it is easy to fall into local optimality, and it is difficult to establish a complete mathematical model; the bionic algorithm mainly includes the ant colony algorithm and the particle swarm algorithm. Due to the low calculation efficiency of the bionic algorithm, it is difficult to meet the real-time requirements of unmanned aerial vehicle trajectory planning; the control algorithm mainly includes PID control, optimal control, sliding mode control, and model predictive control. Among them, the model predictive control algorithm, as the only control method that can explicitly handle constraints at present, has become the recognized standard for dealing with complex constrained variable control problems. However, when the model predictive control algorithm is applied to unmanned aerial vehicle trajectory planning, there is a problem of weak global path planning ability, and it is only applicable to local path planning with good real-time performance. To sum up, the current research on unmanned aerial vehicle trajectory planning generally has problems such as large model calculations, weak global planning ability, insufficient real-time performance, single application environment, and poor formation and maintenance ability of formations, which has a certain gap from the real task requirements. Summary of the Invention

[0004] In order to overcome the deficiencies of the prior art, the present invention provides a fusion optimization method for multi-unmanned aerial vehicle cooperative trajectory planning. In order to overcome the deficiencies of a single algorithm, the present invention provides an intelligent optimization algorithm based on the fusion of an optimized A* algorithm, a model predictive control algorithm, and a Standoff algorithm, realizing multi-unmanned aerial vehicle cooperative trajectory planning in a three-dimensional complex environment.

[0005] This invention comprehensively considers the global planning and real-time planning capabilities of UAV trajectories, integrates and optimizes the A* algorithm, model predictive control algorithm and Standoff algorithm, draws on their strengths and overcomes their weaknesses, and proposes an intelligent fusion algorithm that solves the problems of weak global planning capabilities, poor real-time performance and insufficient formation formation and maintenance capabilities of UAV trajectories. The fusion algorithm has high autonomy and good robustness, and is more in line with real mission requirements.

[0006] The technical solution adopted by the present invention to solve the technical problem includes the following steps:

[0007] Step S1: Build a UAV motion model

[0008] In three-dimensional space, the drone's position, velocity, pitch angle, and heading angle together construct the drone's motion model. Let the OXYZ coordinate system be the three-dimensional space coordinate system where the drone is located, where the origin O represents the center of the drone's mission area, the X-axis points to the north, the Z-axis points to the east, and the Y-axis points vertically upward.

[0009] When examining the motion of a drone, the drone is considered as a point mass and a three-dimensional discretized drone kinematic model is established. The sampling time is △t, and the drone motion equation is as follows:

[0010]

[0011] Among them, (x(k), y(k), z(k)) represents the position coordinates of the UAV at time k; (x(k+1), y(k+1), z(k+1)) represents the position coordinates of the UAV at time k+1; v(k) represents the real-time speed of the UAV at time k; s(k) represents the state sampling of the UAV at time k; S represents the feasible state set; u(k) represents the decision input of the UAV at time k; U represents the feasible input set; the pitch angles θ(k) and θ(k+1) are the angles between the velocity vector of the UAV and the XOZ plane at time k and time k+1 respectively; the heading angle and is the angle between the projection vector of the velocity vector of the UAV at time k and time k+1 on the XOZ plane and the positive direction of the X axis; a(k+1) represents the acceleration of the UAV at time k+1;

[0012] Step S2: Initialize the information of our UAV and the established target, obtain the state of the UAV itself and the relative state of the UAV and the target, and form the total state Q all ;

[0013] The drone's own state includes the position component (x r ,y r ,z r ), UAV speed v r , the pitch angle of the drone θr The heading angle of the UAV The relative state between the UAV and the target includes the relative distance d r and the relative azimuth angle q r ; The total state composition is

[0014] Step S3: Input the total state Q all into the optimized algorithm model based on the fusion of the optimized A* algorithm, model predictive control algorithm and Standoff algorithm. The hierarchical structure of the fusion algorithm is as Figure 1 shown; The multi-UAV cooperative trajectory planning system of the present invention is divided into a trajectory planning layer and a path planning layer. The trajectory planning is carried out based on the optimized A* algorithm, and the result is a static geometric path independent of time; The path planning is carried out based on the fusion of the model predictive control algorithm and the Standoff algorithm. The model predictive control algorithm solves the problem of large-scale real-time optimal control within a finite time, and realizes optimized motion control by using preview ability in a constrained, non-linear, model-uncertain and unforeseen environment, generating a real-time path planning applicable to the actual flight of multi-UAV formations. As one of the main formation control algorithms, the Standoff algorithm maximizes the sensor detection range based on the safety distance and reduces the probability of target loss;

[0015] Step S4: Multi-UAV formation planning. Implant the sudden obstacle model into the prior static obstacle model to verify the real-time obstacle avoidance ability of the fusion algorithm applied to multi-UAV cooperative trajectory planning. The specific steps of multi-UAV cooperative formation planning are as follows:

[0016] The specific trajectory planning process of step 3 is as follows:

[0017] S31: Considering the UAV collision avoidance constraints comprehensively, construct a multi-UAV global trajectory planning model in three-dimensional space based on the A* algorithm; Carry out sparse variable-step curve optimization of the A* algorithm based on the compressed search space and smoothing processing method; Add the relevant cost weights of the A* algorithm heuristic functions G(x) and H(x), and introduce the weight method to improve the algorithm operation efficiency; Based on this, the optimized A* algorithm is applied to the multi-UAV global trajectory planning in three-dimensional space, generating a global route planning composed of multiple intermediate waypoints, and hand over the intermediate waypoints to the route storage unit in the S32 stage;

[0018] S32: Take out the next waypoint of the current UAV position from the global trajectory planning points saved in step S31 as the temporary target point for the model predictive control algorithm to apply to the UAV real-time trajectory planning, and carry out the UAV local real-time path planning based on the model predictive control model;

[0019] S33: During the local real-time path planning process, the Standoff algorithm based on the Lyapunov vector field is used to control the formation flight of the UAV swarm, maximizing the UAV sensor observation coverage;

[0020] S34: Establish a three-dimensional complex environment with prior obstacle information, construct an obstacle model, and verify the effectiveness and real-time performance of the fusion optimization algorithm model applied to multi-UAV cooperative path planning;

[0021] The global path planning of the UAV in step S31 is carried out based on the optimized A* algorithm model, and the specific modeling is as follows:

[0022] S31-1 Initialize the basic information of the UAV, including the starting point (x1, y1, z1), speed v1, and heading angle pitch angle θ1 of the UAV; establish a search fan of the UAV sensor, the search radius of the fan is the current UAV search step, the vertical angle along the route is set to l1 times the maximum pitch angle, and the horizontal angle is set to l2 times the maximum heading angle; when the search fan is in a safe area, the maximum step L is preferentially selected max Carry out the search. If the search fan contains no-fly zones, based on the criterion that the number of no-fly zones contained in the search fan is inversely proportional to the search step, dynamically adjust the search step, that is: the more the number of no-fly zones, the shorter the search step. Initialize the UAV search fan range, determine the number of sub-sectors, determine the step selection, etc. The specific modeling process is as follows:

[0023] To simplify the modeling operation, it is assumed that our UAV and the target always remain in the same plane. At this time, the search task is compressed to a two-dimensional plane; the current position of our UAV is denoted as point B, the position of target j is denoted as point C, the movement direction of the UAV is BP, and the maximum range angle of the UAV search fan is set as ∠PBD, denoted as BC represents the flight direction of the UAV to trace the target, and ∠PBC represents the deviation angle of the UAV to trace target j. The UAV search fan range is simplified to an angle constraint, and the specific calculation is as follows:

[0024]

[0025] Among them, represents the UAV search fan angle, and the size of the search fan angle needs to be determined according to the following formula:

[0026]

[0027] Among them, L is the search step; v represents the UAV speed magnitude; ω max represents the maximum constraint angular velocity of the UAV; α is the step adjustment proportionality coefficient.

[0028] When the search step size is L, the number m of sub-sectors to be searched and expanded is calculated as follows:

[0029]

[0030] Among them, represents the sector angle of the search sub-sector;

[0031] Determine the current search step size. The A* algorithm variable step size optimization satisfies the following formula:

[0032] L max = L min + m max ·△L

[0033] Among them, L max represents the maximum search step size of the algorithm; L min represents the minimum search step size of the algorithm; m max represents the number of sub-sectors corresponding to the maximum step size; △L represents the unit transformation step size;

[0034] The variable step size A* optimization algorithm is specifically modeled as:

[0035]

[0036] Among them, m u represents the number of expanded nodes that fall into the no-fly zone when using the current step size of the algorithm for trial; β represents the step size adjustment coefficient, satisfying 1 ≤ β ≤ m max .

[0037] S31-2 A* algorithm node sparse optimization. According to the UAV flight constraints, including the minimum track segment length and the maximum turning angle constraint, the search space is reduced. The specific steps are as follows:

[0038] 1) Determine the expandable area from the current node. The search radius is the current UAV search step size. Set the vertical angle along the route to k1 times the maximum climb / dive angle, and the horizontal angle is set to k2 times the maximum course angle;

[0039] 2) Divide the sectors in the climb / dive direction of the expandable area into K1 equal parts, and divide the fan surfaces in the turning direction into K2 equal parts to obtain K1*K2 points; The values of K1 and K2 should be selected appropriately. Selecting K1 and K2 between [3, 5] can meet the requirements. The larger K1 and K2 are, the greater the probability of finding an ideal route, but at the same time, it increases the storage memory requirement and search time. Take the K1*K2 points as the nodes to be expanded;

[0040] 3) Calculate the cost value from the current node to the expanded node, and select the node with the minimum cost function on each fan surface;

[0041] 4) For the nodes selected in step 3), determine whether they meet the constraints of the minimum near-ground safety distance and the maximum flight safety distance of the UAV. If they do not meet the requirements, discard them directly; if they meet the requirements, perform trajectory planning based on these nodes.

[0042] S31-3 A* algorithm trajectory curve optimization; use the turning shortcut curve method to replace the UAV's broken-line trajectory, determine the center C(k) of the tangent sphere of the trajectory point, judge the turning direction of the trajectory point, and obtain the tangent point coordinates M1 and M2.

[0043] 1) Determine the tangent sphere C(k) of the trajectory point

[0044] The coordinate position of the turning point A of the i-th UAV i is (x i , y i , z i ). Set the heading angle and pitch angle θ r of the UAV. The turning radius of the trajectory is R. The calculation formula for the coordinate of the center of the turning tangent sphere of the UAV is as follows:

[0045]

[0046] Among them, C 顺 (k) represents the coordinate of the center of the clockwise turning sphere; C 逆 (k) represents the coordinate of the center of the counterclockwise turning sphere; θ r and represent the pitch angle and heading angle of the UAV; (x i (k), y i (k), z i (k)) represents the position coordinate of the i-th UAV at the turning moment;

[0047] 2) Judge the turning direction of the trajectory point;

[0048] Use the mixed vector product algorithm to judge the turning direction of the two adjacent segments of the trajectory before and after the turning point A i of the UAV. The current trajectory point is the turning point A i of the i-th UAV, then the previous trajectory segment is represented as A i-1 A i , and the next trajectory segment is represented as A i A i+1 , δ = [δ x , δ y , δ z T and ε = [ε x , ε y , ε z T are the unit vectors in the directions of the two adjacent trajectory segments respectively. Determine the turning direction of the UAV by calculating the following formula:​​

[0049]

[0050] Among them, sign represents the sign function;

[0051] ①γ(δ,ε)<0: indicates that the current UAV is along the track segment A i-1 A i Fly to A i A i+1 A counterclockwise turn is required;

[0052] ②γ(δ,ε)>0: indicates that the current UAV is along the track segment A i-1 A i Fly to A i A i+1 A clockwise turn is required;

[0053] ③γ(δ,ε)=0:indicates the current track segment A i-1 A i Fly to A i A i+1 On the same straight line, but according to the constraints of the drone's maximum heading angle and maximum pitch angle, the drone will not turn 180 degrees, so it means that the drone is going straight and does not need to turn, that is, the current track node is a non-track turning point;

[0054] 3) Obtain the coordinates of the tangent point

[0055] During the UAV track turning smoothing process, the turning arc is required to be consistent with the previous track segment A. i-1 A i and the next track segment A i A i+1 The tangent points are set as M1 and M2. After the drone track is smoothed, the drone track is changed from the original two segments A i-1 A i With A i A i+1 Become the following three sections:

[0056] ①A i-1 The M1 flight path segment continues to maintain a straight flight state;

[0057] ② From M1 to M2, the flight path is performed in a circle with a radius of R;

[0058] ③M2A i+1 The track segment performs straight-line flight.

[0059] According to the calculation formula of the center coordinates of the UAV turning ball, the coordinates of the center position of the turning ball C are calculated. i , the radius of the sphere is R, set q i ,q i+1They are respectively the track segment A i-1 A i and A i A i+1 The unit vectors on A, the vector q' and the vector q i are perpendicular, the vector q″ and the vector q i+1 are perpendicular. Assuming that the vector q i to q i+1 is clockwise rotation, the coordinates of the tangent points M1 and M2 are calculated by the following formula:

[0060]

[0061] S31-4 uses the weight method to optimize the A* algorithm, adding the weights a g , b h of the relevant costs of H(x) and G(x), and the optimized heuristic function is expressed as the following formula:

[0062]

[0063] Among them, G(x) is a function of the true cost of the UAV from the starting position to the current position, and H(x) represents a function of the estimated cost of the UAV from the current position to the target point; a g , b h are respectively the weights of the true cost and the estimated cost, and the value range of a g is [0.5, 1], and c div is the weight ratio, and its value is greater than 1;

[0064] In three-dimensional space, the extended calculation of G(x) is as follows:

[0065]

[0066] Among them, l represents the side length of the map grid;

[0067] In three-dimensional space, H(x) is expressed as follows:

[0068]

[0069] Among them, (x r , y r , z r ) represents the current UAV track coordinates; (x b , y b , z b ) represents the target position coordinates;

[0070] Step S32 extracts the global trajectory planning point information of the UAV, takes the next waypoint of the current UAV position as the temporary end point of the local real-time path planning, and uses the model predictive control algorithm to complete the real-time planning of the UAV; after the UAV reaches the temporary target point, it then takes out the next waypoint from the trajectory storage unit as the target point for path planning, and continues this process until it reaches the last waypoint of the UAV trajectory. The local real-time path planning process is as follows:

[0071] S32-1: Initialize the discrete motion model of the UAV;

[0072] S32-2: Take out the next waypoint of the current UAV position from the global trajectory planning information and set it as the temporary target point of the local path;

[0073] S32-3: Use the model predictive control algorithm to construct the local path planning model of the UAV;

[0074] S32-4: Based on the fusion of the optimized A* algorithm and the model predictive control algorithm, realize the global real-time trajectory planning of the UAV;

[0075] The modeling of the model predictive control algorithm is as follows:

[0076]

[0077] Among them, and are the velocity and angular velocity control inputs of the i-th UAV within the prediction horizon H; v i (k + p|k) is the current velocity of the UAV; v i (k + p + 1|k) is the velocity of the UAV within the prediction horizon; ω i (k + p|k) is the angular velocity of the UAV; ω i (k + p + 1|k) is the angular velocity of the UAV within the prediction horizon; is the current heading angle of the UAV; is the heading angle of the UAV within the prediction horizon; θ i (k + p|k) is the current pitch angle of the UAV; θ i (k + p + 1|k) is the pitch angle of the UAV within the prediction horizon; (x i (k + p + 1|k), y i (k + p + 1|k), z i (k + p + 1|k)) are the three-dimensional position coordinates of the UAV within the prediction horizon; (x i (k + p|k), y i (k + p|k), z i (k + p|k)) are the three-dimensional motion coordinates of the current UAV; τ v and τ ωare the proportional adjustment factors of the UAV speed and angular velocity respectively, and the sampling time is △t;

[0078] The modeling constraints for the real-time UAV trajectory planning based on the model predictive control algorithm are as follows:

[0079]

[0080] Among them, and are the speed and angular velocity control inputs of the i-th UAV within the prediction horizon H; and are the maximum and minimum speed constraints of the UAV, is the maximum angular velocity constraint of the UAV; (x i (k + p + 1|k), y i (k + p + 1|k), z i (k + p + 1|k)) are the three-dimensional position coordinates of the formation UAV i within the prediction horizon; (x j (k + p + 1|k), y j (k + p + 1|k), z j (k + p + 1|k)) are the three-dimensional position coordinates of the formation UAV j within the prediction horizon; R a is the minimum safe distance of the UAV; (o xj , o yj , o zj ) is the center position of the obstacle, o rj is the radius of the obstacle; (x i (k), y i (k), z i (k)) are the initial position coordinates of the UAV, v i (k) is the initial speed of the UAV, is the initial heading angle of the UAV, θ i (k) is the initial pitch angle of the UAV. At time k, solve the above model predictive control algorithm model to obtain the optimization results of the UAV speed and angular velocity control quantities, and output the results to the UAV motion control unit;

[0081] In the process of the UAV local real-time trajectory planning in step S33, the Standoff algorithm is integrated to control multiple UAVs to carry out cooperative formation planning to achieve the best search formation maintenance of multiple UAVs. The specific process is as follows:

[0082] S33-1: Initialize the UAV cluster information;

[0083] S33-2: Initialize the global UAV trajectory planning using the optimized A* algorithm, and carry out the local real-time optimal trajectory planning using the model predictive control algorithm;

[0084] S33-3: During the process of local real-time optimal trajectory planning, the Standoff algorithm is applied to the formation and maintenance control of UAV formations, enabling the UAV swarm to be evenly distributed around the target, and then maximizing the search and monitoring of multi-UAV formation sensors. The specific modeling is as follows:

[0085] Based on the Standoff algorithm, multi-UAV formation planning is carried out. The UAV formation flies in a spiral shape with a rotation radius of D. r , and the corresponding Lyapunov energy function is the distance function L d (x, y, z), as shown in the following formula:

[0086]

[0087] where r is the radial distance between the UAV position (x r , y r , z r ) and the rotation center position (x d , y d , z d ), ξ represents the allowable error of formation coordination;

[0088] For simplicity of calculation, the present invention assumes that the multi-UAV formation is distributed in the same plane. Therefore, only the influence of phase angle positioning needs to be considered: the phase angles of any two UAVs are φ i and φ j , and the expected relative phase angle is φ z . Then, based on the Lyapunov stability theory, the multi-UAV cooperative formation phase function is calculated as follows:

[0089]

[0090] where μ p represents the difference between the relative phase angle of two UAVs and the expected relative phase angle, and N represents the number of UAVs in the formation.

[0091] The UAV phase angular velocity is calculated as follows:

[0092]

[0093]

[0094] where is the phase angular velocity of UAV i, is the phase angular velocity of UAV j, v r is the real-time speed of the UAV, and k is the function coefficient;

[0095] The UAV speed is calculated as follows:

[0096] v i = v r

[0097] v j = k·(φ i - φ j - φ z )·D r + v r

[0098] where v i is the speed of UAV i, and v j is the speed of UAV j;

[0099] The optimal expected speed of the UAV is calculated by combining the predicted speeds of multiple UAVs with the speed correction term of the rotation center. The calculation expression is as follows:

[0100]

[0101] where is the optimal expected speed value of the UAV, is the predicted speed value of the UAV, is the speed correction value of the rotation center. The optimal predicted speed v t , the optimal heading angle and the optimal pitch angle θ t are calculated according to the following formula:

[0102]

[0103]

[0104]

[0105] The multi-UAV collaborative formation planning in step S4 is as follows:

[0106] S4-1: Construct a UAV flight environment model;

[0107] S4-2: Based on the UAV flight environment model, establish a UAV formation collision avoidance constraint model. The collision avoidance constraint model includes the minimum safe distance between UAVs, the minimum safe distance from the ground for UAVs, and the minimum safe distance from obstacles for UAVs. Real-time judgment is made on whether there are emergencies affecting the safety planning of the UAV formation. If the emergency makes the UAV unable to continue flying according to the established formation plan, go to step S4-3. If there is no emergency or there is an emergency but it does not affect the safety planning of the UAV formation, continue with the established collaborative flight path planning;

[0108] S4-3: Conduct multi-UAV formation reconstruction, analyze the triggering conditions and costs of multi-UAV formation reconstruction, design UAV formation reconstruction planning based on the minimum cost objective, and conduct multi-UAV formation expected position planning based on the Standoff algorithm. By comparing the actual position information with the expected position information, the "feedback-correction" mechanism is used to correct the UAV position;

[0109] The steps of constructing the UAV flight environment model in step S4-1 are as follows:

[0110] Considering that drones need to fly at ultra-low altitudes to avoid radar detection, complex ground environments and obstacles become the main threats to drone trajectory planning. This paper establishes a map model based on the uneven terrain. To improve the robustness of the method, a safety buffer zone is set around the drone. At the same time, a static obstacle model and a sudden obstacle model are established. The static obstacle model is approximated by a cylinder, and the center of the plane is set as P o , the coordinates are [P ox ,P oy ], radius and height are expressed as P or With P oz Indicates that a collision zone is set up around (with L od and △H od Indicated by) and threat area (indicated by L OD and △H OD (L) od Expressed as the minimum approach safety distance, △H od It is expressed as the minimum height approach distance, that is, the distance between the drone and the static obstacle is less than L od or △H od When the UAV collided; L OD Expressed as the maximum threat distance of static obstacles, △H OD It is expressed as the maximum threat height of static obstacles, that is, the distance between the UAV and the static obstacle is less than L OD or △H OD When the UAV is in danger of collision, the sudden obstacle model is approximated by a sphere, and the center of the sphere is set to P t , the specific coordinates are [P tx ,P ty ,P tz ], with a radius of R t , also set up a collision zone (with a radius of R p The threat zone (represented by a sphere with a radius of R w sphere representation);

[0111] Step S4-2 constructs a multi-UAV formation constraint model;

[0112] Based on the UAV flight environment model, a UAV formation collision avoidance constraint model is established, as shown in the following formula:

[0113]

[0114] Among them, (x i (k),y i (k),z i (k)) represents the current UAV position coordinates; R a Indicates the minimum collision avoidance safety distance of the drone; (x j (k),y j (k),z j (k)) represents the position coordinates of the adjacent UAVs; z all (k) represents the ground coordinate (x i (k),y i (k)) height; △H d Indicates the minimum safe distance of the UAV near the ground; [P tx ,P ty ,P tz ] is the coordinate of the sudden obstacle position; R w is the radius of the sudden obstacle threat area; [P ox ,P oy ] is the center coordinate of the static obstacle model plane, P oz is the static obstacle height;

[0115] Multi-UAV formation planning is carried out based on the Standoff algorithm, requiring the UAVs to rotate and move forward in a formation circling manner. The multi-UAV formation circling planning constraint model is as follows:

[0116]

[0117] Among them, t i represents the time required for UAVs to build a formation, t j represents the predicted time required for drones to form a formation; (x i ,y i ,z i ) represents the actual position of the drone; represents the predicted position of the UAV formation; E x ,E y ,E z ,E t Indicates the position and time error of formation planning;

[0118] Step S4-3: Analyze the triggering conditions and costs of multi-UAV formation reconstruction, and carry out formation reconstruction planning based on cost minimization;

[0119] (1) Formation reconstruction triggering conditions

[0120] Due to insufficient prior information, a multi-UAV collaborative formation may encounter the impact of sudden obstacles, and the obstacles may be large. If the UAVs pass through according to the established formation, the distance between the UAVs will be less than the minimum safe distance between the UAVs, or the distance between the UAVs and the obstacle will be less than the minimum safe distance. Under the premise of ensuring stable flight of the formation, it is necessary to initiate the formation reconstruction plan and select the optimized formation formation. When the formation reconstruction is completed, that is, the UAVs leave the sudden obstacle threat area, the multi-UAV "circumvent" obstacle task is completed, and the UAVs continue to fly along the established track.

[0121] (2) Formation planning cost

[0122] Set formation planning cost It consists of two parts: conventional planning and reconstruction planning. The cost is set in the interval [0, j) for conventional trajectory flight. In the interval [j, J), based on emergencies, multi-UAV formations need to be reconstructed. In the interval [J, k), the UAVs complete the formation planning and continue to fly according to the established formation. The specific UAV formation planning cost is calculated as follows:

[0123]

[0124] Where x i (k+j|k) represents the J-1 step state of the drone; x g Indicates the terminal target state; u i (k+j|k) represents the UAV’s J-1-step control input; A i With B i is a symmetric positive definite weight matrix; w=(w1,w2,w3) T is the weight vector; represents the cost of environmental threats; represents the cost of energy consumption; It represents the UAV height cost, which is calculated as follows:

[0125]

[0126] In the formula, (x i ,y i ,z i ) represents the coordinates of the current UAV track point;

[0127]

[0128] In the formula, (x l ,y l ,z l ) represents the coordinates of the given target;

[0129]

[0130] In the formula, z i represents the current track altitude; △H max represents the maximum flight altitude; Z1 and Z2 represent altitude penalty values, △H d represents the minimum safe distance of the UAV near the ground;

[0131] (3) During the local real-time path planning process, a multi-UAV formation reconstruction planning model is established based on the fusion of the model predictive control algorithm and the Standoff algorithm. The minimum cost guidance is introduced to design the UAV formation reconstruction planning. The specific local real-time path planning model of the multi-UAV formation is as follows:

[0132]

[0133] Among them, and are the velocity and angular velocity control inputs of the i-th UAV within the prediction horizon H; (x i (k + p + 1|k), y i (k + p + 1|k), z i (k + p + 1|k)) are the three-dimensional position coordinates of the formation UAV i within the prediction horizon; (x i (k + p|k), y i (k + p|k), z i (k + p|k)) are the current three-dimensional position coordinates of the formation UAV i; are the velocity components of x, y, and z of the UAV i within the prediction horizon; is the velocity correction value of the rotation center of the UAV formation; are the optimal expected velocity component values of x, y, and z of the UAV i within the prediction horizon; v t (k + p|k) is the optimal expected velocity of the UAV; ω i (k + p|k) is the optimal expected angular velocity of the UAV; τ v and τ ω are the proportional adjustment factors of the UAV velocity and angular velocity respectively; the sampling time is △t; is the control input cost; is the formation planning cost, is the coverage of the UAV monitoring target, and the specific calculation is as follows;

[0134]

[0135] Among them, L t represents the distance from the sensor to the target, P f represents the probability that the sensor effectively detects the target, P w represents the probability that the sensor misdetects the target, P f , Pw ∈(0,1], L max represents the maximum detection distance of the sensor, L min represents the fully effective detection distance of the sensor, that is, when the distance between the sensor and the target is less than L min P f = 1

[0136] The beneficial effects of the present invention are as follows: By leveraging the advantages of different intelligent algorithms, a fusion algorithm rule is formulated to form a more excellent fusion algorithm, making up for the functional shortcomings of a single algorithm; optimizing the application of the fusion algorithm to the multi-UAV cooperative trajectory planning in a three-dimensional complex environment enables the UAV swarm trajectory planning to achieve real-time collision avoidance and formation control, maximizing the detection coverage of the UAVs and obtaining a safer and more feasible trajectory planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0137] Figure 1 is the framework diagram of the multi-UAV cooperative trajectory planning of the present invention.

[0138] Figure 2 is the framework diagram of the multi-UAV cooperative trajectory planning of the present invention.

[0139] Figure 3 is the simulation diagram of the cooperative trajectory planning of two UAVs based on the fusion algorithm of the present invention, Figure 3 (a) is the top view, Figure 3 (b) is the trajectory planning diagram. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0140] The present invention will be further described below in conjunction with the drawings and embodiments.

[0141] This embodiment provides a fusion optimization method for multi-UAV cooperative trajectory planning, and the process is as Figure 1 shown, including the following steps:

[0142] Step S1: Construct a UAV motion model

[0143] In a three-dimensional space, the physical description quantities of the UAV motion model include position, velocity, pitch angle, and heading angle. Let the OXYZ coordinate system be the three-dimensional space coordinate system where the UAV is located, where the origin O represents the center of the UAV mission area, the X-axis points to the due north direction, the Z-axis points to the due east direction, and the Y-axis points to the vertically upward direction. Set the sampling time as Δt = 0.25s, then its specific motion model is expressed as the following formula:

[0144]

[0145] Among them, (x(k), y(k), z(k)) represents the position coordinates of the UAV at time k; (x(k + 1), y(k + 1), z(k + 1)) represents the position coordinates of the UAV at time k + 1; v(k) represents the real-time speed of the UAV at time k; s(k) represents the state sampling of the UAV at time k; S represents the set of feasible states; u(k) represents the decision input of the UAV at time k; U represents the set of feasible inputs; the pitch angles θ(k) and θ(k + 1) are the angles between the velocity vectors of the UAV at times k and k + 1 and the XOZ plane respectively; the heading angle and are the angles between the projection vectors of the velocity vectors of the UAV at times k and k + 1 on the XOZ plane and the positive direction of the X-axis; a(k + 1) represents the acceleration of the UAV at time k + 1.

[0146] Step S2: Initialize the information of our UAV and the target, specifically including: the initial positions of both UAVs are [0m, 0m, 0m], the established target position is [400m, 420m, 400m], the initial pitch angle and yaw angle of the UAV are both π / 4 rad, the initial speed is 25 m / s, and the minimum turning radius is 10 m. Obtain the relative state between the UAV and the established target, including the relative distance d r and the relative azimuth angle q r , and the total state

[0147] represents the position vector between the UAV and the established target, with the direction pointing from the UAV to the established target, d r represents the magnitude of the distance between the UAV and the target, q r represents the relative azimuth angle between the UAV and the target, which is the angle between the UAV velocity vector and the distance vector . The specific relative situation data can be represented by d r and q r as follows:

[0148]

[0149]

[0150]

[0151] Among them, is the position vector of our UAV in three-dimensional space, is the UAV velocity vector, v r is the speed magnitude, θ r is the pitch angle, is the yaw angle, (x r , y r , zr ) is the position coordinate of the UAV in three-dimensional space; is the position vector of the established target in three-dimensional space, (x b , y b , z b ) is the three-dimensional position coordinate of the established target.

[0152] Step S3: Input the total state into the fusion optimization algorithm model based on the optimized A* algorithm, model predictive control algorithm, and Standoff algorithm to establish the architecture of the multi-UAV cooperative trajectory planning system.

[0153] Step S31 comprehensively considers the UAV collision avoidance constraints, optimizes the A* algorithm based on the compressed search space method, smoothing method, and weight setting method, constructs the UAV global trajectory planning model accordingly, and delivers the generated global trajectory planning intermediate waypoints to the UAV route storage unit.

[0154] S31-1 Establish the UAV sensor search fan, the fan search radius is the current UAV search step size, set the vertical angle along the route as l1 = 1.5 times the maximum pitch angle, and the horizontal angle is set as l2 = 1.5 times the maximum heading angle. When the search fan is in the safe area, preferentially select the maximum step size L max = 25m to conduct the search. If the search fan contains no-fly zones, reduce the search step size L according to the number of no-fly zones included. The algorithm optimization includes determining the UAV search fan range, determining the number of sub-sectors, and selecting the determined step size.

[0155] For simplicity in modeling and calculation, in this example, it is assumed that our UAV and the target are in the same plane. The current position of our UAV is denoted as point B, the position of target j is denoted as point C, the UAV movement direction is BP, and the maximum range angle of the UAV search fan is set as ∠PBD, denoted as BC represents the flight direction of the UAV tracing the target, and ∠PBC represents the deviation angle of the UAV tracing target j. Based on this, the UAV search fan range can be simplified to an angle constraint, and the specific calculation is as follows:

[0156]

[0157] Among them, represents the UAV search fan angle.

[0158] Based on the three-dimensional complex environment, first, the size of the search fan angle needs to be determined according to the following formula:

[0159]

[0160] Among them, L is the search step size; v represents the UAV speed magnitude, taking v = 25m / s; ωmax Indicates the maximum constrained angular velocity of the UAV, take ω max =0.2rad / s; α is the step size adjustment proportional coefficient, take α=0.2.

[0161] When the step size is L, the number m of extended sub-sectors to be searched is calculated according to the following formula.

[0162]

[0163] in, Indicates the search sub-sector angle, take

[0164] Determine the current search step size. The variable step size A* algorithm step size selection satisfies the following formula:

[0165] L max =L min +m max △L

[0166] Among them, L max Indicates the maximum step length of the algorithm search, take L max =25m;L min Indicates that the algorithm searches for the minimum step size, taking L min =5m;m max Indicates the number of sub-sectors corresponding to the maximum step length, take m max =10; △L represents the unit transformation step, and △L=2m is taken.

[0167] The calculation method of the variable step size A* algorithm is specifically expressed as follows:

[0168]

[0169] Where m u It represents the number of expansion nodes that fall into the no-fly zone when the algorithm uses the current step length trial; β represents the step length adjustment coefficient, β = 3.

[0170] The S31-2 A* algorithm node sparse optimization reduces the search space based on the UAV flight constraints, including the minimum track segment length and the maximum turning angle constraints. The specific steps are as follows:

[0171] 1) Determine the expandable area from the current node, with a search radius equal to the current drone search step size, and set the vertical angle along the route to 1.5 times the maximum climb / dive angle, and the horizontal angle to 1.5 times the maximum turn angle;

[0172] 2) Divide the climb / dive sector of the expandable area into three equal parts, and divide the turning sector into five equal parts, to obtain 15 expansion nodes;

[0173] 3) Calculate the cost value from the current node to the expanded node, and select the node with the minimum cost function on each sector.

[0174] 4) For the node selected in 3), determine whether it meets the constraints of the minimum flight altitude and the maximum flight distance. If it does not meet the requirements, discard it directly; if it meets the requirements, perform trajectory planning based on this node.

[0175] S31-3 Use the turning shortcut curve method to replace the polygonal path of the UAV trajectory, carry out the path curve optimization of the A* algorithm, determine the center C(k) of the tangent sphere of the trajectory point, judge the turning direction of the trajectory point, and obtain the tangent point coordinates M1 and M2. The specific process is as follows:

[0176] 1) Determine the tangent sphere C(k) of the trajectory point

[0177] Assume that the trajectory turning point is A i The coordinate position of is (x i , y i , z i ), set the heading angle and pitch angle θ r of the UAV, and the trajectory turning radius is R = 10m. Then the calculation formula for the coordinates of the center of the tangent sphere of the UAV turning is shown as follows.

[0178]

[0179] Among them, C 顺 (k) represents the coordinate of the center of the clockwise turning sphere; C 逆 (k) represents the coordinate of the center of the counterclockwise turning sphere; R represents the turning radius of the UAV trajectory; θ r and represent the pitch angle and heading angle of the UAV; (x i (k), y i (k), z i (k)) represents the position coordinate of the UAV at the turning moment.

[0180] 2) Judge the turning direction of the trajectory point

[0181] In this example, the mixed vector product algorithm is used to judge the turning direction of the two adjacent segments of the trajectory before and after the trajectory turning point A i . Assume that the current trajectory point is O i , the previous segment of the trajectory is A i-1 A i , and the next segment of the trajectory is A i A i+1 , δ = [δ x , δ y , δ z T and ε = [ε x , ε y , εz ] T are the unit vectors in the directions of the two track segments, respectively. The turning direction of the drone can be determined by calculating the following formula:

[0182]

[0183] Here, sign represents the sign function.

[0184] ①γ(δ,ε)<0: indicates that the current UAV is along the track segment A i-1 A i Fly to A i A i+1 A counterclockwise turn is required;

[0185] ②γ(δ,ε)>0: indicates that the current UAV is along the track segment A i-1 A i Fly to A i A i+1 A clockwise turn is required;

[0186] ③γ(δ,ε)=0: Indicates that the current track node is a non-track turning point.

[0187] 3) Obtain the coordinates of the tangent point

[0188] Determine the turning sphere center coordinate C according to the calculation formula of the sphere center coordinate of the UAV turning and cutting ball i , the radius of the sphere is R = 10m, set q i ,q i+1 Track segment A i-1 A i With A i A i+1 The unit vector on the vector q' is the same as the vector q i Perpendicular, vector q″ and vector q i+1 Vertical, assuming vector q i to q i+1 If the rotation is clockwise, the coordinates of the tangent points M1 and M2 can be calculated as follows:

[0189]

[0190] S31-4 uses the weight method to optimize the A* algorithm and adds the weight a of the cost related to H(x) and G(x) g ,b h , the optimization heuristic function is expressed as follows:

[0191]

[0192] Among them, a g =0.85, b h =0.15, cdiv =5.67.

[0193] Compared with the traditional A* algorithm, in three-dimensional space, the expansion calculation of G(x) is as follows:

[0194]

[0195] Where, l=5m.

[0196] In three-dimensional space, H(x) is expressed as follows:

[0197]

[0198] Among them, (x r ,y r ,z r ) represents the current UAV track coordinates; (x b ,y b ,z b ) represents the target position coordinates.

[0199] Step S32 hierarchically integrates and optimizes the A* algorithm and the model predictive control algorithm. The results obtained from the global trajectory planning process in S31 are sent to the trajectory storage unit of the drone trajectory planning module. Each time, the next waypoint at the current drone position is extracted from the storage unit as a temporary target point, and the model predictive control algorithm is used to implement local real-time path planning for the drone. The specific hierarchical fusion process is as follows:

[0200] S32-1: Set the global trajectory planning point information based on the optimized A* algorithm as the temporary end point of the local path planning;

[0201] S32-2: Construct a UAV local path planning prediction model based on the UAV speed v, heading angle The pitch angle θ is used as input, and the single-step optimization model in the rolling optimization process of the model predictive control algorithm is used to obtain the optimization results of the control variables of the drone speed and angular velocity. The results are then output to the drone motion control unit to achieve local real-time path planning of the drone. The specific model predictive control algorithm is modeled as follows:

[0202]

[0203] in, and is the speed and angular velocity control input of the i-th UAV in the prediction time domain H; v i (k+p|k) is the current speed of the drone; v i (k+p+1|k) is the speed of the UAV in the prediction time domain; ω i (k+p|k) is the angular velocity of the drone; ω i(k + p + 1|k) is the angular velocity of the UAV within the prediction horizon; is the current heading angle of the UAV; is the heading angle of the UAV within the prediction horizon; θ i (k + p|k) is the current pitch angle of the UAV; θ i (k + p + 1|k) is the pitch angle of the UAV within the prediction horizon; (x i (k + p + 1|k), y i (k + p + 1|k), z i (k + p + 1|k)) are the three-dimensional position coordinates of the UAV within the prediction horizon; (x i (k + p|k), y i (k + p|k), z i (k + p|k)) are the current three-dimensional motion coordinates of the UAV; τ v and τ ω are respectively the speed and angular velocity proportional adjustment factors of the UAV, τ v = 2, τ ω = 3; The sampling time is △t = 0.25s.

[0204] The local real-time path planning constraint conditions of the UAV based on the model predictive control algorithm are as follows:

[0205]

[0206] Among them, and are the speed and angular velocity control inputs of the i-th UAV within the prediction horizon H; and are the maximum and minimum speed constraints of the UAV, is the maximum angular velocity constraint of the UAV; (x i (k + p + 1|k), y i (k + p + 1|k), z i (k + p + 1|k)) are the three-dimensional position coordinates of the formation UAV i within the prediction horizon; (x j (k + p + 1|k), y j (k + p + 1|k), z j (k + p + 1|k)) are the three-dimensional position coordinates of the formation UAV j within the prediction horizon; R a = 15m is the minimum safety distance of the UAV, (o xj , o yj , o zj ) is the center position of the obstacle, o rj = 50m is the radius of the obstacle, (x i (k), y i (k), z i(k)) is the initial position coordinate of the UAV, v i (k) = 25m / s is the initial speed of the drone, is the initial heading angle of the UAV, θ i (k) = π / 4rad is the initial pitch angle of the UAV.

[0207] Step S33 uses the Standoff algorithm to build a multi-UAV formation control model. The specific multi-UAV collaborative formation trajectory planning process is as follows:

[0208] S33-1: Using the optimized A* algorithm to carry out the global trajectory planning of UAV;

[0209] S33-2: Use model optimization algorithms to carry out local real-time path planning of UAVs;

[0210] S33-3: Based on the local real-time path planning of UAVs, the Standoff algorithm is used to calculate the phase distribution value of the multi-UAV formation, and then the formation position of the UAV group is determined to realize the coordinated formation control of UAVs.

[0211] This example uses the Standoff algorithm to plan a multi-UAV formation. The UAV formation flies in a spiral shape, and the rotation radius is set to D. r =25m, the corresponding Lyapunov energy function is the distance function, as shown below:

[0212]

[0213] Where r is the position of the drone (x r ,y r ,z r ) and the rotation center position (x d ,y d ,z d ), ξ represents the allowable error of formation coordination, and ξ=0.8m.

[0214] To simplify the calculation, this example assumes that the drone group is distributed in the same plane, so only the influence of phase angle positioning needs to be considered. The phase angles of any two drones are φ i and φ j , the expected relative phase angle is φ z , then the phase function of the multi-UAV cooperative formation is calculated based on the Lyapunov stability theory as follows:

[0215]

[0216] This example is two UAVs flying in formation, N=2, φ z =π.

[0217] The angular velocity of any two drones can be calculated as follows:

[0218]

[0219]

[0220] in, is the phase angular velocity of UAV i, is the phase angular velocity of UAV j, v is the real-time velocity of UAV, k is the function coefficient, and its value is k=0.5.

[0221] The speed of any two drones can be calculated as follows:

[0222] v i =v

[0223] v j =k·(φ i -φ j -φ z )·D r +v

[0224] Among them, v i is the speed of drone i, v j is the speed of UAV j.

[0225] In this example, the optimal expected speed of the drone can be calculated by combining the predicted speeds of multiple drones with the rotation center speed correction term. The calculation expression is as follows:

[0226]

[0227] in, is the optimal expected speed of the UAV, Predict the speed value for the drone, is the rotation center speed correction value.

[0228] Multi-UAV formation predicted speed v t , heading angle and pitch angle θ t It can be calculated according to the following formula:

[0229]

[0230]

[0231]

[0232] Step S4: In this example, a sudden obstacle model is added to the 3D complex map model to verify the real-time and effectiveness of the fusion algorithm applied to multi-UAV formation trajectory planning. The specific steps are as follows:

[0233] S4-1: Construct a UAV flight environment model;

[0234] S4-2: Based on the UAV flight environment model, a UAV formation collision avoidance constraint model is established to determine in real time whether the UAV group encounters an emergency situation and needs to reconfigure the formation. If so, proceed to step S4-3; if not, continue with collaborative trajectory planning;

[0235] S4-3: Analyze the triggering conditions and costs of multi-UAV formation reconstruction, use the model predictive control algorithm to carry out real-time obstacle avoidance planning, and carry out multi-UAV formation expected position planning based on the Standoff algorithm, and use the "feedback-correction" mechanism to correct the UAV position.

[0236] Step S4-1 constructs a UAV flight environment model;

[0237] This example builds a map model based on the uneven terrain, adds four static obstacle models, all of which are approximated by cylinders, and the center of the static obstacle 1 plane is set to P o1 , its coordinates are [100m, 150m], the radius is 20m, and the height is 450m; the center of the static obstacle 2 plane is set to P o2 , its coordinates are [210m, 370m], the radius is 20m, and the height is 450m; the center of the static obstacle 3 plane is set to P o3 , its coordinates are [300m, 360m], the radius is 20m, and the height is 450m; the center of the static obstacle 4 plane is set to P o1 , its coordinates are [400m, 410m], the radius is 20m, and the height is 450m; the maximum threat distance of static obstacles is 10m. Add a sudden obstacle model and use a sphere to approximate it, with the center of the sphere set to P t The specific location coordinates are [200m, 270m, 130m], and the radius is 50m.

[0238] Step S4-2: Constructing a multi-UAV formation constraint model

[0239] Based on the UAV flight environment model, a UAV formation collision avoidance constraint model is established, as shown in the following formula.

[0240]

[0241] Among them, (x i (k),y i (k),z i (k)) represents the current UAV position coordinates; R a Indicates the minimum collision avoidance safety distance of the UAV, R a =15m;(x j (k),y j (k),zj (k)) represents the position coordinates of the adjacent UAVs; z all (k) represents the ground coordinate (x i (k),y i (k)) height; △H d Indicates the minimum safe distance of the drone near the ground, △H d =10m.

[0242] In the process of multi-UAV formation planning, the UAVs are required to rotate and move forward in a formation circle. The multi-UAV formation planning constraint calculation is as follows:

[0243]

[0244] Among them, t i represents the time required for UAVs to build a formation, t j represents the predicted time required for drones to form a formation; x i ,y i ,z i Indicates the actual position of the drone; represents the predicted position of the UAV formation; E x ,E y ,E z ,E t Indicates the formation planning position and time error.

[0245] Step S4-3-1 Formation reconstruction triggering conditions

[0246] Due to insufficient prior information, a multi-UAV coordinated formation may encounter the impact of sudden obstacles, especially large ones. If the distance between UAVs falls below the minimum safe distance between them, or the distance between UAVs and obstacles falls below the minimum safe distance, formation reconfiguration planning must be initiated to optimize the formation while ensuring stable flight. Once formation reconfiguration is complete, the UAVs exit the sudden obstacle threat zone, the multi-UAV "circumvention" task is complete, and the UAVs resume flight on their intended trajectory.

[0247] Step S4-3-2 Formation Planning Cost

[0248] In this example, based on a discretized UAV motion model, the cost function for reconfiguring a multi-UAV formation is constructed as follows. The cost is set in the interval [0, j) for conventional flight paths. In the interval [j, J) , the multi-UAV formation requires reconfiguration planning due to unexpected situations. In the interval [J, k) , the UAVs complete formation planning and continue flying in the established formation.

[0249]

[0250] Where xi (k + j|k) represents the state of the UAV at step J - 1; x g represents the terminal target state; u i (k + j|k) represents the control input of the UAV at step J - 1; A i and B i are symmetric positive definite weight matrices; w = (w1, w2, w3) T is the weight vector; represents the cost of environmental threat; represents the cost of energy consumption; represents the cost of the UAV altitude, and the specific calculation is as follows:

[0251]

[0252] where, (x i , y i , z i ) represents the coordinates of the current waypoint;

[0253]

[0254] where, (x j , y j , z j ) represents the coordinates of the target point;

[0255]

[0256] where, z i represents the altitude of the current waypoint; H i represents the terrain altitude corresponding to the current waypoint; H min and H max represent the minimum and maximum flight altitudes; Z1 and Z2 represent altitude penalty values.

[0257] Step S4 - 3 - 3 Establish the formation reconstruction model

[0258] In the process of local real - time path planning, a multi - UAV formation reconstruction planning model is established based on the fusion of the model predictive control algorithm and the Standoff algorithm. The minimum - cost guidance is introduced to design the UAV formation reconstruction planning. The specific multi - UAV formation local real - time path planning model is as follows:

[0259]

[0260] where, and are the velocity and angular velocity control inputs of the i - th UAV within the prediction horizon H; (x i (k + p + 1|k), y i (k + p + 1|k), z i(k + p + 1|k)) is the three-dimensional position coordinate of the formation UAV i within the prediction time domain; (x i (k + p|k), y i (k + p|k), z i (k + p|k)) is the current three-dimensional position coordinate of the formation UAV i; are the velocity components of UAV i in x, y, and z within the prediction time domain; is the velocity correction value of the rotation center of the UAV formation; are the optimal expected velocity component values of UAV i in x, y, and z within the prediction time domain; v t (k + p|k) is the optimal expected velocity of the UAV; ω i (k + p|k) is the optimal expected angular velocity of the UAV; τ v and τ ω are respectively the velocity and angular velocity proportional adjustment factors of the UAV, τ v = 2, τ ω = 3; The sampling time is △t = 0.25s; is the control input cost; is the formation planning cost, is the coverage of the UAV monitoring target, and the specific calculation is as follows:

[0261]

[0262] where, L t represents the distance from the sensor to the target, P f represents the probability that the sensor effectively detects the target, P w represents the probability that the sensor misdetects the target, P f , P w ∈ (0, 1]. L max = 90m represents the maximum detection distance of the sensor,

[0263] L min = 40m represents the fully effective detection distance of the sensor, that is, when the distance between the sensor and the target is less than L min = 40m, P f = 1.

[0264] This example mainly completes the cooperative trajectory planning of multiple UAVs in a three-dimensional complex environment based on the fusion optimization algorithm.

[0265] In the example, the optimized fusion algorithm is applied to the UAV trajectory planning structure as Figure 2 shown.

[0266] In the example, after using the optimized A* algorithm, compared with the traditional A* algorithm in the same state, the number of planning nodes was reduced from 105 to 72, the planning time was shortened from 2.69s to 2.55s, and the total planned distance was reduced from 2125m to 1944m, with the total distance reduced by about 8.5%.

[0267] In this example, the prediction step of the model predictive control algorithm is 10 steps, the number of sampling times is 100 times, the number of state variables is 3, the number of control variables is 3, which are the speed, pitch angle and heading angle of the UAV respectively, and the sampling period △t = 0.25s.

[0268] In this example, a sudden obstacle is added at [200m, 270m, 130m]. The minimum safety distance of the drone is 15m, and the maximum communication distance is 90m.

[0269] In the example, the simulation realizes the collaborative trajectory planning of multiple UAVs, solving the problem of poor real-time obstacle avoidance capability of the Standoff algorithm. The fusion algorithm is applied to the collaborative trajectory planning of multiple UAVs. The specific UAV trajectory planning is as follows Figure 3 As shown in the figure, two UAVs perform trajectory planning based on a fusion algorithm, which enables them to avoid sudden obstacles in real time, and the formation is formed and maintained stably, meeting the real-time requirements of multi-UAV collaborative trajectory planning.

[0270] The above are merely preferred embodiments of the present invention. The scope of protection of the present invention is not limited to the above embodiments. All technical solutions based on the principles of the present invention are within the scope of protection of the present invention. It should be noted that for those skilled in the art, various improvements and modifications that do not depart from the principles of the present invention should be considered within the scope of protection of the present invention.

Claims

1. A fusion optimization method for multi-UAV cooperative trajectory planning, characterized in that Including the following steps: Step S1: Construct a UAV motion model; In three-dimensional space, the UAV's position, velocity, pitch angle, and heading angle jointly construct a UAV motion model. Let the OXYZ coordinate system be the three-dimensional space coordinate system where the UAV is located. The origin O represents the center of the UAV mission area, the X-axis points to the due north direction, the Z-axis points to the due east direction, and the Y-axis points vertically upward; When examining the UAV's motion, the UAV is regarded as a particle, and a three-dimensional discretized UAV kinematic model is established. The sampling time is △t, and the UAV motion equation is as follows: Among them, (x(k), y(k), z(k)) represents the position coordinates of the UAV at time k; (x(k + 1), y(k + 1), z(k + 1)) represents the position coordinates of the UAV at time k + 1; v(k) represents the real-time speed of the UAV at time k; s(k) represents the state sampling of the UAV at time k; S represents the set of feasible states; u(k) represents the decision input of the UAV at time k; U represents the set of feasible inputs; the pitch angles θ(k) and θ(k + 1) are the angles between the velocity vectors of the UAV at times k and k + 1 and the XOZ plane respectively; the heading angle and are the angles between the projection vectors of the velocity vectors of the UAV at times k and k + 1 on the XOZ plane and the positive direction of the X-axis; a(k + 1) represents the acceleration of the UAV at time k + 1; Step S2: Initialize the own UAV and the established target information, obtain the state of the UAV itself and the relative state between the UAV and the target, and form the total state Q all ; The self - state of the UAV includes the position components (x r , y r , z r ) in a three - dimensional coordinate system, the UAV speed magnitude v r , the UAV pitch angle θ r , and the UAV heading angle The relative state between the UAV and the target includes the relative distance d r and the relative azimuth angle q r ; The total state composed is Step S3: The total state Q all is input into an optimized algorithm model that integrates the optimized A* algorithm, the model predictive control algorithm, and the Standoff algorithm. The multi-UAV cooperative trajectory planning system is divided into a trajectory planning layer and a path planning layer. The trajectory planning is carried out based on the optimized A* algorithm, and the result is a static geometric path independent of time. The path planning is carried out based on the integration of the model predictive control algorithm and the Standoff algorithm. The model predictive control algorithm solves the problem of large-scale real-time optimal control within a finite time, and realizes optimized action control by using preview capabilities in a constrained, nonlinear, model-uncertain, and unforeseeable environment, generating a real-time path planning applicable to the actual flight of multi-UAV formations. As one of the main formation control algorithms, the Standoff algorithm maximizes the sensor detection range based on the safety distance and reduces the probability of target loss; Step S4: Multi-UAV formation planning; Implant a sudden obstacle model into the prior static obstacle model to verify the real-time obstacle avoidance ability of the fusion algorithm applied to multi-UAV cooperative trajectory planning.

2. The fusion optimization method for multi-UAV cooperative trajectory planning according to claim 1, characterized in that: The specific trajectory planning process of step 3 is as follows: S31: Considering the UAV collision avoidance constraints comprehensively, construct a multi-UAV global trajectory planning model in three-dimensional space based on the A* algorithm; carry out sparse variable-step curve optimization of the A* algorithm based on the compressed search space and smoothing processing method; add relevant cost weights to the heuristic functions G(x) and H(x) of the A* algorithm, and introduce a weighting method to improve the algorithm operation efficiency; based on this, optimize the application of the A* algorithm to multi-UAV global trajectory planning in three-dimensional space, generate a global route plan composed of multiple intermediate waypoints, and hand over the intermediate waypoints to the route storage unit in the S32 stage; S32: Take out the next waypoint of the current UAV's location from the global trajectory planning points saved in step S31 as the temporary target point for applying the model predictive control algorithm to the UAV's real-time trajectory planning, and carry out local real-time path planning of the UAV based on the model predictive control model; S33: During the process of carrying out local real-time path planning, use the Standoff algorithm based on the Lyapunov vector field to control the formation flight of the UAV swarm to maximize the UAV sensor observation coverage; S34: Establish a three-dimensional complex environment with prior obstacle information, construct an obstacle model, and verify the effectiveness and real-time performance of the fusion optimization algorithm model applied to multi-UAV cooperative trajectory planning.

3. The fusion optimization method for multi-UAV collaborative trajectory planning according to claim 2, characterized in that: The UAV global trajectory planning in step S31 is carried out based on an optimized A* algorithm model, and the specific modeling is as follows: S31-1 Initialize the basic information of the UAV, including the starting point (x1, y1, z1), speed v1, and heading angle of the UAV Pitch angle θ1; establish a search sector for the UAV sensor, with the search radius of the sector being the current search step of the UAV. Set the vertical angle along the route to be l1 times the maximum pitch angle, and the horizontal angle to be l2 times the maximum heading angle; when the search sector is in a safe area, preferentially select the maximum step length L max Conduct the search. If there is a no-fly zone in the search sector, based on the criterion that the number of no-fly zones contained in the search sector is inversely proportional to the search step length, dynamically adjust the search step length, that is: the more no-fly zones are contained, the shorter the search step length; initialize the search sector range of the UAV, determine the number of sub-sectors, and determine the step length selection. The specific modeling process is as follows: Our drone and the target always remain in the same plane. At this time, the search task is compressed to a two-dimensional plane. The current position of our drone is denoted as point B, the position of target j is denoted as point C, the moving direction of the drone is BP, and the maximum range angle of the drone's search sector is set as ∠PBD, denoted as BC represents the flight direction of the drone to trace the target, and ∠PBC represents the deviation angle of the drone to trace target j. The range of the drone's search sector is simplified to an angle constraint, and the specific calculation is as follows: Among them, represents the search sector angle of the UAV. The size of the search sector angle needs to be determined according to the following formula: where L is the search step size; v represents the magnitude of the UAV speed; ω max represents the maximum constrained angular velocity of the UAV; α is the step size adjustment proportionality coefficient; When the search step size is L, the number m of sub-sectors to be searched and expanded is calculated as follows: Among them, represents the search sub-sector sector angle; Determine the current search step size, and the variable-step optimization of the A* algorithm satisfies the following formula: L max = L min + m max · ΔL Among them, L max represents the maximum step size for algorithm search; L min represents the minimum step size for algorithm search; m max represents the number of sub-sectors corresponding to the maximum step size; △L represents the unit transformation step size; The specific modeling of the variable-step A* optimization algorithm is: where m u represents the number of extended nodes that fall within the no-fly zone when using the current step size of the algorithm for exploration; β represents the step size adjustment coefficient, satisfying 1 ≤ β ≤ m max , S31-2 A* algorithm node sparse optimization; According to the UAV flight constraints, including the minimum track segment length and the maximum turning angle constraint, reduce the search space. The specific steps are as follows: 1) Determine the expandable area from the current node. The search radius is the current UAV search step size. Set the vertical angle along the route to k1 times the maximum climb / dive angle, and the horizontal angle to k2 times the maximum heading angle; 2) Divide the sectors of the climbing / diving directions of the expandable areas into K1 equal parts, and divide the sectors of the turning directions into K2 equal parts to obtain K1 * K2 points; The values of K1 and K2 should be selected appropriately. Selecting K1 and K2 within [3, 5] can meet the requirements. The larger K1 and K2 are, the greater the probability of finding an ideal flight path, but at the same time, it increases the storage memory requirement and search time. Take the K1 * K2 points as the nodes to be expanded; 3) Calculate the cost value from the current node to the expanded node, and select the node with the minimum cost function on each sector; 4) For the nodes selected in step 3), judge whether they meet the constraints of the minimum near-ground safety distance and the maximum flight safety distance of the UAV. If they do not meet the requirements, discard them directly; If they meet the requirements, perform flight path planning based on these nodes; S31-3 A* algorithm trajectory curve optimization; Use the turning shortcut curve method to replace the polyline flight path of the UAV, determine the center C(k) of the tangent sphere of the flight path point, judge the turning direction of the flight path point, and obtain the tangent point coordinates M1 and M2; 1) Determine the tangent sphere C(k) of the flight path point The turning point A of the trajectory of the i-th drone i has the coordinate position of (x i , y i , z i ). Set the heading angle of the drone and the pitch angle θ r . The turning radius of the trajectory is R. The calculation of the coordinates of the center of the turning tangent sphere of the drone is as follows: Among them, C 顺 (k) represents the center coordinate of the clockwise turning ball; C 逆 (k) represents the center coordinate of the counterclockwise turning ball; θ r and represent the pitch angle and heading angle of the UAV; (x i (k), y i (k), z i (k)) represents the position coordinate of the UAV i at the turning moment; 2) Judge the turning direction of the flight path point; Use the mixed vector product algorithm to determine the turning point A of the flight path i The turning directions of two adjacent flight paths before and after. The current flight path point is the turning point A of the i-th UAV's flight path i , then the previous flight path segment is represented as A i-1 A i , and the next flight path segment is represented as A i A i+1 , δ = [δ x , δ y , δ z T and ε = [ε x , ε y , ε z T are the unit vectors in the directions of the previous and next flight path segments respectively. Determine the turning direction of the UAV by calculating the following formula:​​ Among them, sign represents the sign function; ①γ(δ,ε)<0: It means that the current UAV flies along track segment A i-1 A i to A i A i+1 and needs to turn counterclockwise; ②γ(δ,ε)>0: It means that the current UAV flies along track segment A i-1 A i flies to A i A i+1 needs to turn clockwise; ③γ(δ,ε) = 0: It indicates the current flight path segment A i-1 A i flies to A i A i+1 on the same straight line. However, according to the constraints of the maximum heading angle and maximum pitch angle of the UAV, there will be no 180° turn of the UAV. Therefore, it indicates that the UAV moves straight without turning, that is, the current flight path node is a non-flight path turning point; 3) Obtain the tangent point coordinates During the smooth processing of the UAV flight path turning, it is required that the turning arc be tangent to the previous flight path segment A i-1 A i and the next flight path segment A i A i+1 Both are tangent, and the tangent points are set as M1 and M2. After the UAV flight path is smoothly processed, the UAV flight path changes from the original two segments A i-1 A i and A i A i+1 become the following three segments: ①A i-1 The track segment of M1 continues to maintain a straight flight state; ② Execute circular flight with a radius of R for the flight path segment from M1 to M2; ③M2A i+1 The track segment performs straight flight; Calculate the position coordinates C of the turning ball center according to the calculation formula of the center coordinates of the turning ball of the UAV i , the turning radius is R, and q is set i , q i+1 are the unit vectors on the track segments A i-1 A i and A i A i+1 respectively. The vector q' is perpendicular to the vector q i , and the vector q” is perpendicular to the vector q i+1 . Assume that the rotation from the vector q i to q i+1 is clockwise. Then the coordinates of the tangent points M1 and M2 are calculated by the following formula: S31-4 Optimize the A* algorithm using the weight method, and add the weights a g , b h related to the costs of H(x) and G(x). The optimized heuristic function is expressed as the following formula: Among them, G(x) is a function of the true cost of the UAV from the starting position to the current position, and H(x) represents a function of the estimated cost of the UAV from the current position to the target point; a g , b h are the weights of the true cost and the estimated cost respectively. The value range of a g is [0.5, 1], and c div is the weight ratio, and its value is greater than 1; In three-dimensional space, the expansion calculation of G(x) is as follows: Among them, l represents the side length of the map grid; In three-dimensional space, H(x) is expressed as follows: Among them, (x r , y r , z r ) represents the current UAV flight path coordinates; (x b , y b , z b ) represents the target position coordinates.

4. The fusion optimization method for multi-UAV cooperative flight path planning according to claim 2, wherein: In the step S32, extract the information of the global flight path planning points of the UAV, use the next flight path point of the current UAV position as the temporary end point of the local real-time path planning, and use the model predictive control algorithm to complete the real-time planning of the UAV; After the UAV reaches the temporary target point, then take out the next flight path point from the flight path storage unit as the target point for path planning, and continue until reaching the last flight path point of the UAV flight path. The process of local real-time path planning is as follows: S32-1: Initialize the discrete motion model of the UAV; S32-2: Take out the next flight path point of the current UAV position from the global flight path planning information and set it as the temporary target point of the local path; S32-3: Use the model predictive control algorithm to construct the local path planning model of the UAV; S32-4: Based on the fusion of the optimized A* algorithm and the model predictive control algorithm, realize the global real-time flight path planning of the UAV; The modeling of the model predictive control algorithm is as follows: Among them, and are the velocity and angular velocity control inputs of the i-th unmanned aerial vehicle (UAV) within the prediction horizon H; v i (k + p|k) is the current velocity of the UAV; v i (k + p + 1|k) is the velocity of the UAV within the prediction horizon; ω i (k + p|k) is the angular velocity of the UAV; ω i (k + p + 1|k) is the angular velocity of the UAV within the prediction horizon; is the current heading angle of the UAV; is the heading angle of the UAV within the prediction horizon; θ i (k + p|k) is the current pitch angle of the UAV; θ i (k + p + 1|k) is the pitch angle of the UAV within the prediction horizon; (x i (k + p + 1|k), y i (k + p + 1|k), z i (k + p + 1|k)) are the three-dimensional position coordinates of the UAV within the prediction horizon; (x i (k + p|k), y i (k + p|k), z i (k + p|k)) are the three-dimensional motion coordinates of the current UAV; τ v and τ ω are the proportional adjustment factors of the UAV velocity and angular velocity respectively, and the sampling time is △t; The modeling constraint conditions for the real-time flight path planning of the UAV based on the model predictive control algorithm are as follows: Among them, and are the velocity and angular velocity control inputs of the \(i\)-th unmanned aerial vehicle (UAV) within the prediction horizon \(H\); and are the maximum and minimum velocity constraints of the UAV, is the maximum angular velocity constraint of the UAV; \((x i (k + p + 1|k), y i (k + p + 1|k), z i (k + p + 1|k)) are the three-dimensional position coordinates of the formation UAV \(i\) within the prediction horizon; \((x j (k + p + 1|k), y j (k + p + 1|k), z j (k + p + 1|k)) are the three-dimensional position coordinates of the formation UAV \(j\) within the prediction horizon; \(R a is the minimum safe distance of the UAV; \((o xj , o yj , o zj ) is the center position of the obstacle, and \(o rj is the radius of the obstacle; \((x i (k), y i (k), z i (k)) are the initial position coordinates of the UAV, \(v i (k) is the initial velocity of the UAV, is the initial heading angle of the UAV, and \(\theta i (k) is the initial pitch angle of the UAV. At time \(k\), the above model predictive control algorithm model is solved to obtain the optimization results of the control quantities of the UAV velocity and angular velocity magnitudes, and the results are output to the UAV motion control unit.

5. The fusion optimization method for multi-UAV cooperative flight path planning according to claim 2, wherein: In the step S33, during the local real-time flight path planning of the UAV, fuse the Standoff algorithm to control multiple UAVs to carry out cooperative formation planning to achieve the best search formation maintenance of multiple UAVs. The specific process is as follows: S33-1: Initialize the UAV cluster information; S33-2: Use the optimized A* algorithm to initialize the global flight path planning of the UAV, and use the model predictive control algorithm to carry out local real-time optimal flight path planning; S33-3: During the local real-time optimal trajectory planning process, the Standoff algorithm is applied to the formation and maintenance control of UAV formations, enabling the UAV swarm to be evenly distributed around the target, and then maximizing the search and monitoring of multi-UAV formation sensors. The specific modeling is as follows: Based on the Standoff algorithm, multi-UAV formation planning is carried out, and the UAV formation flies in a spiral shape with a rotation radius of D r , and the corresponding Lyapunov energy function is the distance function L d (x, y, z), as shown in the following formula: where r is the radial distance between the UAV position (x r , y r , z r ) and the rotation center position (x d , y d , z d ), ξ represents the allowable error for formation cooperation; It is assumed that multiple UAVs are arranged in the same plane, so only the influence of phase angle positioning needs to be considered: the phase angles of any two UAVs are φ i and φ j , and the expected relative phase angle is φ z . Then, based on the Lyapunov stability theory, the phase function of the multi-UAV cooperative formation is calculated as follows: where μ p represents the difference between the relative phase angle of two UAVs and the expected relative phase angle, and N represents the number of UAVs in the formation; The calculation formula for the phase angular velocity of the UAV is as follows: Among them, is the phase angular velocity of UAV i, is the phase angular velocity of UAV j, v r is the real-time speed of the UAV, and k is the function coefficient; The calculation formula for the UAV speed is as follows: v i = v r v j = k·(φ i - φ j - φ z )·D r + v r Among them, v i is the speed of UAV i, and v j is the speed of UAV j; The optimal expected speed of the UAV is calculated by combining the predicted speeds of multiple UAVs with the rotational center speed correction term. The calculation expression is as follows: Among them, is the optimal expected speed value of the UAV, is the predicted speed value of the UAV, is the speed correction value of the rotation center; it is calculated according to the following formula: Among them, v t is the optimal predicted speed of the multi-UAV formation, is the optimal heading angle, and θ t is the optimal pitch angle.

6. The fusion optimization method for multi-UAV collaborative trajectory planning according to claim 2, characterized in that: The steps of the multi-UAV collaborative formation planning in step S4 are as follows: S4-1: Construct a UAV flight environment model; S4-2: Based on the UAV flight environment model, establish a UAV formation collision avoidance constraint model. The collision avoidance constraint model includes the minimum safe distance between UAVs, the minimum safe distance from the ground for UAVs, and the minimum safe distance from obstacles for UAVs. Real-time judgment is made on whether there are sudden situations affecting the safe planning of the UAV formation. If the sudden situation causes the UAVs to be unable to continue flying according to the established formation plan, go to step S4-3. If there is no sudden situation or there is a sudden situation but it does not affect the safe planning of the UAV formation, continue with the established collaborative trajectory planning; S4-3: Carry out multi-UAV formation reconstruction, analyze the triggering conditions and costs of multi-UAV formation reconstruction, design a UAV formation reconstruction plan based on the minimum cost objective, and at the same time carry out the expected position planning of the multi-UAV formation based on the Standoff algorithm. By comparing the actual position information with the expected position information, the "feedback - correction" mechanism is used to correct the positions of the UAVs.

7. The fusion optimization method for multi-UAV collaborative trajectory planning according to claim 6, characterized in that: The steps for constructing the UAV flight environment model in step S4-1 are as follows: Build a map model based on the uneven terrain. Set up a safety buffer zone around the UAV. At the same time, establish a static obstacle model and a sudden obstacle model. The static obstacle model is approximated by a cylinder, and the center of the plane is set as P o , with coordinates [P ox ,P oy . The radius and height are represented by P or and P oz . Set up a collision area and a threat area around it. L od represents the minimum approach safety distance, and △H od represents the minimum height approach distance. That is, when the distance between the UAV and the static obstacle is less than L od or △H od , the UAV collides; L OD represents the maximum threat distance of the static obstacle, and △H OD represents the maximum threat height of the static obstacle. That is, when the distance between the UAV and the static obstacle is less than L OD or △H OD , there may be a risk of collision for the UAV. The sudden obstacle model is approximated by a sphere, and the center of the sphere is set as P t , with specific coordinates [P tx ,P ty ,P tz . The radius is R t . Similarly, set up a collision area and a threat area. The collision area is represented by a sphere with a radius of R p , and the threat area is represented by a sphere with a radius of R w .

8. The fusion optimization method for multi-UAV collaborative trajectory planning according to claim 6, characterized in that: Step S4-2 constructs a multi-UAV formation constraint model; Based on the UAV flight environment model, establish a UAV formation collision avoidance constraint model, as shown in the following formula: Among them, (x i (k), y i (k), z i (k)) represents the current UAV position coordinates; R a represents the minimum collision avoidance safety distance of the UAV; (x j (k), y j (k), z j (k)) represents the adjacent UAV position coordinates; z all (k) represents the height of the ground coordinate (x i (k), y i (k)); △H d represents the minimum safe distance of the UAV from the ground; [P tx , P ty , P tz is the position coordinates of the sudden obstacle; R w is the threat area radius of the sudden obstacle; [P ox , P oy is the plane center coordinate of the static obstacle model, and P oz is the height of the static obstacle; Based on the Standoff algorithm, carry out multi-UAV formation planning, requiring the UAVs to rotate and advance in a formation surrounding manner. The surrounding planning constraint model for the multi-UAV formation is as follows: Among them, t i represents the time required for the UAV to form a formation, and t j represents the predicted time required for the UAV to form a formation; (x i , y i , z i ) represents the actual position of the UAV; represents the predicted position of the UAV formation; E x , E y , E z , E t represent the errors between the formation planning position and time.

9. According to the fusion optimization method for multi-UAV collaborative trajectory planning described in claim 6, it is characterized in that: Step S4-3: Analyze the triggering conditions and costs of multi-UAV formation reconstruction, and carry out formation reconstruction planning based on minimizing the cost; (1) Triggering conditions for formation reconstruction Due to insufficient prior information, the multi-UAV collaborative formation may be affected by sudden obstacles, and the obstacle is relatively large. If passing through according to the established formation of the UAVs, the distance between UAVs is less than the minimum safe distance between aircraft, or the distance between the UAV and the obstacle is less than the minimum safe distance. On the premise of ensuring the stable flight of the formation, it is necessary to start the formation reconstruction plan and select an optimized formation. After the formation reconstruction is completed, that is, when the UAVs are out of the threat area of the sudden obstacle and the task of the multi-UAVs "circumventing" the obstacle is completed, the UAVs continue to fly along the established trajectory; (2) Formation planning cost Set the formation planning cost It consists of two parts: conventional planning and reconfiguration planning. The set cost is for conventional track flight in the interval [0, j). In the interval [j, J), due to emergencies, the multi-UAV formation needs to carry out reconfiguration planning. In the interval [J, k), the UAVs complete the formation planning and continue to fly according to the established formation shape. The specific calculation of the UAV formation planning cost is shown in the following formula: where x i (k + j|k) represents the state of the UAV at the J-1 step; x g represents the terminal target state; u i (k + j|k) represents the control input of the UAV at the J-1 step; A i and B i are symmetric positive definite weight matrices; w = (w1, w2, w3) T is the weight vector; represents the cost of environmental threat; represents the cost of energy consumption; represents the cost of the UAV altitude, and the specific calculation is as follows: Wherein, (x i , y i , z i ) represents the coordinates of the current UAV flight path point; Wherein, (x l , y l , z l ) represents a given target coordinate; where z i represents the current track altitude; ΔH max represents the maximum flight altitude; Z1 and Z2 represent altitude penalty values, and ΔH d represents the minimum safe distance of the UAV from the ground; (3) During the local real-time path planning process, a multi-UAV formation reconstruction planning model is established based on the fusion of the model predictive control algorithm and the Standoff algorithm. The minimum cost guidance is introduced to design the UAV formation reconstruction planning. The specific local real-time path planning model for multi-UAV formations is as follows: Among them, and are the velocity and angular velocity control inputs of the i-th UAV within the prediction horizon H; (x i (k + p + 1|k), y i (k + p + 1|k), z i (k + p + 1|k)) are the three-dimensional position coordinates of the formation UAV i within the prediction horizon; (x i (k + p|k), y i (k + p|k), z i (k + p|k)) are the current three-dimensional position coordinates of the formation UAV i; are the velocity components of x, y, z of UAV i within the prediction horizon; is the velocity correction value of the rotation center of the UAV formation; are the optimal expected velocity component values of x, y, z of UAV i within the prediction horizon; v t (k + p|k) is the optimal expected velocity of the UAV; ω i (k + p|k) is the optimal expected angular velocity of the UAV; τ v and τ ω are the proportional adjustment factors of the UAV velocity and angular velocity respectively; the sampling time is △t; is the control input cost; is the formation planning cost, f1 i is the coverage of the UAV monitoring target, and the specific calculation is as follows; Among them, L t represents the distance from the sensor to the target, and P f represents the probability that the sensor effectively detects the target, and P w represents the probability that the sensor falsely detects the target, and P f , P w ∈(0, 1], and L max represents the maximum detection distance of the sensor, and L min represents the complete effective detection distance of the sensor, that is, when the distance between the sensor and the target is less than L min , P f = 1.

Images