CN105700537A - Unmanned plane attitude control filtering unit and method - Google Patents

Abstract

The invention discloses an unmanned plane attitude control filtering unit and method, and relates to a design for an anti-interference filtering method for unmanned plane attitude control. The invention aims at multi-source interference of an unmanned plane, such as atmosphere turbulence force moment, execution mechanism noise, measurement noise, and model nondeterminacy. The method comprises the steps: firstly building an unmanned plane attitude mathematic model comprising the multi-source interference; secondly building a multi-source interference moment mathematic model; thirdly designing an anti-interference filter for the unmanned plane attitude; finally designing an optimal controller, and carrying out feedback control. The method is strong in anti-interference capability, is high in control precision, and can be used for unmanned plane attitude control.

Description

UAV Attitude controls filter unit and method

Technical field

The present invention relates to a kind of UAV Attitude and control filter unit and method, can be applicable in the UAV Attitude control containing multi-source interference such as atmospheric turbulance moment, actuator noise, measurement noise and model uncertainties。

Background technology

Miniature self-service gyroplane has the characteristic such as VTOL, hovering, it is possible to performs task at narrow spaces such as drives, is with a wide range of applications。Along with the expansion of application, the intelligence degree demand of miniature self-service gyroplane also increases day by day, and the miniature self-service gyroplane of intelligence autonomous, high becomes the focus of research entirely。As complicated multi-input multi-output control system, miniature self-service gyroplane has close coupling, non-linear, control difficulty characteristic, and Dynamic Modeling is complicated。High-precision model is the basis that high accuracy controls further, and the target that UAV Attitude controls is to make the body coordinate system of unmanned plane follow the tracks of navigation channel coordinate system, consequently facilitating unmanned plane carries out the tasks of science being correlated with。

UAV Attitude controls the interference that faces and includes: the external disturbance moments such as atmospheric turbulance moment, and model uncertainty item, actuator noise and measurement noise etc. are from internal interference；And multi-source interference is very big on the impact of UAV Attitude, it is impossible to reach the requirement that its high-precision attitude controls。In existing UAV Attitude anti-interference filtration method, there is external disturbance moment excessively simply and the problem such as the ability BOUNDED DISTURBANCES not considering model uncertainty item。

Summary of the invention

The technology of the present invention solves problem: overcomes and existing there is, for UAV Attitude control method, the problem that multi-source disturb that do not take into full account, a kind of UAV Attitude control filter unit of design and method。

The technical solution of the present invention is: a kind of UAV Attitude controls filter unit and method, and including filter unit and anti-interference filtration method thereof, described filter unit includes UAV Attitude control mathematical model, anti-interference filter and optimal controller；First described anti-interference filtration method mainly comprises the steps that, sets up the UAV Attitude mathematical model containing multi-source interference；Secondly, the mathematical model of disturbance torque is set up；Again, set up the mathematical model of the UAV Attitude passage containing multi-source interference of augmentation, afterwards, design UAV Attitude anti-interference filter；Finally, design optimal controller, carry out feedback control；Specifically comprise the following steps that

(1) the UAV Attitude mathematical model containing multi-source interference is set up

$\stackrel{\·}{x}\left(t\right)=Mx\left(t\right)+N\[u\left(t\right)+w\left(t\right)+d\left(t\right)\]$

Y (t)=Hx (t)+v (t)

Wherein, x (t) is the state variable of the UAV Attitude mathematical model containing multi-source interference, and y (t) is the output variable of the UAV Attitude mathematical model containing multi-source interference, coefficient matrix

$M=\left[\begin{array}{cc}{0}_{3\×3}& I_{3\×3}\\ -J_0^{-1}D& -J_0^{-1}C\end{array}\right],$ $D=\left[\begin{array}{ccc}(J_{0y}-J_{0z}){\mathrm{\ω}}_0^2& 0& 0\\ 0& 0& 0\\ 0& 0& (J_{0v}-J_{0x}){\mathrm{\ω}}_0^2\end{array}\right],$

