CN106406341A - Flight control method for quadrotor unmanned aerial vehicle - Google Patents

Abstract

The present invention provides a flight control method for a quadrotor unmanned aerial vehicle. The method includes the following steps that: S10, the kinetic model of the quadrotor aerial vehicle is built, and a kinetic equation of the QUAV (quadrotor aerial vehicle) is obtained; and S20, a compound control-based control method is designed to control four independent control channels converted by the kinetic equation in the step S10, the control modes of the control channels are respectively height PID, roll ADRC, pitch ADRC and yaw EACS-PID, and the rotation speed of four rotors is controlled through the conversion of control quantity, so that attitude control can be achieved. With the method provided by the invention adopted, the quadrotor unmanned aerial vehicle can adapt to external environment change; the yaw EACS-PID can realize the adaptive adjustment of control parameters; the roll ADRC and pitch ADRC can carry out disturbance rejection in a more active way; and the height PID can keep excellent anti-interference capability and robustness, avoid too high complexity of a program, reduce the processor load of the quadrotor unmanned aerial vehicle and improve the operating efficiency of hardware.

Description

The flight control method of four rotor unmanned aircrafts

Technical field

The present invention relates to unmanned vehicle technical field is and in particular to flight controlling party to four rotor unmanned aircrafts Method.

Background technology

Four rotor unmanned aircrafts (Quadrotor Unmanned Aerial Vehicle, QUAV) are a kind of by no In addition the not manned vehicle of autonomous flight realized by self-sensor device to line Remote equipment, and it has 6 degree of freedom, 4 controls Input, drives the differential moment producing to realize its elevating movement and tumbling motion, the anti-twisted power of generation by 4 brshless DC motors Square realizes yawing rotation, is Nonlinear Underactuated System.This kind of aircraft is widely used in military and civilian field.Four rotors are no Fixed-wing unmanned vehicle compared by people's aircraft, and due to energy VTOL, the requirement of take-off and landing is relatively low, and motility is high, There is higher adaptability under complicated physical features.Current is PID control (Proportional- using most control methods Integral-derivative Control), it passes through to identify target, then detects the gap of present situation and target, then with taking action Eliminate it.PID control structure is simple, and control technology is ripe, and robustness is preferable.But, flew in four rotor unmanned aircrafts Cheng Dangzhong, the parameter in the middle of controller is difficult to automatically adjust to adapt to extraneous change, thus is extremely difficult to predetermined target, shadow Ring control effect.

Content of the invention

Present invention seek to address that technical problem present in prior art.

For solve the problems, such as PID control can not according to aircraft exterior environmental change Adaptive Modulation so that The control parameter of PID is carried out self-adaptative adjustment to realize aircraft adjust automatically, the present invention is based on optimum-worst Formica fusca system System (Best-worst Ant System, BWAS) algorithm is optimizing pid control parameter.

The present invention proposes a kind of flight control method of quadrotor, is based on optimum-worst ant system (Best-worst Ant System, BWAS) algorithm, to optimize the flight control method of pid control parameter, controls four rotors no Man-machine flight attitude process, adapts to external environment change, realizes control parameter self-adaptative adjustment, keep anti-interference well Ability and robustness, improve the flight quality of four rotor unmanned aircrafts.

The flight control method of described four rotor unmanned aircrafts, mainly by the master controller mould of four rotor unmanned aircrafts Block, to execute, specifically includes following steps：

S10：Set up the kinetic model of quadrotor, the kinetics equation of QUAV is；

Wherein,θ, ψ are respectively roll angle, the angle of pitch and the yaw angle of four rotor unmanned aircrafts, and l is its barycenter to rotation The distance at wing center, I_x、I_y、I_zFor inertia master away from Ω_iFor i-th rotor rotating speed, Fi is the lift of i-th rotor generation, rotation The lift that the wing produces is directly proportional to rotary-wing transmission velocity squared, I_RFor rotary inertia, n₁For its lift coefficient, n₂For reaction torque system Number；

