Detailed Description
The present application will be described in further detail with reference to the drawings and examples, in order to make the objects, technical solutions and advantages of the present application more apparent. It should be understood that the specific embodiments described herein are for purposes of illustration only and are not intended to limit the scope of the application.
As shown in fig. 1, the method for forming a manned unmanned aerial vehicle in a formation and aggregation manner provided by the application comprises the following steps in one embodiment:
And 102, acquiring a track planning task of a manned unmanned aerial vehicle formation, wherein the track planning task comprises a long aircraft track, a long aircraft speed, an initial pose of each assistant aircraft, a speed of each assistant aircraft and expected formation parameters.
The number of iterations may be set to 1 or other value at this time.
And 104, obtaining a position set of each wing aircraft according to the long aircraft track and the expected formation parameters, and updating the particle state by taking the position set of each wing aircraft as a sub-population according to a particle swarm algorithm to obtain the current pose of each wing aircraft.
Specifically, discretizing a long aircraft track, combining with expected formation parameters of the assistant aircraft to obtain a position set of each assistant aircraft, updating a particle state by taking the position set of each assistant aircraft as a sub-population according to a particle swarm algorithm, wherein the particle state comprises a particle speed and a particle position, and obtaining the current pose of each assistant aircraft according to the particle state, wherein the current pose comprises the current position and the current course angle.
The desired formation parameters of the bureau include the priority of each bureau, so that a collection of bureau positions is available.
Each of the bureaus has a corresponding position set, and one bureau corresponds to one sub-group, and the current pose of the corresponding bureau is obtained according to the particle state in the sub-group.
And 106, calculating the flight distance of each assistant machine based on the Dubin theory according to the speed of the long machine, the initial pose of each assistant machine, the speed of each assistant machine and the current pose of each assistant machine, and calculating the target fitness function value of each assistant machine.
The method comprises the steps of calculating the flight distance of each assistant machine according to the initial pose of each assistant machine and the current pose of each assistant machine, and calculating the target fitness function value of each assistant machine based on the Dubins theory according to the flight distance of each assistant machine, the speed of each assistant machine and the speed of the long machine.
And step 108, introducing a multi-population cooperation mechanism according to the target fitness function values of each bureau, and calculating the coordination fitness function values of the formation.
Specifically, according to the target fitness function values of the various bureaus, a multi-group cooperation mechanism is introduced to meet the intra-formation anti-collision constraint and the outer-formation obstacle avoidance constraint to obtain global optimal set nodes of the various bureaus, and according to the global optimal set nodes of the various bureaus, the cooperative fitness function values of the formation are obtained.
Preferably, the constraint conditions met further include meeting boundary condition constraints, turning performance constraints, intra-formation collision avoidance constraints, and extra-formation obstacle avoidance constraints.
Step 110, when the collaborative fitness function value of the formation is less than or equal to a preset first threshold, outputting a Dubins curve as a formation assembly track according to the initial pose of each wing plane and the current pose of each wing plane.
Dubin theory is a prior art from which Dubins curves can be derived.
The method comprises the steps of judging that a cooperative fitness function value of a formation is larger than a preset first threshold value, updating iteration times, updating the current pose of each wing machine according to a particle swarm algorithm, updating a target fitness function value of each wing machine based on a Dubin theory, updating the cooperative fitness function value of the formation according to the updated target fitness function value, and re-judging until preset conditions are met, and outputting a Dubins curve as a formation assembly track according to the initial pose of each wing machine and the current pose of each wing machine.
Preferably, after updating the target fitness function value of each plane, the method further comprises the steps of obtaining initial temperature, obtaining current temperature based on a simulated annealing algorithm according to the updated target fitness function value, and outputting a Dubins curve as a collaborative aggregation track according to the initial pose of each plane and the current pose of each plane when the current temperature is judged to be smaller than or equal to a preset second threshold value.
The method comprises the steps of obtaining energy change amounts of two iterative processes according to a target fitness function value and an updated target fitness function value, judging according to a Metropolis criterion, taking updated values of various population positions as new global optimal solutions when the energy change amounts are smaller than 0, otherwise, taking the criterion probability as the global optimal solution, and then carrying out temperature-withdrawal operation to obtain the current temperature.
Preferably, the method further comprises the steps of updating iteration times when the current temperature is judged to be larger than a preset second threshold value, updating the current pose of each wing plane according to a particle swarm algorithm, updating the target fitness function value of each wing plane based on the Dubins theory, updating the current temperature based on a simulated annealing algorithm according to the updated target fitness function value, and re-judging until preset conditions are met, and outputting a Dubins curve as a formation assembly track according to the initial pose of each wing plane and the current pose of each wing plane.
Preferably, the preset condition comprises that the formed collaborative fitness function value is smaller than or equal to a preset first threshold value or the current temperature is smaller than or equal to a preset second threshold value.
Preferably, the preset condition further comprises that the iteration number is equal to a preset third threshold value.
The unmanned aerial vehicle formation method can plan formation tracks, avoid collision among formations, meet the requirements of speed, heading and formation among the formations, and achieve effective formation of the unmanned aerial vehicle formation, under the Dubin-CSAPSO algorithm, the maximum relative distance error of the unmanned aerial vehicle is only 0.4 m, and compared with the heuristic direct search algorithm, the Dubin-CSAPSO algorithm can effectively avoid collision among formations, each internal aerial vehicle and obstacles, and achieve safe formation of the unmanned aerial vehicle formation.
It should be understood that, although the steps in the flowchart of fig. 1 are shown in sequence as indicated by the arrows, the steps are not necessarily performed in sequence as indicated by the arrows, and are not strictly limited to the order shown. Moreover, at least some of the steps in fig. 1 may include multiple sub-steps or stages that are not necessarily performed at the same time, but may be performed at different times, nor do the order in which the sub-steps or stages are performed necessarily performed in sequence, but may be performed alternately or alternately with at least a portion of other steps or sub-steps of other steps.
Further preferably, when the target fitness function value of each of the bureaus is calculated and the number of times of calculation is equal to or greater than two, the following determination is made simultaneously:
1) According to the updated target fitness function value, updating the cooperative fitness function value of the formation, and judging the cooperative fitness function value of the formation;
2) Updating the current temperature according to the updated target fitness function value, and judging the current temperature;
3) Judging the iteration times;
When the collaborative fitness function value of the formation is smaller than or equal to a preset first threshold value, or the current temperature is smaller than or equal to a preset second threshold value, or the iteration number is equal to a preset third threshold value, outputting a Dubin curve as a formation track according to the initial pose of each wing plane and the current pose of each wing plane.
It should be noted that, the first threshold, the second threshold, and the third threshold are related to the coordinated fitness function value of the formation, the current temperature, and the number of iterations, respectively, and may be specifically set according to actual situations.
Further preferably, after the Dubins curve is output as the formation integrated track, the method further comprises the step of designing a track tracking control law of a double-loop structure according to the formation integrated track so that the position and the gesture of each plane track the formation integrated track motion, wherein the double loop comprises an outer loop and an inner loop, the outer loop establishes a track tracking guidance law based on a vector field method, and the inner loop takes the track tracking guidance law as input to design a gesture tracking control law.
That is, tracking control is also included after the track planning of the platoon assembly.
The application designs a manned unmanned aerial vehicle formation aggregation method aiming at the problem of unmanned aerial vehicle formation aggregation control by taking unmanned aerial vehicle formation execution aerial tasks as a background. Firstly, a multi-population cooperation mechanism is introduced into a particle swarm algorithm by considering head-end formation constraint, kinematic constraint and collision constraint conditions of an assembly, a CSAPSO assembly route planning algorithm is designed based on a Dubin curve, then a nonlinear path tracking control method with a double-loop structure is designed by taking the assembly route obtained through planning as an expected route, and finally, the validity and superiority of the designed assembly route planning method are simulated and verified by Matlab software, so that the assembly safety assembly can be realized.
In one particular embodiment, the formation build control problem may be divided into two sub-problems, formation build track planning and desired track follow control, as shown in FIG. 2. The MAV/UAV formation team formation gathering task is designed as that a long aircraft MAV (i.e. an unmanned aerial vehicle) flies from a base or in the air according to a pre-planned track, and the UAVs (i.e. unmanned aerial vehicles) of each wing aircraft are distributed at any position in space. After the pilot issues the formation command, the formation command control system performs top-layer track planning, the planned integrated track information is transmitted to the track tracking control module, and the generated track tracking control law is transmitted to the bottom-layer control system of each UAV, so that mapping from task demands to command issuing to formation flight tracks is realized.
In order to realize the aggregation process from an initial arbitrary state to a desired formation, an MAV/UAV formation aggregation control strategy framework is designed, as shown in fig. 3, the aggregation control comprises five layers, wherein the first layer is a formation cooperative task layer and issues formation aggregation task instructions, the second layer is a central computation processing module and is used for computing the relative distance between each UAV state and each environmental state in the formation, the distance between each UAV state and an obstacle, the flight time of each MAV/UAV and the like, the third layer is a track planning module and is used for generating an optimal cooperative aggregation track by using a designed track planning algorithm, the fourth layer is a track tracking module and is used for generating a gesture guidance law by using a designed track tracking control method, and the fifth layer is a UAV control system and is used for realizing corresponding flight actions by the gesture guidance law. To simplify the study problem, the following assumptions are made:
1) Neglecting the influence of aerodynamic forces on MAV and UAV, only considering particle motion equation;
2) Communication delay and packet loss in formation are not considered;
3) To ensure that formation enters the formation holding phase at the same speed, it is assumed that MAV and UAV fly at a constant speed at a predetermined altitude;
4) When the UAV arrives at the collection point, the formation adopts a long-plane control strategy to realize formation maintenance.
Considering the problem of take-off aggregation of an N-frame MAV/UAV formation, a task formation is set to include one MAV and N-1 UAVs, i= { l, 1.
1. Boundary condition constraints
For example, the formation plane aggregation process, knowing the initial pose of MAV and UAV in the formation, the initial moment state can be expressed asIn order to meet the formation and aggregation conditions, the terminal state constraint of the corresponding long machine of the plane is as follows:
wherein the subscripts f and l respectively represent the states of the plane and the long plane, and Deltax and Deltay respectively represent the distances between the long planes in the x direction and the y direction to meet the desired formation parameters.
2. Turning performance constraints
The fixed wing MAV and the UAV realize course deflection through rolling motion in actual flight. The turning radius R of the UAV should satisfy the following turning radius constraints:
Where g is the gravitational acceleration and phimax is the maximum roll angle that meets the maneuver performance of the UAV.
3. Anti-collision constraint in formation
In order to avoid collisions between the internal MAV and the UAV during formation and aggregation, the following constraints need to be satisfied:
wherein xi(t)、yi (t) respectively represents the centroid coordinates of the ith aircraft at the moment t, and L is the minimum safe distance.
4. Formation external obstacle avoidance constraint
Meanwhile, in order to avoid collision between the exterior of formation and obstacles, no-fly zones and the like, the following constraint conditions need to be satisfied:
Where xo、yo represents the obstacle centroid coordinates and Ro is the obstacle maximum threat radius.
The N MAV/UAV formation build track planning problem can be equivalent to determining a build position for each UAV, so that the UAV and the MAV meet the speed, course angle and formation constraint, and the overall evaluation index of all tracks tends to be optimal or suboptimal. In order to better solve the problem of MAV/UAV formation cooperative cluster track planning, the section designs a parallel cooperative simulated annealing particle swarm algorithm to determine an optimal formation cluster node, and generates a formation cluster track by utilizing a Dubins curve.
In a specific embodiment, the unmanned aerial vehicle formation method comprises the following steps:
Step 1, a preset track gammal of the MAV long machine is given, discretization is expressed as (xl(m),yl(m),ψl (m)), m=1, 2,..K, and a collection gammafi(m)＝(xfi(m),yfi(m),ψfi (m) of each UAV collection point of the plane is obtained through calculation of a given collection formation parameter;
And 2, initializing each sub population. Initializing the speed vij and the position xij of all particles in each sub-population, i=1, wherein N-1 represents the ith sub-population, j=1, and M represents the jth particle in the sub-population;
Step 3, updating the speed and the position of particles in each sub-population according to the following formula (5), calculating the distance between two state points of qfi (0) and qfi(xij) by utilizing Dubins curve theory, calculating a target fitness function value Ji of the ith sub-population according to the following formulas (9), (10) and (12), and storing a local optimal pij and a global optimal pig by comparing fitness function values;
Step 4, each sub-population selects the global optimum of each sub-population as a representative, forms a group with other sub-populations in a cooperative mode, and calculates the current formation cooperative fitness function value according to the following formula (13);
step 5, updating the target fitness function value Ji of each sub-population according to the following formulas (9), (10) and (12) to obtain the energy change amount of the front and back states, determining the fitness value of the particles at the current temperature according to the following formula (7) at the current temperature, and then performing temperature-withdrawal operation, wherein T=alpha T;
And 6, checking a termination condition (reaching the maximum iteration time TT, reducing the termination temperature Tf or the cooperative adaptation value function J to meet the threshold epsilon), if the condition is not met, enabling k=k+1 to return to the step 3, and if the condition is met, outputting a cooperative Dubins curve, wherein the output curve is a track curve of a formation of the unmanned aerial vehicle.
Specifically, the invention relates to a manned unmanned aerial vehicle formation method based on a Dubin-CSAPSO algorithm. Consider a five-machine formation in which one man-machine and four unmanned aerial vehicles are provided, the man-machine is used as a long machine of the formation, the man-machine is operated by a pilot in real time and flies according to a preset track, and the unmanned aerial vehicles are used as a plane to be controlled by a control system. Given the known flying tracks of the long aircraft, setting formation forming parameters, optimizing the formation positions of each assistant aircraft relative to the long aircraft by CSAPSO algorithm, generating each assistant aircraft track by adopting Dubins curve, and keeping the expected formation after formation until the last assistant aircraft joins in formation to complete the formation forming and formation task.
The Dubin-CSAPSO formation and accumulation track planning algorithm used in the invention has an algorithm flow shown in figure 4.
MAV is flown as a long-forming machine on a pre-designed course γl, which discretization can be expressed as γl(m)＝(xl(m),yl(m),ψl (m)), where m ε {0,1, 2. According to the formation requirement, each unmanned mechanism thinks that the discrete point is gammafi(m)＝(xfi(m),yfi(m),ψfi (m)), wherein the subscript fi represents the ith unmanned plane of the plane.
As shown in fig. 5, taking the example that the desired formation is a wedge-shaped formation, the MAV is taken as a formation long machine, and each UAV is taken as a assistant machine of the MAV to track a preset track of the long machine to implement formation. The long machine is known to fly along a preset track gammal at an initial state and a fixed speed vl, and the plane set can be represented as ql＝[xl,yl,ψl in a discretized mode, and then fly along a planned track gammafi at an initial state qfi(0)＝[xfi(0),yfi(0),ψfi (0) and a fixed speed vf, wherein the plane set of nodes can be represented as qfi＝[xfi,yfi,ψfi in a discretized mode. Assuming that formation with a long machine is completed at the optimal set point qgi of the ith plane, the relationship between qgi、γl and gammafi can be obtained as follows:
qgi＝γfi(g)＝γl(g)-di (8)
Wherein di＝[Δxi,Δyi, 0 is the desired formation parameter of the ith plane relative to the leader, qgi＝[xi(g),yi(g),ψi (g) ].
The feasible solution of the N-1 unmanned aerial vehicle is used as N-1 sub-population, and each sub-population is updated according to the speed and position according to a standard Particle Swarm Optimization (PSO) in parallel. Assuming that there are M particles making up the population in the D-dimensional target search space, the position and velocity of the jth particle may be represented as Xj＝(xj1,xj2,...,xjD) and Vj＝(vj1,vj2,...,vjD), respectively), the corresponding fitness value may be determined by substituting the particle position into the fitness function. The optimal position searched for by the ith particle is referred to as the individual extremum pbestj＝(pj1,pj2,...,pjD), and the optimal position searched for by the entire population is referred to as the global extremum gbestj＝(gj1,gj2,...,gjD). The position and velocity update formula for each particle is as follows:
Where vj (k) is the current velocity of the particle, xj (k) is the current position of the particle, c1 and c2 are learning factors, and r1 and r2 are uniformly distributed random numbers between [0,1 ]. pj is the optimal position of the particle so far, and pg is the optimal position of all particles so far. Lambda is a compression factor, as shown in the following formula:
Simulated annealing (simulated annealing, SA) algorithm is a random optimization algorithm based on Monte-Carlo iterative solution. The SA algorithm starts from a higher initial temperature, and as the temperature parameter continuously drops, a global optimal solution which enables an objective function to be optimal is randomly found through a certain probability in a solution space kick, and the global optimal solution can be obtained through probability jumping out in the local optimal solution. The Metropolis criterion defines the probability of the energy of a system changing from state i to state j at a certain temperature as:
Wherein Ei and Ej are the energy of the solid in the states i and j, Ej-Ei is the energy change amount, and K is the Boltzmann constant. The SA algorithm receives a 'worsened' solution with a probability, and the behavior is seemingly unreasonable, so that the flexibility of the algorithm is improved to a certain extent, the searching range of the algorithm is enlarged, and the diversity of particles is increased. The simulated annealing strategy enables the algorithm to avoid the defect of being trapped in a local optimal solution in the iterative search process, and improves the possibility and reliability of searching a global optimal solution.
In order to solve for the optimal staging locations for each UAV, it is critical to determine the fitness function of the sub-population.
To successfully achieve formation into the desired configuration, the long and the plane need to arrive at the same time, i.e., the difference in time of flight to each arrival at the collection point is 0. In practice the time-of-flight difference |te | should satisfy the following relation:
|Te|＝|Tf-Tl|<ε (11)
Wherein Tf and Tl respectively represent the gathering time of the plane and the plane, vf and vl respectively represent the flying speeds of the plane and the plane, T (the term) represents the flying time between the initial state and the gathering state, and L (the term) represents the track length between the two point states, which can be obtained by a length formula between the two point states known in Dubins curve theory. Epsilon is a threshold and the determination of qgi concentration points is decisive for |te |. The fitness function of the ith unmanned aerial vehicle is defined as:
Ji＝|Tfi-Tl| (12)
In order to obtain a safe and optimal formation assembly track, the section regards the formation as a population, and each unmanned aerial vehicle in the formation is regarded as a sub-population. In the whole large population, in order to avoid collision among sub-populations and collision with external obstacles and the like, the sub-populations are subjected to cooperative evaluation among the sub-populations after respective fitness functions are calculated, individual extremums obtained by the current sub-populations are shared to form a new group, the individual extremums and the group extremums are updated through correcting the cooperative fitness functions of the formation populations, whether the optimal values obtained by the sub-populations meet formation constraint or not is determined, and therefore the optimal (or suboptimal) values under formation cooperation are found.
In order to plan a safe and flyable formation assembly track, the problems of intra-formation collision constraint and extra-formation obstacle avoidance constraint are considered. The condition that collision does not occur in the formation is the formation inner collision avoidance constraint meeting the formula (3), and the condition that collision does not occur between the outside of the formation, an obstacle, a no-fly zone and the like is the formation outer collision avoidance constraint meeting the formula (4), assuming that the mutual position information and the obstacle information can be obtained in real time between all machines in the formation.
Adding the anti-collision constraint and the obstacle avoidance constraint into the fitness function by using a punishment function, and defining a formation cooperative fitness function as follows:
Wherein kinner and kouter respectively represent penalty factors of internal and external anti-collision of formation, and H is the number of obstacles. When collision occurs inside and outside the formation, at the moment, J1 or J2 is larger than 0, the punishment item plays a role in constraint, when the formation avoids obstacles or no collision occurs in the formation, J1 or J2 is equal to 0, and at the moment, the extremum of the optimal population of each particle swarm is the position of a collecting node of each unmanned aerial vehicle of the formation.
When MAV/UAVs in the formation or UAVs collide with obstacles in the gathering process, the cooperative fitness function J is obviously a large number, the precision of the cooperative fitness function is not met, and the population is updated again for each sub-population. In order to improve algorithm optimizing efficiency, two UAVs which collide are prevented from changing tracks simultaneously, priorities are set for the formation MAV/UAVs, namely, the aircraft with high priorities can keep the original planning tracks, the aircraft with low priorities are re-planned on the basis of collision, the original gathering points do not meet the requirement of safety gathering, and the gathering points meeting the safety need to be recalculated. In the whole MAV/UAV formation, the MAV level is highest, all UAVs are informed of pilot command, then the UAV level is determined according to factors such as UAV type, oil quantity and the like, the UAV level for executing task weight is high, the UAV level with relatively small oil quantity is high, and therefore the UAVs 1, 2, 3 and 4 are determined in sequence.
Under the condition that the initial pose and the assembly pose of the unmanned aerial vehicle and the unmanned aerial vehicle are known, the Dubins curve can be utilized to directly generate an assembly track curve, and the obtained track curve is the assembly track with the optimal assembly, so that the assembly precision of the assembly is met, and the flight safety of the assembly is ensured.
To achieve accurate tracking of the desired flight path by the UAV, the path tracking controller is designed to design a flight path tracking guidance law and a gesture tracking control law for any given geometric route, including straight lines, circles and curves, so that the UAV position and gesture track onto the desired flight path and continue to follow the desired flight path. In the process of tracking a general curve, the unmanned aerial vehicle can have the problem of singularity under specific conditions, namely tracking failure or spin, and in order to avoid the singularity phenomenon, the section takes a single UAV as a research object, takes an integrated track obtained by planning as an expected track, and a track tracking error kinematic equation is as follows:
Where ex,ey represents the longitudinal and lateral offset errors, respectively, χd represents the heading angle of the desired track, κ(s) is the curvature of the virtual target on the desired track,For the difference between the UAV course angle and the desired track course angle, v, ω represent the UAV airspeed and course rate control commands, respectively,Respectively represents the wind speed and azimuth angle of the steady wind field,Representing the virtual target speed on the desired track. As shown in fig. 6.
The track tracking control module is an important component part in the UAV control system, and a UAV nonlinear track tracking controller with a double-loop structure is designed. The outer loop takes the virtual target speed and the course speed as control variables, and designs the UAV track tracking guidance law based on a vector field method, and the inner loop takes the outer loop guidance law as control input, and designs the roll angle control law to track the expected course angle of the outer loop, thereby realizing the tracking of the expected track.
In order to guide the UAV on the desired track, an asymptotically stable vector field is constructed around the desired track as follows:
χd(ey)＝-χ∞tanh(kdey) (15)
Where kd >0 is a gain parameter for controlling the convergence rate of ey, χ∞ e (0, pi/2) is a constant angle, and for any ey:
when both the yaw and azimuth errors are equal to 0, tracking of the desired track is achieved. In the constructed vector field, the desired heading angle of the UAV is determined by χd(ey), and as can be seen from the equation (15), when the UAV is far from the desired track, the azimuth difference of the unmanned aerial vehicle tracks the desired track by χ∞, i.e. the unmanned aerial vehicle moves in the direction perpendicular to the desired path, while as the lateral offset becomes smaller, the unmanned aerial vehicle deflects gradually until it is parallel to the tangential direction of the desired track.
To design a track following control law, a lyapunov function is defined from a track following error model (14) as follows:
The derivation of formula (16) can be obtained:
setting a virtual target speed according to (17)And UAV yaw rate ω:
substitution of formulas (18) and (19) into formula (17) can be obtained
When ey is less than or equal to 0, 0 is less than or equal to χd(ey) is less than or equal to pi/2, and when eyvsinχd(ey is less than or equal to 0, and when ey is less than or equal to 0, pi/2 is less than or equal to χd(ey) is less than or equal to 0, and when eyvsinχd(ey is less than or equal to 0. So for any ey, eyvsinχd(ey) is always less than or equal to 0, namely V1 is more than or equal to 0; in summary, the track following error model (14) is globally asymptotically stable under the control laws (18) and (19), i.eAsymptotically approaches 0.
And designing an inner loop attitude tracking controller based on a nonlinear method to enable the roll angle of the UAV to track the expected heading rate obtained by an outer loop. When the UAV performs horizontal turning maneuver, the roll attitude control system is generally needed to enable the UAV to incline to generate side force by adjusting the aileron so as to realize course deflection of the UAV, which can be expressed as:
Wherein phi is the roll angle of the UAV. Considering UAV control input constraint, the unmanned plane roll angle satisfies the following condition-phimax≤φ≤φmax
To design roll angle tracking control laws, we introduce auxiliary control inputsThe track following error system is obtained by integrating equations (14) and (21):
wherein p is the roll angle speed and u is the roll angle control variable.
The following expression (23) is defined in combination with expression (21), and the derivative of expression (23) is obtained:
Wherein γ=v/g, and ωd is obtained from formula (19).
To obtain a suitable roll angle control law u, the lyapunov function is designed as follows:
wherein lambda >0 and q >1 are optional parameters.
The derivation of V2 can be obtained:
The roll angle control law is designed as follows:
Wherein kp,kc is a positive real number, and substitution of formula (27) into formula (26) yields:
when the parameters meetAndCan realize
In conclusion, the design of the control law (27) can realize that V2 is more than or equal to 0,I.e. the implementation state variable ex→0,ey -0,And phi-phid.
In order to verify the effectiveness of the formation control strategy, matlab simulation is respectively carried out on a track planning algorithm and a track tracking control method. The simulation experiment environment is an InterCore i7,2.30GHz processor, windows10 operating system, and the simulation software is MATLAB R2016b.
When the simulation experiment of the flight path planning algorithm is carried out, the basic parameters of the simulation algorithm are shown in table 1. The section envisions two formation aggregation situations, namely, offshore formation take-off aggregation and aerial formation aggregation with different initial states. Experiment 1 is that no obstacle exists in the take-off and gathering space domain, MAV/UAV ascending and aircraft carrier surface guarantee factors are considered, and finally wedge formation is formed in the gathering area. Experiment 2 shows that MAV/UAV completes formation and formation in different initial states in the airspace with no-fly zone to form wedge formation. The simulation-related parameters of experiments 1 and 2 are shown in tables 2 and 3.
Table 1 simulation base parameters
Table 2 experiment 1 simulation parameters
TABLE 3 experiment 2 simulation initial position parameters
Fig. 7 is a three-dimensional and two-dimensional track layout of experiment 1. As shown in the figure, MAV flies from aircraft carrier to the gathering height and flies according to preset flight path, UAVs 1-4 wait for aircraft carrier surface guarantee and then fly to the gathering height in sequence, and meanwhile gather with MAV in sequence and keep formation to fly continuously, and after UAVs 4 and MAV reach the gathering point, the whole formation safely completes the whole gathering process.
FIG. 8 is a graph of MAV versus UAV1-4 distance over time, where the distance between the MAV and UAV varies over time with UAV1 reaching the staging height as a time starting point. 91s, UAV1 reaches the hub point and MAV-UAV1 is maintained at the desired distance of the formation. In this process, the MAV-UAV1 distance is a minimum of 54m, exceeding the intra-platoon bump distance. Similarly, 137s, 184s, and 234.5s, UAV2, UAV3, and UAV4 reach the hub, respectively, i.e., the relative distance is the desired distance. The integrated take-off time, the ship surface guarantee time and the plane flying integration time are integrated, and the integration time of the whole formation is 303s.
Fig. 9 is a trace layout of experiment 2. In the initial stage, the MAV and the four UAVs are respectively positioned at different positions, at the moment, the MAV pilot issues a formation assembly instruction, and the four UAVs plan an assembly track according to the MAV preset flight track. The algorithm designed in the formation in the figure can avoid the no-fly zone, and all the aircrafts in the formation always keep a safe distance, namely the formation realizes safe formation, and the whole formation time is 126.5s. The algorithm designed in the method can effectively plan a gathering flight path for the whole formation, so that collision between MAV/UAVs in the formation is avoided, collision between the UAVs and obstacles is prevented, the requirements of speed, heading and formation between the MAV and UAVs in the formation are met, and safe and effective gathering is realized.
To verify the superiority of the herein designed trajectory planning algorithm, the herein algorithm was compared to a Collaborative Genetic Algorithm (CGA). Table 4 below shows the aggregate time differences between each UAV and MAV under the action of the algorithm and the collaborative genetic algorithm, and the design algorithm meets the time threshold requirement, and the maximum error is only 0.4m and can be ignored because the UAV has a very fast flying speed. However, under the CGA algorithm, although the time is very small, the maximum distance between the MAV and the UAV reaches 48m, the error is large, the formation cannot be formed, and the formation aggregation cannot be realized. As shown in fig. 10.
TABLE 4 aggregation time differences between UAVs and MAVs
As can be seen from the graph, the heuristic direct search algorithm is used to determine the aggregation point, wherein the planned route generated by the UAV1-3 is the same as the heuristic algorithm, but the UAV4 passes through the no-fly zone, and cannot fly safely, i.e. the planning is failed. The time used by the method is obviously faster than that of a CGA algorithm and a heuristic direct search algorithm, and the planning efficiency is greatly improved.
When the simulation experiment of the track tracking control is carried out, the validity of the designed track tracking control algorithm is verified. The straight line, the circle and Clothoid curves are integrated as a desired track, the simulation time is 110s, the desired flying speed v of the UAV is=20m/s, the initial position of the UAV is p1 = [50,100], and the initial course angle isThe initial roll angle is phi1 =0°, the wind speed of a constant wind field is vw =4m/s, and the azimuth angle of the wind field is determinedThe control parameters are selected to be kd＝0.1,kω＝0.1,kx＝0.5,χ∞＝π/2,kp＝0.5,kc =0.1, the control input constraints ω_e [ -0.49rad/s,0.49rad/s ] and Φ_e [ -pi/4 rad, pi/4 rad ] of the uav. Simulation results are shown.
From fig. 12 and 13, it can be seen that the path tracking control law of the design can accurately track the expected track curve under the conditions of a constant wind field and a UAV kinematics, the tracking model error is kept within 0.2m, the expected track speed is converged to the UAV speed set value of 20m/s under the action of the controller from fig. 14, the course rate changes obviously in the initial stage of tracking the expected curve in fig. 15, the expected track is tracked at the maximum and minimum course rates according to the initial position and the course angle of the UAV, and the expected value is converged after 12.5 s. FIG. 16 is an inner loop roll angle tracking result where the UAV determines the heading rate from the roll angle, otherwise tracking the heading rate yields a roll angle variation curve. In the initial stage, the roll angle of the UAV is rapidly changed from 0 degree to phimin, and then the attitude tracking of the roll angle is realized under the action of a control law.
And (3) taking the formation aggregation track planned in the experiment 2 as a desired track, and carrying out simulation verification on the practicability of the UAV nonlinear track tracking controller.
In the simulation, the flying speed of the MAV and the UAV is 200m/s, and the wind speed of a stable wind field is the azimuth angle of the wind field. In comparison to the initial pose of each UAV in the track planning section, this section assumes that there is an initial error in the initial state of each UAV, and specific MAV and UAV initial state parameters are shown in table 5.
TABLE 5 initial pose parameters
MAV/UAV platoon crew track following effect graphs are shown in FIGS. 17-18. The black solid line in the figure represents the planned formation combined track, the red solid line is the actual tracking track, and each UAV can accurately track the desired track under the action of the track tracking controller. However, as the flying speed of the unmanned aerial vehicle is too high, two curves are overlapped together, so that the expected track and the tracking track are better, clearly and intuitively compared, a local enlarged view of each unmanned aerial vehicle in the initial stage is shown in fig. 18, and as can be seen from fig. 17, under the condition that initial errors exist, each unmanned aerial vehicle asymptotically tends to the expected track, accurately and stably tracks the expected track, the process from the pilot to the gathering task to the gathering track planning throughout the ground is realized, and then to the gathering track tracking control is realized, and the practicability of the nonlinear track tracking controller is verified.
In one embodiment, as shown in fig. 19, there is provided a manned unmanned aerial vehicle formation device, including an acquisition module 1902, an update module 1904, a target fitness function value calculation module 1906, a collaborative fitness function value calculation module 1908, and a collaborative fitness function value calculation module 1910, wherein:
The system comprises an acquisition module 1902, a control module and a control module, wherein the acquisition module 1902 is used for acquiring a track planning task of a manned unmanned aerial vehicle formation group, wherein the track planning task comprises a long aircraft track, a long aircraft speed, an initial pose of each assistant aircraft, a speed of each assistant aircraft and a plane expected formation parameter;
The updating module 1904 is used for obtaining a position set of each wing aircraft according to the long aircraft track and the expected formation parameters of the wing aircraft, updating the particle state by taking the position set of each wing aircraft as a sub-population according to a particle swarm algorithm, and obtaining the current pose of each wing aircraft;
The target fitness function value calculation module 1906 is configured to calculate a flight distance of each plane based on the Dubins curve theory according to the speed of the long plane, the initial pose of each plane, the speed of each plane, and the current pose of each plane, and calculate a target fitness function value of each plane;
a collaborative fitness function value calculation module 1908, configured to introduce a multi-population collaboration mechanism according to the target fitness function values of each of the multiple wing planes, and calculate a collaborative fitness function value of the formation;
and the output module 1910 is configured to output a Dubins curve as a formation assembly track according to the initial pose of each machine and the current pose of each wing machine when the collaborative fitness function value of the formation is determined to be less than or equal to a preset first threshold.
For specific limitations on the unmanned aerial vehicle formation device, reference may be made to the above limitation on the unmanned aerial vehicle formation method, and no further description is given here. Each of the modules in the above-described apparatus may be implemented in whole or in part by software, hardware, and combinations thereof. The above modules may be embedded in hardware or may be independent of a processor in the computer device, or may be stored in software in a memory in the computer device, so that the processor may call and execute operations corresponding to the above modules.
In one embodiment, a computer device is provided, which may be a terminal, and an internal structure diagram thereof may be as shown in fig. 20. The computer device includes a processor, a memory, a network interface, a display screen, and an input device connected by a system bus. Wherein the processor of the computer device is configured to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system and a computer program. The internal memory provides an environment for the operation of the operating system and computer programs in the non-volatile storage media. The network interface of the computer device is used for communicating with an external terminal through a network connection. The computer program when executed by a processor implements a unmanned aerial vehicle formation method. The display screen of the computer equipment can be a liquid crystal display screen or an electronic ink display screen, and the input device of the computer equipment can be a touch layer covered on the display screen, can also be keys, a track ball or a touch pad arranged on the shell of the computer equipment, and can also be an external keyboard, a touch pad or a mouse and the like. The computer device may be a simulation device, the input means inputs relevant information to the simulation device, the processor executes the programs in the memory for combined simulation, and the display screen displays the relevant simulation results.
It will be appreciated by those skilled in the art that the structure shown in FIG. 20 is merely a block diagram of some of the structures associated with the present inventive arrangements and is not limiting of the computer device to which the present inventive arrangements may be applied, and that a particular computer device may include more or fewer components than shown, or may combine some of the components, or have a different arrangement of components.
In an embodiment a computer device is provided comprising a memory storing a computer program and a processor implementing the steps of the method of the above embodiments when the computer program is executed.
In one embodiment, a computer readable storage medium is provided, on which a computer program is stored which, when executed by a processor, implements the steps of the method of the above embodiments.
Those skilled in the art will appreciate that implementing all or part of the above described methods may be accomplished by way of a computer program stored on a non-transitory computer readable storage medium, which when executed, may comprise the steps of the embodiments of the methods described above. Any reference to memory, storage, database, or other medium used in embodiments provided herein may include non-volatile and/or volatile memory. The nonvolatile memory can include Read Only Memory (ROM), programmable ROM (PROM), electrically Programmable ROM (EPROM), electrically Erasable Programmable ROM (EEPROM), or flash memory. Volatile memory can include Random Access Memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in a variety of forms such as Static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double Data Rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link (SYNCHLINK) DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM), among others.
The technical features of the above embodiments may be arbitrarily combined, and all possible combinations of the technical features in the above embodiments are not described for brevity of description, however, as long as there is no contradiction between the combinations of the technical features, they should be considered as the scope of the description.
The above examples illustrate only a few embodiments of the application, which are described in detail and are not to be construed as limiting the scope of the application. It should be noted that it will be apparent to those skilled in the art that several variations and modifications can be made without departing from the spirit of the application, which are all within the scope of the application. Accordingly, the scope of protection of the present application is to be determined by the appended claims.