CN102955477B - Attitude control system and control method of four-rotor aircraft - Google Patents

Abstract

The invention discloses an attitude control system and a control method of a four-rotor aircraft. Firstly, the information of attitude angles of the current aircraft is obtained, and the attitude angles include a roll angle, a pitch angle and a yaw angle; secondly, controllers for the roll angle, the pitch angle and the yaw angle are respectively designed, wherein the controller for the roll angle is the same as that for the pitch angle; and finally, after obtained control inputs of the three controllers are overlapped, a total driving signal is output, thereby driving a motor to operate, so as to control the attitude of the aircraft. A gyroscope and an accelerometer are combined, and a Kalman filter is designed in accordance with models of the attitude angles of the aircraft, so as to conjecture the attitude angles on line. Additionally, variables of the intermediate state of the system can be obtained, and a certain filtering effect is achieved.

Description

A kind of quadrotor attitude control system and control method

Technical field

The present invention relates to a kind of quadrotor attitude control system, belong to flying vehicles control technical field.

Background technology

As far back as the middle of last century, microminiature multi-rotor aerocraft has been subjected to attracting attention of more overseas research institutions, but multi-rotor aerocraft stock size is less, and load capacity is relatively poor, cannot carry traditional high-precision sensor.Until the beginning of this century, the development of MEMS sensor technology makes the research of microminiature multi-rotor aerocraft be broken through.

Gesture stability is the basis that many rotor flyings control.For completing gesture stability, the attitude information of aircraft first must be obtained.The measuring method at current attitude of flight vehicle angle mainly contains: 1, adopt high-precision gyroscope.Although price is relatively low, there is drift in gyroscope itself, increase in time, the data precision can be very poor.2, adopt accelerometer and magnetometer to revise gyro data, thus suppress sensor drift.The accuracy of the method is relevant with the backoff algorithm of employing, generally has Eulerian angle, hypercomplex number, rotating vector, spreading kalman etc.Business-like attitude measurement system mainly contains the MNAV module of Crossbow company of the U.S. and the MTi of Dutch Xsens company, but their price is all up to tens thousand of Renminbi, has had a strong impact on its range of application.

3, the method for image procossing is adopted.The method is by obtaining attitude information comparatively accurately to the process of image in conjunction with respective algorithms, but image processing data amount is comparatively large, and Data Update frequency is slow, not easily meets the needs of aircraft.In addition, because image procossing generally needs to install image capture device and mark etc., the restriction by environment is comparatively large, cannot extensively adopt.

At present, the method for designing of attitude controller is more, as PID control, modern scientist, fuzzy control etc., and achieves certain control effects.But above control method is mostly higher to the accuracy requirement of model, when model perturbation or when being subject to external interference, its Control platform can obviously decline, and has had a strong impact on the range of application of multi-rotor aerocraft.

Summary of the invention

The object of the present invention is to provide the quadrotor attitude control system that a kind of structure is simple, cost is low; Meanwhile, the present invention also proposes a kind of control method based on this control system.

The technical solution realizing the object of the invention is: a kind of quadrotor attitude control system, comprise DC-DC circuit, 3-axis acceleration device, magnetometer, gyroscope, analog to digital converter and microprocessor, described 3-axis acceleration device, magnetometer, gyroscope are connected with microprocessor by analog to digital converter, and 3-axis acceleration device, magnetometer, gyroscope will detect that simulating signal is sent to microprocessor and carries out processing and controlling after analog to digital conversion.

Prioritization scheme further, in quadrotor attitude control system of the present invention, described gyrostatic quantity is three.

A kind of quadrotor attitude control method, comprises the following steps:

The attitude angle information of step one, acquisition current flight device, described attitude angle comprises roll angle, the angle of pitch and crab angle;

Step 2, design the controller of roll angle, the angle of pitch and crab angle respectively, wherein roll angle is identical with the controller of the angle of pitch;

Step 3, the total drive singal of rear output one is superposed to the controlled quentity controlled variable of three controllers obtained in step 2 thus drive motor work to control attitude of flight vehicle.

Prioritization scheme further, in described quadrotor attitude control method, obtains the attitude angle information of current flight device in step one, specific as follows:

(1-1) set up the attitude angle model of aircraft, comprising roll angle model, angle of pitch model and crab angle model, described roll angle model is identical with angle of pitch model to be:

