CN106933104A - A kind of quadrotor attitude based on DIC PID and the mixing control method of position - Google Patents

Abstract

The invention discloses a kind of quadrotor attitude based on DIC PID and the mixing control method of position, comprise the following steps：Step 1：By quadrotor as single rigid body, its non-linear dynamic model is obtained by newton euler equations, establish the input/output relation of system；Step 2：Gesture stability is carried out based on the theoretical inner looping for quadrotor of DIC dynamic inversion controls, inner ring attitude controller is formed；Step 3：Position control is carried out based on the theoretical external loops for quadrotor of PID, outer shroud positioner is formed, the uneoupled control of attitude and position passage is realized；Step 4：Under simulated environment, the control method proposed by step response and tracking elevating screw X -ray inspection X.Mixing control method of the present invention is disturbed to external world has preferable rejection ability with Unmarried pregnancy, disclosure satisfy that the demand of quadrotor gesture stability and position control.

Description

A kind of quadrotor attitude based on DIC-PID is controlled with the mixing of position Method

Technical field

It is more particularly to a kind of to be based on DIC- the present invention relates to quadrotor attitude and the uneoupled control field of position The quadrotor attitude of PID and the mixing control method of position.

Background technology

With the development of aeronautical technology, sensor technology and the communication technology, unmanned plane is received because of its huge application prospect To the extensive concern of people.In all types of unmanned planes, quadrotor mechanical structure is simple, it is easy to dismount, and has VTOL, the function of spot hover, therefore it is also the object that domestic and foreign scholars are keen to research.However, four rotors are one Non-linear, close coupling under-actuated systems, its kinetics equation and be unsatisfactory for additivity with it is homogeneity, therefore to realize it in high precision Attitude it is quite big with position control difficulty.

Regarding to the issue above, scholars both domestic and external have carried out a series of correlations and have ground to the control strategy of quadrotor Study carefully, including Backstepping control, Active Disturbance Rejection Control, adaptive sliding-mode observer etc..Recent studies indicate that being based on dynamic inversion control (Dynamic inversion control, abbreviation DIC) is come to design the controller of four rotors be a kind of effective way.The base of DIC Present principles are to eliminate the non-linear of system by constructing the global state feedback of system, original system is changed into new linear system System.For example, being controlled using the inner-outer loop that DIC theories have separately designed four rotors, the uneoupled control of attitude and position is realized.

But, this simple DIC control strategies can cause control effect not because of external disturbance with modeling uncertainty It is good.Accordingly, it is considered to the method that various control strategy is combined is subject to favoring for scholars.For example：

With quantitative feedback theory camera and the robust controller of four rotor stance loops is devised by DIC is theoretical, and in gas The validity of controller is demonstrated in the case of dynamic Parameter Perturbation.

Introduced in system circuit the method that is combined with DIC of model reference adaptive devise the rotors of PX4 tetra- attitude and Position control is restrained, and simulation result shows that the controller can comparatively fast adapt to external disturbance and change with load.

The content of the invention

In order to overcome deficiency of the prior art, the present invention to provide a kind of quadrotor attitude based on DIC-PID With the mixing control method of position, the control method is disturbed to external world has preferable rejection ability, Neng Gouman with Unmarried pregnancy Sufficient quadrotor gesture stability and the demand of position control.

In order to reach foregoing invention purpose, the technical scheme that its technical problem used is solved as follows：

A kind of quadrotor attitude based on DIC-PID and the mixing control method of position, comprise the following steps：

Step 1：By quadrotor as single rigid body, its nonlinear kinetics mould is obtained by Newton-Euller method Type, establishes the input/output relation of system；

Step 2：Gesture stability is carried out based on the theoretical inner looping for quadrotor of DIC dynamic inversion controls, is formed Inner ring attitude controller；

Step 3：Position control is carried out based on the theoretical external loops for quadrotor of PID, outer ring position control is formed Device processed, realizes the uneoupled control of attitude and position passage；

Step 4：Under simulated environment, the control method proposed by step response and tracking elevating screw X -ray inspection X.

Further, specifically include in step 1：

