A Nonlinear Flight Controller Design for a UFO by Trajectory Linearization Method Part II Controller Design Xiaofei Wu*, Tony Adami, Jacob Campbell, Guan Jianwei and J. J. Zhu, School of EECS, Ohio University *Email:
[email protected] January 2002
2. Plant Model ABSTRACT In this paper, we present an attitude controller design for a 3DOF flight control experiment apparatus, the “Quanser UFO”, as an application of the multi-input, multi-output (MIMO) nonlinear tracking and decoupling control technique based on trajectory linearization method. The controller consists of two parts: a pseudo-inverse of the plant that computes the nominal control, and a linear time-varying regulator that stabilizes and decouples the tracking error dynamics. For regulating the attitude and the body rate tracking errors, inner and outer loop PI controllers are employed, which are designed by time-varying bandwidth PDeigenstructure assignment. The presented trajectory linearization method for tracking error stabilization and decoupling is proved to be an effective method to achieve the “good” flying qualities by simulation tests.
The rigid body rotational equations of motion of the airframe are given below which consists of the kinematics equations (outer loop) φ& = p + q sin(φ ) tan(θ ) + r cos(φ ) tan(θ ) θ& = q cos(φ ) − r sin(φ )
ψ& = q sin(φ ) sec(θ ) + r cos(φ ) sec(θ )
q 2 q q& = I qpp p 2 + I rr r + I qpr pr + g m Tm r r& = I rpq pq + I qr qr + g lr Tl + g nr Tn
(2.2) p r p , I qr , I qr , where the coefficients I pq
1. Introduction
p
This is Part II of a two-part paper. In this part a nonlinear flight controller design project using the “Quanser UFO” will be presented. The UFO is a helicopter model made by Quanser Consulting, Inc. It has 3 propellers driven by DC motors. They are mounted on a triangular frame, as shown in Figure 1.1. The UFO has 3 degrees-of-freedom (DOF), which are represented as the Euler pitch angle θ , roll angle φ , and yaw angle ψ , as shown in Figure 1.2. These angular motions have the following limitations: roll ±18 deg, pitch ±45 deg, and yaw unlimited. Differential thrust of the three propellers is used as actuators to effect attitude changes of the UFO. The attitude of the UFO is measured by three angular optical encoders on the pitch, roll and yaw axes.
q I rpq , g l , g np , g lr , g m , g nr
are
defined
by the mass
3. Pseudoinverse Design When designing a controller for a highly nonlinear plant using linearization methods, it is imperative to ensure that the linearized variables, usually the tracking errors, to be sufficiently small. This can be achieved by inverting the nonlinear dynamic plant to obtain the nominal control for the commanded trajectory. The ideal situation would occur if the plant was modeled exactly and was able to be inverted. This would allow an open loop controller to be developed as seen in figure 3.1 Y(s)
Plant Inverse 1/G(s)
Plant G(s)
Fig. 1.2 Figure 3.1 Ideal Plant Inverse
0-7803-7339-1/02/$17.00 2002 IEEE
q I qpp , I rr , I qpr ,
moments of inertia of the airframe I xx , I yy , I zz , I xz given in appendix. The moments of inertia were identified in Part I of this paper, along with the control effective derivatives, and the closed loop flying quality specifications [4].
R(s) Fig. 1.1
(2.1)
and the dynamic equations (inner loop) p p p& = I pq pq + I qr qr + g lp Tl + g np Tn
y(t)
However, such an inverse is not feasible in real world causal dynamic systems because the plant output depends on its past state, thus the plant inverse would have to know the future state in order to be able to generate the nominal plant Plant Inverse
gamma_
gamm
gamm
gamma_
gamm
omega_
ome
u (t ) + r(t)
Regulator
u~ (t )
- e(t)
gamma2o
Plant
+
u(t)
y(t)
Figure 3.2 Overall system
inputs. Thus, the inverse would be anti-causal. However, a pseudoinverse of the plant is practical. In developing the pseudoinverse for our UFO project, the equations of motion (2.1) and (2.2) are first inverted with respect to the & & derivatives of the nominal state variables φ ,θ ,ψ& and p& , q& , r& to obtain
( )
& p = φ − ψ& sin θ & q = θ cos φ + ψ& sin φ cos θ & r = −θ sin φ + ψ& cos φ cos θ
( ) ( )
(
( ) ( ) ( ) ( )
)
(
(
)
(
)
Tn = I zz r& + I yy − I xx q p + I xz q r − p&
)
) (3.1)
Where the over-bar indicates nominal values. Note that this inverse is exact. However, implementation of the nominal controls requires a pseudo-differentiator to estimate the derivative of a given function. A first-order pseudodifferentiator is represented by equation (3.2), where ω n, diff is the bandwidth of the low pass filter that attenuates high frequency gain, thereby making the pseudo-differentiator causal and realizable. Gdiff(s) =
s 1 s +1 ω n,diff
torqu
omega_n
torqu
omega omega_
gamm
Theta
omeg
omega2to
Psi
P
channal
Phi
q
r
ωn T1/2
3.30
6.90
3.46
3*3.30
3*6.90
3*3.46
0.21
0.12
0.20
0.21/3
0.12/3
0.20/3
Figure 3.3 Simulink implementation of pseudo-differentiat Table 3.1 parameters chose for pseudo-differentiator
4. Tracking error regulator control law design
Tl = I xx p& + I zz − I yy q r − I xz r& + q p Tm = I yy q& + (I xx − I zz ) pr + I xz p 2 − r 2
(
omega_n
(3.2)
Note that the fidelity of the pseudo-inverse is determined by ω n, diff , and is a tradeoff with the high frequency noise attenuation. In this design, there were six pseudo-differentiators used in the pseudoinverse of the plant, one for each Euler angle input, and one for each body rate input. The final design of the pseudoinverse can be seen in figure 3.3. The time constants were chosen to meet the desired flight quality time to half amplitude (t1/2) specifications. Table 3.1 shows the values chosen.
Based on the principle of trajectory linearization for nonlinear dynamic systems [3], we can design tracking error stabilizing controllers to deal with modeling uncertainties, disturbances and initial conditions. The overall system architecture is shown in Figure 3.2. Linear state-feedback proportional and integral controllers are employed and the architecture is applied to each channel in the inner and outer loop systems. First, for outer loop Euler angles control, augment the state variables with the integral of Euler angles and define the state variable vector and control input as
φ int θ int ψ ξ1 = int φ θ ψ ,
p µ1 =q r
where φint = ∫ φdt , θ int = ∫ θ dt , ψ int = ∫ ψ dt . So we have the augmented nonlinear state space model: ξ&1 = f1 (ξ1 , µ1 )
where φ&
int
=φ
θ&int = θ ψ& int = ψ φ& = p + q sin(φ ) tan(θ ) + r cos(φ ) tan(θ ) θ& = q cos(φ ) − r sin(φ )
(4.1)
ψ& = q sin(φ ) sec(θ ) + r cos(φ ) sec(θ ) Using the Matlab function ‘Jacobin’, we can derive the augmented linearized tracking error system: γ&aug = A1γ aug + B1u1 (4.2) where γaug is a vector containing the tracking error state variables, ∫ (φ − φcom )dt (θ − θ com )dt ∫ ∫ (ψ − ψ com )dt γ aug = φ − φcom θ − θ com ψ − ψ com
A1 =
O I ∂f1 = 3 3 ∂ξ1 ξ ,µ O3 A122 1 1
(4.3)
q 2 q q& = I qpp p 2 + I rr r + I qpr pr + g m Tm r r& = I rpq pq + I qr qr + glrTl + g nrTn
Using Matlab function ‘Jacobin’, we can derive the augmented linearized tracking error system: ω& aug = A2ω aug + B2u2 (4.6) where ωaug is the vector containing the tracking error state variables, ∫ ( p − pcom )dt (q − q )dt com ∫ ∫ (r − rcom )dt ω aug = p − pcom q − qcom r − rcom
where O3 denotes a 3×3 zero matrix, I 3 denotes a 3×3 Identity matrix and A122 is given by
A2 =
A122 =
0 0 0 = 0 0 0
( q cos(φ ) − r sin(φ )) tan(θ ) (q sin(φ ) + r cos(φ )) sec 2 (θ ) 0 − sin( ) − cos( ) 0 0 q φ r φ q cos(φ ) − r sin(φ ) sec(θ ) ( q sin(φ ) + r cos(φ )) sec(θ ) tan(θ ) 0
(
)
0 0 0 0 0 0 0 0 0 ∂f1 = B1 = (4.4) ∂µ1 ξ , µ 1 sin(φ ) tan(θ ) cos(φ ) tan(θ ) 1 1 0 cos(φ ) − sin(φ ) 0 sin(φ ) sec(θ ) cos(φ ) sec(θ ) Second, for inner-loop body rate control, augment the state variables with the integral of the body rates and define the state variable vector and control input as pint q int Tl rint ξ2 = , µ2 = Tm , p Tn q r where pint = ∫ pdt , qint = ∫ qdt , rint = ∫ rdt . So we have the augmented nonlinear state space model ξ&2 = f 2 (ξ 2 , µ 2 ) (4.5) where p& int = p
qint = q r&int = r p p p& = I pq pq + I qr qr + glpTl + g npTn
∂f2 ∂ξ2 ξ ,µ 2 2 0 0 0 0 0 0 0 0 0 0 0 0
0 1 0 0 0 1 p p p p I pqq I pqp + Iqrr Iqrq q 2I qpp p + I qprr 0 2Irr r + I qpr p r r I rpqq I rpqp + Iqr r Iqr q 1
0
0
(4.7)
0 0 0 0 0 0 0 0 0 ∂f B2 = 2 = p (4.8) 0 g np ∂µ 2 ξ , µ gl 2 2 0 gq 0 m r 0 g nr gl For outer loop Euler angle errors, design the state feedback control law: u1 = − K1γ aug (4.9) so that the closed-loop system matrix can be determined in de-coupled multi-variable phase-variable (MVPV) canonical form as 0 0 1 0 0 0 0 0 0 0 1 0 0 0 0 0 0 1 Acl1 = 0 0 0 −α112 −α111 0 0 0 0 −α121 0 −α122 0 0 −α131 0 −α132 0
(4.10)
where the coefficients α1 jk ( j = 1,2,3, k = 1,2) are obtained from the closed-loop characteristic equation with desired damping and bandwidth,
λ 2 + α1 j 2 λ + α1 j1 = 0,
j = 1,2,3 .
Considering equations (4.2, 4.3, 4.4, 4.9, 4.10), we can derive gain matrix: K1 = K I 1 K p1
[
K
I1
The PI controller can be realized by implementing the desired control gains in SIMULINK, shown in Figure 5.1
]
α 111 = 0 0
Kp1 =
5. Controller Implementation, Tuning and Testing
α112 −r q
− α 131 S θ α 131 S φ C θ α 131 C φ C θ
0
α 121 C φ − α 121 S φ
−α132Sθ α122Cφ + qSφ + rCφ SφSθ / Cθ α132SφCθ −α122Sφ + qSφ + rCφ Cφ Sθ / Cθ α132CφCθ qSφ + rCφ
( (
Figure 5.1 Outer loop controller
) )
The inner loop PI controller is realized with Simulink as shown in fig. 5.2.
(4.11) where Sφ = sin φ , Cφ = cos φ , Sθ = sin θ , Cθ = cos θ . For inner-loop body rate control, design the state feedback control law: u 2 = − K 2ω aug (4.12)
()
()
()
()
so that the closed-loop system matrix can be determined in de-coupled MVPV canonical form as 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 0 1 Acl2 = 0 −α212 0 0 −α211 0 0 −α 0 0 −α222 0 221 0 −α231 0 0 −α232 0
Figure5.2 Inner loop controller
where the proportional and integral controller gains are used in the Matlab functions.
(4.13)
where the coefficients α 2 jk ( j = 1,2,3, k = 1,2) are obtained from the closed-loop characteristic equation with desired damping and bandwidth,
λ 2 + α 2 j 2 λ + α 2 j1 = 0,
j = 1,2,3 .
Considering equations (4.6, 4.7, 4.8, 4.12, 4.13), we can derive gain matrix: K2 = K I 2 K p2
[
]
0 − Ixxα231 Ixxα211 KI 2 = 0 I yyα221 0 − Ixzα211 0 Izzα231 Kp2 = p p r r Ixx(I pq q +α212) − IxzI rpqq Ixx(I pq p + Iqrp r) − Ixz(I rpqp + Iqr r) IxxIqrp q − Ixz(Iqr q +α232) q q q I yy(2I ppp + I prr) I yyα222 I yy(2Irrr + I qpr p) − I (I p q +α ) + I I r q − I (I p p + I p r) − I (I r p + I r r) − I I p q − I (I r q +α ) 212 232 xz pq zz pq xx pq qr xz pq qr xx qr xz qr
Figure 5.3 overall control system realization
Figure 5.3 shows the Simulink realization of the overall controlled system. The commanded Euler angles are the inputs to the pseudoinverse. For this system, the tuning includes PI controller tuning and pseudoinverse tuning. The starting point is to calculate values for the controller gains and pseudoinverse bandwidths for which we believed would provide the desired system characteristics. In PI controller tuning, the final performance specifications were selected as shown in table 5.1: Channel
Roll
Pitch
Yaw
Time to half (sec)
0.21
0.1-0.12
0.6
Percent Overshoot
undefined
< 15%