$C=\left[\begin{array}{ccc}0& 0& (J_{0y}-J_{0z}-J_{0x}){\mathrm{\ω}}_0\\ 0& 0& 0\\ -(J_{0y}-J_{0z}-J_{0x}){\mathrm{\ω}}_0& 0& 0\end{array}\right],$ $N=\left[\begin{array}{c}{0}_{3\×3}\\ J_0^{-1}\end{array}\right],$ H=[I_3×30_3×3], the nominal rotational inertia matrix J of unmanned plane₀=diag{J_0x, J_0y, J_0z, ω₀Actuator noise and measurement noise, the I of unmanned plane is represented respectively for angular velocity, the u (t) control moment suffered by unmanned plane, the d (t) disturbance torque suffered by unmanned plane, w (t) and v (t)_3×3Unit matrix for 3*3。

(2) mathematical model of disturbance torque is set up

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\ψ}}\left(t\right)=P\mathrm{\ψ}\left(t\right)+\mathrm{\ξ}\left(t\right)\\ d\left(t\right)=Q\mathrm{\ψ}\left(t\right)\end{array}\right.$

Wherein, the state variable of the mathematical model that ψ (t) is disturbance torque, P and Q is coefficient matrix, and ξ (t) is model uncertainty item；

(3) mathematical model of the UAV Attitude passage containing multi-source interference of augmentation is set up