$\left\{\begin{array}{c}\stackrel{.}{x}=\mathrm{Ax}+\mathrm{Bu}=\left[\begin{array}{cccc}-\frac{1}{T}& 0& 0& 0\\ 1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& g& 0& -\frac{K}{m}\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\\ x_4\end{array}\right]+\left[\begin{array}{c}\frac{k}{\mathrm{JT}}\\ 0\\ 0\\ 0\end{array}\right]u\\ y=\mathrm{Cx}=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& 0& g& -1\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\\ x_4\end{array}\right]\end{array}\right.$

In formula, A is the system matrix of roll angle/angle of pitch model, and B is gating matrix, and C is output matrix, and x is state variable, and y is output variable, represent the derivative J moment of inertia of x, T is a time constant, and k is scale-up factor, and K is coefficient of air resistance, and m is vehicle mass, and g is acceleration of gravity, state variable x ₁, x ₂, x ₃, x ₄be respectively angular acceleration, acceleration, angle, sensors observe acceleration, u is control inputs;

Crab angle model is:

$\left\{\begin{array}{c}{\stackrel{.}{x}}_y=A_y{x}_y+B_y{u}_y=\left[\begin{array}{cc}-\frac{T_{s1}+T_{s2}}{T_{s1}{T}_{s2}}& -\frac{1}{T_{s1}{T}_{s2}}\\ 1& 0\end{array}\right]\\ y_y=C_y{x}_y=\left[\begin{array}{cc}0& 1\end{array}\right]\left[\begin{array}{c}{x}_{y1}\\ x_{y2}\end{array}\right]\end{array}\right.\left[\begin{array}{c}{x}_{y1}\\ x_{y2}\end{array}\right]+\left[\begin{array}{c}\frac{k}{T_{s1}{T}_{s2}}s\\ 0\end{array}\right]u_y$

A in formula _y, B _y, C _y, x _y, y _ybe respectively the system matrix of crab angle model, gating matrix, output matrix, state variable, output variable, for x _yderivative.T _s1, T _s2be respectively the time constant of two approximate inertial elements, k _sbe the product of the scale-up factor of two inertial elements, x _y1, x _y2for yaw rate and angle, u _yfor crab angle control inputs;

(1-2) respectively according to the corresponding Kalman filter of the modelling of above-mentioned foundation, estimate the state variable in attitude angle model, the state variable wherein in roll angle model/angle of pitch model comprises attitude angle, angular acceleration, angular velocity and observation acceleration; State variable in crab angle model comprises yaw rate and angle;

A, the Kalman filter set up according to roll angle model/angle of pitch model are:

$\widehat{\stackrel{.}{x}}=\mathrm{Ax}+\mathrm{Bu}+K_z(y-\hat{y})$

In formula, represent the estimated value of state variable x, y and be respectively the output valve of system and the output valve of estimation, K _zfor the kalman gain of roll angle/angle of pitch model;

B, according to crab angle model set up Kalman filter be:

${\widehat{\stackrel{.}{x}}}_y=A_y{x}_y+B_y{u}_y+K_{\mathrm{zy}}(y_y-{\hat{y}}_y)$

In formula, represent state variable x _yestimated value, y _ywith be respectively the output valve of system and the output valve of estimation, K _zyfor the kalman gain of crab angle model;

Prioritization scheme further, in described quadrotor attitude control method, the method designing the controller of roll angle, the angle of pitch and crab angle in step 2 is specially:

(2-1) for roll angle model, angle of pitch model, crab angle model arrange corresponding reference model

Wherein, the reference model of roll angle model, angle of pitch model is as follows:

$\left\{\begin{array}{c}{\stackrel{.}{x}}_m=A_m{x}_m+B_{m}r\\ y_m=C_m{x}_m\end{array}\right.$

In formula, r is reference input, A _m, B _m, C _mfor the state matrix of reference model, x _m, y _mfor state variable and the output of reference model;

Crab angle reference model is as follows:

$\left\{\begin{array}{c}{\stackrel{.}{x}}_{\mathrm{my}}=A_{\mathrm{my}}{x}_{\mathrm{my}}+B_{\mathrm{my}}{r}_y\\ y_{\mathrm{my}}=C_{\mathrm{my}}{x}_{\mathrm{my}}\end{array}\right.$

R in formula _yfor the reference input of crab angle, A _my, B _my, C _myfor the state matrix of reference model, x _my, y _myfor state variable and the output of reference model;

(2-2), utilize the deviation of the state variable in the state variable in reference model and attitude of flight vehicle angle model to design sliding mode controller; Wherein, roll angle, pitch controller are:

u=u _eq+u _nl=u _eq+K _nlf(σ)

In formula, u is the control inputs that roll angle/pitch controller obtains, u _eqfor equivalent inpnt, u _nlfor the non-linear input of sliding mode controller, K _nlfor switching amplitude, f (σ) is switching function;

Crab angle controller is:

u _y=u _eqy+u _nly=u _eqy+K _nlyf _y(σ _y)

U in formula _yfor the control inputs that crab angle controller obtains, u _eqyfor equivalent inpnt, u _nlyfor the non-linear input of sliding mode controller, K _nlyfor switching amplitude, f _y(σ _y) be switching function.

The present invention compared with prior art, its remarkable advantage: 1) the present invention adopts gyroscope in conjunction with accelerometer, and design Kalman filter according to attitude of flight vehicle angle model and infer attitude angle online, the intermediateness variable of system can also be obtained simultaneously, there is certain filter effect;

2) the present invention has the advantage contained much information that cost is low, calculated amount is little, precision is high, obtain;

3) the present invention adopts gyroscope in conjunction with accelerometer, and designs Kalman filter according to attitude of flight vehicle angle model and infer attitude angle online, and calculated amount is less, and system response time is fast.