In body axis system O_BX_BY_BZ_BUnder, the lift F=[F that quadrotor is produced₁ F₂ F₃ F₄]^TPositive O_BZ_BSide To；

Work as F₁=F₂=F₃=F₄, aircraft does VTOL or hovering is moved；

Work as F₂=F₄And F₁≠F₃, aircraft does elevating movement；

Work as F₁=F₃And F ≠ F₄, aircraft does roll motion；

Work as F₁=F₃≠F₂=F₄When, aircraft does yawing rotation；

Based on above-mentioned driving principle, the expression-form of four rotor control inputs is can obtain：

Wherein, L is the distance of each blade normal direction line of rotor centers line, U=[U₁,U₂,U₃,U₄]^TIt is pitching, roll, driftage With always away from input signal, M=[M₁,M₂,M₃,M₄]^TIt is the torque of each rotor；

According to Newton-Euller method, the kinetics equation of quadrotor can be derived：

Wherein, m is four rotor gross masses,It is earth coordinates O_IX_IY_IZ_ITo the transition matrix of body axis system, g is Acceleration of gravity, F_DIt is air drag, I=[I_xx I_yy I_zz]^TIt is rotary inertia, q=[φ θ ψ]^TIt is attitude matrix, D is appearance To the mapping relations of angular speed, S () is revolving victor matrix, and φ, θ and ψ are respectively roll angle, the angle of pitch and yaw angle at state angle.

Further, if not considering the influence of air drag, under low-speed operations or floating state, there are φ ≈ 0, θ ≈ 0, In addition, I can be obtained by four rotor structure symmetry_xx≈I_yy, simultaneously as yaw angle need not be controlled frequently, haveTherefore, Formula (2) can be approximately：

Further, specifically include in step 2：

Set nonlinear system as：

In order to track desired trajectory r (t), defining tracking error is：

E (t)=r (t)-y (t) (5)

If carrying out differential to system output in formula (4), have：

When G (x) can the inverse time, define dynamic inversion control device be：

Wherein, v is the controlled quentity controlled variable of inverse dynamic model, and by formula (6) substitution formula (5), can obtain error dynamics system is：

System pole is set to be located at origin using linear control technique configuration v, order：

V=Ke (9)

Wherein, K is matrix of adjusting,

Therefore, the complete dynamic inversion control device of system is：

Further, in step 2, the controlled volume of inner looping is three-axis attitude angle,

The output vector of inner looping is y_1c=[φ θ ψ]^T,

Inverse dynamic control amount is：

Therefore, the control law of inner looping is：

Further, in step 3, the controlled volume of external loop is three shaft positions and linear velocity, and formula (3) gives four rotations The wing relative to earth coordinates linear acceleration, location variable y_1w=[x y z]^TCan be by expecting that input signal is obtained with attitude angle , the linear acceleration in external loop is：

Use PID control technology can obtain linear acceleration for：

Wherein, K_P、K_IAnd K_DRespectively proportionality coefficient, integral coefficient and differential coefficient；e_wIt is site error；e_vIt is linear velocity Error.

The present invention is allowed to compared with prior art, have the following advantages that and actively imitate due to using above technical scheme Really：

(1) present invention devises the controller of quadrotor attitude and position using DIC-PID control strategies, realizes Its model is uncertain and external disturbance under robust control.

(2) compared to the four-rotor aircraft control system based on PID, control system control effect proposed by the present invention is more Good, robustness is stronger, and stability is more excellent, can solve the problem that the problem that four rotors are easily influenceed by Unmarried pregnancy and external disturbance.

(3) uneoupled control of quadrotor attitude and position is realized, emulation testing demonstrates this with flight experiment The validity of the designed control strategy of invention.

Brief description of the drawings

Technical scheme in order to illustrate more clearly the embodiments of the present invention, below will be to that will use needed for embodiment description Accompanying drawing be briefly described.It is clear that drawings in the following description are only some embodiments of the present invention, for ability For field technique personnel, on the premise of not paying creative work, other accompanying drawings can also be obtained according to these accompanying drawings.It is attached In figure：

Fig. 1 is the stream of a kind of quadrotor attitude based on DIC-PID of the invention and the mixing control method of position Journey schematic diagram；