In order to the kinetics equation of quadrotor is converted into four independent control passages, define four rotor flyings The control input of device is

S20：Design optimizes pid control parameter based on optimum-worst ant system (BWAS) algorithm, then by step S10 In the kinetics equation of four rotor unmanned aircrafts be converted into four independent control passages, four described control passages are respectively For height BWAS-PID, rolling BWAS-PID, pitching BWAS-PID, driftage BWAS-PID；

Wherein pid control parameter is optimized using optimum-worst ant system (BWAS) algorithm and to include procedure below

S21:PID control

Using Increment Type Digital Hydraulic PID control, its expression formula such as formula (3)：

K in formula_pFor proportionality coefficient, e (k) is this deviation, and the corresponding controlled quentity controlled variable of Δ u (k) is u (k), and T is the sampling period, T_iFor integration time constant, T_dFor derivative time constant, PID control it needs to be determined that parameter be respectively K_p、T_iAnd T_d；

S22:Pid control parameter optimizes

Represent the performance indications evaluating control system using formula (4)：

In formula, LP counts for simulation calculation, and DT is simulation calculation step；Then formula (4) conitnuous forms are expressed as formula (5)：

If Formica fusca sum is m, for each Formica fusca, the point in this moment is i, and its respective function value is η_i, the next one can The point j reaching, respective function value is η_j, then

Formica fusca can be moved towards the most direction of pheromone, when there is no pheromone, will be according to the original direction of motion Movement, therefore Formica fusca are when t is in i point towards the probability such as formula (7) of j point movement：

In formula, allowed directly reaches the set of next path point for Formica fusca from place i, and τ is pheromone, τ_ijIt is road The information cellulose content of footpath i to path j, α be pheromone relative importance, if α=0, near i place j will have selected Go out；β is the relative importance of range information, β=0, and Formica fusca is only affected by pheromone and have ignored heuristic information band The skewed popularity coming；If Formica fusca moving direction has barrier, randomly choose other directions, guide if there are pheromone When, then guide action according to it, during optimizing, the movement probability of Formica fusca determines according to formula (7), Δ η_ijT () ＜ 0 represents that Formica fusca exists The neighborhood search of itself site i, perceives and takes action；Δη_ijT () ＞ 0 expression Formica fusca is according to movement probability from its own institute Place i neighborhood movement to j neighborhood；

After n unit of time, local information element more New Policy presses formula (8), (9) develop：

τ_ij(t+n)=(1- ρ) τ_ij(t)+ρΔτ_ij(8)

Wherein ρ ∈ (0,1) is pheromone volatilization parameter, Δ τ_ijIncrease for the pheromone on path (i, j) in this circulation Amount,The pheromone staying on path (i, j) in this circulation for Formica fusca k；

Optimum-worst ant group algorithm is strengthened to a greater extent to optimal solution, is weakened for worst solution, makes The pheromone amount difference that must belong between optimal path and worst path increases further, so so that the search of Formica fusca more For concentrating in the range of the optimal path found out till previous cycle；

After an iteration terminates, Pheromone update is carried out for worst Formica fusca paths traversed, as overall situation letter Breath element updates as formula (10)：

The parameter that wherein ε is introduced into, L_worstAnd L_bestIt is respectively the path of worst Formica fusca and optimum Formica fusca in previous cycle Length, τ (i, j) is the pheromone track amount on path (i, j)；

The structure in path：

Formica fusca k positioned at node i can produce a random number q before the next path of each selection, then from node i arrives The movement rule p ' of node j presses formula (11)：

Q is stochastic variable, is evenly distributed in interval [0,1], q₀∈[0,1]；

Pid control parameter optimal solution can be found by above BWAS algorithm.

