CN202854611U - Four-rotor aircraft attitude control system - Google Patents

Abstract

The utility model discloses a four-rotor aircraft attitude control system, which comprises a DC-DC circuit, a triaxial accelerometer, a magnetometer, a gyroscope, an analog-to-digital converter and a microprocessor, wherein the triaxial accelerometer, the magnetometer and the gyroscope are connected with the microprocessor through the analog-to-digital converter, and the triaxial accelerometer, the magnetometer and the gyroscope send detected analog signals, which are converted by the analog-to-digital converter, to the microprocessor to process and control. The four-rotor aircraft attitude control system disclosed by the utility model is simple, and low in cost.

Description

A kind of quadrotor attitude control system

Technical field

The utility model relates to a kind of quadrotor attitude control system, belongs to aircraft control technology field.

Background technology

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

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

3, the method that adopts image to process.The method can obtain comparatively accurately attitude information by the processing to image in conjunction with respective algorithms, but image deal with data amount is larger, and the Data Update frequency is slow, is difficult for satisfying the needs of aircraft.In addition, generally need image capture device and mark etc. are installed because image is processed, be subjected to the restriction of environment larger, can't extensively adopt.

At present, the method for designing of attitude controller is more, such as PID control, modern control, fuzzy control etc., and has obtained certain control effect.But the above control method mostly accuracy requirement to model is higher, and its control quality can obviously descend when model perturbs or is subject to external interference, has had a strong impact on the range of application of multi-rotor aerocraft.

The utility model content

The purpose of this utility model is to provide a kind of simple in structure, quadrotor attitude control system that cost is low.

The technical solution that realizes the utility model purpose 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 link to each other with microprocessor by analog to digital converter, and 3-axis acceleration device, magnetometer, gyroscope will detect simulating signal and send to microprocessor process and control after analog to digital conversion.

Prioritization scheme further, in the utility model quadrotor attitude control system, described gyrostatic quantity is three.

A kind of quadrotor attitude control method may further comprise the steps:

Step 1, obtain the attitude angle information of 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, thereby the controlled quentity controlled variable of three controllers obtaining in the step 2 is superposeed total driving signal drive motor work of rear output with the control attitude of flight vehicle.

Prioritization scheme in the described quadrotor attitude control method, obtains the attitude angle information of current flight device in the step 1 further, and is 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 the 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,

The derivative J moment of inertia of expression x, T is a time constant, and k is scale-up factor, and K is coefficient of air resistance, and m is vehicle mass, g is acceleration of gravity, state variable x ₁, x ₂, x ₃, x ₄Be respectively angular acceleration, acceleration, angle, sensor observation acceleration, u is control inputs;

The crab angle model is:

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{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}_{\mathrm{<figure-callout\; id="2"\; label="s"\; filenames="121026164428.png"\; state="\{\{state\}\}">s</figure-callout>}2}}\\ 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}_{\mathrm{<figure-callout\; id="2"\; label="s"\; filenames="121026164428.png"\; state="\{\{state\}\}">s</figure-callout>}2}}\\ 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 the formula _y, B _y, C _y, x _y, y _yBe respectively system matrix, gating matrix, output matrix, state variable, the output variable of crab angle model,

Derivative for xy.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 _Y2Be yaw rate and angle, u _yBe the 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 the attitude angle model, wherein the state variable in roll angle model/angle of pitch model comprises attitude angle, angular acceleration, angular velocity and observation acceleration; State variable in the crab angle model comprises yaw rate and angle;

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

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

In the formula,

The estimated value of expression state variable x, y and Be respectively the output valve of system and the output valve of estimation, K _zKalman gain for roll angle/angle of pitch model;

B, the Kalman filter of setting up according to the crab angle model are:

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

In the formula,

Expression state variable x _yEstimated value, y _yWith

Be respectively the output valve of system and the output valve of estimation, K _ZyKalman gain for the crab angle model;

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

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

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

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

R is reference input in the formula, A _m, B _m, C _mBe the state matrix of reference model, x _m, y _mState variable and output for reference model;

The crab angle reference model is as follows:

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{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 the formula _yBe the reference input of crab angle, A _My, B _My, C _MyBe the state matrix of reference model, x _My, y _MyState variable and output for reference model;

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

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

In the formula, u is the control inputs that roll angle/angle of pitch controller obtains, u _EqBe equivalence input, u _NlBe the non-linear input of sliding mode controller, K _NlFor switching amplitude, f (σ) is switching function;

The crab angle controller is:

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

U in the formula _yBe the control inputs that the crab angle controller obtains, u _EqyBe equivalence input, u _NlyBe the non-linear input of sliding mode controller, K _NlyFor switching amplitude, f _y(σ _y) be switching function.

The utility model compared with prior art, its remarkable advantage: 1) the utility model adopts 3-axis acceleration device, magnetometer and gyroscope to combine to control the attitude of aircraft, so that precision is higher; 2) the utility model system have advantages of simple in structure, cost is low, calculated amount is little, precision is high, containing much information of obtaining.

