1

Feedback Linearization and Luenberger Observer of a Quadrotor Unmanned aerial Vehicle Abdellah Mokhtari¤ ,N.K.M’Sirdi¤¤, A.Belaidi¤¤¤ ¤¤ Laboratoire de Robotique de Versailles 10-12 av de l’europe, 78140 Velizy , FRANCE. ¤ University of Science and Technology Oran Algérie. ¤¤¤ E.N.S.E.T Oran Algérie email: [email protected] Abstract — This paper presents a study of performances and characteristics of Luenberger Observer, combined with a classical polynomial controller, on the accurate mathematical representation of the plant. The results show that the Luenberger Observer is e¢cient in dealing with dynamic uncertainties, disturbances and sensor noise when these last are bounded. T he criterion for analysis is based on the observer tracking errors, at steady state and during transients, and the robustness of the performance with respect to the uncertainties of plant. A wind parameters estimation is added to the over all system to reinforce the robustness of the system. Simulation results are provided and trajectories output are analyzed. Keywords— Luenberger observer; exact liearization; estimation

I. INTRODUCTION An unmanned aerial vehicle quadrotor will be required to move in di¤erent environments showing good performance and a great autonomy under a variety of load conditions and unknown disturbances. Developing a control system that can achieve the aforementioned goal is challenging for a variety of reasons: ² The nonlinear behavior of a vehicle subjected to aerodynamic forces and moments. ² The multivariable character of a 6dof vehicle, which lead to an interacting system among di¤erent command channels. ² The consistent amount of uncertainty in both high and low frequencies, due to unknown disturbances introduced by linearization of the nonlinear dynamics. So the main di¢culties of the motion control for high performance positioning system are parametric uncertainties, unmodeled dynamics, and external disturbances. Since the original work by Luenberger [1], the use of state observers proves to be useful in not only system monitoring and regulation but also detecting as well as identifying failures in dynamic systems. Almost all observer designs are based on the mathematical model of the plant. However the presence of disturbances, dynamic uncertainties, and nonlinearities pose great challenges in practical applications. Furthermore, the available state information from measurements, i.e., sensor outputs, usually does not contain full state information and is most often corrupted by noise. This causes further complication in the design of robust controllers for actual systems[2].

There exist in the literature several observer structures based on di¤erent methods: linearization by coordinate transformation and the output injection [3] and variable structure approaches [4]. A sliding mode observer yielding insensitivity to unknown parameter variations and noise, has been proposed by Utkin [5]. Dorling and Zinober [6] compared the full and reduced order Luenberger observers with the Utkin observer. However, that the observer structures above need to include a plant model in their equations inevitably accompanies some practical burdens as follows: without a model, observers cannot be constructed; even if it is available, unless it is accurate enough, a reliable state reconstruction could not be expected; even when a model is accurate enough, the observers could often become too complicated (due to model complexity) to be of practical use, especially on real time basis. It is expected that the Luenberger observer will provide robust state reconstruction in the presence of uncertainties and disturbances since the nonlinear system (quadrotor) has been linearized through Lie derivatives. In this work a combined Luenberger observer; feedback linearization controller and a disturbance estimator is designed in an over all closed loop system to analyze the e¢ciency of such system when dealing with uncertainties; delay time and wind disturbances. II. QUADROTOR DYNAMICS The quadrotor helicopter is shown in 1. Two diagonal motors (1 and 3) are running in the same direction (anticlockwise) whereas the two others (2 and 4) in the clockwise direction to eliminate the anti-torque. On varying the rotor speeds altogether with the same quantity the lift forces will change a¤ecting in this case the altitude z of the system and enabling vertical take-o¤/on landing. Yaw angle is obtained by speeding up the clockwise motors or slowing down depending on the desired angle direction. Tilting around x (roll angle) axe allows the quadrotor to move toward y direction. The sense of direction depend on the sense of angle whether it is positive or negative. Tilting around y (pitch angle) axe allows the quadrotor to move toward x direction. The rotor is the primary source of control and propulsion for the UAV. The Euler angle orientation to the ‡ow provides the forces and moments to control

2

¹g1 (¹ x) = ¹g2 (¹ x) = ¹g3 (¹ x) = ¹g4 (¹ x) =