The quadrotor control method of the present invention, according to formula (8), (9), (10) Formica fusca come continuous updating pheromone, Then Formica fusca usually selects different paths according to different information, when path reaches farthest, and most that of information cellulose content Paths are exactly the optimum solution path of pid control parameter；Wherein optimum-worst ant group algorithm has been carried out to a greater extent to optimal solution Enhancing, for worst solution carried out weaken so that belonging to the pheromone amount difference between optimal path and worst path to enter one Step increases, so so that the search of Formica fusca more concentrates in the range of the optimal path found out till previous cycle, Thus optimal path can be effectively utilized, also just can be with more rapid accurate acquisition pid control parameter optimal solution, then by step The kinetics equation of four rotor unmanned aircrafts in S10 is converted into four independent control passages, and then controls four rotors no The rotating speed of four unidirectional current motors of people's aircraft, solves the defect that its control parameter in PID control is unable to self-adaptative adjustment, Allow unmanned plane to better adapt to the change of external environment in flight, improve the anti-interference of unmanned aerial vehicle control system Property and robustness, have reached preferable flight control effect.

Brief description

The above-mentioned and/or additional aspect of the present invention and advantage will become from reference to the description to embodiment for the accompanying drawings below Substantially and easy to understand, wherein：

Fig. 1 is the four rotor unmanned aircraft overall structure diagrams of the present invention；

Fig. 2 is that the four rotor unmanned aircraft main modular of the present invention constitute schematic diagram；

Fig. 3 is four rotor unmanned aircraft body axis systems and the inertial coodinate system figure of the present invention；

Fig. 4 is the four rotor unmanned aircraft control system architecture schematic diagrams of the present invention；

Fig. 5 is based on BWAS optimization PID control system structure in the four rotor unmanned aircraft control methods of the present invention Schematic diagram；

Fig. 6 is based under BWAS-PID control and based on PID in the four rotor unmanned aircraft control methods of the present invention Driftage angle tracking comparison diagram under control, AS-PID control；

Fig. 7 is based under BWAS-PID control and based on PID in the four rotor unmanned aircraft control methods of the present invention Roll angle Immunity Performance comparison diagram under control, AS-PID control；

Fig. 8 is based on robustness test result figure under PID control；

Fig. 9 is to control lower robustness test result figure based on AS-PID；

Figure 10 is the method for the present invention based on the lower robustness test result figure of BWAS-PID control.

Specific embodiment

In order to be more clearly understood that the above objects, features and advantages of the present invention, below in conjunction with the accompanying drawings and specifically real Mode of applying is further described in detail to the present invention.It should be noted that in the case of not conflicting, the enforcement of the application Feature in example and embodiment can be mutually combined.

Elaborate a lot of details in the following description in order to fully understand the present invention, but, the present invention also may be used To be implemented different from mode described here using other, therefore, protection scope of the present invention is not subject to following public tool The restriction of body embodiment.

Referring to Fig. 1-2, four rotor unmanned aircrafts of the embodiment of the present invention are further described.

As depicted in figs. 1 and 2, four rotor unmanned aircrafts 100 include body 10 and the main control being fixed on body 10 Device module 20, also includes four brushless electric machine control modules 60 being fixed on 10 4 cantilevers of body and is driven by brushless electric machine Rotor 70, in addition, as shown in Fig. 2 four rotor unmanned aircrafts also include being arranged on described body 10 and respectively with described Sensor assembly 40, navigation module 50 and water-cooled-air cooling module 80 that main controller module 20 connects, also include and master controller The wireless communication module 30 of 20 communication connections；Navigation module 50 adopts high-precision GPS satellite navigation system unmanned to four rotors Aircraft 100 is tracked positioning, and provides positional information to described main controller module 20 and navigate, and in navigation procedure Modification and solidification baud rate, it can in addition contain preserve the setting up procedure of baud rate；Described sensor assembly 40 include respectively with institute State Inertial Measurement Unit, baroceptor, electronic compass, Smoke Sensor and the air velocity transducer of main control module 20 connection, Described Inertial Measurement Unit is used for three axis accelerometers of sense aircraft, rolling angular speed, pitch rate, yawrate And course information, described baroceptor is for the height of sense aircraft, the heading device of described electronic compass measurement aircraft Breath, described air velocity transducer is monitored to the wind speed of aircraft location, and described Smoke Sensor is arranged on aircraft On PCB on for detect described circuit board break down generation smog and by smog feedback of the information to described master control Device module 20 processed；Described wireless communication module 30 includes remote control, PPM decoder and PPM receiver, described PPM encoder with Described remote control connects, and four channel control signals of remote control will be wirelessly transmitted to described PPM after PPM encoder encodes Receiver, described PPM receiver is connected with described main controller module；Described motor control module 60 is included for driving flight Four motors of four rotors 70 and the electron speed regulator controlling described four motors work respectively, described electronic speed regulation on device Device is connected to receive motor control signal with described main controller module, described Smoke Sensor be arranged in close on carry-on Circuit board adnexa with detect described circuit board break down generation smog and by smog feedback of the information to described master controller mould Block；Described water-cooled-air cooling module 80 can effectively reduce the heat producing when main controller module 20 and motor control module 60 work Amount.