Description of drawings

Fig. 1 is the structured flowchart of the utility model quadrotor attitude control system;

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

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 utility model 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 link to each other with microprocessor by analog to digital converter, and 3-axis acceleration device, magnetometer, gyroscope will detect simulating signal and send to microprocessor process and control after analog to digital conversion.

1, DC-DC circuit: the DC-DC circuit is the power transfer module of DC-to-DC, and effect is that direct supply is converted into the DC voltage that needs.This example is selected 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 the voltage stabilizing error is in 4%.According to actual needs, adopt two LM2575 to obtain the voltage stabilizing output of 3.3v and 5v.

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

3. 3-axis acceleration device: acceleration is MAV attitude measurement and the important state amount of analyzing the MAV flying quality, and the ADXL335 of selection can be with the full range acceleration measurement of ± 3g.Select suitable bandwidth by the electric capacity on the regulation output pin.The X-axis of the accelerometer that adopts 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 the 0.1uf filter capacitor.

4. magnetometer: selecting first magnetometer of Freescale semiconductor MAG3110, is a small-sized low-power consumption, digital 3 axle magnetometers, has broad dynamic range, can move 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 be adjusted in the sampling interval from 12ms to the several seconds.

5. analog to digital converter: analog to digital converter has used the MCP3204 of 12 bit resolutions, and band SPI serial line interface is 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 will be with the frequency output 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; Lift-over, pitching, crab angle.Leave the GPS input interface, the ADC input pin leaves expansion interface, conveniently increases according to actual needs corresponding sensor.

6. microprocessor: adopt microprocessor that sensing data is processed, infer attitude angle and carry out attitude control, controlled frequency is 400Hz.Selection has the AT91sam7 than high performance-price ratio, and it is Atmel32 position ARM risc processor, and with the high speed Flash of 256k, processing speed can satisfy the needs that attitude is inferred and attitude is controlled.

A kind of control method of quadrotor attitude control system may further comprise the steps:

Step 1, obtain the attitude angle information of 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

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

Derive and approximate linearization process by theory, but the attitude angle of aircraft and the relation approximate representation between the torque are:

$\mathrm{\&tau;}=J\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&phi;}}---\left(1\right)$

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

The steering order signal of now supposing aircraft is first order inertial loop to the torque that obtains, then can controlled command signal to the model of attitude angle.

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

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

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

The observation acceleration a of accelerometer _mComprise moving 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 with speed, then has:

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

K is coefficient of air resistance in the formula.

Following formula is done Laplace transform, obtains the transport function that attitude angle arrives moving acceleration:

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

Thereby obtain roll angle/angle of pitch to the system block diagram between the acceleration that records, as shown in Figure 2, Angular Velocity is input angular velocity, and Roll is the attitude angle that obtains, and g is acceleration of gravity, and Acc is quiet acceleration, a _dBe moving acceleration, a _mBe sensor observation 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{\&CenterDot;}{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 the 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,

Be the derivative of x, J moment of inertia, T are 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, sensor observation acceleration, u is control inputs;

B, set up the crab angle model

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

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{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}\\ {\mathrm{<figure-callout\; id="2"\; label="x\; y"\; filenames="121026164428.png"\; state="\{\{state\}\}">x</figure-callout>}}_y\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}\\ {\mathrm{<figure-callout\; id="2"\; label="x\; y"\; filenames="121026164428.png"\; state="\{\{state\}\}">x</figure-callout>}}_y\end{array}\right]\end{array}\right.$

A in the formula _y, B _y, C _y, x _y, y _yBe respectively system matrix, gating matrix, output matrix, state variable, the output variable of crab angle model,

Be 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 _Y2Be yaw rate and angle, u _yBe the 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 the attitude angle model, wherein the state variable in roll angle model/angle of pitch model comprises attitude angle, angular acceleration, angular velocity and observation acceleration; State variable in the crab angle model comprises yaw rate and angle;

A, the Kalman filter of setting up according to roll angle model, angle of pitch model are expressed as

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

In the formula

The estimated value of expression state variable x, A, B are system matrix and the input matrix of system state equation in the formula (6), y and

Be respectively the output valve of system and the output valve of estimation, K _zBe kalman gain, can be expressed as

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

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

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

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

B, the Kalman filter of setting up according to the crab angle model are:

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

In the formula,

Expression state variable x _yEstimated value, y _yWith

Be respectively the output valve of system and the output valve of estimation, K _ZyKalman gain for the crab angle model; The design of the Kalman filter of crab angle model is identical with the roll angle method, just kalman gain K _ZyTo find the solution be according to crab angle model and its corresponding weighting matrices Q _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 corresponding reference model is set

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

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

R is reference input in the formula, A _m, B _m, C _mBe the state matrix of reference model, x _m, y _mState variable and output for reference model;

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

