CN116382334A

CN116382334A - Fusion optimization method for multi-unmanned aerial vehicle collaborative track planning

Info

Publication number: CN116382334A
Application number: CN202310302720.2A
Authority: CN
Inventors: 李波; 宋超; 马云红; 杨帆; 康培棋; 杨慧林; 万开方; 高晓光
Original assignee: Northwestern Polytechnical University
Current assignee: Northwestern Polytechnical University
Priority date: 2023-03-24
Filing date: 2023-03-24
Publication date: 2023-07-04

Abstract

The invention provides a fusion optimization method for collaborative flight path planning of multiple unmanned aerial vehicles, which comprises the steps of constructing an unmanned aerial vehicle motion model, initializing information of an unmanned aerial vehicle and a set target, acquiring the state of the unmanned aerial vehicle and the relative state of the unmanned aerial vehicle and the target to form a total state, inputting the total state into an optimization algorithm model fused with a model predictive control algorithm based on an optimization A algorithm and a stand off algorithm, and completing formation planning of the multiple unmanned aerial vehicles. According to the invention, by means of the advantages of different intelligent algorithms, a fusion algorithm rule is formulated, so that a fusion algorithm with more excellent performance is formed, and the function short board of a single algorithm is made up; the optimization fusion algorithm is applied to the collaborative flight path planning of multiple unmanned aerial vehicles in a three-dimensional complex environment, so that the unmanned aerial vehicle group flight path planning realizes real-time collision prevention and formation control, the unmanned aerial vehicle detection coverage area is maximized, and safer and more feasible flight path planning is obtained.

Description

Fusion optimization method for multi-unmanned aerial vehicle collaborative track planning

Technical Field

The invention relates to the field of unmanned aerial vehicle autonomous control, in particular to a method for performing track planning on multiple unmanned aerial vehicles based on fusion of an optimization A-based algorithm, a model predictive control algorithm and a stand off algorithm.

Background

With the increasing complexity of task environments and increasing task demands, by means of the fact that a single unmanned aerial vehicle is difficult to complete a specific flight task, intelligent cooperative control of multiple unmanned aerial vehicles gradually becomes a research focus in the unmanned intelligent field. The flight path planning is used as a key technology for researching the cooperative control of multiple unmanned aerial vehicles, and has very important significance especially for the cooperative flight path planning research of the multiple unmanned aerial vehicles in a complex environment.

In recent years, related researchers have achieved partial research results in multi-unmanned aerial vehicle collaborative track planning, such as: the algorithm A, the artificial potential field method, the bionic algorithm, the control algorithm and the like are applied to unmanned aerial vehicle track planning. The algorithm A is used as an offline unmanned aerial vehicle track planning core algorithm, has good global planning capacity, is only suitable for searching a known path of an obstacle, cannot avoid a sudden obstacle in real time, and has a large number of algorithm application scenes in a two-dimensional environment; the artificial potential field method is applied to unmanned aerial vehicle collaborative track planning, so that local optimum is easily trapped, and a complete mathematical model is difficult to build; the bionic algorithm mainly comprises an ant colony algorithm and a particle swarm algorithm, and is difficult to meet the real-time requirement of unmanned aerial vehicle track planning due to low calculation efficiency; the control algorithm mainly comprises PID control, optimal control, sliding mode control and model prediction control, wherein the model prediction control algorithm is used as the only control method capable of explicitly processing constraint at present and has become a recognized standard for processing the control problem of complex constraint variables, but when the model prediction control algorithm is applied to unmanned aerial vehicle flight path planning, the problem of weak global path planning capability exists, and the model prediction control algorithm is only applied to local path planning with good instantaneity. In summary, the problems of large model calculation amount, weak global planning capability, insufficient real-time performance, single application environment and poor formation and maintenance capability existing in unmanned aerial vehicle track planning research at present have a certain gap from the real task requirements.

Disclosure of Invention

In order to overcome the defects of the prior art, the invention provides a fusion optimization method for multi-unmanned aerial vehicle collaborative track planning. In order to overcome the defect of a single algorithm, the invention provides an intelligent optimization algorithm based on the fusion of an optimization A-type algorithm, a model predictive control algorithm and a standby off algorithm, and realizes the collaborative track planning of multiple unmanned aerial vehicles in a three-dimensional complex environment.

According to the intelligent fusion algorithm, the overall planning capability and the real-time planning capability of the unmanned aerial vehicle track are comprehensively considered, the A-type algorithm, the model predictive control algorithm and the stand off algorithm are fused, the advantages and the disadvantages are taken into account, the intelligent fusion algorithm is provided, the problems that the overall planning capability of the unmanned aerial vehicle track is weak, the real-time performance is poor, the formation and maintenance capability of the unmanned aerial vehicle track is insufficient are solved, the autonomy of the fusion algorithm is high, the robustness is good, and the real task requirements are met.

The technical scheme adopted by the invention for solving the technical problems comprises the following steps:

step S1: constructing unmanned aerial vehicle motion model

In a three-dimensional space, the unmanned aerial vehicle position, speed, pitch angle and course angle jointly construct an unmanned aerial vehicle motion model, an OXYZ coordinate system is set as the three-dimensional space coordinate system where the unmanned aerial vehicle is located, wherein an origin O represents the center of a task area of the unmanned aerial vehicle, an X axis points to the north direction, a Z axis points to the east direction, and a Y axis points to the vertical upward direction;

When the unmanned aerial vehicle motion is considered, the unmanned aerial vehicle is regarded as particles, a three-dimensional discretization unmanned aerial vehicle kinematic model is established, the sampling time is delta t, and the unmanned aerial vehicle motion equation is as follows:

wherein (x (k), y (k), z (k)) represents the position coordinates of the unmanned aerial vehicle at the moment k; (x (k+1), y (k+1), z (k+1)) represent the position coordinates of the unmanned aerial vehicle at time k+1; v (k) represents the real-time speed of the unmanned aerial vehicle at the moment k; s (k) represents state sampling of the unmanned aerial vehicle at the moment k; s represents a feasible state set; u (k) represents decision input of the unmanned plane at the moment k; u represents a feasible input set; pitch angle θ (k) and θ (k+1) are dividedThe included angles between the velocity vector of the unmanned aerial vehicle at the moment k and the moment k+1 and the XOZ plane are respectively shown; course angle

And->

The positive included angle between the projection vector of the velocity vector of the unmanned aerial vehicle at the moment k and the moment k+1 on the XOZ plane and the X axis is formed; a (k+1) represents the acceleration of the unmanned aerial vehicle at the time k+1;

step S2: initializing information of the unmanned aerial vehicle and a set target, acquiring the state of the unmanned aerial vehicle and the relative state of the unmanned aerial vehicle and the target, and forming a total state Q _all ；

The state of the drone itself includes a position component (x _r ,y _r ,z _r ) Unmanned aerial vehicle speed size v _r Unmanned aerial vehicle pitch angle theta _r Course angle of unmanned aerial vehicle

The relative state of the unmanned aerial vehicle and the target comprises a relative distance d _r Relative azimuth q _r The method comprises the steps of carrying out a first treatment on the surface of the The total state of composition is->

Step S3: will total state Q _all Inputting an optimization algorithm model based on fusion of an optimization A algorithm, a model predictive control algorithm and a standby off algorithm, wherein the hierarchy structure of the fusion algorithm is shown in figure 1; the method divides a multi-unmanned plane collaborative track planning system into a track planning layer and a path planning layer, and the track planning is developed based on an optimization A-type algorithm, so that a static geometric path irrelevant to time is obtained; the path planning is developed based on the fusion of a model predictive control algorithm and a standby off algorithm, the model predictive control algorithm solves the problem of large-scale real-time optimal control in limited time, utilizes preview capability to realize optimal action control in constraint, nonlinear, model uncertain and unpredictable environments, generates a real-time path planning suitable for the actual flight of a multi-unmanned aerial vehicle formation,the standby off algorithm is used as one of main algorithms for formation control, the maximization of the detection range of the sensor is realized based on the safety distance, and the target loss probability is reduced;

step S4: multiple unmanned aerial vehicle formation planning. The sudden obstacle model is implanted into the prior static obstacle model, the verification fusion algorithm is applied to the real-time obstacle avoidance capability of multi-unmanned aerial vehicle collaborative track planning, and the specific multi-unmanned aerial vehicle collaborative formation planning steps are as follows:

The specific track planning process in the step 3 is as follows:

s31: comprehensively considering collision avoidance constraint of unmanned aerial vehicles, and constructing a multi-unmanned aerial vehicle global track planning model in a three-dimensional space based on an A-algorithm; developing A algorithm sparse variable step curve optimization based on a compression search space and a smoothing method; adding a related cost weight of an A-algorithm heuristic function G (x) and H (x), and introducing a weight method to improve the algorithm operation efficiency; based on the method, an optimization A algorithm is applied to three-dimensional space multi-unmanned aerial vehicle global track planning, global track planning consisting of a plurality of intermediate waypoints is generated, and the intermediate waypoints are handed over to an S32-stage route storage unit;