Fig. 2 is quadrotor force diagram in the present invention；

Fig. 3 is the control system figure of quadrotor in the present invention；

Fig. 4 is the inner looping control system figure of quadrotor in the present invention；

Fig. 5 is position control trajectory diagram of the quadrotor under gust disturbances in the present invention；

Fig. 6 is the three-dimensional pursuit path figure of quadrotor in the present invention；

Fig. 7 is the change schematic diagram of quadrotor attitude angle in the present invention；

Fig. 8 is quadrotor experiment porch flight test design sketch in the present invention.

Specific embodiment

Below with reference to accompanying drawing of the invention, clear, complete description is carried out to the technical scheme in the embodiment of the present invention And discussion, it is clear that as described herein is only a part of example of the invention, is not whole examples, based on the present invention In embodiment, the every other implementation that those of ordinary skill in the art are obtained on the premise of creative work is not made Example, belongs to protection scope of the present invention.

As shown in figure 1, the invention discloses the mixing control of a kind of quadrotor attitude based on DIC-PID and position Method processed, comprises the following steps：

Step 1：By quadrotor as single rigid body, its nonlinear kinetics mould is obtained by Newton-Euller method Type, establishes the input/output relation of system；

Step 2：Gesture stability is carried out based on the theoretical inner looping for quadrotor of DIC dynamic inversion controls, is formed Inner ring attitude controller；

Step 3：Position control is carried out based on the theoretical external loops for quadrotor of PID, outer ring position control is formed Device processed, realizes the uneoupled control of attitude and position passage；

Step 4：Under simulated environment, the control method proposed by step response and tracking elevating screw X -ray inspection X.

Generally quadrotor is modeled as six degree of freedom rigid body in the world.As shown in Fig. 2 it is along from bodily movement of practising Wushu To the free movement in three dimensions in the presence of lift, then specifically include in step 1：

In body axis system O_BX_BY_BZ_BUnder, the lift F=[F that quadrotor is produced₁ F₂ F₃ F₄]^TPositive O_BZ_BSide To；

Work as F₁=F₂=F₃=F₄, aircraft does VTOL or hovering is moved；

Work as F₂=F₄And F₁≠F₃, aircraft does elevating movement；

Work as F₁=F₃And F ≠ F₄, aircraft does roll motion；

Work as F₁=F₃≠F₂=F₄When, aircraft does yawing rotation；

Based on above-mentioned driving principle, the expression-form of four rotor control inputs is can obtain：

Wherein, L is the distance of each blade normal direction line of rotor centers line, U=[U₁,U₂,U₃,U₄]^TIt is pitching, roll, driftage With always away from input signal, M=[M₁,M₂,M₃,M₄]^TIt is the torque of each rotor；

According to Newton-Euller method, the kinetics equation of quadrotor can be derived：

Wherein, m is four rotor gross masses,It is earth coordinates O_IX_IY_IZ_ITo the transition matrix of body axis system, g is Acceleration of gravity, F_DIt is air drag, I=[I_xx I_yy I_zz]^TIt is rotary inertia, q=[φ θ ψ]^TIt is attitude matrix, D is appearance To the mapping relations of angular speed, S () is revolving victor matrix, and φ, θ and ψ are respectively roll angle, the angle of pitch and yaw angle at state angle.

Further, if not considering the influence of air drag, under low-speed operations or floating state, there are φ ≈ 0, θ ≈ 0, In addition, I can be obtained by four rotor structure symmetry_xx≈I_yy, simultaneously as yaw angle need not be controlled frequently, haveTherefore, Formula (2) can be approximately：

Further, specifically include in step 2：

Set nonlinear system as：

In order to track desired trajectory r (t), defining tracking error is：

E (t)=r (t)-y (t) (5)

If carrying out differential to system output in formula (4), have：

When G (x) can the inverse time, define dynamic inversion control device be：

Wherein, v is the controlled quentity controlled variable of inverse dynamic model, and by formula (6) substitution formula (5), can obtain error dynamics system is：

System pole is set to be located at origin using linear control technique configuration v, order：

V=Ke (9)

Wherein, K is matrix of adjusting,

Therefore, the complete dynamic inversion control device of system is：