g71 g81 Fig. 1. Helicoptère quadrotor.

g91

the altitude and position of the system. The absolute position is described by three coordinates (x 0 ; y0 ; z0 );and its attitude by Euler angles (Ã ; µ; Á); under the conditions : (¡¼ · Ã < ¼) for yaw (¡ ¼2 < µ < ¼2 ) for pitch (¡ ¼2 < Á < ¼2 ) for roll Using Newton law the dynamic equations of the system can be represented as: X m V_0 = Fext (1) X J !_ = ¡! £ J ! + T ext (2) P P with F ext ; T ext, represent the external forces and moments. Referring to V. Misler et. al [7], the general MIMO nonlinear system is of the form:

¢

= f¹(¹ x) +

x¹

4 X

(3)

¹gi (¹ x)¹ ui

i=1

y

= h(¹ x ) = col(x 0 ; y 0 ; z0 ; Ã)

x¹ = col(x0 ; y 0 ; z0 ; Ã; µ; Á; u 0 ; v0 ; w0 ; ³ ; »; p; q; r) 2

6 6 6 6 6 6 6 6 6 6 6 6 f¹(¹ x) = 6 6 6 6 6 6 6 6 6 6 6 4

u0 v0 w0 q sin Á sec µ + r cos Á sec µ q cos Á ¡ r sin Á p + q sin Á tan µ + r cos Á tan µ Ax 7 m + g 1 (Ã; µ; Á)³ Ay 8 m + g1 (Ã; µ; Á)³ Az + g18 (Ã; µ; Á)³ m » 0 I y¡ Iz A qr + I xp Ix Aq I z¡I x I y pr + I y Ix ¡ Iy Iz

pq +

Ar Iz

3 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 5

col(0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 1; 0; 0; 0) d col(0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0) Ix d col(0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0) Iy 1 col(0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0 ) Iz

1 (cos Á cos à sin µ + sin Á sin Ã) m 1 = ¡ (cos Á sin µ sin à ¡ cos à sin Á) m 1 = ¡ (cos µ cos Á) m = ¡

The real control signals (u1 ; u 2 ; u 3 ; u 4) have been replaced by (¹ u1 ; u¹ 2 ; u¹3 ; u¹ 4 ) to avoid singularity in lie transformation matrices when using exact linearization . In that case u1 has been delayed by double integrator. The others control signals will keep unchanged. u1 ³_ »_

= ³

u2 u3

= u¹ 2 = u¹ 3

u4

= u¹ 4

= » = u¹ 1

The Input-Output linearization uses full state feedback to globally linearize the nonlinear dynamics of selected controlled outputs. Each of the output channels is di¤erentiated a su¢cient number of times until a control input component appears in the resulting equation. Using the Lie derivative , feedback linearization will transform the nonlinear system into a linear and non-interacting system which is known as Brunovsky form: d4 x 0 dt4 d4 y 0 dt4 d4z 0 dt4 d2Ã dt2

=

v1

=

v2

=

v3

=

v4

(4)

v 1 ; v2 ; v3 ; v 4 ; represent the new input control signals. Obviously the state vector need the primary state ( x 0 ; y 0 ; z0 ; Ã) and their successive derivatives to be compared with the desired state tra jectories when developing the control law. On adopting a classical polynomial control law, the closed loop system will be represented by the following equation:

3

v1 v2

(4)

(3)

(3)

= x d ¡ ¸ 3 (x 0 ¡ x d ) ¡ ¸ 2 (Ä x 0 ¡ xÄ d ) ¡¸ 1 (x_ 0 ¡ x_ d ) ¡ ¸ 0 (x 0 ¡ x d ) (4)

(3)

(3)

= y d ¡ ¸3 (y0 ¡ y d ) ¡ ¸2 (Äy0 ¡ yÄd ) ¡¸ 1 (y_0 ¡ y_ d ) ¡ ¸ 0 (y 0 ¡ yd )

v3 v4

For a vector z = [x1 ; x 2 ; x3 ; x 4 ; x5 ; x 6 ]; a well-known result from linear system theory is that, for a linear timeinvariant (LTI) system (6) with the dynamics

(4)

