CN106444826A - Flight control method of QUAV (Quadrotor Unmanned Aerial Vehicle) - Google Patents

Abstract

The invention provides a flight control method of a QUAV. The method comprises the following steps that S10) a dynamical model and a dynamic equation of the QUAV are established; and S20) a compound control based control manner is designed to control four independent control channels converted from the dynamic equation in the step S10), control manners for the four channels include height PID, tumbling ADRC, pitching ADRC and yaw EACS-PID respectively, and conversion of control quantities is controlled to adjust the rotating speed of the four rotors and further achieve attitude control. Via the method of the invention, the QUAV adapts to the external environment change, yaw EACS-PID can realize adaptive adjustment of control parameters, tumbling ADRC and pitching ADRC can resist to interference more actively, height PID can be used to maintain high anti-interference capability and robustness and avoid the process from being too complex, the processor load of the QUAV is reduced, and the operation efficiency of hardware is improved.

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 right Pid control parameter carries out self-adaptative adjustment to realize aircraft adjust automatically, and the present invention then introduces EACS (Elitist Ant Colony System, elite Ant ColonySystem) algorithm to be optimizing pid control parameter.

ADRC (Active Disturbance Rejection Control, Active Disturbance Rejection Control) is that Chinese scholar Han Jing is clear A kind of control method that Mr. proposes, it is that " inside disturbing " and " disturbing outward " is regarded as system " total disturbance ", then with expansion shape Disturbance estimated by state observer (Extended State Observer, ESO), then cancels out.This anti-interference more active, Very big overshoot will not be caused.

The present invention propose a kind of flight control method of quadrotor, be based on PID control, EACS-PID control and The composite control method of the compositions such as ADRC, controls the flight attitude process of four rotor wing unmanned aerial vehicles, adapts to external environment change, from Dynamic be adjusted, keep good capacity of resisting disturbance and robustness, it is to avoid program is excessively complicated, reduces by four rotor unmanned flight The processor load of device, improves the operational efficiency of hardware.

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, 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, I_RFor 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：Design four that the control mode based on complex controll converts come kinetics equation in rate-determining steps S10 Independent control passage, the control mode of described four passages is respectively height PID, rolling ADRC, pitching ADRC, driftage EACS-PID, the rotating speed adjusting four rotors through the conversion and control of controlled quentity controlled variable to reach gesture stability, specifically includes with lower section Formula：

S21:Altitude channel adopts PID control

Using Increment Type Digital Hydraulic PID control altitude channel, its expression formula is：

U (k)=u (k-1)+Δ u (k) (3)

U (k)=u (k-1)+K_p[e(k)-e(k-1)]+K_ie(k)

+K_d[e(k)-2e(k-1)+e(k-2)] (4)

K in formula_p、K_iAnd K_dControl parameter for PID control；

S22:Jaw channel adopts EACS-PID to control

PID control adopts Increment Type Digital Hydraulic PID control, and its expression formula is：

Wherein K_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；

Evaluate the performance indications of control system using (6) formula：

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

Formula (6) conitnuous forms are formula (7)：

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：

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 Mobile, then Formica fusca when t is in i point towards the probability of j point movement is：

In formula, allowed directly reaches the set of next path point for Formica fusca from place i, and α is the relatively heavy of pheromone Want degree, β is the relative importance of range information；When α=0, the place j near i would be possible to be selected, and is similarly to Random greedy algorithm, when β=0, then Formica fusca is only affected by pheromone and be have ignored the skewed popularity that heuristic information is brought；As When fruit Formica fusca moving direction has barrier, then randomly choose other directions, when guiding if there are pheromone, then guide according to it Action；During optimizing, the movement probability of Formica fusca determines according to formula (9), if Δ η_ijT () ＜ 0 represents Formica fusca in itself site i Neighborhood search, perceive and take action；If Δ η_ij(t) ＞ 0 represent Formica fusca according to movement probability from the neighbour of its own site i The mobile neighborhood to point j in domain；

Local information element more New Policy is：