Navigation module 50 can provide four rotor wing unmanned aerial vehicles current positional information, main controller module 20 be four rotors no The core of man-machine 100 control systems, its effect is responsible for gathering three axis accelerometers, the roll angle speed that sensor detects The attitude angular rate of the compositions such as rate, pitch rate and yawrate and course information real-time resolving, further according to detecting The flight information being sent by remote control, in conjunction with the control program controlling lower control method based on BWAS-PID, calculates reality Output motor control signal controls turning of 4 motors to electron speed regulator, then electron speed regulator according to the control signal obtaining Speed, thus realizing the control of the lift and torque that 4 rotors are produced, brushless electric machine its rotating speed can be controlled by PWM thus Reach the size to power and moment produced by each rotor to be controlled.

Specifically, described main controller module 20 and described motor control module 60 are respectively arranged with temperature element (figure In for illustrating), described temperature element is connected to realize temperature acquisition with described main controller module 20, described main controller module 20 adjust the cooling power of described water-cooled-air cooling module 80 according to collected temperature information.Both when temperature element detects Need during the temperature drift of main controller module 20 and described motor control module 60 to lift the cooling work(of water-cooled-air cooling module 80 Rate is to accelerate to cool, if reducing cooling work(when the temperature of main controller module 20 and described motor control module 60 is low Rate, so can ensure that main controller module 20 and described motor control module 60 are operated within the scope of suitable temperature.

Specifically, water-cooled-air cooling module is lowered the temperature first with water-cooling system, reaches and effective object temperature after water temperature raises When spending close, discharge all of water, now will be lowered the temperature using air cooling system.So it is effectively reduced main controller module 20 The temperature rise caused by heat producing when working with motor control module 60.

Four rotor unmanned aircrafts of the present embodiment, control signal is wirelessly sent to PPM by PWM mode and connects by remote control Receipts machine, PPM encoder transports to main controller module 20 by after the control signal decoding received by PPM receipts machine, meanwhile, constitutes four The height of rotor unmanned aircraft real-time attitude information, rolling, pitching, driftage by etc. recorded and transmitted to master by sensor assembly Controller module 20, in conjunction with the control program of the control method under being controlled based on BWAS-PID, main controller module COMPREHENSIVE CALCULATING After real-time attitude information and control signal information, output motor control signal is to electron speed regulator, and then electron speed regulator is according to obtaining The motor control signal obtaining controls the rotating speed of 4 motors, thus realizing the control of the lift and torque that 4 rotors are produced, no Brush motor can control its rotating speed thus the size reaching to power and moment produced by each rotor is controlled by PWM, Thus realizing automatically adapting to external environment change, reach preferable control effect.

After the main control module COMPREHENSIVE CALCULATING real-time attitude information of described four rotor unmanned aircrafts and control signal information Output motor control signal is comprised the following steps with controlling the method for unmanned vehicle：

S10：Body axis system according to quadrotor as shown in Figure 3 and inertial coodinate system, set up four rotor flyings The kinetic model of device, the kinetics equation of QUAV is；

Wherein, ifθ, ψ are respectively roll angle, the angle of pitch and the yaw angle of four rotor unmanned aircrafts, and l is for its barycenter extremely The distance of rotor centers, Ix, Iy, Iz are inertia master is i-th rotor rotating speed away from, Ω i, and Fi is the lift that i-th rotor produces, The lift that rotor produces is directly proportional to rotary-wing transmission velocity squared, and IR is rotary inertia, n₁For its lift coefficient, n₂For reaction torque Coefficient；

In order to the kinetics equation of quadrotor is converted into four independent control passages, define four rotor flyings The control input of device is

S20：As shown in figure 4, design optimizes pid control parameter based on optimum-worst ant system algorithm, then by step The kinetics equation of four rotor unmanned aircrafts in S10 is converted into four independent control passages, four described control passages It is respectively height BWAS-PID, rolling BWAS-PID, pitching BWAS-PID, driftage BWAS-PID；

The BWAS Optimizing Mode of each control passage is as shown in figure 5, wherein r (K) refers to the Pre-tracking by remote control The input of control targe signal, that is, want the flight parameter of the QUAV of realization, for example preferable height value, course angle, the angle of pitch Or roll angle etc., y (k) refers to the output of actual signal, and e (k) refers to the deviation of output signal and input signal, and u (k) refers to BWAS optimizes the controlled quentity controlled variable of PID control, wherein adopts optimum-worst ant system (BWAS) algorithm to optimize pid control parameter Including procedure below

S21:PID control

Using Increment Type Digital Hydraulic PID control, its expression formula such as formula (3)：

K in formula_pFor proportionality coefficient, e (k) is this deviation, and the corresponding controlled quentity controlled variable of Δ u (k) is u (k), and T is the sampling period, T_iFor integration time constant, T_dFor derivative time constant, PID control it needs to be determined that parameter be respectively K_p、T_iAnd T_d；

S22:Pid control parameter optimizes

Represent the performance indications evaluating control system using formula (4)：

In formula, LP counts for simulation calculation, and DT is simulation calculation step；Then formula (4) conitnuous forms are expressed as formula (5)：

If Formica fusca sum is m, for each Formica fusca, the point in this moment is i, and its respective function value is η_i, the next one can The point j reaching, respective function value is η_j, then

Formica fusca can be moved towards the most direction of pheromone, when there is no pheromone, will be according to the original direction of motion Movement, therefore Formica fusca are when t is in i point towards the probability such as formula (7) of j point movement：

In formula, allowed directly reaches the set of next path point for Formica fusca from place i, and τ is pheromone, τ_ijIt is road The information cellulose content of footpath i to path j, α be pheromone relative importance, if α=0, near i place j will have selected Go out；β is the relative importance of range information, β=0, and Formica fusca is only affected by pheromone and have ignored heuristic information band The skewed popularity coming；If Formica fusca moving direction has barrier, randomly choose other directions, guide if there are pheromone When, then guide action according to it, during optimizing, the movement probability of Formica fusca determines according to formula (7), Δ η_ijT () ＜ 0 represents that Formica fusca exists The neighborhood search of itself site i, perceives and takes action；Δη_ijT () ＞ 0 expression Formica fusca is according to movement probability from its own institute Place i neighborhood movement to j neighborhood；

After n unit of time, local information element more New Policy presses formula (8), (9) develop：

τ_ij(t+n)=(1- ρ) τ_ij(t)+ρΔτ_ij(8)

Wherein ρ ∈ (0,1) is pheromone volatilization parameter, Δ τ_ijIncrease for the pheromone on path (i, j) in this circulation Amount,The pheromone staying on path (i, j) in this circulation for Formica fusca k；

Optimum-worst ant group algorithm is strengthened to a greater extent to optimal solution, is weakened for worst solution, makes The pheromone amount difference that must belong between optimal path and worst path increases further, so so that the search of Formica fusca more For concentrating in the range of the optimal path found out till previous cycle；

After an iteration terminates, Pheromone update is carried out for worst Formica fusca paths traversed, as overall situation letter Breath element updates as formula (10)：

The parameter that wherein ε is introduced into, L_worstAnd L_bestIt is respectively the path of worst Formica fusca and optimum Formica fusca in previous cycle Length, τ (i, j) is the pheromone track amount on path (i, j)；

The structure in path：

Formica fusca k positioned at node i can produce a random number q before the next path of each selection, then from node i arrives The movement rule p ' of node j presses formula (11)：

Q is stochastic variable, is evenly distributed in interval [0,1], q₀∈[0,1]；

Pid control parameter optimal solution can be found by above BWAS algorithm.

The quadrotor control method of the present invention, according to formula (8), (9), (10) Formica fusca come continuous updating pheromone, Then Formica fusca usually selects different paths according to different information, when path reaches farthest, and most that of information cellulose content Paths are exactly the optimum solution path of pid control parameter；Wherein optimum-worst ant group algorithm has been carried out to a greater extent to optimal solution Enhancing, for worst solution carried out weaken so that belonging to the pheromone amount difference between optimal path and worst path to enter one Step increases, so so that the search of Formica fusca more concentrates in the range of the optimal path found out till previous cycle, Thus can more effectively utilize optimal path, also just can be with more rapid accurate acquisition pid control parameter optimal solution, then by step The kinetics equation of four rotor unmanned aircrafts in S10 is converted into four independent control passages, and then controls four rotors no The rotating speed of four unidirectional current motors of people's aircraft, solves the defect that its control parameter in PID control is unable to adjust automatically, makes Obtain the change that unmanned plane can better adapt to external environment in flight, improve the vulnerability to jamming of unmanned aerial vehicle control system And robustness, reach preferable flight control effect.

In order to verify the control effect of four rotor unmanned aircraft flight control methods proposed by the present invention, using build Four rotor unmanned aircraft model machines are tested, and have carried out multiple scheme experiments respectively, specific as follows：

As shown in fig. 6, in the yaw angle tracking test carrying out, employing three kinds of control methods and contrasted, PID control Under the dynamic performance index of yaw angle aircraft pursuit course be：Overshoot is 6.7%, and the rise time is 1.13s, and time to peak is 1.71s, regulating time is 4.26s；The dynamic performance index of yaw angle aircraft pursuit course under AS-PID controls is：Overshoot is 0.1%, the rise time is 2.58s, and time to peak is 2.73s, and regulating time is 3.17s；Yaw angle under BWAS-PID control The dynamic performance index of aircraft pursuit course is：Overshoot is 0.1%, and the rise time is 0.95s, and time to peak is 1.12s, during regulation Between be 1.42s.From data above, under BWAS-PID control, for PID control and AS-PID control, overshoot Amount is low, and regulating time is short, few to the time described in peaking, adjusts quick and precisely, the General Promotion dynamic property of control system.

As shown in fig. 7, taking roll angle as a example, to test the vulnerability to jamming under three kinds of control methods control, it can be seen that Interference rejection ability outline under BWAS-PID controls is better than AS-PID control, is better than PID control.Fig. 8, Fig. 9 and Figure 10 are for three kinds not With the robustness test of control method, in PID control, the excursion of overshoot is ± 0.4%, adjusts taking the angle of pitch as a example The excursion of section time is ± 0.02s；During AS-PID controls, the excursion of overshoot is ± 0.2%, the change of regulating time Change scope is ± 0.01s；During BWAS-PID controls, the excursion of overshoot is ± 0.1%, and the excursion of regulating time is ±0.01s；According to the above system analyzed and understand under BWAS-PID control, preferably, the system under AS-PID controls is secondary for robustness It.

To sum up, the present invention proposes a kind of flight control method of quadrotor, is based on optimum-worst ant system (Best-worst Ant System, BWAS) algorithm, to optimize the flight control method of pid control parameter, controls four rotors no Man-machine flight attitude process, adapts to external environment change, realizes control parameter self-adaptative adjustment, keep anti-interference well Ability and robustness, improve the flight quality of four rotor unmanned aircrafts.

These are only the preferred embodiments of the present invention, be not limited to the present invention, for those skilled in the art For member, the present invention can have various modifications and variations.Within all creative spirit in the present invention and principle, that is made is any Modification, equivalent, improvement etc., should be included within the scope of the present invention.

Claims (1)

1. a kind of flight control method of four rotor unmanned aircrafts, comprises the following steps：

S10：Set up the kinetic model of quadrotor, the kinetics equation of QUAV is；

Wherein,θ, ψ are respectively roll angle, the angle of pitch and the yaw angle of four rotor unmanned aircrafts, and l is its barycenter to rotor The distance of the heart, I_x、I_y、I_zFor inertia master away from Ω_iFor i-th rotor rotating speed, Fi is the lift of i-th rotor generation, and rotor produces Raw lift is directly proportional to rotary-wing transmission velocity squared, I_RFor rotary inertia, n₁For its lift coefficient, n₂For anti-twisted moment coefficient；

In order to the kinetics equation of quadrotor is converted into four independent control passages, define quadrotor Control input is

$\left[\begin{array}{c}{U}_1\\ U_2\\ U_3\\ U_4\end{array}\right]=\left[\begin{array}{c}{F}_1+F_2+F_3+F_4\\ F_4-F_2\\ F_3-F_1\\ F_2+F_4-F_1-F_3\end{array}\right]=\left[\begin{array}{c}{n}_1\underset{i=1}{\overset{4}{\Σ}}{{\mathrm{\Ω}}_i}^2\\ n_1({{\mathrm{\Ω}}_4}^2-{{\mathrm{\Ω}}_2}^2)\\ n_1({{\mathrm{\Ω}}_3}^2-{{\mathrm{\Ω}}_1}^2)\\ n_2({{\mathrm{\Ω}}_1}^2-{{\mathrm{\Ω}}_2}^2+{{\mathrm{\Ω}}_3}^2-{{\mathrm{\Ω}}_4}^2)\end{array}\right]---\left(2\right)$

S20：Design optimizes pid control parameter based on optimum-worst ant system (BWAS) algorithm, then by step S10 The kinetics equation of four rotor wing unmanned aerial vehicles is converted into four independent control passages, and four described control passages are respectively height BWAS-PID, rolling BWAS-PID, pitching BWAS-PID, driftage BWAS-PID；

Wherein pid control parameter is optimized using optimum-worst ant system (BWAS) algorithm and to include procedure below

S21:PID control

Using Increment Type Digital Hydraulic PID control, its expression formula such as formula (3)：

$\begin{array}{c}\mathrm{\Δ}u\left(k\right)=K_p\{\[e\left(k\right)-e\left(k-1\right)\]+\frac{T}{T_i}e\left(k\right)+\frac{T_d}{T}\[e\left(k\right)\\ -2e\left(k-1\right)+e\left(k-2\right)\]\}\end{array}---\left(3\right)$

K in formula_pFor proportionality coefficient, e (k) is this deviation, and the corresponding controlled quentity controlled variable of Δ u (k) is u (k), and T is the sampling period, T_iFor Integration time constant, T_dFor derivative time constant, PID control it needs to be determined that parameter be respectively K_p、T_iAnd T_d；

S22:Pid control parameter optimizes

Represent the performance indications evaluating control system using formula (4)：

$\mathrm{\η}={\mathrm{DT}}^2\underset{i=1}{\overset{LP}{\Σ}}i\left|e\left(i\right)\right|---\left(4\right)$

In formula, LP counts for simulation calculation, and DT is simulation calculation step,

Then formula (4) conitnuous forms are expressed as formula (5)：

$\mathrm{\η}={\∫}_0^{\mathrm{\∞}}t\left|e\left(t\right)\right|dt---\left(5\right)$

If Formica fusca sum is m, for each Formica fusca, the point in this moment is i, and its respective function value is η_i, next up to point J, respective function value is η_j, then

${\mathrm{\Δ\η}}_{ij}={\mathrm{\η}}_i-{\mathrm{\η}}_j,\∀i,j---\left(6\right)$

Formica fusca can be moved towards the most direction of pheromone, when not having pheromone, will move according to the original direction of motion, Therefore Formica fusca when t is in i point towards the probability such as formula (7) of j point movement：

$p_{ij}\left(t\right)=\frac{{{\mathrm{\τ}}_{ij}}^{\mathrm{\α}}\left(t\right){{\mathrm{\Δ\η}}_{ij}}^{\mathrm{\β}}\left(t\right)}{\underset{l\∈allowed}{\mathrm{\Σ}}{{\mathrm{\τ}}_{il}}^{\mathrm{\α}}\left(t\right){{\mathrm{\Δ\η}}_{il}}^{\mathrm{\β}}\left(t\right)},j\∈allowed---\left(7\right)$

In formula, allowed directly reaches the set of next path point for Formica fusca from place i, and τ is pheromone, τ_ijIt is that path i arrives The information cellulose content of path j, α is the relative importance of pheromone, if α=0, the place j near i will be selected；β is The relative importance of range information, β=0, Formica fusca only affected by pheromone and have ignored that heuristic information brings inclined Tropism；If Formica fusca moving direction has barrier, randomly choose other directions, when guiding if there are pheromone, then press Guide action according to it, during optimizing, the movement probability of Formica fusca determines according to formula (7), Δ η_ijT () ＜ 0 represents that Formica fusca is located at itself The neighborhood search of place i, perceives and takes action；Δη_ijT () ＞ 0 expression Formica fusca is according to movement probability from its own site i Neighborhood movement to j neighborhood；

After n unit of time, local information element more New Policy presses formula (8), (9) develop：

τ_ij(t+n)=(1- ρ) τ_ij(t)+ρΔτ_ij(8)

${\mathrm{\Δ\τ}}_{ij}=\underset{k=1}{\overset{m}{\Σ}}{\mathrm{\Δ\τ}}_{ij}^k---\left(9\right)$

In formula, ρ ∈ (0,1) is pheromone volatilization parameter, Δ τ_ijPheromone increment on path (i, j) in circulating for this,The pheromone staying on path (i, j) in this circulation for Formica fusca k；

After an iteration terminates, Pheromone update, as global information element are carried out for worst Formica fusca paths traversed Update as formula (10)：

$\mathrm{\τ}(i,j)=(1-\mathrm{\ρ})\mathrm{\τ}(i,j)-\mathrm{\ϵ}\frac{L_{worst}}{L_{best}}---\left(10\right)$

The parameter that in formula, ε is introduced into, L_worstAnd L_bestIt is respectively the path of worst Formica fusca and optimum Formica fusca in previous cycle, τ (i, j) is the pheromone track amount on path (i, j)；

The structure in path：

Formica fusca k positioned at node i can produce a random number q before the next path of each selection, then from node i is to node The movement rule p ' of j presses formula (11)：

$p^{\′}=\left\{\begin{array}{cc}\underset{j\∈allowed}{\mathrm{argmax}}\left\{{\[{\mathrm{\τ}}_{ij}\]}^{\mathrm{\α}}{\[{\mathrm{\η}}_{ij}\]}^{\mathrm{\β}}\right\},& q\≤q_0\\ p_{ij},& q>q_0\end{array}\right.---\left(11\right)$

Q is stochastic variable, is evenly distributed in interval [0,1], q₀∈[0,1]；

Pid control parameter optimal solution can be found by above BWAS algorithm.