$\left\{\begin{array}{c}\stackrel{\·}{\stackrel{\‾}{x}}\left(t\right)=\stackrel{\‾}{M}\stackrel{\‾}{x}\left(t\right)+\stackrel{\‾}{N}\[u\left(t\right)+w\left(t\right)+d\left(t\right)\]+\stackrel{\‾}{G}\mathrm{\ξ}\left(t\right)\\ \stackrel{\‾}{y}\left(t\right)=\stackrel{\‾}{H}\stackrel{\‾}{x}\left(t\right)+v\left(t\right)\end{array}\right.$

Wherein, $\stackrel{\‾}{x}\left(t\right)=\left[\begin{array}{c}x\left(t\right)\\ \mathrm{\ψ}\left(t\right)\end{array}\right],$ $\stackrel{\‾}{y}\left(t\right)=y\left(t\right),$ Coefficient matrix $\stackrel{\‾}{M}=\left[\begin{array}{cc}M& NQ\\ 0_{6\×6}& P\end{array}\right],$ $\stackrel{\‾}{N}=\left[\begin{array}{c}N\\ 0_{6\×6}\end{array}\right],$

$\stackrel{\‾}{G}=\left[\begin{array}{c}{0}_{6\×6}\\ I_{6\×6}\end{array}\right],$ $\stackrel{\‾}{H}=\left[\begin{array}{cc}H& 0_{3\×6}\end{array}\right];$

(4) anti-interference filter that design UAV Attitude controls:

$\left\{\begin{array}{c}\stackrel{\·}{\widehat{\stackrel{\‾}{x}}}\left(t\right)=\stackrel{\‾}{M}\widehat{\stackrel{\‾}{x}}\left(t\right)+\stackrel{\‾}{N}u\left(t\right)+K\[\stackrel{\‾}{y}\left(t\right)-\widehat{\stackrel{\‾}{y}}\left(t\right)\]\\ \widehat{\stackrel{\‾}{y}}\left(t\right)=\stackrel{\‾}{H}\widehat{\stackrel{\‾}{x}}\left(t\right)\end{array}\right.$

Wherein,ForEstimated value,ForEstimated value, K is the gain matrix of the anti-interference filter for UAV Attitude passage；

(5) design optimal controller, carries out feedback control:

$u\left(t\right)=L\hat{x}\left(t\right)-\widehat{\mathrm{\ψ}}\left(t\right)$

Wherein, L is the gain matrix of optimal controller,The estimated value of mathematical model state variable x (t) of UAV Attitude passage for disturbing containing multi-source,Estimated value for mathematical model state variable ψ (t) of external disturbance moment。

Present invention advantage compared with prior art is in that: (1) present invention has taken into full account that the multi-sources such as the atmospheric turbulance moment, actuator noise, measurement noise and the model uncertainty item that contain in UAV Attitude disturb, and establishes the UAV Attitude mathematical model containing multi-source interference；Overcome the limitation that existing model does not take into full account that multi-source disturbs；

(2) present invention devises UAV Attitude anti-interference filter and optimal controller, it is possible to effective compensation is from atmospheric turbulance moment and suppresses the internal interference from actuator noise, measurement noise and model uncertainty item；Overcome the problem that existing method does not take into full account that multi-source disturbs, improve the overall performance of system。

Accompanying drawing explanation

The UAV Attitude that Fig. 1 is the present invention controls filter unit and Method And Principle figure。

The UAV Attitude that Fig. 2 is the present invention controls anti-interference filtration method flow diagram。

Detailed description of the invention

As it is shown in figure 1, be that the UAV Attitude of the present invention controls filter unit and Method And Principle figure, filter unit of the present invention includes UAV Attitude control mathematical model, anti-interference filter and optimal controller；Anti-interference filtration method of the present invention initially sets up the UAV Attitude mathematical model considering multiple disturbance torque with noise, output it the input as designed anti-interference filter, in UAV Attitude mathematical model, the estimated value of the mathematical model state variable of the estimated value of state variable and external disturbance moment sends into designed optimal controller, and the output of optimal controller is opened and controlled to carry out feedback control in the mathematical model of loop to anti-interference filter and UAV Attitude。

As in figure 2 it is shown, the UAV Attitude of the present invention of the present invention controls anti-interference filtration method flow diagram, comprise the following steps:

(1) the UAV Attitude mathematical model containing multi-source interference is set up

The control target of UAV Attitude is that the body coordinate system making unmanned plane follows the tracks of navigation channel coordinate system, and the kinetics equation based on this UAV Attitude can be expressed as:

$J_0\stackrel{\·}{\mathrm{\ω}}\left(t\right)=-\mathrm{\ω}{\left(t\right)}^{\×}{J}_0\mathrm{\ω}\left(t\right)+u\left(t\right)+d\left(t\right)$

Wherein, J₀For the nominal inertia matrix of unmanned plane, ω (t)=[ω_sc]^scFor the expression under the body coordinate system of unmanned plane of the unmanned plane absolute angular velocities, θ₀、ψ₀It is the body coordinate system Eulerian angles relative to navigation channel coordinate system of unmanned plane respectively, ω₀For navigation channel angular velocity, ω^xT () is corresponding multiplication cross matrix, the u (t) control moment suffered by unmanned plane, the d (t) external disturbance moment suffered by unmanned plane；

So UAV Attitude kinetics equation can be expressed as:

$\stackrel{\·}{\mathrm{\ω}}\left(t\right)=J_0^{-1}\[-\mathrm{\ω}{\left(t\right)}^{\×}{J}_0\mathrm{\ω}\left(t\right)+u\left(t\right)+d\left(t\right)\]$

Considering further that unmanned plane actuator noise and measurement noise, UAV Attitude mathematical model can be expressed as:

$\stackrel{\·}{x}\left(t\right)=Mx\left(t\right)+N\[u\left(t\right)+w\left(t\right)+d\left(t\right)\]$

Y (t)=Hx (t)+v (t)

Wherein, x (t) is the state of flight variable (such as three axis angular rates) of the UAV Attitude mathematical model containing multi-source interference, y (t) is the output variable (including the angle of pitch of unmanned plane, yaw angle and roll angle) of the UAV Attitude mathematical model containing multi-source interference, coefficient matrix $M=\left[\begin{array}{cc}{0}_{3\×3}& I_{3\×3}\\ {-J}_0^{-1}D& {-J}_0^{-1}C\end{array}\right],$ $D=\left[\begin{array}{ccc}(J_{0y}-J_{0z}){\mathrm{\ω}}_0^2& 0& 0\\ 0& 0& 0\\ 0& 0& (J_{0y}-J_{0x}){\mathrm{\ω}}_0^2\end{array}\right],$ $C=\left[\begin{array}{ccc}0& 0& (J_{0y}-J_{0z}-J_{0x}){\mathrm{\ω}}_0\\ 0& 0& 0\\ -(J_{0y}-J_{0z}-J_{0x}){\mathrm{\ω}}_0& 0& 0\end{array}\right],$ H=[I_3×30_3×3], the nominal rotational inertia matrix J of unmanned plane₀=diag{J_0x, J_0y, J_0z, w (t) and v (t) represents actuator noise and measurement noise, the I of unmanned plane respectively_3×3Unit matrix for 3*3。

(2) mathematical model of external disturbance moment is set up

Owing to unmanned plane operates in default navigation channel, so UAV Attitude is subject to the impact of the external disturbance moments such as atmospheric turbulance moment, outside disturbance torque can be modeled and is described as the form of state-space model:

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\ψ}}\left(t\right)=P\mathrm{\ψ}\left(t\right)+\mathrm{\ξ}\left(t\right)\\ d\left(t\right)=Q\mathrm{\ψ}\left(t\right)\end{array}\right.$

Wherein, the state variable (as atmospheric turbulance moment, air drag moment, atmospheric pressure act on the moment that body produces) of the mathematical model that ψ (t) is disturbance torque, P and Q is coefficient matrix, and ξ (t) is model uncertainty item；

(3) the UAV Attitude mathematical model containing multi-source interference of augmentation is set up

By the UAV Attitude mathematical model disturbed containing multi-source and the mathematical model simultaneous of external disturbance moment, obtain:

$\left\{\begin{array}{c}\stackrel{\·}{\stackrel{\‾}{x}}\left(t\right)=\stackrel{\‾}{M}\stackrel{\‾}{x}\left(t\right)+\stackrel{\‾}{N}\[u\left(t\right)+w\left(t\right)+d\left(t\right)\]+\stackrel{\‾}{G}\mathrm{\ξ}\left(t\right)\\ \stackrel{\‾}{y}\left(t\right)=\stackrel{\‾}{H}\stackrel{\‾}{x}\left(t\right)+v\left(t\right)\end{array}\right.$

Wherein, $\stackrel{\‾}{x}\left(t\right)=\left[\begin{array}{c}x\left(t\right)\\ \mathrm{\ψ}\left(t\right)\end{array}\right],$ $\stackrel{\‾}{y}\left(t\right)=y\left(t\right),$ Coefficient matrix $\stackrel{\‾}{M}=\left[\begin{array}{cc}M& NQ\\ 0_{6\×6}& P\end{array}\right],$ $\stackrel{\‾}{N}=\left[\begin{array}{c}N\\ 0_{6\×6}\end{array}\right],$ $\stackrel{\‾}{G}=\left[\begin{array}{c}{0}_{6\×6}\\ I_{6\×6}\end{array}\right],$ $\stackrel{\‾}{H}=\left[\begin{array}{cc}H& 0_{3\×6}\end{array}\right];$

(4) anti-interference filter that design controls for UAV Attitude:

$\left\{\begin{array}{c}\stackrel{\·}{\widehat{\stackrel{\‾}{x}}}\left(t\right)=\stackrel{\‾}{M}\widehat{\stackrel{\‾}{x}}\left(t\right)+\stackrel{\‾}{N}u\left(t\right)+K\[\stackrel{\‾}{y}\left(t\right)-\widehat{\stackrel{\‾}{y}}\left(t\right)\]\\ \widehat{\stackrel{\‾}{y}}\left(t\right)=\stackrel{\‾}{H}\widehat{\stackrel{\‾}{x}}\left(t\right)\end{array}\right.$

Wherein,ForEstimated value,ForEstimated value, K is the gain matrix of anti-interference filter, and K can be solved by the method for POLE PLACEMENT USING；

(5) design optimal controller, carries out feedback control:

$u\left(t\right)=L\hat{x}\left(t\right)-\widehat{\mathrm{\ψ}}\left(t\right)$

Wherein,The estimated value of UAV Attitude mathematical model state variable x (t) for disturbing containing multi-source,For the estimated value of mathematical model state variable ψ (t) of external disturbance moment, L is the gain matrix of optimal controller,V is symmetric positive definite matrix, i.e. V=V^T> 0, unknown matrix S are following Li Kadi (Riccati) non trivial solution:

$S\stackrel{\‾}{M}+{\stackrel{\‾}{M}}^{T}S-S\stackrel{\‾}{N}{V}^{-1}\stackrel{\‾}{N}S+W=0$

Wherein, W is symmetric positive semidefinite matrix, i.e. W=W^T≥0。

The content not being described in detail in description of the present invention belongs to the known prior art of professional and technical personnel in the field。

Claims (1)

1. a UAV Attitude controls filter unit and method, it is characterised in that: including filter unit and anti-interference filtration method thereof, described filter unit includes UAV Attitude control mathematical model, anti-interference filter and optimal controller；First described anti-interference filtration method mainly comprises the steps that, sets up the UAV Attitude mathematical model containing multi-source interference；Secondly, the mathematical model of disturbance torque is set up；Again, set up the mathematical model of the UAV Attitude passage containing multi-source interference of augmentation, afterwards, design UAV Attitude anti-interference filter；Finally, design optimal controller, carry out feedback control；Specifically comprise the following steps that

(1) the UAV Attitude mathematical model containing multi-source interference is set up

$\stackrel{\·}{x}\left(t\right)=Mx\left(t\right)+N\[u\left(t\right)+w\left(t\right)+d\left(t\right)\]$

Y (t)=Hx (t)+v (t)

Wherein, x (t) is the state variable of the UAV Attitude mathematical model containing multi-source interference, and y (t) is the output variable of the UAV Attitude mathematical model containing multi-source interference, coefficient matrix $M=\left[\begin{array}{cc}{0}_{3\×3}& I_{3\×3}\\ -J_0^{-1}D& -J_0^{-1}C\end{array}\right],$ $D=\left[\begin{array}{ccc}(J_{0y}-J_{0z}){\mathrm{\ω}}_0^2& 0& 0\\ 0& 0& 0\\ 0& 0& (J_{0y}-J_{0x}){\mathrm{\ω}}_0^2\end{array}\right],$ $C=\left[\begin{array}{ccc}0& 0& (J_{0y}-J_{0z}-J_{0x}){\mathrm{\ω}}_0\\ 0& 0& 0\\ -(J_{0y}-J_{0z}-J_{0x}){\mathrm{\ω}}_0& 0& 0\end{array}\right],$ $N=\left[\begin{array}{c}{0}_{3\×3}\\ J_0^{-1}\end{array}\right],$ H=[I_3×30_3×3], the nominal rotational inertia matrix J of unmanned plane₀=diag{J_0x, J_0y, J_0z, ω₀Actuator noise and measurement noise, the I of unmanned plane is represented respectively for angular velocity, the u (t) control moment suffered by unmanned plane, the d (t) disturbance torque suffered by unmanned plane, w (t) and v (t)_3×3Unit matrix for 3*3。

(2) mathematical model of disturbance torque is set up

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\ψ}}\left(t\right)=P\mathrm{\ψ}\left(t\right)+\mathrm{\ξ}\left(t\right)\\ d\left(t\right)=Q\mathrm{\ψ}\left(t\right)\end{array}\right.$

Wherein, the state variable of the mathematical model that ψ (t) is disturbance torque, P and Q is coefficient matrix, and ξ (t) is model uncertainty item。

(3) mathematical model of the UAV Attitude passage containing multi-source interference of augmentation is set up

$\left\{\begin{array}{c}\stackrel{\·}{\stackrel{\‾}{x}}\left(t\right)=\stackrel{\‾}{M}\stackrel{\‾}{x}\left(t\right)+\stackrel{\‾}{N}\[u\left(t\right)+w\left(t\right)+d\left(t\right)\]+\stackrel{\‾}{G}\mathrm{\ξ}\left(t\right)\\ \stackrel{\‾}{y}\left(t\right)=\stackrel{\‾}{H}\stackrel{\‾}{x}\left(t\right)+v\left(t\right)\end{array}\right.$

Wherein, $\stackrel{\‾}{x}\left(t\right)=\left[\begin{array}{c}x\left(t\right)\\ \mathrm{\ψ}\left(t\right)\end{array}\right],$ $\stackrel{\‾}{y}\left(t\right)=y\left(t\right),$ Coefficient matrix $\stackrel{\‾}{M}=\left[\begin{array}{cc}M& NQ\\ 0_{6\×6}& P\end{array}\right],$ $\stackrel{\‾}{N}=\left[\begin{array}{c}N\\ 0_{6\×6}\end{array}\right],$ $\stackrel{\‾}{G}=\left[\begin{array}{c}{0}_{6\×6}\\ I_{6\×6}\end{array}\right],$ $\stackrel{\‾}{H}=\[\begin{array}{cc}H& 0_{3\×6}\end{array}\].$

(4) anti-interference filter that design UAV Attitude controls:

$\left\{\begin{array}{c}\stackrel{\·}{\widehat{\stackrel{\‾}{x}}}\left(t\right)=\stackrel{\‾}{M}\widehat{\stackrel{\‾}{x}}\left(t\right)+\stackrel{\‾}{N}u\left(t\right)+K\[\stackrel{\‾}{y}\left(t\right)-\widehat{\stackrel{\‾}{y}}\left(t\right)\]\\ \widehat{\stackrel{\‾}{y}}\left(t\right)=\stackrel{\‾}{H}\widehat{\stackrel{\‾}{x}}\left(t\right)\end{array}\right.$

Wherein,ForEstimated value,ForEstimated value, K is the gain matrix of the anti-interference filter for UAV Attitude passage。

(5) design optimal controller, carries out feedback control:

$u\left(t\right)=L\hat{x}\left(t\right)-\widehat{\mathrm{\ψ}}\left(t\right)$

Wherein, L is the gain matrix of optimal controller,The estimated value of mathematical model state variable x (t) of UAV Attitude passage for disturbing containing multi-source,Estimated value for mathematical model state variable ψ (t) of external disturbance moment。