According to time scale separation principle, the controller of quadrotor is designed successively from inside to outside, as shown in Figure 3. Wherein, inner looping carries out gesture stability, and external loop carries out position control.The design of inner looping is for the appearance of four rotors of calming State, controlled volume is three-axis attitude angle.As shown in figure 4, in step 2, the controlled volume of inner looping is three-axis attitude angle,

The output vector of inner looping is y_1c=[φ θ ψ]^T,

Inverse dynamic control amount is：

Therefore, the control law of inner looping is：

Further, the design of external loop is for the position of four rotors of calming, in step 3, the controlled volume of external loop It is three shaft positions and linear velocity, formula (3) gives linear acceleration of four rotors relative to earth coordinates, location variable y_1w= [x y z]^TCan be by expecting that input signal is obtained with attitude angle, the linear acceleration in external loop is：

Use PID control technology can obtain linear acceleration for：

Wherein, K_P、K_IAnd K_DRespectively proportionality coefficient, integral coefficient and differential coefficient；e_wIt is site error；e_vIt is linear velocity Error.

In order to verify the validity of put forward mixing control method, four rotors are flown under the simulated environment of Matlab2012b The control performance of row device is verified that the parameter of selected quadrotor is as shown in table 1 in emulation：

Parameter	L(m)	m(kg)	Ixx(kg.m2)	Iyy(kg.m2)	Izz(kg.m2)
						Numerical value	0.250	0.612	0.0045	0.0045	0.0053

The physical parameter of the quadrotor of table 1

Case 1：Given is (1.5m, 1m, 0.5m) with reference to step signal, and mixing control proposed by the present invention is respectively adopted Method goes to track this signal with PID control strategy of the prior art.For more preferable simulation practical flight, introduced in input Amplitude is 0.05 meter of gust disturbances signal, and simulation result is as shown in Figure 5.It can be seen that two kinds of controllers are presented Go out big overshoot.Comparatively, the response speed of DIC-PID controllers is signifi-cantly more rapidly than PID controller.Meanwhile, DIC-PID Stabilization time be less than 5s, and steady-state error is not more than 2%.This explanation proposes control and calculates herein in the case of gust disturbances Method can still provide metastable response.

Case 2：Given three-dimensional reference locus are an elevating screw line, and equation of locus is as follows：

To verify the antijamming capability of DIC-PID, an external interference of 0.1m/s2, whole emulation are introduced in simulations Continue 30s, as a result as shown in Figure 6.It can be seen that in the starting stage, reference locus exist larger with simulation track Error, needs to indicate that the error is caused by four rotor tracing deviations here.But say on the whole, the response of DIC-PID controllers Speed quickly, can quickly and accurately track reference locus.In addition, Fig. 7 gives the change of quadrotor attitude angle Situation, inner ring tends to stabilization during 4.3s.Meanwhile, Fig. 7 also gives four control signals of passage.

Flight experiment is set up on the basis of upper one section test environment, and the quadrotor in experiment is by repacking, dress It is loaded with GPS, flies control, damping block, number biography etc..

Four rotors are set along such track flight：Starting point-A-B-C-D-E- landing points.First determine under simulated environment DIC-PID controller parameters, then control algolithm is converted into by C code by the code transformation function of Matlabde is written to winged control In carry out verification experimental verification.Here it may be noted that the equipment for lacking measurement noise interference due to laboratory, therefore add in simulations different Noise signal simulate actual disturbance, finally select one group of optimal or suboptimum controller parameter.

Fig. 8 gives the result of flight experiment, it can be seen that according to the controller quadrotor base of present invention design Originally desired trajectory can be tracked, is capable of achieving to control effectively four rotor attitudes with position.

The above, the only present invention preferably specific embodiment, but protection scope of the present invention is not limited thereto, Any one skilled in the art the invention discloses technical scope in, the change or replacement that can be readily occurred in, Should all be included within the scope of the present invention.Therefore, protection scope of the present invention should be with scope of the claims It is defined.

Claims (6)

1. the mixing control method of a kind of quadrotor attitude based on DIC-PID and position, it is characterised in that including with Lower step：