τ_ij(t)=(1- ξ) τ_ij(t-1)+ξτ₀(10)

Wherein ξ ∈ (0,1), τ₀For pheromone initial value；

The effect of local information element more New Policy is：Formica fusca is through path (i, j), the pheromone τ in this path_ijWill subtract Few, thus other Formica fuscas choose the probability in this path to reduce again；

Global information element more New Policy is：

Wherein ρ is pheromone volatilization parameter, Δ τ_ij ^bestThe pheromone increasing to path (i, j) for elitist ants；

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 of node j is：

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

Pid control parameter optimal solution is found by above EACS algorithm；

S23 rolling passage and pitch channel adopt ADRC, set roll angle θ and value respectively value θ of angle of pitch Φ_d、Φ_d, First with roll angle θ_dCalculating rolling channel value specific algorithm is：

S231) transition process arranging (TD)：

Transition process arranging such as below equation (13),

In formula, r is velocity factor, and h is filtering factor, and H is integration step, wherein h=3H～7H, and it is bigger, filter effect Better, but it is bigger to follow the tracks of signal phase loss；

Wherein set fst () as u=fst (v₁,v₂,r,h)：

S232) state estimation and total disturbance (ESO)：

Take α 1=0.5, α 2=0.25, δ value is moderate, λ 1, λ 2, λ 3 are observer parameter, the precision of ESO is higher, then Control performance is better；

S233) the formation (NLSEF) of controlled quentity controlled variable：

Wherein

δ ＞ 0 in above formula；

In NLSEF, typically take α₁=0.75, α₂=1.25 or α₁=0.5, α₂The value of=1.5, δ is identical with ESO； k₁、k₂For controller gain coefficient；B is the penalty coefficient of system；

In addition again pitch channel value is calculated with angle of pitch Φ, will roll angle θ_dReplace with angle of pitch Φ_dAnd repeat step S231, S232 and S233.

The flight control method of the quadrotor of the present invention, based on groups such as PID control, EACS-PID control and ADRC The complex controll becoming, in step S22 and S23, EACS-PID controls and can obtain optimal control ginseng by self-adaptative adjustment Number, ADRC is then that " inside disturbing " and " disturbing outward " is regarded as system " total disturbance ", then cancels out again.This anti-interference is more Ground actively, will not cause very big overshoot, so that QUAV keeps good capacity of resisting disturbance in flight course, improves Itself robustness.Pass through step S21 simultaneously, keep avoiding program excessively complicated while good control performance, reduce by four rotations The processor load of wing unmanned plane, improves the operational efficiency of hardware.

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 the PID control structural representation in the four rotor unmanned aircraft control method camber passages of the present invention；

Fig. 6 is the PID being optimized based on EACS in jaw channel in the four rotor unmanned aircraft control methods of the present invention Control structure schematic diagram；

Fig. 7 is in aircraft rolling passage and pitch channel in the four rotor unmanned aircraft control methods of the present invention ADRC structural representation；

Fig. 8 is the height tracing Comparative result figure based on PID control, EACS-PID control and ADRC；

Fig. 9 is the driftage angle tracking comparison diagram based on PID control, EACS-PID control and ADRC；

Figure 10 is the rolling angle tracking comparison diagram based on PID control, EACS-PID control and ADRC；

Figure 11 is the pitching angle tracking contrast based on PID control, EACS-PID control and ADRC；

Figure 12 is the vulnerability to jamming test curve of four control passages of four rotor unmanned aircrafts；

Figure 13 is robustness test curve under PID control and EACS-PID control for four control passages；