s32: taking out the global track planning point saved in the step S31 from the next track point of the current unmanned aerial vehicle position, using the next track point as a model predictive control algorithm to be applied to a temporary target point of unmanned aerial vehicle real-time track planning, and carrying out unmanned aerial vehicle local real-time path planning based on a model predictive control model;

s33: in the process of developing local real-time path planning, controlling unmanned aerial vehicle group formation flight by adopting a stand off algorithm based on Lyapunov vector field, and maximizing unmanned aerial vehicle sensor observation coverage;

s34: establishing a three-dimensional complex environment with priori obstacle information, constructing an obstacle model, and verifying the effectiveness and instantaneity of the fusion optimization algorithm model applied to the collaborative flight path planning of multiple unmanned planes;

Step S31 of unmanned aerial vehicle global track planning is based on optimization A algorithm model expansion, and specific modeling is as follows:

s31-1 initializing unmanned aerial vehicle basic information including a starting point (x ₁ ,y ₁ ,z ₁ ) Speed of rotationDegree v ₁ Course angle

Pitch angle theta ₁ The method comprises the steps of carrying out a first treatment on the surface of the Establishing an unmanned aerial vehicle sensor search sector, wherein the sector search radius is the current unmanned aerial vehicle search step length, and setting the opening angle along the vertical direction of a route as l of the maximum pitch angle ₁ The opening angle in the horizontal direction is set to be l of the maximum course angle ₂ Doubling; when the search sector is in the safe area, the maximum step length L is preferably selected _max Searching is carried out, if the search sector contains no-fly zones, the searching step length is dynamically regulated based on the criterion that the number of the no-fly zones contained in the search sector is inversely proportional to the searching step length, namely: the greater the number of contained no-fly zones, the shorter the search step. Initializing a search sector range of the unmanned aerial vehicle, determining the number of sub-sectors, determining step length selection and the like, wherein the specific modeling process is as follows:

in order to simplify modeling operation, setting that the unmanned aerial vehicle and the target are always kept in the same plane, and compressing a search task to a two-dimensional plane at the moment; the current position of the unmanned aerial vehicle on the my side is marked as a point B, the position of a target j is marked as a point C, the moving direction of the unmanned aerial vehicle is BP, the angle of the maximum range of the search sector of the unmanned aerial vehicle is set as < PBD, and the current position is marked as

BC represents the flight direction of the unmanned aerial vehicle tracking target, and < PBC represents the offset angle of the unmanned aerial vehicle tracking target j, the search sector range of the unmanned aerial vehicle is simplified into angle constraint, and the following formula is specifically calculated:

wherein,,

the search sector angle of the unmanned aerial vehicle is represented, and the size of the search sector angle is required to be determined according to the following steps:

wherein L is the searching step length; v represents the speed of the unmanned aerial vehicle; omega _max Representing the maximum constrained angular velocity of the unmanned aerial vehicle; alpha is the step size adjustment scaling factor.

When the search step length is L, the number m of extended sub-sectors to be searched is calculated as follows:

wherein,,

representing a search sub-sector angle;

determining the current searching step length, wherein the variable step length optimization of the algorithm A satisfies the following formula:

L _max ＝L _min +m _max ·△L

wherein L is _max Representing an algorithm searching maximum step size; l (L) _min Representing an algorithm searching minimum step size; m is m _max Representing the number of sub-sectors corresponding to the maximum step size; Δl represents a unit transform step size;

the variable step length a optimization algorithm is specifically modeled as:

wherein m is _u Representing the number of expansion nodes falling in the no-fly zone when the current step length of the algorithm is used for probing; beta represents a step length adjustment coefficient, and satisfies that beta is more than or equal to 1 and less than or equal to m _max 。

S31-2A algorithm node sparse optimization. According to unmanned aerial vehicle flight constraint, including minimum track section length, biggest turning angle constraint reduces search space, and specific steps are as follows:

1) Determining an expandable area from a current node, searching a current unmanned aerial vehicle searching step length with a radius, and setting an opening angle in the vertical direction of a route as k of a maximum climbing/diving angle ₁ Multiple of waterSetting the opening angle of the flat direction as k of the maximum course angle ₂ Doubling;

2) K for a sector of the expandable region in the climb/dive direction ₁ Aliquoting, and carrying out K on the sector in the turning direction ₂ Aliquoting to obtain K ₁ *K ₂ A plurality of points; k (K) ₁ And K is equal to ₂ The value is properly selected, K ₁ And K is equal to ₂ In [3,5 ]]The selection between the two can meet the requirement, K ₁ And K is equal to ₂ The larger the ideal route is found, the larger the probability is, but the storage memory requirement and the search time are increased, and K is calculated ₁ *K ₂ The points are used as nodes to be expanded;

3) Calculating a cost value from a current node to an expansion node, and selecting a node with the minimum cost function on each sector;

4) Judging whether the node selected in the step 3) meets the minimum near-ground safety distance constraint of the unmanned aerial vehicle and the maximum flight safety distance constraint of the unmanned aerial vehicle or not, and if not, directly discarding the node; if yes, carrying out track planning according to the node;

S31-3A algorithm trajectory curve optimization; using a turning shortcut curve method to replace a broken line track of the unmanned aerial vehicle, determining a track point tangent sphere center C (k), judging the turning direction of the track point, and solving a tangent point coordinate M ₁ ,M ₂ ；

1) Determining track point cutting ball C (k)

Track turning point A of ith unmanned aerial vehicle _i Is (x) _i ,y _i ,z _i ) Setting course angle of unmanned aerial vehicle

And pitch angle theta _r The track turning radius is R, and the coordinates of the turning and ball cutting sphere center of the unmanned aerial vehicle are calculated as follows:

wherein C is _Cis-cis (k) Representing coordinates of a clockwise-rotated sphere center; c (C) _{Reverse direction} (k) Representing the coordinates of the center of a counterclockwise turn; θ _r And (3) with

Representing the pitch angle and the course angle of the unmanned aerial vehicle; (x) _i (k),y _i (k),z _i (k) A) position coordinates representing the turning moment of the unmanned plane i;

2) Judging the turning direction of the track point;

judging the track turning point A by adopting a mixed vector integration algorithm _i The turning direction of the two sections of tracks adjacent to each other in front and back, wherein the current track point is the track turning point A of the ith unmanned aerial vehicle _i Then the previous track segment is denoted as A _i-1 A _i The latter track segment is denoted as A _i A _i+1 ，δ＝[δ _x ,δ _y ,δ _z ] ^T And ε= [ ε ] _x ,ε _y ,ε _z ] ^T The unit vectors in the directions of the front track section and the rear track section are respectively, and the turning direction of the unmanned aerial vehicle is determined by calculating the following formula:

wherein sign represents a sign function;

①γ(δ,ε)<0: representing current unmanned aerial vehicle along track section A _i-1 A _i Fly to A _i A _i+1 A counterclockwise turn is required;

②γ(δ,ε)>0: representing current unmanned aerial vehicle along track section A _i-1 A _i Fly to A _i A _i+1 Clockwise turning is required;

(3) γ (δ, ε) =0: representing 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 constraint conditions of the maximum course angle and the maximum pitch angle of the unmanned aerial vehicle, the 180-degree turning condition of the unmanned aerial vehicle cannot occur, so that the unmanned aerial vehicle can directly move without turning, namely the current track node is a non-track turning point;

3) Solving for the coordinates of the tangent points

In the course of unmanned aerial vehicle track turning smoothing treatment, the turning arc is required to be in front ofTrack segment A _i-1 A _i And the subsequent track segment A _i A _i+1 Are all tangent, and the tangent point is set as M ₁ ,M ₂ After the unmanned aerial vehicle track is subjected to smoothing treatment, the unmanned aerial vehicle track is formed by two original sections A _i-1 A _i And A is a _i A _i+1 The following three steps are changed:

①A _i-1 M ₁ the track section continues to maintain a straight-line flight state;

(2) from M ₁ To M ₂ The track segment performs a circumferential flight around a radius R;

③M ₂ A _i+1 the track segment performs a straight line flight.

Calculating the turning sphere center position coordinate C according to the calculation formula of the turning sphere center coordinate of the unmanned aerial vehicle _i Setting q when the radius of the winding ball is R _i ,q _i+1 Track segment A respectively _i-1 A _i And A is a _i A _i+1 Unit vector above, vector q' and vector q _i Perpendicular, vector q' and vector q _i+1 Perpendicular, assume vector q _i To q _i+1 Is rotated clockwise, then the tangent point M ₁ ,M ₂ The coordinates are calculated by the following formula:

s31-4 optimizing an A-algorithm by using a weight method, and adding a weight a of H (x) and G (x) related costs _g ,b _h The optimization heuristic is expressed as:

wherein G (x) is a function of the real cost of the unmanned aerial vehicle from the starting position to the current position, and H (x) is a function of the estimated cost of the unmanned aerial vehicle from the current position to the target point; a, a _g ,b _h Weights of real cost and predicted cost respectively, a _g The value range of (2) is [0.5,1 ] ]，c _div The weight ratio is a value larger than 1;

in three-dimensional space, the G (x) expansion is calculated as follows:

wherein, l is expressed as the side length of the map grid;

in three-dimensional space, H (x) represents the following formula:

wherein, (x) _r ,y _r ,z _r ) Representing current unmanned aerial vehicle track coordinates; (x) _b ,y _b ,z _b ) Representing the coordinates of the target position;

step S32 is to extract the information of the global track planning point of the unmanned aerial vehicle, take the next track point of the current unmanned aerial vehicle position as a temporary terminal point of local real-time path planning, and complete the real-time planning of the unmanned aerial vehicle by adopting a model predictive control algorithm; after the unmanned aerial vehicle reaches the temporary target point, the next route point is taken out from the route storage unit as the target point to carry out route planning, and the route planning is continuously carried out until the last route point of the unmanned aerial vehicle route is reached, wherein the local real-time route planning process is as follows:

s32-1: initializing a discrete motion model of the unmanned aerial vehicle;

s32-2: taking out the next route point of the current unmanned aerial vehicle position from the global route planning information, and setting the next route point as a local route temporary target point;

s32-3: constructing a local path planning model of the unmanned aerial vehicle by adopting a model predictive control algorithm;

s32-4: the unmanned aerial vehicle global real-time track planning is realized by fusing an optimization A-type algorithm and a model predictive control algorithm;

The model predictive control algorithm is modeled as follows:

wherein,,

and->

The speed and angular speed of the ith unmanned aerial vehicle in the prediction time domain H are controlled and input; v _i (k+p|k) is the current speed of the unmanned aerial vehicle; v _i (k+p+ 1|k) is the speed of the drone in the prediction time domain; omega _i (k+p|k) is the unmanned angular velocity; omega _i (k+p+ 1|k) is the angular velocity of the drone in the predicted time domain; />

The current course angle of the unmanned aerial vehicle is; />

The course angle of the unmanned aerial vehicle in the prediction time domain is set; θ _i (k+p|k) is the current pitch angle of the unmanned aerial vehicle; θ _i (k+p+ 1|k) is the pitch angle of the unmanned aerial vehicle in the predicted time domain; (x) _i (k+p+1|k),y _i (k+p+1|k),z _i (k+p+ 1|k)) is the three-dimensional position coordinates of the unmanned aerial vehicle in the prediction time domain; (x) _i (k+p|k),y _i (k+p|k),z _i (k+p|k)) is the current three-dimensional motion coordinate of the unmanned aerial vehicle; τ _v And τ _ω The speed and angular speed ratio adjusting factors of the unmanned aerial vehicle are respectively, and the sampling time is delta t;

the real-time flight path planning modeling constraint conditions of the unmanned aerial vehicle based on the model predictive control algorithm are as follows:

wherein,,

and->

Is the ith unmanned aerial vehicleA speed and angular speed control input in a predicted time domain H; />

And->

For maximum minimum speed constraint of unmanned aerial vehicle, +.>

Constraint for maximum angular velocity of the unmanned aerial vehicle; (x) _i (k+p+1|k),y _i (k+p+1|k),z _i (k+p+ 1|k)) is the three-dimensional position coordinates of the formation unmanned aerial vehicle i in the prediction time domain; (x) _j (k+p+1|k),y _j (k+p+1|k),z _j (k+p+ 1|k)) is the three-dimensional position coordinates of the formation unmanned aerial vehicle j in the prediction time domain; r is R _a The minimum safety distance of the unmanned aerial vehicle is set; (o) _xj ,o _yj ,o _zj ) Is the center position of the obstacle, o _rj Is the obstacle radius; (x) _i (k),y _i (k),z _i (k) Is the initial position coordinate of the unmanned aerial vehicle, v _i (k) For the initial speed of the unmanned aerial vehicle, +.>

For the initial course angle theta of the unmanned aerial vehicle _i (k) Solving the model predictive control algorithm model for the initial pitch angle of the unmanned aerial vehicle at the moment k to obtain an optimization result of the control quantity of the speed and the angular speed of the unmanned aerial vehicle, and outputting the result to a unmanned aerial vehicle motor control unit;

in the process of unmanned aerial vehicle local real-time track planning, the step S33 fuses a stand off algorithm to control a plurality of unmanned aerial vehicles to develop collaborative formation planning, so that the optimal search formation maintenance of the plurality of unmanned aerial vehicles is realized, and the specific process is as follows:

s33-1: initializing unmanned aerial vehicle cluster information;

s33-2: initializing a global track planning of the unmanned aerial vehicle by using an optimization A-algorithm, and carrying out local real-time optimal track planning by using a model predictive control algorithm;

s33-3: in the process of local real-time optimal track planning, the method is applied to unmanned aerial vehicle formation and maintenance control based on a standby off algorithm, so that unmanned aerial vehicle groups are uniformly distributed around a target, and further, the maximum search monitoring of a plurality of unmanned aerial vehicle formation sensors is completed, and the specific modeling is as follows:

Developing multi-unmanned aerial vehicle formation planning based on standby off algorithm, wherein unmanned aerial vehicle formation flies in a spiral mode, and the rotating radius is D _r The corresponding Lyapunov energy function is a distance function L _d (x, y, z) as shown in the following formula:

where r is the unmanned position (x _r ,y _r ,z _r ) And the rotation center position (x _d ,y _d ,z _d ) Is provided for the radial distance of (a),

ζ represents a formation cooperative allowance error;

in order to simplify the operation, the invention sets that the multiple unmanned aerial vehicle formations are distributed in the same plane, so that only the influence of phase angle positioning is considered: the phase angles of any two unmanned aerial vehicles are phi respectively _i And phi _j The expected relative phase angle is phi _z Calculating a multi-unmanned aerial vehicle cooperative formation phase function based on Lyapunov stability theory as follows:

wherein mu _p The difference value between the relative phase angles of the two unmanned aerial vehicles and the expected phase angle is represented, and N represents the number of unmanned aerial vehicles in formation.

The unmanned aerial vehicle phase angle speed is calculated as follows:

wherein,,

for unmanned plane i phase angle speed,/>

For unmanned plane j phase angle speed, v _r The real-time speed of the unmanned aerial vehicle is obtained, and k is a function coefficient;

the unmanned aerial vehicle speed is calculated as follows:

v _i ＝v _r

v _j ＝k·(φ _i -φ _j -φ _z )·D _r +v _r

wherein v is _i Is the speed of the unmanned plane i, v _j Is the j speed of the unmanned plane;

calculating the optimal expected speed of the unmanned aerial vehicle by combining the predicted speed of the unmanned aerial vehicle with the rotational center speed correction term, wherein the calculation expression is as follows:

Wherein,,

for the optimal desired speed value of the unmanned aerial vehicle +.>

For the predicted speed value of the unmanned aerial vehicle,

is the rotational center speed correction value. Optimal prediction speed v for multi-unmanned aerial vehicle formation _t Optimal heading angle->

And an optimal pitch angle theta _t Calculated according to the following formula:

the step S4 of the multi-unmanned aerial vehicle collaborative formation planning comprises the following steps:

s4-1: constructing an unmanned aerial vehicle flight environment model;

s4-2: establishing an unmanned aerial vehicle formation collision prevention constraint model based on an unmanned aerial vehicle flight environment model, wherein the collision prevention constraint model comprises a minimum safety distance among unmanned aerial vehicles, a minimum near-ground safety distance of the unmanned aerial vehicles and a minimum safety distance of an obstacle of the unmanned aerial vehicles, judging whether an emergency condition affects unmanned aerial vehicle formation safety planning in real time, and if the emergency condition enables the unmanned aerial vehicle to continue to fly according to a given formation planning, turning to step S4-3, and continuing the given collaborative flight planning without the emergency condition or without affecting unmanned aerial vehicle formation safety planning;

s4-3: performing multi-unmanned aerial vehicle formation reconstruction, analyzing multi-unmanned aerial vehicle formation reconstruction triggering conditions and cost, designing unmanned aerial vehicle formation reconstruction planning based on a minimum cost target, performing multi-unmanned aerial vehicle formation expected position planning based on a standby off algorithm, and correcting the unmanned aerial vehicle position by using a feedback-correction mechanism through comparison of actual position information and expected position information;

The step S4-1 of constructing the unmanned aerial vehicle flight environment model comprises the following steps of:

considering that an unmanned aerial vehicle needs to fly as ultra-low as possible in order to avoid radar detection, a complex ground environment and obstacles thereof become a main threat for unmanned aerial vehicle track planning. Map establishment based on relief and uneven topographyThe model is used for setting up a safety buffer area around the unmanned plane in order to improve the robustness of the method. Simultaneously establishing a static obstacle model and a burst obstacle model, wherein the static obstacle model is approximated by a cylinder, and the plane center is set as P _o The coordinates are [ P ] _ox ,P _oy ]P for radius and height _or And P _oz Indicating that the surrounding establishes a collision zone (with L _od And DeltaH _od Represented) and threat zone (represented by L _OD And DeltaH _OD Representation), L _od Expressed as minimum near-safe distance, ΔH _od Expressed as a minimum height approach distance, i.e. the distance of the drone from the static obstacle is less than L _od Or DeltaH _od When the unmanned aerial vehicle collides; l (L) _OD Expressed as the maximum threat distance of a static obstacle, ΔH _OD Expressed as a maximum threat height of static obstacle, i.e. the distance of the drone from the static obstacle is less than L _OD Or DeltaH _OD When the unmanned aerial vehicle possibly has collision risk, the sudden obstacle model is approximated by a sphere, and the sphere center is set to be P _t Specific coordinates are [ P _tx ,P _ty ,P _tz ]Radius of R _t Also set up are collision zones (with radius R _p Represented by spheres) and threat zones (represented by a radius R _w Is represented by a sphere);

s4-2, constructing a multi-unmanned aerial vehicle formation constraint model;

establishing an unmanned aerial vehicle formation collision prevention constraint model based on an unmanned aerial vehicle flight environment model, wherein the model is specifically shown as follows:

wherein, (x) _i (k),y _i (k),z _i (k) A) representing the current position coordinates of the unmanned aerial vehicle; r is R _a Representing the minimum collision avoidance safety distance of the unmanned aerial vehicle; (x) _j (k),y _j (k),z _j (k) Representing the position coordinates of adjacent unmanned aerial vehicles; z _all (k) Representing ground coordinates (x) _i (k),y _i (k) A) the height of the floor; deltaH _d Representing the minimum safety distance of the unmanned plane near the ground; [ P ] _tx ,P _ty ,P _tz ]Position coordinates of burst obstacles; r is R _w Threat zone radius for bursty obstacles; [ P ] _ox ,P _oy ]Is the plane center coordinate of the static obstacle model, P _oz Is a static obstacle height;

developing multi-unmanned aerial vehicle formation planning based on a stardoff algorithm, requiring unmanned aerial vehicles to rotate and advance in a formation surrounding mode, wherein a multi-unmanned aerial vehicle formation surrounding planning constraint model is as follows:

wherein t is _i Indicating the time required for the unmanned mechanism to build the queue, t _j The prediction time required by the formation of the unmanned aerial vehicle is represented; (x) _i ,y _i ,z _i ) Representing the actual position of the unmanned aerial vehicle;

representing a predicted location of the unmanned aerial vehicle formation; e (E) _x ,E _y ,E _z ,E _t Representing formation planning position and time errors;

Step S4-3: analyzing a multi-unmanned aerial vehicle formation reconstruction triggering condition and cost, and developing formation reconstruction planning based on cost minimization;

(1) Formation reconfiguration trigger conditions

Due to insufficient prior information, the cooperative formation of multiple unmanned aerial vehicles possibly encounters the influence of sudden obstacles, the obstacle shape is larger, if the unmanned aerial vehicles pass according to the established formation of the unmanned aerial vehicles, the distance between the unmanned aerial vehicles is smaller than the minimum safe distance, or the distance between the unmanned aerial vehicles and the obstacle is smaller than the minimum safe distance, the formation reconstruction planning is required to be started on the premise of ensuring the stable flight of the formation, and the formation is selected and optimized; after formation reconstruction is completed, namely the unmanned aerial vehicle breaks away from the sudden obstacle threat zone, the task of 'detouring' the obstacle by the unmanned aerial vehicle is completed, and the unmanned aerial vehicle continues to fly with a set flight path;

(2) Formation planning cost

Setting formation planning cost

The method comprises the steps of conventional planning and reconstruction planning, wherein the set cost is the conventional track flight in the interval of [0, J), reconstruction planning is required to be carried out based on the formation of multiple unmanned aerial vehicles in the interval of [ J, J), the formation planning is completed by the unmanned aerial vehicles in the interval of [ J, k), the flight is continued according to the formation of the established formation, and the specific unmanned aerial vehicle formation planning cost is calculated as shown in the following formula:

Wherein x is _i (k+j|k) represents the unmanned plane J-1 step status; x is x _g Representing a terminal target state; u (u) _i (k+j|k) represents a drone J-1 step control input; a is that _i And B is connected with _i Is a symmetric positive weighting matrix; w= (w) ₁ ,w ₂ ,w ₃ ) ^T Is a weight vector;

representing an environmental threat cost; />

Representing energy consumption costs; />

Representing the unmanned aerial vehicle altitude cost, and specifically calculating the following formula:

in (x) _i ,y _i ,z _i ) Representing current unmanned aerial vehicle track point coordinates;

in (x) _l ,y _l ,z _l ) Representing the coordinates of a given target;

wherein z is _i Representing a current track altitude; deltaH _max Representing a maximum flying height; z is Z ₁ And Z is ₂ Represents a high penalty value, ΔH _d Representing the minimum safety distance of the unmanned plane near the ground;

(3) In the process of local real-time path planning, a multi-unmanned aerial vehicle formation reconstruction planning model is built based on fusion of a model prediction control algorithm and a stand off algorithm, minimum cost is introduced to guide and design unmanned aerial vehicle formation reconstruction planning, and the specific multi-unmanned aerial vehicle formation local real-time path planning model is as follows:

wherein,,

and->

The speed and angular speed of the ith unmanned aerial vehicle in the prediction time domain H are controlled and input; (x) _i (k+p+1|k),y _i (k+p+1|k),z _i (k+p+ 1|k)) is the three-dimensional position coordinates of the formation unmanned aerial vehicle i in 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 unmanned aerial vehicle i;

The velocity components of x, y and z in the prediction time domain are used for the unmanned aerial vehicle i;

forming a rotational center speed correction value for the unmanned aerial vehicle;

the optimal expected speed component values of x, y and z of the unmanned aerial vehicle i in a prediction time domain are obtained; v _t (k+p|k) is the optimal desired speed of the drone; omega _i (k+p|k) is the optimal desired angular velocity of the drone; τ _v And τ _ω The speed and angular speed proportion adjustment factors of the unmanned aerial vehicle are respectively; sampling time is delta t; />

To control input cost; />

Planning costs for formation->

Monitoring target coverage for the unmanned aerial vehicle, and specifically calculating the following formula;

wherein L is _t Representing the distance of the sensor to the target, P _f Representing the probability that the sensor is effectively detecting the target, P _w Representing probability of false detection of target by sensor, P _f ,P _w ∈(0,1]，L _max Indicating the maximum detection distance of the sensor, L _min Indicating that the sensor is fully detecting the effective distance, i.e. the sensor is less than L from the target _min At the time P _f ＝1。

The invention has the beneficial effects that by means of the advantages of different intelligent algorithms, a fusion algorithm rule is formulated to form a fusion algorithm with more excellent performance, and the function short board of a single algorithm is made up; the optimization fusion algorithm is applied to the collaborative flight path planning of multiple unmanned aerial vehicles in a three-dimensional complex environment, so that the unmanned aerial vehicle group flight path planning realizes real-time collision prevention and formation control, the unmanned aerial vehicle detection coverage area is maximized, and safer and more feasible flight path planning is obtained.

Drawings

FIG. 1 is a diagram of a multi-unmanned cooperative track planning framework in accordance with the present invention.

Fig. 2 is a diagram of a multi-drone collaborative trajectory planning framework of the present invention.

Fig. 3 is a simulation diagram of collaborative flight path planning of two unmanned aerial vehicles based on a fusion algorithm, fig. 3 (a) is a top view, and fig. 3 (b) is a flight path planning diagram.

Detailed Description

The invention will be further described with reference to the drawings and examples.

The example provides a fusion optimization method for collaborative flight path planning of multiple unmanned aerial vehicles, and the flow is shown in fig. 1, and comprises the following steps:

step S1: constructing unmanned aerial vehicle motion model

In the three-dimensional space, the physical description quantity of the unmanned plane motion model comprises position, speed, pitch angle and course angle. The OXYZ coordinate system is set as a three-dimensional space coordinate system where the unmanned aerial vehicle is located, wherein an origin O represents the center of a task area of the unmanned aerial vehicle, an X axis points to the north direction, a Z axis points to the east direction, and a Y axis points to the vertical upward direction. Setting the sampling time to Δt=0.25 s, the specific motion model is expressed as:

wherein (x (k), y (k), z (k)) represents the position coordinates of the unmanned aerial vehicle at the moment k; (x (k+1), y (k+1), z (k+1)) represent the position coordinates of the unmanned aerial vehicle at time k+1; v (k) represents the real-time speed of the unmanned aerial vehicle at the moment k; s (k) represents state sampling of the unmanned aerial vehicle at the moment k; s represents a feasible state set; u (k) represents decision input of the unmanned plane at the moment k; u represents a feasible input set; the pitch angles theta (k) and theta (k+1) are respectively included angles between velocity vectors of the unmanned aerial vehicle at the moment k and the moment k+1 and an XOZ plane; course angle

And->

Projection direction of velocity vector of unmanned plane at k time and k+1 time on XOZ planeThe positive included angle between the quantity and the X axis; a (k+1) represents the acceleration of the unmanned aerial vehicle at time k+1.

Step S2: initializing information of the unmanned aerial vehicle and the target, which specifically comprises the following steps: the initial positions of the two unmanned aerial vehicles are [0m,0m ]]The set target positions are [400m,420m,400m]The initial pitch angle and yaw angle of the unmanned aerial vehicle are pi/4 rad, the initial speed is 25m/s, and the minimum turning radius is 10m. Acquiring the relative state of the unmanned aerial vehicle and the set target comprises a relative distance d _r Relative azimuth q _r Total state of

Representing a position vector between the unmanned aerial vehicle and the intended target, the direction being directed by the unmanned aerial vehicle to the intended target, d _r Represents the distance between the unmanned plane and the target, q _r Representing the relative azimuth angle of the unmanned aerial vehicle and the target, and being the speed vector of the unmanned aerial vehicle +.>

And distance vector->

Can be used for specific relative situation data>

d _r And q _r Description of:

wherein,,

for the position vector of the my unmanned aerial vehicle in three-dimensional space,/-for>

Is the speed vector of the unmanned aerial vehicle, v _r For the speed, θ _r For pitch angle, < >>

Is a yaw angle (x) _r ,y _r ,z _r ) The position coordinates of the three-dimensional space of the unmanned aerial vehicle; />

For the position vector of a given target in three dimensions, (x) _b ,y _b ,z _b ) Is the three-dimensional position coordinates of the given target.

Step S3: will always state

And inputting a fusion optimization algorithm model based on an optimization A algorithm, a model predictive control algorithm and a standby off algorithm, and establishing a multi-unmanned aerial vehicle collaborative track planning system framework.

Step S31 comprehensively considers collision avoidance constraint of the unmanned aerial vehicle, optimizes an A-algorithm based on a compressed search space method, a smoothing method and an add weight method, constructs a global track planning model of the unmanned aerial vehicle according to the A-algorithm, and transmits a generated global track planning intermediate route point to an unmanned aerial vehicle route storage unit.

S31-1, establishing an unmanned aerial vehicle sensor search sector, wherein the sector search radius is the current unmanned aerial vehicle search step length, and setting the opening angle along the vertical direction of a route as l of the maximum pitch angle ₁ =1.5 times, the opening angle in the horizontal direction is set to l of the maximum heading angle ₂ =1.5 times. When the search sector is in the safe area, the maximum step length L is preferably selected _max Search was performed =25m. If searchThe sectors contain no-fly zones, the searching step length L is reduced according to the number of the no-fly zones, the algorithm optimization comprises the steps of determining the range of the unmanned aerial vehicle searching sector, determining the number of the sub-sectors and selecting the determined step length.

To simplify modeling operations, the present example sets that the my drone is in the same plane as the target. The current position of the unmanned aerial vehicle on the my side is marked as a point B, the position of a target j is marked as a point C, the moving direction of the unmanned aerial vehicle is BP, the angle of the maximum range of the search sector of the unmanned aerial vehicle is set as < PBD, and the current position is marked as

BC represents the flight direction of the unmanned aerial vehicle tracking target, and Tilt PBC represents the offset angle of the unmanned aerial vehicle tracking target j. Based on the above, the range of the search sector of the unmanned aerial vehicle can be simplified into an angle constraint, and the following formula is specifically calculated:

wherein,,

indicating the search sector angle of the unmanned aerial vehicle.

Based on a three-dimensional complex environment, the size of the search sector angle is firstly determined according to the following steps:

wherein L is the searching step length; v represents the speed of the unmanned plane, and v=25m/s is taken; omega _max Represents the maximum constraint angular velocity of the unmanned plane and takes omega _max =0.2 rad/s; α is a step size adjustment scaling factor, taking α=0.2.

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

Wherein,,

representing the sector angle of the search sub-sector, taking +.>

Determining the current searching step length, and selecting the algorithm step length with variable step length A to meet the following formula:

L _max ＝L _min +m _max ·△L

wherein L is _max Representing the algorithm searching the maximum step length, taking L _max ＝25m；L _min Representing the algorithm searching the minimum step length, taking L _min ＝5m；m _max Representing the number of sub-sectors corresponding to the maximum step length, taking m _max =10; Δl represents a unit conversion step, and Δl=2m.

The variable step length a algorithm calculation method is specifically expressed as the following formula:

wherein m is _u Representing the number of expansion nodes falling in the no-fly zone when the current step length of the algorithm is used for probing; β represents a step size adjustment coefficient, β=3.

S31-2A algorithm node sparse optimization, wherein the search space is reduced according to unmanned aerial vehicle flight constraint including constraint of minimum track segment length and maximum turning angle, and the specific steps are as follows:

1) Determining an expandable area from a current node, wherein the searching radius is the searching step length of the current unmanned aerial vehicle, setting the opening angle in the vertical direction of a route to be 1.5 times of the maximum climbing/diving angle, and setting the opening angle in the horizontal direction to be 1.5 times of the maximum turning angle;

2) 3 equally dividing a sector in the climbing/diving direction of the expandable region, and 5 equally dividing a sector in the turning direction to obtain 15 expansion nodes;

4) Judging whether the node selected in the step 3) meets the constraint of the lowest flying height and the maximum flying distance, and if not, directly discarding the node; if yes, track planning is carried out according to the node.

S31-3, substituting a turning shortcut curve method for the unmanned aerial vehicle track broken line path, developing A-algorithm path curve optimization, determining a track point tangent sphere center C (k), judging the turning direction of the track point, and solving a tangent point coordinate M ₁ ,M ₂ The specific process is as follows:

1) Determining track point cutting ball C (k)

Assume track turning point A _i Is (x) _i ,y _i ,z _i ) Setting course angle of unmanned aerial vehicle

And pitch angle theta _r And if the track turning radius is R=10m, calculating the coordinates of the turning and ball cutting sphere center of the unmanned aerial vehicle as shown in the following formula.

Wherein C is _Cis-cis (k) Representing coordinates of a clockwise-rotated sphere center; c (C) _{Reverse direction} (k) Representing the coordinates of the center of a counterclockwise turn; r is represented as the track turning radius of the unmanned aerial vehicle; θ _r And (3) with

Representing the pitch angle and the course angle of the unmanned aerial vehicle; (x) _i (k),y _i (k),z _i (k) A position coordinate indicating the turning time of the unmanned aerial vehicle.

2) Judging the turning direction of the track point

The present example uses a hybrid vector integration algorithm to determine the track turning point A _i The turning direction of the two sections of tracks adjacent to each other. Assume the current track point O _i The previous section of track is A _i-1 A _i The next section of track is A _i A _i+1 ，δ＝[δ _x ,δ _y ,δ _z ] ^T And ε= [ ε ] _x ,ε _y ,ε _z ] ^T The unit vectors in the directions of the front track section and the rear track section are respectively, and then the turning direction of the unmanned aerial vehicle can be determined by calculating the following formula:

where sign represents a sign function.

(3) γ (δ, ε) =0: indicating that the current track node is a non-track turning point.

3) Solving for the coordinates of the tangent points

Determining turning sphere center coordinates C according to an unmanned aerial vehicle turning and cutting sphere center coordinate calculation formula _i The radius of the winding ball is R=10m, q is set _i ,q _i+1 Track segment A respectively _i-1 A _i And A is a _i A _i+1 Unit vector above, vector q' and vector q _i Perpendicular, vector q' and vector q _i+1 Vertical, assume vector q _i To q _i+1 Is rotated clockwise, then the tangent point M ₁ ,M ₂ The coordinates may be calculated by the following formula:

wherein a is _g ＝0.85，b _h ＝0.15，c _div ＝5.67。

In three dimensions, compared to the conventional a-algorithm, the G (x) expansion computes the following:

where l=5m.

In three-dimensional space, H (x) represents the following formula:

wherein, (x) _r ,y _r ,z _r ) Representing current unmanned aerial vehicle track coordinates; (x) _b ,y _b ,z _b ) Representing the target position coordinates.

And step S32, hierarchically fusing an optimization A algorithm and a model predictive control algorithm. And (3) sending the result obtained in the S31 global track planning process to a track storage unit of the unmanned aerial vehicle track planning module, taking out the next track point of the current unmanned aerial vehicle position from the track storage unit each time, and taking the next track point as a temporary target point, and realizing the unmanned aerial vehicle local real-time path planning based on a model prediction control algorithm. The specific layering fusion process is as follows:

S32-1: setting global track planning point information based on an optimization A-algorithm as a temporary end point of local path planning;

s32-2: constructing a local path planning prediction model of the unmanned aerial vehicle by using the speed v and the course angle of the unmanned aerial vehicle

And the pitch angle theta is used as input, a single-step optimization model in the rolling optimization process of a model predictive control algorithm is utilized to obtain an optimization result about the control quantity of the speed and the angular speed of the unmanned aerial vehicle, and the result is output to a unmanned aerial vehicle motor control unit to realize the local real-time path planning of the unmanned aerial vehicle. The specific model predictive control algorithm is modeled as follows: />

Wherein,,

and->

The current course angle of the unmanned aerial vehicle is; />

The course angle of the unmanned aerial vehicle in the prediction time domain is set; θ _i (k+p|k) is the current pitch angle of the unmanned aerial vehicle; θ _i (k+p+ 1|k) is the pitch angle of the unmanned aerial vehicle in the predicted time domain; (x) _i (k+p+1|k),y _i (k+p+1|k),z _i (k+p+ 1|k)) is the three-dimensional position coordinates of the unmanned aerial vehicle in the prediction time domain; (x) _i (k+p|k),y _i (k+p|k),z _i (k+p|k)) is the current three-dimensional motion coordinate of the unmanned aerial vehicle; τ _v And τ _ω Respectively, the speed and angular speed proportional adjustment factors of the unmanned aerial vehicle, tau _v ＝2，τ _ω =3; the sampling time is Δt=0.25 s.

The constraint conditions of the unmanned aerial vehicle local real-time path planning based on the model predictive control algorithm are as follows:

wherein,,

and->

The speed and angular speed of the ith unmanned aerial vehicle in the prediction time domain H are controlled and input; />

And->

For maximum minimum speed constraint of unmanned aerial vehicle, +.>

Constraint for maximum angular velocity of the unmanned aerial vehicle; (x) _i (k+p+1|k),y _i (k+p+1|k),z _i (k+p+ 1|k)) is the three-dimensional position coordinates of the formation unmanned aerial vehicle i in the prediction time domain; (x) _j (k+p+1|k),y _j (k+p+1|k),z _j (k+p+ 1|k)) is the three-dimensional position coordinates of the formation unmanned aerial vehicle j in the prediction time domain; r is R _a =15m is the minimum safe distance of the unmanned aerial vehicle, (o _xj ,o _yj ,o _zj ) Is the center position of the obstacle, o _rj =50m is the obstacle radius, (x _i (k),y _i (k),z _i (k) Is the initial position coordinate of the unmanned aerial vehicle, v _i (k) =25m/s is the initial speed of the unmanned aerial vehicle, +.>

For the initial course angle theta of the unmanned aerial vehicle _i (k) =pi/4 rad is the initial pitch angle of the drone.

Step S33, a multi-unmanned aerial vehicle formation control model is built by using a standby off algorithm, and the specific multi-unmanned aerial vehicle collaborative formation track planning process is as follows:

s33-1: developing the global track planning of the unmanned aerial vehicle by using an optimization A-x algorithm;

s33-2: carrying out unmanned aerial vehicle local real-time path planning by using a model optimization algorithm;

S33-3: based on unmanned aerial vehicle local real-time path planning, calculating a plurality of unmanned aerial vehicle formation phase distribution values by using a stand off algorithm, further determining the formation position of the unmanned aerial vehicle group, and realizing unmanned aerial vehicle cooperative formation control.

In the embodiment, multiple unmanned aerial vehicle formation planning is carried out based on a standby off algorithm, unmanned aerial vehicle formation flies in a spiral mode, and the rotation radius is set to be D _r =25m, the corresponding Lyapunov energy function is a distance function, as shown in the following formula:

ζ represents the formation synergy allowing error, taking ζ=0.8m.

In order to simplify the operation, the present example sets that the unmanned aerial vehicle groups are distributed in the same plane, so only the influence of phase angle positioning needs to be considered. The phase angles of any two unmanned aerial vehicles are phi respectively _i And phi _j The expected relative phase angle is phi _z Calculating a multi-unmanned aerial vehicle cooperative formation phase function based on Lyapunov stability theory as follows:

this example is a two unmanned aerial vehicle formation flight, n=2, Φ _z ＝π。

The angular velocity of any two unmanned aerial vehicles is calculated as follows:

wherein,,

for unmanned plane i phase angle speed,/>

For the phase angle speed of the unmanned aerial vehicle j, v is the real-time speed of the unmanned aerial vehicle, k is a function coefficient, and the value is k=0.5.

The speed of any two unmanned aerial vehicles is calculated as follows:

v _i ＝v

v _j ＝k·(φ _i -φ _j -φ _z )·D _r +v

wherein v is _i Is the speed of the unmanned plane i, v _j Is the speed of the unmanned aerial vehicle j.

The optimal expected speed of the unmanned aerial vehicle can be calculated through the combination of the predicted speed of the unmanned aerial vehicle and the rotational center speed correction term, and the calculation expression is as follows:

wherein,,

for the optimal desired speed value of the unmanned aerial vehicle +.>

For the predicted speed value of the unmanned aerial vehicle,

is the rotational center speed correction value.

Multi-unmanned aerial vehicle formation prediction speed v _t Course angle

And pitch angle theta _t Can be calculated according to the following formula:

/>

step S4: in the three-dimensional complex map model, a burst obstacle model is added in the embodiment, and the verification fusion algorithm is applied to the instantaneity and the effectiveness of multi-unmanned aerial vehicle formation track planning, and specifically comprises the following steps:

s4-1: constructing an unmanned aerial vehicle flight environment model;

s4-2: and establishing an unmanned aerial vehicle formation collision prevention constraint model based on the unmanned aerial vehicle flight environment model, and judging whether the unmanned aerial vehicle group encounters an emergency or not in real time to carry out formation reconstruction planning. If yes, turning to step S4-3, if not, continuing collaborative track planning;

s4-3: analyzing reconstruction triggering conditions and costs of the multi-unmanned aerial vehicle formation, carrying out real-time obstacle avoidance planning by using a model predictive control algorithm, carrying out multi-unmanned aerial vehicle formation expected position planning based on a standby off algorithm, and correcting the unmanned aerial vehicle position by using a feedback-correction mechanism.

S4-1, constructing an unmanned aerial vehicle flight environment model;

the example establishes a map model based on the relief and uneven topography, adds 4 static obstacle models, approximates by a cylinder, and the plane center of the static obstacle 1 is set as P _o1 Its coordinates are [100m,150m]Radius 20m, height 450m; the plane center of the static obstacle 2 is set as P _o2 Its coordinates are [210m,370m]Radius 20m, height 450m; the plane center of the static obstacle 3 is set as P _o3 Its coordinates are [300m,360m]Radius 20m, height 450m; the plane center of the static obstacle 4 is set as P _o1 Its coordinates are [400m,410m]Radius of 20m and height of 450m; the maximum threat distances of static obstacles are 10m. Adding a burst obstacle model and approximating the burst obstacle model by a sphere, wherein the center of the sphere is set to be P _t The specific position coordinates are [200m,270m,130m ]]The radius was 50m.

S4-2, constructing a multi-unmanned aerial vehicle formation constraint model

And establishing an unmanned aerial vehicle formation collision prevention constraint model based on the unmanned aerial vehicle flight environment model, wherein the model is specifically shown in the following formula.

Wherein, (x) _i (k),y _i (k),z _i (k) A) representing the current position coordinates of the unmanned aerial vehicle; r is R _a Represents the minimum collision avoidance safety distance of the unmanned aerial vehicle, R _a ＝15m；(x _j (k),y _j (k),z _j (k) Representing the position coordinates of adjacent unmanned aerial vehicles; z _all (k) Representing ground coordinates (x) _i (k),y _i (k) A) the height of the floor; deltaH _d Represents the minimum safety distance of the unmanned plane near the ground, delta H _d ＝10m。

In the multi-unmanned aerial vehicle formation planning process, unmanned aerial vehicles are required to rotationally advance in a formation surrounding mode, and multi-unmanned aerial vehicle formation planning constraints are calculated as follows:

wherein t is _i Indicating the time required for the unmanned mechanism to build the queue, t _j The prediction time required by the formation of the unmanned aerial vehicle is represented; x is x _i ,y _i ,z _i Representing the actual position of the unmanned aerial vehicle;

representing a predicted location of the unmanned aerial vehicle formation; e (E) _x ,E _y ,E _z ,E _t Indicating the formation plan position and time error.

Step S4-3-1 formation reconfiguration triggering condition