4) the present invention adopts reference model synovial membrane controller gesture stability algorithm, has stronger robustness, and flying quality is very little by the restriction of environment.

Accompanying drawing explanation

Fig. 1 is the structured flowchart of quadrotor attitude control system of the present invention;

Fig. 2 is the system block diagram between roll angle/angle of pitch to the acceleration recorded;

Fig. 3 is the Kalman filter block scheme of roll angle/angle of pitch;

Fig. 4 is the controlling party block diagram of the synovial membrane controller of roll angle/angle of pitch;

Embodiment

Below in conjunction with accompanying drawing, the present invention is described in further detail.

As shown in Figure 1, a kind of quadrotor attitude control system, comprise DC-DC circuit 1,3-axis acceleration device 3, magnetometer 4, gyroscope 2, analog to digital converter 5 and microprocessor 6, described 3-axis acceleration device, magnetometer, gyroscope are connected with microprocessor by analog to digital converter, and 3-axis acceleration device, magnetometer, gyroscope will detect that simulating signal is sent to microprocessor and carries out processing and controlling after analog to digital conversion.

1, DC-DC circuit: DC-DC circuit is the power transfer module of DC-to-DC, effect is DC voltage direct supply being converted into needs.This example selects voltage stabilizing chip LM2575, and its maximum input voltage is 45v, and maximum output current can reach 1A, and output voltage 3.3v, 5v, 12v, 15v are adjustable, and voltage stabilizing error is within 4%.According to actual needs, the voltage stabilizing adopting two panels LM2575 to obtain 3.3v and 5v exports.

2. gyroscope: gyro used is a complete function of ADI company, angular rate sensor with low cost (gyroscope), for measured angular speed, this gyroscope survey scope reaches ± 300 °/s, antijamming capability is strong, there is temperature correction function, drift error is little, can meet the flight control performance demand of multi-rotor aerocraft.Utilize resistance on external capacitor and sheet to form a low-pass filter for limiting ADXRS610 rate response bandwidth, bandwidth is set to 361Hz.

3. 3-axis acceleration device: acceleration is MAV attitude measurement and the important state amount analyzing MAV flying quality, the ADXL335 of selection can with the full range acceleration measurement of ± 3g.Suitable bandwidth is selected by the electric capacity on regulation output pin.The X-axis of accelerometer adopted and the bandwidth range of Y-axis are 0.5Hz to 1600Hz, and the bandwidth range of Z axis is 0.5Hz to 550Hz.In order to reduce noise, degree of will speed up sensor bandwidth is set as 50HZ, namely uses 0.1uf filter capacitor.