Figure 14 be four control passages ADRC control under robustness test curve.

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 complex controll side based on compositions such as PID control, EACS-PID control and ADRC The control program of method, calculates actual output motor control signal to electron speed regulator, then electron speed regulator is according to acquisition Control signal controls the rotating speed of 4 motors, thus realizing the control of the lift and torque that 4 rotors are produced, brushless electric machine can So that its rotating speed is controlled thus the size reaching to power and moment produced by each rotor is controlled by PWM.

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 controlling party of the composite control method based on compositions such as PID control, EACS-PID control and ADRC Case, after main controller module COMPREHENSIVE CALCULATING real-time attitude information and control signal information, output motor control signal is to electronic speed regulation Device, then electron speed regulator control the rotating speed of 4 motors according to the motor control signal obtaining, thus realizing 4 rotors are produced Raw lift and the control of torque, brushless electric machine can control its rotating speed thus reaching to produced by each rotor by PWM The size of power and moment is controlled, thus realizing automatically adapting to external environment change, reaches 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, and n1 is its lift coefficient, and n2 is 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：Design four that the control mode based on complex controll converts come kinetics equation in rate-determining steps S10 Independent control passage, as shown in figure 4, the control mode of described four passages is respectively height PID, rolling ADRC, pitching ADRC, driftage EACS-PID, that is, altitude channel just have a PID control, using using ADRC control, pitch channel adopts rolling passage ADRC controls, and jaw channel adopts EACS-PID to control, wherein Z_d、θ_d、Φ_d、ψ_dIt is respectively and flown by four rotors that remote control sets The height set of row device, roll angle setting value, angle of pitch setting value and yaw angle setting value, Z, θ, Φ, ψ are four rotor flyings The height actual value of device, roll angle actual value, angle of pitch actual value and yaw angle actual value, then the conversion and control through controlled quentity controlled variable The rotating speed adjusting four rotors, to reach gesture stability, specifically includes in the following manner：

S21:Altitude channel adopts PID control

Using Increment Type Digital Hydraulic PID control altitude channel, as shown in figure 5, its expression formula is：

U (k)=u (k-1)+Δ u (k) (3)

U (k)=u (k-1)+K_p[e(k)-e(k-1)]+K_ie(k)

+K_d[e(k)-2e(k-1)+e(k-2)] (4)

K in formula_p、K_iAnd K_dControl parameter for PID control；

S22:Jaw channel adopts EACS-PID to control

PID control adopts Increment Type Digital Hydraulic PID control, as shown in fig. 6, its expression formula is：

Wherein K_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；

Evaluate the performance indications of control system using (6) formula：

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

Formula (6) conitnuous forms are formula (7)：

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：

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 Mobile, then Formica fusca when t is in i point towards the probability of j point movement is：

In formula, allowed directly reaches the set of next path point for Formica fusca from place i, and α is the relatively heavy of pheromone Want degree, β is the relative importance of range information；When α=0, the place j near i would be possible to be selected, and is similarly to Random greedy algorithm, when β=0, then Formica fusca is only affected by pheromone and be have ignored the skewed popularity that heuristic information is brought；As When fruit Formica fusca moving direction has barrier, then randomly choose other directions, when guiding if there are pheromone, then guide according to it Action；During optimizing, the movement probability of Formica fusca determines according to formula (9), if Δ η_ijT () ＜ 0 represents Formica fusca in itself site i Neighborhood search, perceive and take action；If Δ η_ij(t) ＞ 0 represent Formica fusca according to movement probability from the neighbour of its own site i The mobile neighborhood to point j in domain；

Local information element more New Policy is：

τ_ij(t)=(1- ξ) τ_ij(t-1)+ξτ₀(10)

Wherein ξ ∈ (0,1), τ₀For pheromone initial value；

The effect of local information element more New Policy is：Formica fusca is through path (i, j), the pheromone τ in this path_ijWill subtract Few, thus other Formica fuscas choose the probability in this path to reduce again；

Global information element more New Policy is：

Wherein ρ is pheromone volatilization parameter, Δ τ_ij ^bestThe pheromone increasing to path (i, j) for elitist ants；Actual feelings Under condition, take suitable λ value can obtain more preferable solution path in the case that iterationses are less；

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 of node j is：

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

Pid control parameter optimal solution is found by above EACS algorithm；

S23 rolling passage and pitch channel adopt ADRC, as shown in fig. 7, wherein θ_d、Φ_dFor roll angle θ and angle of pitch Φ Setting value, first with roll angle θ_dCalculate rolling channel value, specific algorithm is：