(3)

z(t) _ (5)

(3)

= x d ¡ ¸ 3 (z0 ¡ z d ) ¡ ¸ 2 (Äz0 ¡ zÄd ) ¡¸ 1 (z_0 ¡ z_d ) ¡ ¸0 (z 0 ¡ zd ) Ä d ¡ ¸5 ( Ã_ ¡ Ã_ d ) ¡ ¸4 (à ¡ à d ) = Ã

The choice of ¸ i is decisive in de…ning the dynamic of the system since the poles placement are based on.

=

Az(t) + Bv(t)

y(t) = 2 A1 6 04£ 4 A=6 4 04£ 4 02£ 4

Cz(t) + Dv(t)

0 4£4 04£ 4 A1 04£ 4 0 4£4 A1 0 2£4 02£ 4 2 3 0 3£4 6 B1 7 6 7 6 0 3£4 7 6 7 6 B2 7 6 7 B =6 7 6 0 3£4 7 6 B3 7 6 7 4 0 1£4 5 B2

III. Luenberger State Observer A reliable state estimate for a process is indispensable not only for control purpose, but also for other applications such as navigation of spacecraft, monitoring, and fault diagnosis in mechanical systems. However, since the mathematical model ( f ,g) is only an approximation to the physical process and the actual plant is usually a¤ected by external disturbances. Very often when dealing with state space realizations of dynamic systems in real time, it is necessary to manipulate the state vector x. In most of the case the compete measure will be costly or di¢cult to realize. In that case an observer may be used to obtain an estimate state which will replace the non measured one. For the practical control implementation the measured quantities are state variables x 0 ; y0 ; z0 and à which represent the translational motion and rotation around z axis respectively. It is true that non measurable signals can be obtained by successive differentiation ,unfortunately they will be contaminated by the measurement noise to such a degree that it can no longer be used. In fact, the accuracy of the state estimation largely depends on how well the physics of a real plant is copied to the estimator structure[8]. A. Observer Model

C=

£

C1

x1 x_ 1

= col(x0 ; y 0 ; z0 ) = x 2 = col(x_ 0 ; y_0 ; z_0 )

x_ 2

x_ 5

= x 3 = col(Ä x 0 ; yÄ0 ; zÄ0 ) ... ... ... = x 4 = col( x 0 ; y 0 ; z 0 ) 2 3 v1 = 4 v2 5 v3 = Ã = x 6 = Ã_

x_ 6

= v4

x_ 3 x_ 4 x5

C2

2

0 A1 = 4 0 0

2

0 4£ 3

B1

=

B2

=

B3

=

B4

=

£ £ £ £

C3

0 4£ 3

3 · 0 0 5 0 ; A2 0 1

1 0 0 1 0 0

3 2 1 6 0 7 6 7 6 C1 = 6 4 0 5 ; C1 = 4 0

1

0

0

0

0

1

0

0

0

0

1

0

0

0

0

1

3 2 0 6 1 7 7 ; C3 = 6 5 4 0 0

C4 1 0

04£1

¸

¤ ¤ ¤ ¤

3 2 0 6 0 7 7 ; C4 = 6 5 4 1 0

3 0 0 7 7 0 5 1

z^(t) = y^(t) =

A^z(t) + Bv(t) + L(y(t) ¡ y^(t)) ; z^(0) = z^0 C z^(t) + Dv(t)

can be designed by placing the poles of the observer at any desired location such that the error signals exhibit the desired dynamics [9]. ¢

z^(t) = (A ¡ LC ) z^(t) + (B ¡ LD)v(t) + Ly(t) (6)

¤

and an observable (A,C) pair, a stable linear Luenberger observer, which is given by ¢

The dynamic model of the quadrotor after linearization can be represented in state form where [x 1 ; x 5 ] is the measured state vector:

0 4£3

3 0 4£2 0 4£2 7 7 0 4£2 5 A2

The estimation error is: e = z ¡ z^

The error dynamic equation is:

^ e_ = (A ¡ LC )e = Ae

4