Due to insufficient prior information, the cooperative formation of multiple unmanned aerial vehicles possibly encounters the influence of sudden obstacles, the obstacle shape is large, if the unmanned aerial vehicles pass according to the established formation of the unmanned aerial vehicles, the distance between the unmanned aerial vehicles is smaller than the minimum inter-machine safety distance, or the distance between the unmanned aerial vehicles and the obstacle distance are smaller than the minimum safety distance, the formation reconstruction planning needs to be started on the premise of ensuring the stable flight of the formation, and the formation is selected and optimized. After formation reconfiguration is completed, namely the unmanned aerial vehicle breaks away from the sudden obstacle threat zone, the task of multiple unmanned aerial vehicles for 'detouring' the obstacle is completed, and the unmanned aerial vehicle continues to fly with a set flight path.

Step S4-3-2 formation planning cost

According to the discretization unmanned aerial vehicle motion model, the multi-unmanned aerial vehicle formation reconstruction cost function expression is constructed according to the following formula. Setting the cost to be conventional track flight in the [0, J) interval, carrying out reconstruction planning based on the formation of multiple unmanned aerial vehicles in emergency in the [ J, J) interval, completing formation planning by the unmanned aerial vehicles in the [ J, k) interval, and continuing to fly according to the established formation.

representing an environmental threat cost; />

Representing energy consumption costs; />

wherein, (x) _i ,y _i ,z _i ) Representing the current track point coordinates;

wherein, (x) _j ,y _j ,z _j ) Representing coordinates of a target point;

wherein z is _i Representing the current track point altitude; h _i Representing a terrain elevation corresponding to the current track point; h _min And H is _max Representing minimum and maximum fly heights; z is Z ₁ And Z is ₂ Representing a high penalty value.

S4-3-3 building formation reconstruction model

In the process of local real-time path planning, a multi-unmanned aerial vehicle formation reconstruction planning model is built based on fusion of a model prediction control algorithm and a stand off algorithm, minimum cost is introduced to guide and design unmanned aerial vehicle formation reconstruction planning, and the specific multi-unmanned aerial vehicle formation local real-time path planning model is as follows:

Wherein,,

and->

The speed and angular speed of the ith unmanned aerial vehicle in the prediction time domain H are controlled and input; (x) _i (k+p+1|k),y _i (k+p+1|k),z _i (k+p+ 1|k)) isForming three-dimensional position coordinates of the unmanned aerial vehicle i in a 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 unmanned aerial vehicle i;

the optimal expected speed component values of x, y and z of the unmanned aerial vehicle i in a prediction time domain are obtained; v _t (k+p|k) is the optimal desired speed of the drone; omega _i (k+p|k) is the optimal desired angular velocity of the drone; τ _v And τ _ω Respectively, the speed and angular speed proportional adjustment factors of the unmanned aerial vehicle, tau _v ＝2，τ _ω =3; sampling time Δt=0.25 s; />

To control input cost; />

Planning costs for formation->

Monitoring target coverage for the unmanned aerial vehicle, and specifically calculating the following formula:

wherein L is _t Representing the distance of the sensor to the target, P _f Representing the probability that the sensor is effectively detecting the target, P _w Representing probability of false detection of target by sensor, P _f ,P _w ∈(0,1]。L _max =90m represents the maximum detection distance of the sensor,

L _min =40m means that the sensor is fully detecting the effective distance, i.e. the sensor is less than L from the target _min When=40m, P _f ＝1。

The method mainly completes the collaborative flight path planning of multiple unmanned aerial vehicles in a three-dimensional complex environment based on a fusion optimization algorithm.

In an example, the optimization fusion algorithm is applied to the unmanned aerial vehicle track planning structure as shown in fig. 2.

In the example, after the optimization a-algorithm is adopted, compared with the traditional a-algorithm in the same state, the number of planning nodes is reduced from 105 to 72, the planning time is shortened from 2.69s to 2.55s, the total planning range is reduced from 2125m to 1944m, and the total planning range is reduced by about 8.5%.

In the example, the prediction step length of the model prediction control algorithm is 10 steps, the sampling times are 100 times, the number of state quantities is 3, the number of control quantities is 3, and the control quantities are respectively the speed, the pitch angle and the course angle of the unmanned aerial vehicle, and the sampling period Deltat=0.25 s.

In the example, 1 burst obstacle is added, the positions are [200m,270m,130m ], the minimum safety distance of the unmanned aerial vehicle is 15m, and the maximum communication distance is 90m.

In the example, the simulation realizes the collaborative flight path planning of multiple unmanned planes, and solves the problem of poor real-time obstacle avoidance capability of the stand off algorithm. The fusion algorithm is applied to multi-unmanned aerial vehicle collaborative track planning, and the specific unmanned aerial vehicle track planning is shown in fig. 3. The two unmanned aerial vehicles develop the track planning based on the fusion algorithm, burst obstacles can be avoided in real time, and the formation is formed and kept stable, so that the real-time requirement of the collaborative track planning of multiple unmanned aerial vehicles is met.

The above is only a preferred embodiment of the present invention, and the protection scope of the present invention is not limited to the above examples, and all technical solutions belonging to the concept of the present invention belong to the protection scope of the present invention. It should be noted that modifications and adaptations to the invention without departing from the principles thereof are intended to be within the scope of the invention as set forth in the following claims.

Claims

1. The fusion optimization method for the collaborative flight path planning of the multiple unmanned aerial vehicles is characterized by comprising the following steps of:

step S1: constructing an unmanned aerial vehicle motion model;

And->

Step S3: will total state Q _all Inputting an optimization algorithm model based on fusion of an optimization A-algorithm, a model predictive control algorithm and a standby off algorithm, dividing a multi-unmanned-plane collaborative flight path planning system into a flight path planning layer and a path planning layer, and expanding the flight path planning based on the optimization A-algorithm, so that a static geometric path irrelevant to time is obtained; the path planning is developed based on fusion of a model predictive control algorithm and a stand off algorithm, the model predictive control algorithm solves the problem of large-scale real-time optimal control in limited time, optimal action control is realized by utilizing preview capability in constrained, nonlinear, model uncertain and unpredictable environments, a real-time path planning suitable for actual flight of a multi-unmanned aerial vehicle formation is generated, the stand off algorithm is used as one of main formation control algorithms, the maximization of the detection range of a sensor is realized based on a safe distance, and the target loss probability is reduced;

Step S4: planning multi-unmanned aerial vehicle formation;

and implanting a sudden obstacle model into the prior static obstacle model, and verifying the real-time obstacle avoidance capability of the fusion algorithm applied to the collaborative track planning of the multiple unmanned aerial vehicles.

2. The fusion optimization method for multi-unmanned aerial vehicle collaborative track planning according to claim 1, wherein the fusion optimization method is characterized in that: the specific track planning process in the step 3 is as follows:

s34: and establishing a three-dimensional complex environment with priori obstacle information, constructing an obstacle model, and verifying the effectiveness and instantaneity of the fusion optimization algorithm model applied to the collaborative flight path planning of the multiple unmanned aerial vehicles.

3. The fusion optimization method for multi-unmanned aerial vehicle collaborative track planning according to claim 2, wherein the fusion optimization method is characterized in that: step S31 of unmanned aerial vehicle global track planning is based on optimization A algorithm model expansion, and specific modeling is as follows:

s31-1 initializing unmanned aerial vehicle basic information including a starting point (x ₁ ,y ₁ ,z ₁ ) Velocity v ₁ Course angle

Pitch angle theta ₁ The method comprises the steps of carrying out a first treatment on the surface of the Establishing an unmanned aerial vehicle sensor search sector, wherein the sector search radius is the current unmanned aerial vehicle search step length, and setting the opening angle along the vertical direction of a route as l of the maximum pitch angle ₁ The opening angle in the horizontal direction is set to be l of the maximum course angle ₂ Doubling; when the search sector is in the safe area, the maximum step length L is preferably selected _max To search if the sector is searchedAnd the number of the no-fly zones contained in the search sector is inversely proportional to the search step length, and the search step length is dynamically regulated, namely: the more the number of the no-fly zones is contained, the shorter the search step length is; initializing a search sector range of the unmanned aerial vehicle, determining the number of sub-sectors, determining step length selection and the like, wherein the specific modeling process is as follows:

The unmanned aerial vehicle and the target are always kept in the same plane, and the search task is compressed to a two-dimensional plane at the moment; the current position of the unmanned aerial vehicle on the my side is marked as a point B, the position of a target j is marked as a point C, the moving direction of the unmanned aerial vehicle is BP, the angle of the maximum range of the search sector of the unmanned aerial vehicle is set as < PBD, and the angle is marked as theta _max Not less than 0; BC represents the flight direction of the unmanned aerial vehicle tracking target, and < PBC represents the offset angle of the unmanned aerial vehicle tracking target j, the search sector range of the unmanned aerial vehicle is simplified into angle constraint, and the following formula is specifically calculated:

-θ _max ≤θ≤θ _max

wherein θ represents a search sector angle of the unmanned aerial vehicle, and the size of the search sector angle is determined according to the following formula:

wherein L is the searching step length; v represents the speed of the unmanned aerial vehicle; omega _max Representing the maximum constrained angular velocity of the unmanned aerial vehicle; alpha is a step length adjusting proportionality coefficient;

wherein Δθ represents the search sub-sector angle;

L _max ＝L _min +m _max ·△L

wherein L is _max Representing algorithmic searchesMaximum step length; l (L) _min Representing an algorithm searching minimum step size; m is m _max Representing the number of sub-sectors corresponding to the maximum step size; Δl represents a unit transform step size;

The variable step length a optimization algorithm is specifically modeled as:

wherein m is _u Representing the number of expansion nodes falling in the no-fly zone when the current step length of the algorithm is used for probing; beta represents a step length adjustment coefficient, and satisfies that beta is more than or equal to 1 and less than or equal to m _max ，

S31-2A algorithm node sparse optimization;

according to unmanned aerial vehicle flight constraint, including minimum track section length, biggest turning angle constraint reduces search space, and specific steps are as follows:

1) Determining an expandable area from a current node, searching a current unmanned aerial vehicle searching step length with a radius, and setting an opening angle in the vertical direction of a route as k of a maximum climbing/diving angle ₁ The opening angle in the horizontal direction is set to k of the maximum course angle ₂ Doubling;

1) Determining track point cutting ball C (k)

2) Judging the turning direction of the track point;

wherein sign represents a sign function;

3) Solving for the coordinates of the tangent points

In the course of unmanned aerial vehicle track turning smoothing treatment, the required turning arc is about to be identical to the previous track section A _i-1 A _i And the subsequent track segment A _i A _i+1 Are all tangent, and the tangent point is set as M ₁ ,M ₂ After the unmanned aerial vehicle track is subjected to smoothing treatment, the unmanned aerial vehicle track is formed by two original sections A _i-1 A _i And A is a _i A _i+1 The following three steps are changed:

③M ₂ A _i+1 the track section executes straight line flight;

calculating the turning sphere center position coordinate C according to the calculation formula of the turning sphere center coordinate of the unmanned aerial vehicle _i Setting q when the radius of the winding ball is R _i ,q _i+1 Track segment A respectively _i-1 A _i And A is a _i A _i+1 Unit vector above, vector q' and vector q _i Vertical, vectorq' and vector q _i+1 Perpendicular, assume vector q _i To q _i+1 Is rotated clockwise, then the tangent point M ₁ ,M ₂ The coordinates are calculated by the following formula:

wherein G (x) is a function of the real cost of the unmanned aerial vehicle from the starting position to the current position, and H (x) is a function of the estimated cost of the unmanned aerial vehicle from the current position to the target point; a, a _g ,b _h Weights of real cost and predicted cost respectively, a _g The value range of (2) is [0.5,1 ]]，c _div The weight ratio is a value larger than 1;

in three-dimensional space, the G (x) expansion is calculated as follows:

wherein, l is expressed as the side length of the map grid;

in three-dimensional space, H (x) represents the following formula:

4. The fusion optimization method for multi-unmanned aerial vehicle collaborative track planning according to claim 2, wherein the fusion optimization method is characterized in that:

S32-1: initializing a discrete motion model of the unmanned aerial vehicle;

the model predictive control algorithm is modeled as follows:

wherein,,

and->

The current course angle of the unmanned aerial vehicle is; />

wherein,,

and->

The speed and angular speed of the ith unmanned aerial vehicle in the prediction time domain H are controlled and input;

and->

For maximum minimum speed constraint of unmanned aerial vehicle, +.>

For the initial course angle theta of the unmanned aerial vehicle _i (k) And solving the model predictive control algorithm model at the moment k for the initial pitch angle of the unmanned aerial vehicle to obtain an optimization result of the control quantity of the speed and the angular speed of the unmanned aerial vehicle, and outputting the result to a unmanned aerial vehicle motor control unit.

5. The fusion optimization method for multi-unmanned aerial vehicle collaborative track planning according to claim 2, wherein the fusion optimization method is characterized in that:

S33-1: initializing unmanned aerial vehicle cluster information;

developing multi-unmanned aerial vehicle formation planning based on standby off algorithm, wherein unmanned aerial vehicle formation flies in a spiral mode, and the rotating radius is D _r Corresponding toLyapunov energy function is distance function L _d (x, y, z) as shown in the following formula:

ζ represents a formation cooperative allowance error;

the configuration of multiple unmanned aerial vehicle formations is distributed in the same plane, so only the influence of phase angle positioning needs to be considered: the phase angles of any two unmanned aerial vehicles are phi respectively _i And phi _j The expected relative phase angle is phi _z Calculating a multi-unmanned aerial vehicle cooperative formation phase function based on Lyapunov stability theory as follows:

wherein mu _p The difference value between the relative phase angles of the two unmanned aerial vehicles and the expected phase angle is represented, and N represents the number of the unmanned aerial vehicles in formation;

The unmanned aerial vehicle phase angle speed is calculated as follows:

wherein,,

i phase angle speed for unmanned aerial vehicle，/>

the unmanned aerial vehicle speed is calculated as follows:

v _i ＝v _r

v _j ＝k·(φ _i -φ _j -φ _z )·D _r +v _r

wherein,,

for the optimal desired speed value of the unmanned aerial vehicle +.>

Predicting a speed value for the unmanned aerial vehicle, < >>

Is a rotational center speed correction value; calculated according to the following formula:

wherein v is _t An optimal predicted speed is formed for multiple unmanned aerial vehicles,

for optimum heading angle, θ _t Is the optimal pitch angle.

6. The fusion optimization method for multi-unmanned aerial vehicle collaborative track planning according to claim 2, wherein the fusion optimization method is characterized in that: the step S4 of the multi-unmanned aerial vehicle collaborative formation planning comprises the following steps:

s4-1: constructing an unmanned aerial vehicle flight environment model;

S4-3: and (3) carrying out multi-unmanned aerial vehicle formation reconstruction, analyzing multi-unmanned aerial vehicle formation reconstruction triggering conditions and cost, designing unmanned aerial vehicle formation reconstruction planning based on a minimum cost target, carrying out multi-unmanned aerial vehicle formation expected position planning based on a standby off algorithm, and correcting the unmanned aerial vehicle position by using a feedback-correction mechanism through comparison of actual position information and expected position information.

7. The fusion optimization method for multi-unmanned aerial vehicle collaborative track planning according to claim 6, wherein the fusion optimization method is characterized in that: the step S4-1 of constructing the unmanned aerial vehicle flight environment model comprises the following steps of:

relief-based relief and uneven topography constructionThe map model is built, a safety buffer zone is arranged around the unmanned aerial vehicle, a static obstacle model and a burst obstacle model are simultaneously built, the static obstacle model is approximated by a cylinder, and the plane center is set as P _o The coordinates are [ P ] _ox ,P _oy ]P for radius and height _or And P _oz Indicating that collision zone and threat zone are set up around, L _od Expressed as minimum near-safe distance, ΔH _od Expressed as a minimum height approach distance, i.e. the distance of the drone from the static obstacle is less than L _od Or DeltaH _od When the unmanned aerial vehicle collides; l (L) _OD Expressed as the maximum threat distance of a static obstacle, ΔH _OD Expressed as a maximum threat height of static obstacle, i.e. the distance of the drone from the static obstacle is less than L _OD Or DeltaH _OD When the unmanned aerial vehicle possibly has collision risk, the sudden obstacle model is approximated by a sphere, and the sphere center is set to be P _t Specific coordinates are [ P _tx ,P _ty ,P _tz ]Radius of R _t Also set up collision zone and threat zone, collision zone uses radius R _p Is represented by a sphere of radius R _w Is represented by a sphere.

8. The fusion optimization method for multi-unmanned aerial vehicle collaborative track planning according to claim 6, wherein the fusion optimization method is characterized in that: s4-2, constructing a multi-unmanned aerial vehicle formation constraint model;

9. The fusion optimization method for multi-unmanned aerial vehicle collaborative track planning according to claim 6, wherein the fusion optimization method is characterized in that:

(1) Formation reconfiguration trigger conditions

(2) Formation planning cost

Setting formation planning cost f ₃ ⁱ The method comprises the steps of conventional planning and reconstruction planning, wherein the set cost is the conventional track flight in the interval of [0, J), reconstruction planning is required to be carried out based on the formation of multiple unmanned aerial vehicles in the interval of [ J, J), the formation planning is completed by the unmanned aerial vehicles in the interval of [ J, k), the flight is continued according to the formation of the established formation, and the specific unmanned aerial vehicle formation planning cost is calculated as shown in the following formula:

representing an environmental threat cost; />

Representing energy consumption costs; />

in (x) _l ,y _l ,z _l ) Representing the coordinates of a given target;

Wherein,,

and->

To control input cost; f (f) ₃ ⁱ Planning a cost for formation, f ₁ ⁱ Monitoring target coverage for the unmanned aerial vehicle, and specifically calculating the following formula;