S231) transition process arranging (TD)：

Transition process arranging such as below equation (13)

In formula, r is velocity factor, and h is filtering factor, and H is integration step, wherein h=3H～7H；It is bigger, filter effect Better, but it is bigger to follow the tracks of signal phase loss；

Wherein set fst () as u=fst (v₁,v₂,r,h)：

S232) state estimation and total disturbance (ESO)：

Take α₁=0.5, α₂=0.25 to obtain more preferable algorithm controls performance, and λ 1, λ 2, λ 3 are observer parameter, ESO's Precision is higher, then control performance is better；

S233) the formation (NLSEF) of controlled quentity controlled variable：

Wherein

δ ＞ 0 in above formula；

In NLSEF, typically take α₁=0.75, α₂=1.25 or α₁=0.5, α₂The value of=1.5, δ is identical with ESO； k₁、k₂For controller gain coefficient, k₁Increase, system response accelerates；k₂Then it is used for suppressing the overshoot in transient process；B is system Penalty coefficient；

In addition again pitch channel value is calculated with angle of pitch Φ, will roll angle θ_dReplace with angle of pitch Φ_dAnd repeat step S231, S232 and S233.

The flight control method of the quadrotor of the present invention, based on groups such as PID control, EACS-PID control and ADRC The complex controll becoming, in step S22 and S23, EACS-PID controls and can obtain optimal control ginseng by self-adaptative adjustment Number, ADRC is then that " inside disturbing " and " disturbing outward " is regarded as system " total disturbance ", then cancels out again.This anti-interference is more Ground actively, will not cause very big overshoot, so that QUAV keeps good capacity of resisting disturbance in flight course, improves certainly Body robustness.Pass through step S21 simultaneously, keep avoiding program excessively complicated while good control performance, reduce by four rotors The processor load of unmanned plane, improves the operational efficiency of hardware.Driftage EACS-PID can realize control parameter self-adaptative adjustment, Rolling ADRC, pitching ADRC can carry out anti-interference in the way of more initiative, and height PID is keeping good capacity of resisting disturbance and Shandong Program is avoided excessively complicated while rod.

In order to verify the control effect of four rotor unmanned aircrafts proposed by the present invention and its control method, using build Four rotor unmanned aircraft model machines are tested.Carry out multiple scheme experiments respectively, specific as follows：

In altitude channel, such as Fig. 8, employ ADRC algorithm, pid control algorithm and EACS-PID control algolithm carry out height Degree follows the tracks of contrast test, and the height tracing curve from PID control understands, its time to peak is 0.65s, and overshoot is 2.5%, Rise time is 0.51s, not only can reach preferable control and require, compare other two kinds of control algolithms it is also possible to reduce journey The complexity of sequence design.Also use similar thinking in the middle of the design of driftage corner channel, such as Fig. 9, due to PID control Can show bad, adjustment time is long, it is 4.31s.EACS-PID control under yaw angle aircraft pursuit course, time to peak is 0.36s, overshoot is 1.1%, and the rise time is 0.29s, and its dynamic property and ADRC are close, and EACS-PID control algolithm Complexity is less than ADRC algorithm, and parameter is less, then have chosen EACS-PID and control to control driftage corner channel.In the same manner, by Figure 10 and Figure 11 understands, selects ADRC algorithm to control rolling passage and pitch channel more suitable.Compound control by design System processed carries out vulnerability to jamming test, adds lasting stochastic signal as interference, as shown in figure 12, four passages in test process The steady-state error of corresponding control system is all within ± 2%.Robustness test is carried out by the multiplex control system of design, such as Shown in Figure 13 and Figure 14.In the control system of altitude channel, the excursion of overshoot is ± 1.0%, the change of regulating time Change scope is ± 0.01s；In the control system of jaw channel, the excursion of overshoot is ± 0.2%, the change of regulating time Change scope is ± 0.01s；In the control system of rolling passage, the excursion of overshoot is ± 0.1%, the change of regulating time Change scope is ± 0.02s；In the control system of pitch channel, the excursion of overshoot is ± 0.1%, the change of regulating time Change scope is ± 0.01s.Understand that the control system robustness of four passages is all preferable according to above analysis.