4. magnetometer: selecting first magnetometer of TM MAG3110, is a small-sized low-power consumption, digital 3 axle magnetometers, has broad dynamic range, can run in the printed circuit board (PCB) (PCB) of external magnetic field.MAG3110 comprises the I2C serial line interface of standard, can measure the magnetic field, position up to 10 Gausses, and output data rate (ODR) can reach 80Hz.Corresponding output data rate can adjust in the sampling interval from 12ms to several seconds.

5. analog to digital converter: analog to digital converter employs the MCP3204 of 12 bit resolutions, band SPI serial line interface, when supply voltage is 5V; Maximum sampling rate can reach 100Ksps.In order to strengthen the wind loading rating of miniature MAVS, need to improve the sampling rate of ADC, the attitude measurement system of design is by with the rate-adaptive pacemaker data of 400HZ.Signal is with analog-and digital-(RS-232) two kinds of formatted outputs.Simulating signal comprises three axis accelerometer voltage, three axis magnetometer voltage, accelerometer voltage.Digital output comprises tri-axis angular rate; Rolling, pitching, crab angle.Leave GPS input interface, ADC input pin leaves expansion interface, conveniently increases corresponding sensor according to actual needs.

6. microprocessor: adopt microprocessor to process sensing data, infer and attitude angle and carry out gesture stability, controlled frequency is 400Hz.Select the AT91sam7 had compared with high performance-price ratio, it is Atmel32 position ARM risc processor, and with the high speed Flash of 256k, processing speed can meet the needs of attitude supposition and gesture stability.

A control method for quadrotor attitude control system, comprises the following steps:

The attitude angle information of step one, acquisition current flight device, described attitude angle comprises roll angle, the angle of pitch and crab angle, is specially:

(1-1) the attitude angle model of aircraft is set up, comprising roll angle model, the angle of pitch

Model and crab angle model

A, set up roll angle model, angle of pitch model

By theory deduction and approximate linearization process, the relation between the attitude angle of aircraft and torque can approximate representation be:

$\mathrm{\τ}=J\stackrel{..}{\mathrm{\φ}}---\left(1\right)$

In formula, τ is the torque that aircraft obtains, J moment of inertia, and φ is attitude angle.

Now suppose that the steering order signal of aircraft is first order inertial loop to the torque obtained, then can obtain the model of steering order signal to attitude angle.

$G_{\mathrm{u\φ}}=\frac{\mathrm{\φ}\left(s\right)}{u\left(s\right)}=\frac{k}{(1+\mathrm{Ts})s^2}---\left(2\right)$

G in formula _{u φ}represent the transport function of control inputs signal u to attitude angle φ, k is scale-up factor, and T is a time constant.

For suppressing the attitude angle φ presumption error produced by the drift of single gyro data, this patent proposes to introduce accelerometer and compensates, and then utilizes Kalman filter to infer attitude angle online.For this reason, first we will will speed up data read and introduce dummy vehicle.

The observation acceleration a of accelerometer _mcomprise dynamic acceleration a _dwith quiet acceleration, g is acceleration of gravity, and φ is attitude angle, and its relation can be provided by following formula.

a _m=g _φ+a _d （3）

Consider air resistance, and resistance coefficient is directly proportional to speed, then have:

ma _d=mgφ-K∫a _ddt （4）

In formula, K is coefficient of air resistance.

Laplace transform is done to above formula, obtains the transport function of attitude angle to dynamic acceleration:

$G_{{\mathrm{\φa}}_d}=\frac{a_d\left(s\right)}{\mathrm{\φ}\left(s\right)}=\frac{\frac{\mathrm{mg}}{K}s}{\frac{m}{K}s+1}---\left(5\right)$

Thus the system block diagram obtained between roll angle/angle of pitch to the acceleration recorded, as shown in Figure 2, Angular Velocity is input angular velocity, and Roll is the attitude angle obtained, and g is acceleration of gravity, and Acc is quiet acceleration, a _dfor dynamic acceleration, a _mfor sensors observe acceleration.

According to formula (2) (3) (5), select angular velocity and acceleration to export as system, then can obtain the model of quadrotor roll angle or the angle of pitch:

$\left\{\begin{array}{c}\stackrel{.}{x}=\mathrm{Ax}+\mathrm{Bu}=\left[\begin{array}{cccc}-\frac{1}{T}& 0& 0& 0\\ 1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& g& 0& -\frac{K}{m}\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\\ x_4\end{array}\right]+\left[\begin{array}{c}\frac{k}{\mathrm{JT}}\\ 0\\ 0\\ 0\end{array}\right]u\\ y=\mathrm{Cx}=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& 0& g& -1\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\\ x_4\end{array}\right]\end{array}\right.---\left(6\right)$