$\stackrel{\&CenterDot;}{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 satisfies following condition:

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

Then formula (10) can be expressed as:

$\stackrel{\&CenterDot;}{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）

Can tracking target value r for the output that makes reference model, needing the straight-through gain of Adjustment System is 1.When time during approximates infinity, the state variable convergence of system is stable, 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）

Thereby can obtain K ₂Value

${\mathrm{<figure-callout\; id="2"\; label="K"\; filenames="121026164428.png"\; state="\{\{state\}\}">K</figure-callout>}}_2={(-C_m{A}_m^{-1}B)}^{-1}---\left(17\right)$

B then _mCan be obtained by following formula

$B_m={\mathrm{<figure-callout\; id="2"\; label="BK"\; filenames="121026164428.png"\; state="\{\{state\}\}">BK</figure-callout>}}_2=B{(-C_m{A}_m^{-1}B)}^{-1}---\left(18\right)$

A _mValue can regulate by emulation experiment and obtain.

B, crab angle reference model are as follows:

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{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 the formula _yBe the reference input of crab angle, A _My, B _My, C _MyBe the state matrix of reference model, x _My, y _MyState variable and output for reference model; The method for solving of crab angle model and the method for solving of roll angle/angle of pitch are similar, only need roll angle in the solution procedure/matrix A of angle of pitch model, B, C are replaced with A _y, B _y, C _yCan obtain A _My, B _My, C _My

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

Because sliding mode controller (SMC) disturbs to external world and modeling error has preferably robustness, therefore adopt the realization of sliding mode controller design method to the tracking of reference model state variable.Be the accurate tracking of realize target value in SMC control, introduce integral element to improve the tracking performance of system, we introduce a new state variable ε for this reason _yBe output bias e _yIntegration, namely

${\stackrel{\&CenterDot;}{\mathrm{\&epsiv;}}}_y=y-y_m---\left(19\right)$

Then the bias system of expansion is

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

U in the formula _s=-(K ₁X+K ₂R-u)

Definition switching function σ ∈ R is

σ=Se _s （21）

$\stackrel{\&CenterDot;}{\mathrm{\&sigma;}}=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 equivalence input u _EqCan obtain u by formula (23) _Eq=-(SB _s) ^-1SA _se _s+ K ₁X+K ₂R (23)

With u _EqBringing formula (19) into obtains

${\stackrel{\&CenterDot;}{e}}_s=\{I-B_s{\left({\mathrm{SB}}_s\right)}^{-1}S\}{A}_s{e}_s---\left(24\right)$

Can get system by following formula and have stable zero point.

For selecting diverter surface, introduce method for optimally controlling, namely selecting Optimal Feedback gain F is diverter surface S.P is the solution of Riccati equation in the formula, and S satisfies SB _s0.

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

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

The non-linear input of SMC

u _nl=K _nlf(σ) （27）

K in the formula Chinese style _NlFor switching amplitude, f (σ) is switching function, common selection function sign, but sign function can produce very large shake in actual applications, impact control quality.We adopt a smooth function

$f\left(\mathrm{\&sigma;}\right)=\frac{\mathrm{\&sigma;}}{\left|\mathrm{\&sigma;}\right|+\mathrm{\&delta;}}---\left(28\right)$

δ is the smooth function weight in the formula.

Roll angle, angle of pitch controller are expressed as:

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

In the formula, u is the controlled quentity controlled variable that controller obtains, u _EqBe equivalence input, u _NlBe the non-linear input of sliding mode controller, K _NlFor switching amplitude, f (σ) is switching function;

Also corresponding reducing of corresponding nonlinear Control amount when selecting suitable δ to make state variable near diverter surface, thus establishment thrashing.As shown in Figure 4, SMC is sliding mode controller, and Kalman is Kalman filter, and Real System is the 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 then the integration of the output angle y of aircraft and reference model output angle difference export control inputs u as the state variable of sliding mode controller SMC, thereby finish the attitude control of aircraft.

The crab angle controller is:

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

U in the formula _yBe the control inputs that the crab angle controller obtains, u _EqyBe equivalence input, u _NlyBe the non-linear input of sliding mode controller, K _NlyFor switching amplitude, f _y(σ _y) be switching function.

Step 3, thereby the controlled quentity controlled variable of three controllers obtaining in the step 2 is superposeed total driving signal drive motor work of rear output with the control attitude of flight vehicle.

Claims (2)

1. quadrotor attitude control system, it is characterized in that: comprise DC-DC circuit, 3-axis acceleration device, magnetometer, gyroscope, analog to digital converter and microprocessor, described 3-axis acceleration device, magnetometer, gyroscope link to each other with microprocessor by analog to digital converter, and 3-axis acceleration device, magnetometer, gyroscope send to microprocessor with the simulating signal that detects and process and control after analog to digital conversion.

2. quadrotor attitude control system according to claim 1, it is characterized in that: described gyrostatic quantity is three.

Images