To sum up, four rotor unmanned aircraft control methods proposed by the present invention, are a kind of based on PID control, EACS-PID Control the composite control method with compositions such as ADRC, for controlling the flight attitude of four rotor unmanned aircrafts, keeping very well Capacity of resisting disturbance and while robustness, it is to avoid program is excessively complicated, reduces the processor load of four rotor wing unmanned aerial vehicles, carries The operational efficiency of high hardware.

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, ifθ, ψ 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 at center, Ix, Iy, Iz are inertia master away from Ω i is i-th rotor rotating speed, and Fi is the lift of i-th rotor generation, rotor The lift producing is directly proportional to rotary-wing transmission velocity squared, and IR is 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

S20：Four independences that the control mode based on complex controll for the design converts come kinetics equation in rate-determining steps S10 Control passage, the control mode of described four passages is respectively height PID, rolling ADRC, pitching ADRC, driftage EACS- PID, the rotating speed adjusting four rotors through the conversion and control of controlled quentity controlled variable to reach gesture stability, specifically includes in the following manner：

S21:Altitude channel adopts PID control

Using Increment Type Digital Hydraulic PID control altitude channel, its expression formula is：

U (k)=u (k-1)+Δ u (k) (3)

U (k)=u (k-1)+K_p[e(k)-e(k-1)]+K_ie(k)

+K_d[e(k)-2e(k-1)+e(k-2)] (4)

K in formula_p、K_iAnd K_dControl parameter for PID control；

S22:Jaw channel adopts EACS-PID to control

PID control adopts Increment Type Digital Hydraulic PID control, and its expression formula is：

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

Wherein K_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；

Evaluate the performance indications of control system using (6) formula：

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

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

Formula (6) conitnuous forms are formula (7)：