Step 1：By quadrotor as single rigid body, its non-linear dynamic model is obtained by Newton-Euller method, really The input/output relation of erection system；

Step 2：Gesture stability is carried out based on the theoretical inner looping for quadrotor of DIC dynamic inversion controls, inner ring is formed Attitude controller；

Step 3：Position control is carried out based on the theoretical external loops for quadrotor of PID, outer shroud positioner is formed, Realize the uneoupled control of attitude and position passage；

Step 4：Under simulated environment, the control method proposed by step response and tracking elevating screw X -ray inspection X.

2. the mixing controlling party of a kind of quadrotor attitude based on DIC-PID according to claim 1 and position Method, it is characterised in that specifically include in step 1：

In body axis system O_BX_BY_BZ_BUnder, the lift F=[F that quadrotor is produced₁ F₂ F₃ F₄]^TPositive O_BZ_BDirection；

Work as F₁=F₂=F₃=F₄, aircraft does VTOL or hovering is moved；

Work as F₂=F₄And F₁≠F₃, aircraft does elevating movement；

Work as F₁=F₃And F ≠ F₄, aircraft does roll motion；

Work as F₁=F₃≠F₂=F₄When, aircraft does yawing rotation；

Based on above-mentioned driving principle, the expression-form of four rotor control inputs is can obtain：