The estimation error will converge to zero if A^ = (A ¡ LC ) has all its eigenvalues in the left-half plane. The observer design refers to the selection of the gain matrix L, using, the pole placement method. The main challenge in these applications is that the observer design is heavily dependent on the accuracy of the mathematical model of the plant, in this case, the A, B, and C matrices. Therefore the rate convergence of z~(t) = z ¡ z^ to zero can be arbitrarily chosen by designing L appropriately. It follows that z^(t) ! z(t) exponentially fast as t ! 1 with a rate that depends on the matrix Â. This results is valid for any matrix A and any initial condition z0 as long as (C,A) is an observable pair and A,C are known .

deduced: 2 3 2 p 0 4 q 5=4 0 r 1

B. Output States Reconstruction

with x the measured state vector and its successive derivatives ,´ 1 and ´ 2 are the wind disturbances vector (AÄx Äy ,A Ä z ) and (Ap ; Aq ;Ar ) respectively. ,A

The Luenberger observer designed above is in fact a state estimator with partial state feedback (x 0 ; y0 ; z0 ; Ã) taken as a measured state .The observer make an estimation of the state needed by the control law to calculate the tracking error between the desired trajectories (x1d ; x2d ; x3d ; x4d ; x 5d ; x 6d ) and the estimated trajectories (x1 ; x 2 ; x3 ; x 4 ; x5 ; x 6 ): Unfortunately the estimated states does not involve all the output states. In that case, to complete the full state output , the missed variables (µ; Á; p; q; r) from x¹ vector(3) have been calculated through the estimated values and from the nonlinear system of equation3, without taking the perturbation into account. So: from (12) µ and Á can be deduced:

Áe µe

Ã