$\mathrm{\η}={\∫}_0^{\mathrm{\∞}}t\left|e\left(t\right)\right|dt---\left(7\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：

${\mathrm{\Δ\η}}_{ij}={\mathrm{\η}}_i-{\mathrm{\η}}_j,\∀i,j---\left(8\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, Then Formica fusca when t is in i point towards the probability of j point movement is：

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

In formula, allowed directly reaches the set of next path point for Formica fusca from place i, and α is the relatively important journey of pheromone Degree, β is the relative importance of range information；When α=0, the place j near i would be possible to be selected, and is similarly to random Greedy algorithm, when β=0, then Formica fusca is only affected by pheromone and be have ignored the skewed popularity that heuristic information is brought；If ant When ant moving direction has barrier, then randomly choose other directions, when guiding if there are pheromone, then guide row according to it Dynamic；During optimizing, the movement probability of Formica fusca determines according to formula (9), if Δ η_ijT () ＜ 0 represents Formica fusca itself site i's Neighborhood search, perceives and takes action；If Δ η_ij(t) ＞ 0 represent Formica fusca according to movement probability from the neighborhood of its own site i The mobile neighborhood to point j；

Local information element more New Policy is：

τ_ij(t)=(1- ξ) τ_ij(t-1)+ξτ₀(10)

Wherein ξ ∈ (0,1), τ₀For pheromone initial value；

The effect of local information element more New Policy is：Formica fusca is through path (i, j), the pheromone τ in this path_ijWill reduce, Thus other Formica fuscas choose the probability in this path to reduce again；

Global information element more New Policy is：

${\mathrm{\τ}}_{ij}\left(t\right)=(1-\mathrm{\ρ}){\mathrm{\τ}}_{ij}(t-1)+\underset{k=1}{\overset{m}{\Σ}}{{\mathrm{\Δ\τ}}_{ij}}^k+{{\mathrm{\λ\Δ\τ}}_{ij}}^{best}$

Wherein ρ is pheromone volatilization parameter, Δ τ_ij ^bestThe pheromone increasing to path (i, j) for elitist ants；

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 of j is：

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

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

Pid control parameter optimal solution is found by above EACS algorithm；

S23 rolling passage and pitch channel adopt ADRC, set roll angle θ and the value of angle of pitch Φ is respectively θ_d、Φ_d, first to turn over Roll angle θ_dCalculate rolling channel value, specific algorithm is：

S231) transition process arranging (TD)：

Transition process arranging such as below equation (13)

$\left\{\begin{array}{c}{v}_1(k+1)=v_1\left(k\right)+{\mathrm{Hv}}_2\left(k\right)\\ v_2(k+1)=v_2\left(k\right)+Hfst\left(v_1\right(k)-\\ {\mathrm{\θ}}_d\left(k\right),v_2\left(k\right),r,h)\end{array}\right.---\left(13\right)$

In formula, r is velocity factor, and h is filtering factor, and H is integration step, wherein h=3H～7H；

Wherein set fst () as u=fst (v₁,v₂,r,h)：

$\left\{\begin{array}{c}d=rh\\ d_0=dh={\mathrm{rh}}^2\\ y=v_1+{\mathrm{hv}}_2\\ a_0={(d^2+8r|y\left|\right)}^{\frac{1}{2}}\end{array}\right.---\left(14\right)$

$a=\left\{\begin{array}{cc}{v}_2+\frac{(a_0-d)}{2},& \left|y\right|>d_0\\ v_2+\frac{y}{h},& \left|y\right|\≤d_0\end{array}\right.---\left(15\right)$

$fst(v_1,v_2,r,h)=\left\{\begin{array}{cc}-\frac{ra}{d},& \left|a\right|\≤d\\ -r\mathrm{sgn}\left(a\right),& \left|a\right|>d\end{array}\right.---\left(16\right)$

S232) state estimation and total disturbance (ESO)：

$\left\{\begin{array}{c}{e}_0=z_1\left(k\right)-y\left(k\right)\\ z_1(k+1)=z_1\left(k\right)+H\left(z_2\right(k)-{\mathrm{\λ}}_1{e}_0)\\ z_2(k+1)=z_2\left(k\right)+H\left(z_3\right(k)-\\ {\mathrm{\λ}}_{2}fal(e_0,{\mathrm{\α}}_1,\mathrm{\δ})+bu\left(k\right))\\ z_3(k+1)=z_3\left(k\right)-{\mathrm{H\λ}}_{3}fal(e_0,{\mathrm{\α}}_2,\mathrm{\δ})\end{array}\right.---\left(17\right)$

Take α₁=0.5, α₂=0.25, λ₁、λ₂、λ₃For observer parameter；

S233) the formation (NLSEF) of controlled quentity controlled variable：

$\left\{\begin{array}{c}{e}_1=v_1\left(k\right)-z_1\left(k\right)\\ e_2=v_2\left(k\right)-z_2\left(k\right)\\ u_0=k_{1}fal(e_1,{\mathrm{\α}}_1,\mathrm{\δ})+k_{2}fal(e_2,{\mathrm{\α}}_2,\mathrm{\δ})\\ u\left(k\right)=u_0-\frac{z_3\left(k\right)}{b}\end{array}\right.---\left(18\right)$

Wherein

$fal(e,\mathrm{\α},\mathrm{\δ})=\left\{\begin{array}{cc}|e{|}^{\mathrm{\α}}\mathrm{sgn}\left(e\right),& \left|e\right|>\mathrm{\δ}\\ \frac{e}{{\mathrm{\δ}}^{1-\mathrm{\α}}},& \left|e\right|\≤\mathrm{\δ}\end{array}\right.---\left(19\right)$

δ ＞ 0 in above formula；

In NLSEF, take α₁=0.75, α₂=1.25 or α₁=0.5, α₂The value of=1.5, δ is identical with ESO；k₁、k₂For control Device gain coefficient processed；B is the penalty coefficient of system；

In addition again pitch channel value is calculated with angle of pitch Φ, will roll angle θ_dReplace with angle of pitch Φ_dAnd repeat step S231, S232 and S233.