In formula, A is the system matrix of roll angle model, and B is gating matrix, and C is output matrix, and x is state variable, and y is output variable, for the derivative of x, J moment of inertia, T is a time constant, and K is coefficient of air resistance, and k is scale-up factor, and m is vehicle mass, and g is acceleration of gravity, state variable x ₁, x ₂, x ₃, x ₄be respectively angular acceleration, acceleration, angle, sensors observe acceleration, u is control inputs;

B, set up crab angle model

Suppose that in crab angle model, control inputs is the superposition of two first order inertial loops to crab angle, then can obtain the second-order model of crab angle.

$\left\{\begin{array}{c}{\stackrel{.}{x}}_y=A_y{x}_y+B_y{u}_y=\left[\begin{array}{cc}-\frac{T_{s1}+T_{s2}}{T_{s1}{T}_{s2}}& -\frac{1}{T_{s1}{T}_{s2}}\\ 1& 0\end{array}\right]\\ y_y=C_y{x}_y=\left[\begin{array}{cc}0& 1\end{array}\right]\left[\begin{array}{c}{x}_{y1}\\ x_{y2}\end{array}\right]\end{array}\right.\left[\begin{array}{c}{x}_{y1}\\ x_{y2}\end{array}\right]+\left[\begin{array}{c}\frac{k}{T_{s1}{T}_{s2}}s\\ 0\end{array}\right]u_y$

A in formula _y, B _y, C _y, x _y, y _ybe respectively the system matrix of crab angle model, gating matrix, output matrix, state variable, output variable, for x _yderivative.T _s1, T _s2be respectively the time constant of two approximate inertial elements, k _sbe the product of the scale-up factor of two inertial elements, x _y1, x _y2for yaw rate and angle, u _yfor crab angle control inputs;

(1-2), respectively according to the corresponding Kalman filter of the modelling of above-mentioned foundation, estimate the state variable in attitude angle model, the state variable wherein in roll angle model/angle of pitch model comprises attitude angle, angular acceleration, angular velocity and observation acceleration; State variable in crab angle model comprises yaw rate and angle;

A, according to roll angle model, angle of pitch model set up Kalman filter be expressed as

$\widehat{\stackrel{.}{x}}=\mathrm{Ax}+\mathrm{Bu}+K_z(y-\hat{y})---\left(7\right)$

In formula represent the estimated value of state variable x, A, B are system matrix and the input matrix of system state equation in formula (6), y and be respectively the output valve of system and the output valve of estimation, K _zfor kalman gain, can be expressed as

K _z=PC ^TR ^-1 (8)

In formula, C is formula (6) system state equation output matrix, and P is the solution of algebraic Riccati equation

AP+PA ^T+BQB ^T-PC ^TRCP=0 (9)

In Riccati equation, matrix Q, R are respectively the variance of system output (gyroscope and accelerometer data) and control input end noise, represent the degree of confidence of parameters.Relatively accurate attitude angle information then can be obtained by the weighting coefficient adjusting matrix.Kalman filter has certain filter effect simultaneously, can eliminate the sensor noise of high frequency, improve the accuracy of the attitude angle of supposition.As shown in Figure 3, y is the angular velocity that obtains of sensor and acceleration information, and u is the output controlled quentity controlled variable of controller, and A, B, C are the attitude of flight vehicle angle model that formula (6) represents, Kz is the kalman gain that formula (8) obtains, then can obtain the estimation of the state variable of system

B, according to crab angle model set up Kalman filter be:

${\widehat{\stackrel{.}{x}}}_y=A_y{x}_y+B_y{u}_y+K_{\mathrm{zy}}(y_y-{\hat{y}}_y)$

In formula, represent state variable x _yestimated value, y _ywith be respectively the output valve of system and the output valve of estimation, K _zyfor the kalman gain of crab angle model; The design of the Kalman filter of crab angle model is identical with roll angle method, just kalman gain K _zyto solve be weighting matrices Q according to crab angle model and its correspondence _y, R _yobtain.

Step 2, design the controller of roll angle, the angle of pitch and crab angle respectively, wherein roll angle is identical with the controller of the angle of pitch;

(2-1) for roll angle model, angle of pitch model, crab angle model arrange corresponding reference model

The reference model of A, roll angle model, angle of pitch model is as follows:

$\left\{\begin{array}{c}{\stackrel{.}{x}}_m=A_m{x}_m+B_{m}r\\ y_m=C_m{x}_m\end{array}\right.$

In formula, r is reference input, A _m, B _m, C _mfor the state matrix of reference model, x _m, y _mfor state variable and the output of reference model;

Make C _m=C, e=x _m-x, then the deviation differential expressions of backfeed loop can be expressed as:

$\stackrel{.}{e}=A_{m}e+(A-A_m)x+\mathrm{Bu}-B_{m}r---\left(11\right)$

U is control inputs, supposes that this reference model meets following condition:

A _m-A=BK ₁,B _m=BK ₂ （12）

Then formula (10) can be expressed as:

$\stackrel{.}{e}=A_{m}e-B(K_{1}x+K_{2}r-u)---\left(13\right)$

Introduce the generalized inverse matrix B of B ⁺, obtain K by formula (11) ₁expression formula

K ₁=B ⁺(A _m-A) （14）

For enabling the output tracking target value r of reference model, the straight-through gain of adjustment System is needed to be 1.When time approximates infinity, the state variable convergence of system is stablized, namely

A _mx _m+ B _mr=0, so

x _m=-A _m ^-1B _mr （15）

Bring y into _m

y _m=-C _mA _m ^-1B _mr=-C _mA _m ^-1BK ₂r （16）

Thus can K be obtained ₂value

$K_2={(-C_m{A}_m^{-1}B)}^{-1}---\left(17\right)$

Then B _mcan be obtained by following formula

$B_m={\mathrm{BK}}_2=B{\left({-C}_m{A}_m^{-1}B\right)}^{-1}---\left(18\right)$

A _mvalue by emulation experiment regulate obtain.

B, crab angle reference model are as follows:

$\left\{\begin{array}{c}{\stackrel{.}{x}}_{\mathrm{my}}=A_{\mathrm{my}}{x}_{\mathrm{my}}+B_{\mathrm{my}}{r}_y\\ y_{\mathrm{my}}=C_{\mathrm{my}}{x}_{\mathrm{my}}\end{array}\right.$

R in formula _yfor the reference input of crab angle, A _my, B _my, C _myfor the state matrix of reference model, x _my, y _myfor state variable and the output of reference model; The method for solving of crab angle model and the method for solving of roll angle/angle of pitch similar, only roll angle in the solution procedure/matrix A of angle of pitch model, B, C need be replaced with A _y, B _y, C _ya can be obtained _my, B _my, C _my.

(2-2), utilize the deviation of the state variable in the state variable in reference model and attitude of flight vehicle angle model to design sliding mode controller;

To disturb to external world due to sliding mode controller (SMC) and modeling error has good robustness, therefore adopt the tracking that sliding mode controller design method realizes reference model state variable.Be the accurate tracking of realize target value in SMC controls, introducing integral element is to improve the tracking performance of system, and we introduce a new state variable ε for this reason _yfor output bias e _yintegration, namely

${\stackrel{.}{\mathrm{\ϵ}}}_y=y-y_m---\left(19\right)$

The bias system then expanded is

${\stackrel{.}{e}}_s=\left[\begin{array}{c}\stackrel{.}{e}\\ {\stackrel{.}{\mathrm{\ϵ}}}_y\end{array}\right]=\left[\begin{array}{cc}{A}_m& 0\\ C_m& 0\end{array}\right]\left[\begin{array}{c}e\\ {\mathrm{\ϵ}}_y\end{array}\right]+\left[\begin{array}{c}B\\ 0\end{array}\right]u_s=A_e{e}_s+B_s{u}_s---\left(20\right)$

U in formula _s=-(K ₁x+K ₂r-u)

Definition switching function σ ∈ R is

σ=Se _s （21）

$\stackrel{.}{\mathrm{\σ}}=S{A}_s{e}_s-{\mathrm{SB}}_s(K_{1}x+K_{2}r-u)---\left(22\right)$

When system arrives desirable sliding mode its equivalent inpnt u _eqcan be obtained by formula (23)

To u _eq=-(SB _s) ^-1sA _se _s+ K ₁x+K ₂r (23)

By u _eqbring formula (19) into obtain

${\stackrel{.}{e}}_s=\{I-B_s{\left(S{B}_s\right)}^{-1}S\}{A}_s{e}_s---\left(24\right)$

System has stable zero point can be obtained fom the above equation.

For selecting diverter surface, introducing method for optimally controlling, namely selecting Optimal Feedback gain F to be diverter surface S.In formula, P is the solution of Riccati equation, and S meets SB _s>0.

$F=S=B_s^{T}P---\left(25\right)$

${\mathrm{PA}}_s+A_s^{T}P-{\mathrm{PB}}_s{B}_s^{T}P+Q=0---\left(26\right)$

The non-linear input of SMC

u _nl=K _nlf(σ) （27）

K in formula Chinese style _nlfor switching amplitude, f (σ) is switching function, usually selection function sign, but sign function can produce very large shake in actual applications, affects Control platform.We adopt a smooth function

$f\left(\mathrm{\σ}\right)=\frac{\mathrm{\σ}}{\left|\mathrm{\σ}\right|+\mathrm{\δ}}---\left(28\right)$

In formula, δ is smooth function weight.

Roll angle, pitch controller are expressed as:

u=u _eq+u _nl=u _eq+K _nlf(σ)

In formula, u is the controlled quentity controlled variable that controller obtains, u _eqfor equivalent inpnt, u _nlfor the non-linear input of sliding mode controller, K _nlfor switching amplitude, f (σ) is switching function;

By selecting suitable δ to make state variable close to nonlinear Control amount corresponding during diverter surface also corresponding reduction, thus effectively inhibit thrashing.As shown in Figure 4, SMC is sliding mode controller, and Kalman is Kalman filter, and Real System is practical flight device.R is reference input, the state variable that Kalman filter is inferred with the difference of the state variable x of reference model, and the output angle y of aircraft and reference model export the state variable of integration as sliding mode controller SMC of angle difference, then export control inputs u, thus complete the gesture stability of aircraft.

Crab angle controller is:

u _y=u _eqy+u _nly=u _eqy+K _nlyf _y(σ _y)

U in formula _yfor the control inputs that crab angle controller obtains, u _eqyfor equivalent inpnt, u _nlyfor the non-linear input of sliding mode controller, K _nlyfor switching amplitude, f _y(σ _y) be switching function.

Step 3, the total drive singal of rear output one is superposed to the controlled quentity controlled variable of three controllers obtained in step 2 thus drive motor work to control attitude of flight vehicle.

Claims (1)

1. the control method of a quadrotor attitude control system, this system comprises DC-DC circuit, 3-axis acceleration device, magnetometer, gyroscope, analog to digital converter and microprocessor, described 3-axis acceleration device, magnetometer, gyroscope are connected with microprocessor by analog to digital converter, the simulating signal detected is sent to microprocessor by 3-axis acceleration device, magnetometer, gyroscope after analog to digital conversion carries out processing and controlling, it is characterized in that, comprise the following steps:

The attitude angle information of step one, acquisition current flight device, described attitude angle comprises roll angle, the angle of pitch and crab angle, is specially:

(1-1) set up the attitude angle model of aircraft, comprising roll angle model, angle of pitch model and crab angle model, described roll angle model is identical with angle of pitch model to be:

$\left\{\begin{array}{c}\stackrel{\·}{x}=\mathrm{Ax}+\mathrm{Bu}=\left[\begin{array}{cccc}-\frac{1}{T}& 0& 0& 0\\ 1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& g& 0& -\frac{K}{m}\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\\ x_4\end{array}\right]+\left[\begin{array}{c}\frac{k}{\mathrm{JT}}\\ 0\\ 0\\ 0\end{array}\right]u\\ y=\mathrm{Cx}=\begin{array}{c}\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& 0& g& -1\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\\ x_4\end{array}\right]\end{array}\end{array}\right.$

In formula, A is the system matrix of roll angle model, and B is gating matrix, and C is output matrix, and x is state variable, and y is output variable, represent the derivative of x, J moment of inertia, T is a time constant, and K is coefficient of air resistance, and k is scale-up factor, and m is vehicle mass, and g is acceleration of gravity, state variable x ₁, x ₂, x ₃, x ₄be respectively angular acceleration, acceleration, angle, sensors observe acceleration, u is control inputs;

Crab angle model is:

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_y=A_y{x}_y+B_y{u}_y=\left[\begin{array}{cc}-\frac{T_{s1}+T_{s2}}{T_{s1}{T}_{s2}}& -\frac{1}{T_{s1}{T}_{s2}}\\ 1& 0\end{array}\right]\left[\begin{array}{c}{x}_{y1}\\ x_{y2}\end{array}\right]+\left[\begin{array}{c}\frac{k_s}{T_{s1}{T}_{s2}}\\ 0\end{array}\right]u_y\\ y_y=C_y{x}_y=\left[\begin{array}{cc}0& 1\end{array}\right]\left[\begin{array}{c}{x}_{y1}\\ x_{y2}\end{array}\right]\end{array}\right.$

A in formula _y, B _y, C _y, x _y, y _ybe respectively the system matrix of crab angle model, gating matrix, output matrix, state variable, output variable, for x _yderivative; T _s1, T _s2be respectively the time constant of two inertial elements, k _sbe the product of the gain of two inertial elements, x _y1, x _y2for yaw rate and angle, u _yfor crab angle control inputs;

(1-2) respectively according to the corresponding Kalman filter of the modelling of above-mentioned foundation, estimate the state variable in attitude angle model, the state variable wherein in roll angle model/angle of pitch model comprises attitude angle, angular acceleration, angular velocity and observation acceleration; State variable in crab angle model comprises yaw rate and angle;

A, the Kalman filter set up according to roll angle model/angle of pitch model are:

$\widehat{\stackrel{\·}{x}}=\mathrm{Ax}+\mathrm{Bu}+K_z(y-\hat{y})$

In formula, represent the estimated value of state variable x, y and be respectively the output valve of system and the output valve of estimation, K _zfor the kalman gain of roll angle/angle of pitch model;

B, according to crab angle model set up Kalman filter be:

${\widehat{\stackrel{\·}{x}}}_y=A_y{x}_y+B_y{u}_y+K_{\mathrm{zy}}(y_y-{\hat{y}}_y)$

In formula, represent state variable x _yestimated value, y _ywith be respectively the output valve of system and the output valve of estimation, K _zyfor the kalman gain of crab angle model;

Step 2, design the controller of roll angle, the angle of pitch and crab angle respectively, wherein roll angle is identical with the controller of the angle of pitch, is specially:

(2-1) be respectively roll angle model, angle of pitch model, crab angle model arrange corresponding reference model

Wherein, the reference model of roll angle model, angle of pitch model is as follows:

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_m=A_m{x}_m+B_{m}r\\ y_m=C_m{x}_m\end{array}\right.$

In formula, r is the reference input of roll angle or the angle of pitch, A _m, B _m, C _mfor the state matrix of reference model, x _m, y _mfor state variable and the output of reference model;

The reference model of crab angle model is as follows:

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_{\mathrm{my}}=A_{\mathrm{my}}{x}_{\mathrm{my}}+B_{\mathrm{my}}{r}_y\\ y_{\mathrm{my}}=C_{\mathrm{my}}{x}_{\mathrm{my}}\end{array}\right.$

R in formula _yfor the reference input of crab angle, A _my, B _my, C _myfor the state matrix of reference model, x _my, y _myfor state variable and the output of reference model;

(2-2), utilize the deviation of the state variable in the state variable in reference model and attitude of flight vehicle angle model to design sliding mode controller;

Wherein, roll angle, pitch controller are:

u＝u _eq+u _nl＝u _eq+K _nlf(σ)

In formula, u is the control inputs that roll angle/pitch controller obtains, u _eqfor equivalent inpnt, u _nlfor the non-linear input of sliding mode controller, K _nlfor switching amplitude, f (σ) is switching function;

Crab angle controller is:

u _y＝u _eqy+u _nly＝u _eqy+K _nlyf _y(σ _y)

U in formula _yfor the control inputs that crab angle controller obtains, u _eqyfor equivalent inpnt, u _nlyfor the non-linear input of sliding mode controller, K _nlyfor switching amplitude, f _y(σ _y) be switching function;

Step 3, the total drive singal of rear output one is superposed to the control inputs of three controllers obtained in step 2 thus drive motor work to control attitude of flight vehicle.