! ^ x ¡ cos( Ã)^ ^ y) ¡m(sin(Ã)^ = arcsin (7) u1 Ã ! ^ y) 1 ¡m(cos(Ã^ )^ x + sin( Ã)^ = arcsin cos Áe u1

The determination of (p; q; r) can³be found ´ from the _ _ _ transformation matrix [7] which need Ã; µ; Á :These lasts can be evaluated from (12) and the third derivatives ^ ^ ... ... ( x 0 ; y 0 ) i.e. :

8 9 ^ ... > > ^ + cos(Ã)cos(Á ^ > m x (sin(Á )sin(µ )sin( Ã) ))+ > 0 e > e e > < = ^ ... ^ ^ y m 0 (cos(Áe )sin(Ã) ¡ sin(Áe )cos(Ã)sin(µ e))+ > > > > ^ > > : ; _ 1 sin(Á )cos 2(µe ) ¡ sin(µ e )³ cos(Á ) Ãu e e _µ e = ¡ cos(µ e)cos2 (Áe )u1 (8) 8 9 ^ < ¡m... ^ 1 cos(Á )sin(µ e)+ = x 0 sin(Ã^ ) + Ãu e ^ ... : ; ^ ³ sin(Áe ) + mcos(Ã ) y 0 _Áe = (9) u1 cos(Áe) So from the following matrix equation (p; q; r) can be

3¡1 2 sin(Áe ) sec(µe ) cos(Áe ) sec(µe ) 5 6 cos(Áe ) ¡ sin(Áe ) 4 sin(Áe ) tan(µe ) cos(Áe ) tan(µe )

¢ 3 ^ Ã 7 µ_ e 5 Á_ e (10)

C. Wind Parameters Estimation In fact the …nal model computed by feedback linearization di¤ers from (4) in presence of perturbation since linearization will not be exact in that case. The system with perturbation is then represented as: (11)

x_ = Ax + B1 ´ 1 + B2 ´ 2

2

6 6 6 A=6 6 6 4

x = col(x 1 ; x2 ; x 3 ; x4;x 5 ; x6) 0 0 0 ¡¸ 0 0 0

I 0 0 ¡¸ 1 0 0

0 I 0 ¡¸ 2 0 0

0 0 I ¡¸ 3 0 0

0 0 0 0 0 ¡¸4

¸ i are the control gains. 2 3 2 3 0 0 6 0 7 6 0 7 6 7 6 7 6 0 7 6 0 7 6 7 6 7 B1 = 6 7 ; B 2 = 6 M2 7 6 M1 7 6 7 4 0 5 4 0 5 0 M3 M1 M3 a14 a24 a34 a45

2

3 2 0 a14 1 = 4 0 m 0 5 ; M 2 = 4 a24 1 0 0 m a34 £ ¤ 0 a a = 45 46 1 m

0

0 0 0 0 1 ¡¸ 5

a15 a25 a35

3 7 7 7 7 7 7 5

3 0 0 5 0

= (³ SÁCÃ Sµ ¡ ³ C ÁSÃ)=(mIx ); a15 = ¡(³C ÃCµ)=(mIy ) = (³ SÁSÃSµ + ³ CÁCÃ)=(mIx ); a25 = ¡(³ SÃCµ)=(mIy ) = (³ SÁCµ)=(mIx ); a35 = (³ Sµ)(mIy ) = SÁ=(Iy Cµ); a46 = CÁ=(Iz Cµ)

A problem of primary importance is the selection of the quadratic Lyapunov function. From equation (12), the Lyapunov function which assure the convergence is chosen as: 1 1 V = x~ T P x~ + ´~ T1 ¡1 ~´ 1 + ~´ T2 ¡1 ~´ 2 (12) 2 2 x~ represents the tracking errors between the estimated and desired values. On computing V_ and making V_ < 0 the adaptation law for tuning parameters is deduced: ¢

¢

T 1 T ~´ 1 = ¡2¡ ¡1 ~ ; ~´ 2 = ¡2¡¡ ~ 1 B1 P x 2 B2 P x

Finally the closed loop system with the combined controller-observer-estimator is presented in …g-2.

5

measured trajectory estimated trajectory desired trajectory 5

z pos(m)

0 -5 -10 Fig. 2. The over all closed loop system

-15 1 0.5

IV. Simulation and Results

0

The constant parameters of the quadrotor are: m g

= =

0

y pos(m) -0.5

x pos(m)

-1

2

2Kg; Ix = Iy = Iz = 1:241 6N=ras=s / ; d = 0:1m

-1

2

9:81m=s

Fig. 3. Trajectories behavior without disturbances

(¸ 0 ; ¸ 1 ; ¸ 2 ; ¸3 ); (¸4 ; ¸ 5 ) represent the coe¢cients of the polynomial (s + 10)4 and (s + 5)2 respectively . The reference tra jectory is chosen as:

y0d z0d Ãd

1

1 t cos( ) 2 2 1 t = sin( ) 2 2 t = ¡1 ¡ 10 ¼ = 3 =

¡5:5; ¡5:5; 5:5; ¡6:6; ¡6:6; ¡6:6; ¡7:7; ¡7:7; ¡7:7; ¡8:8; ¡8:8; ¡8:8; ¡4:95; ¡4:95

The estimation gains for wind disturbance are: £ ¤ ¡2 = 2:10 ¡4 2:10 ¡4 4

¡

Ap

Aq

An application has been established without and with disturbances and with uncertainties to see the performance and robustness of the sliding observer. A. Without disturbance Taking for this case (Ax = Ay = Az = 0); (Ap = Aq = Ar = 0); the following results are obtained(…g-3,4,5,6): B. With disturbances For the following aerodynamic disturbances (Moments) Ap = 0:8; Aq = 1; Ar = 0:8; the following results are obtained(…g-7,8):

0.4

0 0

5

10

15

20

25

30

time(sec)

¾ Ar

0.6

0.2

The desired poles for the closed loop system used for determination of L are: ½

measured yaw angle estimated yaw angle desired yaw angle

0.8

Yaw angle (°)

x 0d

P 14£1 =

1

Fig. 4. Yaw trajectory

¢

C. With uncertainties An uncertainties of 20% have been made on the mass m and inertial coe¢cients Ix ; Iy ; Iz to show the behavior of the observer-control law combination toward error modelling. So the following results are obtained (…g-9): It is noted from …gures obtained without perturbation, (…g-3, 4, 5,6),that the Luenberger mode observer gives satisfying results especially when poles placement are chosen judiciously. This can be shown from tracking error trajectories which vanished after a …nite time with a perfect convergency. When wind disturbances are introduced the results in …g-7 re‡ect the robustness of the mixed observercontroller, this can be con…rmed by the tracking error convergence. The estimation of wind parameters around x axis (Ap ), around y axis (Aq) and around z axis (Ar ) is presented in …g-8. It shows that the system dynamic behav-

1

x error y error z error psi error

0.5

Ap estimation

6

2 estimated value real value 0

Aq estimation

Tracking error

-2

0

0 5

20

Ar estimation

-1 0

5

10

15

80

100

0

0 1

20

40

60

80

100

estimated value real value

0 -1

20

time (sec)

60

estimated value real value

-5

-0.5

40

0

20

40

60

80

100

time(sec)

Fig. 5. Tracking error for x,y,z,Ã

Fig. 8. Wind parameters e stimation for Ap=0.8; Aq=0.5; Ar=1 v1 v2 v3 v4

3500 3000 2500

1

x error y error z error psi error

0.8 0.6 0.4

1500

Tracking errors

control signals

2000

1000 500 0 -500

0.2 0 -0.2 -0.4

-1000

-0.6

-1500

-0.8 2

4

6

8

10

12

time(sec)

-1 0

10

20

30

40

50

60

70

time(sec) Fig. 6. Control signals v1; v2; v3; v4 Fig. 9. Tracking errors for 20% incertainties on m; Ix ; Iy; Iz 1

x error y error z error psi error

Tracking error

0.5

0

-0.5

-1 0

5

10

15

20

ior is sensitive toward aerodynamic moments disturbances. However the estimation of translational wind forces given by Ax, Ay and Az is not represented since the observercontroller shown their e¢ciency to overcome these type of perturbation without need of estimation procedure. The observer robustness has been also tested by the introduction of uncertainties of 20% on the parameters m; Ix ; Iy ; Iz (…g-9). The convergence of output state vector is obtained even though the non-robustness exact linearization when uncertainties on system parameters occur. However attention must be paid on the choice of L to avoid noise ampli…cation around desired trajectories. V. CONCLUSION

time (sec) Fig. 7. Tracking error with wind disturbances

In this paper, a feedback linearization using Luenberger observer has been applied to a quadrotor unmanned aer-

7

ial vehicle. The observer has been used to rebuilt the non measured variables which are necessary to the control law. An adaptive estimator is added to the over all system to estimate the e¤ect of the external disturbances such as wind. The whole observer-estimator-control constitute an original suggestion of control system with minimum sensors used. The robustness study has been realized on simulation taking into account uncertainties, disturbances, with a corrupted measured state by noise. The results obtained show the convergence in …nite time of estimated values and a satisfying tracking errors of desired trajectories. they show also that the estimator added to the control reinforce the robustness and stability of the over all system. References [1] D. Luenberger, “Observers for multivariable systems”,IEEE Trans. Autom. Control, 11, 190-197, 1966. [2] Jong-Rae Kim, "Model-Error Control Synthesis: A New Approach to Robust Control", PhD of Texas A&M University August 2002 [3] A. J. Krener and A. Isidori, “Linearization by Output Injection and Nonlinear Observers,” Systems and Control Letters, vol. 3, pp. 47-52, 1983. [4] B. L. Walcott and S. H. Zak, “State Observation of Nonlinear Uncertain Dynamical Systems,”IEEE Transactions on Automatic Control, vol.32, pp. 166-170, 1987. [5] Utkin. V. I., Sliding Modes in Control and Optimization, Springer-Verlag, Berlin, 1992. [6] Dorling, C. M. and Zinober, A.S. I., "A comparaison study of the sensitivity of observers, Proceeding First IASTED Symposium on Applied Control and Identi…cation, Copenhagen, 6.32-6.37, 1983. [7] V. Mistler, A. Benallegue, N. K. M’Sirdi, ”Exac t Linearization and Noninteracting Control of a 4 Rotors Helicopter via Dynamic Feedback”, IEEE 10th IEEE International Workshop on RobotHuman Interactive Communication (September 18-21,2001 Bordeaux and Paris). [8] S .J. Kwon, W. K. Chung, "Combined Synthesis of State Estimator and Perturbation Observer", Journal of Dynamic Syste ms, Measurement, and Control. , Vol. 125 . ASME,March 2003. [9] T. Kailath, Linear Systems, Prentice-Hall, Englewood Cli¤s,NJ, 1980.