$\left\{\begin{array}{c}{U}_1=F_1+F_2+F_3+F_4\\ U_2=(F_2-F_4)L\\ U_3=(F_1-F_3)L\\ U_4=M_2+M_4-M_1-M_3\end{array}\right.---\left(1\right)$

Wherein, L is the distance of each blade normal direction line of rotor centers line, U=[U₁,U₂,U₃,U₄]^TFor pitching, roll, driftage with it is total Away from input signal, M=[M₁,M₂,M₃,M₄]^TIt is the torque of each rotor；

According to Newton-Euller method, the kinetics equation of quadrotor can be derived：

$\left\{\begin{array}{c}m\stackrel{\·\·}{r}=R_I^B\left(U_1{Z}_B\right)-{\mathrm{mgZ}}_I+F_D\\ I\stackrel{\·\·}{q}={\left[\begin{array}{ccc}{U}_2& U_3& U_4\end{array}\right]}^T-S\left(\stackrel{\·}{D}q\right)I\stackrel{\·}{D}q\end{array}\right.---\left(2\right)$

Wherein, m is four rotor gross masses,It is earth coordinates O_IX_IY_IZ_ITo the transition matrix of body axis system, g adds for gravity Speed, F_DIt is air drag, I=[I_xx I_yy I_zz]^TIt is rotary inertia, q=[φ θ ψ]^TIt is attitude matrix, D is arrived for attitude angle The mapping relations of angular speed, S () is revolving victor matrix, and φ, θ and ψ are respectively roll angle, the angle of pitch and yaw angle.

3. the mixing controlling party of a kind of quadrotor attitude based on DIC-PID according to claim 2 and position Method, it is characterised in that if not considering the influence of air drag, under low-speed operations or floating state, has φ ≈ 0, θ ≈ 0, separately Outward, I can be obtained by four rotor structure symmetry_xx≈I_yy, simultaneously as yaw angle need not be controlled frequently, haveTherefore, it is public Formula (2) can be approximately：

$\begin{array}{c}\stackrel{\·\·}{x}=\frac{U_1}{m}(\mathrm{\θ}\cos \mathrm{\ψ}+\mathrm{\φ}\sin \mathrm{\ψ}),\stackrel{\·\·}{y}=\frac{U_1}{m}(\mathrm{\θ}\sin \mathrm{\ψ}-\mathrm{\φ}\cos \mathrm{\ψ})\\ \stackrel{\·\·}{z}=\frac{U_1}{m}-g,\stackrel{\·\·}{\mathrm{\φ}}=\frac{U_2}{I_{xx}},\stackrel{\·\·}{\mathrm{\θ}}=\frac{U_3}{I_{yy}},\stackrel{\·\·}{\mathrm{\ψ}}=\frac{U_4}{I_{zz}}\end{array}---\left(3\right).$

4. the mixing controlling party of a kind of quadrotor attitude based on DIC-PID according to claim 3 and position Method, it is characterised in that specifically include in step 2：

Set nonlinear system as：

$\left\{\begin{array}{c}\stackrel{\·}{x}=f\left(x\right)+g\left(x\right)u\\ y=h\left(x\right)\end{array}\right.---\left(4\right)$

In order to track desired trajectory r (t), defining tracking error is：

E (t)=r (t)-y (t) (5)

If carrying out differential to system output in formula (4), have：

$\stackrel{\·}{y}=\frac{\∂h}{\∂x}\stackrel{\·}{x}=\frac{\∂h}{\∂x}f\left(x\right)+\frac{\∂h}{\∂x}g\left(x\right)u=F\left(x\right)+G\left(x\right)u---\left(6\right)$

When G (x) can the inverse time, define dynamic inversion control device be：

$u=G{\left(x\right)}^{-1}(-F(x)+\stackrel{\·}{r}+v)---\left(7\right)$

Wherein, v is the controlled quentity controlled variable of inverse dynamic model, and by formula (6) substitution formula (5), can obtain error dynamics system is：

$\stackrel{\·}{e}=-v---\left(8\right)$

System pole is set to be located at origin using linear control technique configuration v, order：

V=Ke (9)

Wherein, K is matrix of adjusting,

Therefore, the complete dynamic inversion control device of system is：

$u=G{\left(x\right)}^{-1}(-F(x)+\stackrel{\·}{r}+Ke)---\left(10\right).$

5. the mixing controlling party of a kind of quadrotor attitude based on DIC-PID according to claim 3 and position Method, it is characterised in that in step 2, the controlled volume of inner looping is three-axis attitude angle,

The output vector of inner looping is y_1c=[φ θ ψ]^T,

Inverse dynamic control amount is：

$v_1=K_1{e}_1=\left[\begin{array}{c}{K}_{\mathrm{\φ}}({\mathrm{\φ}}_c-\mathrm{\φ})\\ K_{\mathrm{\θ}}({\mathrm{\θ}}_c-\mathrm{\θ})\\ K_{\mathrm{\ψ}}({\mathrm{\ψ}}_c-\mathrm{\ψ})\end{array}\right]---\left(11\right)$

Therefore, the control law of inner looping is：

$\begin{array}{c}{u}_1=G{\left(x\right)}^{-1}(-F(x)+\stackrel{\·}{r}+Ke)\\ =\left[\begin{array}{ccc}1& \sin \mathrm{\φ}\tan \mathrm{\θ}& cos\mathrm{\φ}tan\mathrm{\θ}\\ 0& \cos \mathrm{\φ}& -sin\mathrm{\φ}\\ 0& \sin \mathrm{\φ}/\cos \mathrm{\θ}& cos\mathrm{\φ}/\cos \mathrm{\θ}\end{array}\right]\left[\begin{array}{c}{K}_{\mathrm{\φ}}({\mathrm{\φ}}_c-\mathrm{\φ})\\ K_{\mathrm{\θ}}({\mathrm{\θ}}_c-\mathrm{\θ})\\ K_{\mathrm{\ψ}}({\mathrm{\ψ}}_c-\mathrm{\ψ})\end{array}\right]\end{array}---\left(12\right).$

6. the mixing controlling party of a kind of quadrotor attitude based on DIC-PID according to claim 3 and position Method, it is characterised in that in step 3, the controlled volume of external loop is three shaft positions and linear velocity, and formula (3) gives four rotors Relative to the linear acceleration of earth coordinates, location variable y_1w=[x y z]^TCan be by expecting that input signal is obtained with attitude angle , the linear acceleration in external loop is：

$a={\left[\begin{array}{ccc}\stackrel{\·\·}{x}& \stackrel{\·\·}{y}& \stackrel{\·\·}{z}\end{array}\right]}^T---\left(13\right)$

Use PID control technology can obtain linear acceleration for：

A=K_Pe_w+K_I∫e_w+K_De_v (14)

Wherein, K_P、K_IAnd K_DRespectively proportionality coefficient, integral coefficient and differential coefficient；e_wIt is site error；e_vFor linear velocity is missed Difference.