[ieee iecon 2006 - 32nd annual conference on ieee industrial electronics - paris, france...

6
A Coupled Nonlinear Discrete-Time Controller and Observer Designs for Under- actuated Autonomous vehicles with Application to a Quadrotor Aerial Robot M’hammed Guisser Hicham Medromi Hassan Ifassiouen Janah Saadi Nour-eddine radhy Equipe Architecture des Systèmes Equipe Architecture des Systèmes Equipe Architecture des Systèmes Equipe Automatique et Productique Laboratoire d'automatique et d'informatique Université Hassan II Ain Chock Université Hassan II Ain Chock Université Hassan II Ain Chock Université Hassan II Ain Chock Université Hassan II Ain Chock ENSEM, BP. 8118, Oasis Casablanca, MOROCOO ENSEM, BP. 8118, Oasis Casablanca, MOROCOO ESTC, B.P. 8012 Oasis Casablanca, MOROCOO ENSEM, BP. 8118, Oasis Casablanca, MOROCOO BP 5366 - Maârif Casablanca, MOROCOO [email protected] [email protected] [email protected] [email protected] [email protected] Abstract—This paper addresses the problem of a combined discrete-time nonlinear tracking controller-observer design procedure for a class of underactuated autonomous vehicles with configuration (position and orientation) measurements and modelled via Lagrangian approach; we show the asymptotic stability of the closed loop-dynamics using Lyapunov analysis to guarantee the desired objectives of the tracking controller coupled with the observer (software sensor). The combined observer-control design techniques are applied to an autonomous quadrotor aerial robot. Simulation results are also provided to show the effectiveness of the proposed techniques. I. INTRODUCTION The control of underactuated vehicles (systems with a smaller number of control inputs than the number of independent generalized coordinates) is an active subject of research in automatic control and robotic. The study of these systems is motivated by the fact that it is usually costly and often not even practical to fully actuate autonomous vehicles due to weight, reliability, complexity and efficiency considerations. Typical examples of underactuated systems include robotic manipulators, wheeled robots, walking robots, spacecraft, aircraft, helicopters, missiles, surface vessels and underwater vehicles. The problem of trajectory tracking for nonlinear underactuated autonomous vehicles, which is concerned with the design of control laws that force a vehicle to reach and follow a geometric path with an associated timing law is especially challenging because most of these systems are not completely feedback linearizable, therefore standard tools used to control nonlinear systems such as feedback linearization [5], [6] and integrator backstepping [7] are not directly applicable. The classical approach for trajectory tracking of underactuated vehicles utilizes local linearization and decoupling of the multi- variable model to steer the same number of degrees of freedom as the number of available control inputs which can be done using standard linear (nonlinear) control method. Several examples of nonlinear trajectory tracking controllers for underactuated vehicles in continuous-time have been reported in the literature [1], [2], [3], [4]. The output feedback control problem for nonlinear systems due to its importance in many practical applications considerable attention in the literature [9], [10] and [11] where measurement of all the state variables is not available for technological and cost reasons. Output feedback control design usually involves two related problems: observer design and controller design which uses estimated state and output as feedback. Unlike linear systems, separation principle does not generally hold for nonlinear systems. One has often to consider special classes of nonlinear systems to solve the observer design problem as well as the output feedback control problem. In this paper, a combined nonlinear discrete-time tracking controller and observer design is shown to produce globally stable closed-loop dynamics for a class of underactuted autonomous vehicles using Euler approximate discrete-time model of the underactuated Lagrangian system, when the sampling period should typically be sufficiently small in order to get a good approximation of the exact discrete-time model. The proposed combined observer-controller is very practical since it is given in discrete-time and it is easy to implement using digital computers with A/D and D/A converters (sampler and zero order hold). The control algorithm is based on the input-output linearization and the observer design is based on the pole placement technique. The paper is arranged into five sections: Section II presents the problem statement. Section III contains the main result of the paper and presents the combined tracking controller-observer. Section IV applies the observer-control design techniques to an autonomous quadrotor aircraft and presents some simulation results. Finally, the conclusions are given in section V. II. PROBLEM STATEMENT Consider an underactuated mechanical system modelled as a rigid body subject to external generalized forces. Let 1 (, , ) n n q q q = \ denote the configuration vector of the underactuated vehicle, kin T is the (positive semidefinite) kinetic energy and U is the potential energy. The Lagrangian of the underactuated vehicle is 1 (,) () () 2 T kin qq T U qMqq Uq = = L (1) where () Mq is the (positive definite symmetric) inertia matrix. The Euler-Lagrange equation for this underactuated system is as the following ( ) () d Fqu dt q q = L L (2) where p u \ is the control input with p n < . Here p denotes the number of control inputs that is less than the number of configuration variables n and () n p Fq × \ denotes the (non-square) matrix of the external forces. The equation of motion of this underactuated mechanical system can be derived as 1 1-4244-0136-4/06/$20.00 '2006 IEEE

Upload: nour-eddine

Post on 10-Dec-2016

212 views

Category:

Documents


0 download

TRANSCRIPT

Page 1: [IEEE IECON 2006 - 32nd Annual Conference on IEEE Industrial Electronics - Paris, France (2006.11.6-2006.11.10)] IECON 2006 - 32nd Annual Conference on IEEE Industrial Electronics

A Coupled Nonlinear Discrete-Time Controller and Observer Designs for Under-actuated Autonomous vehicles with Application to a Quadrotor Aerial Robot

M’hammed Guisser Hicham Medromi Hassan Ifassiouen Janah Saadi Nour-eddine radhy Equipe Architecture des

Systèmes Equipe Architecture des

Systèmes Equipe Architecture des

Systèmes Equipe Automatique et

Productique Laboratoire d'automatique et

d'informatique Université Hassan II Ain

Chock Université Hassan II Ain

Chock Université Hassan II Ain

Chock Université Hassan II Ain

Chock Université Hassan II Ain

Chock ENSEM, BP. 8118, Oasis Casablanca, MOROCOO

ENSEM, BP. 8118, Oasis Casablanca, MOROCOO

ESTC, B.P. 8012 Oasis Casablanca, MOROCOO

ENSEM, BP. 8118, Oasis Casablanca, MOROCOO

BP 5366 - Maârif Casablanca, MOROCOO

[email protected] [email protected] [email protected] [email protected] [email protected]

Abstract—This paper addresses the problem of a combined discrete-time nonlinear tracking controller-observer design procedure for a class of underactuated autonomous vehicles with configuration (position and orientation) measurements and modelled via Lagrangian approach; we show the asymptotic stability of the closed loop-dynamics using Lyapunov analysis to guarantee the desired objectives of the tracking controller coupled with the observer (software sensor). The combined observer-control design techniques are applied to an autonomous quadrotor aerial robot. Simulation results are also provided to show the effectiveness of the proposed techniques.

I. INTRODUCTION

The control of underactuated vehicles (systems with a smaller number of control inputs than the number of independent generalized coordinates) is an active subject of research in automatic control and robotic. The study of these systems is motivated by the fact that it is usually costly and often not even practical to fully actuate autonomous vehicles due to weight, reliability, complexity and efficiency considerations. Typical examples of underactuated systems include robotic manipulators, wheeled robots, walking robots, spacecraft, aircraft, helicopters, missiles, surface vessels and underwater vehicles. The problem of trajectory tracking for nonlinear underactuated autonomous vehicles, which is concerned with the design of control laws that force a vehicle to reach and follow a geometric path with an associated timing law is especially challenging because most of these systems are not completely feedback linearizable, therefore standard tools used to control nonlinear systems such as feedback linearization [5], [6] and integrator backstepping [7] are not directly applicable. The classical approach for trajectory tracking of underactuated vehicles utilizes local linearization and decoupling of the multi-variable model to steer the same number of degrees of freedom as the number of available control inputs which can be done using standard linear (nonlinear) control method. Several examples of nonlinear trajectory tracking controllers for underactuated vehicles in continuous-time have been reported in the literature [1], [2], [3], [4]. The output feedback control problem for nonlinear systems due to its importance in many practical applications considerable attention in the literature [9], [10] and [11] where measurement of all the state variables is not available for technological and cost reasons. Output feedback control design usually involves two related problems: observer design and controller design which uses estimated state and output as feedback. Unlike linear systems, separation

principle does not generally hold for nonlinear systems. One has often to consider special classes of nonlinear systems to solve the observer design problem as well as the output feedback control problem.

In this paper, a combined nonlinear discrete-time tracking controller and observer design is shown to produce globally stable closed-loop dynamics for a class of underactuted autonomous vehicles using Euler approximate discrete-time model of the underactuated Lagrangian system, when the sampling period should typically be sufficiently small in order to get a good approximation of the exact discrete-time model. The proposed combined observer-controller is very practical since it is given in discrete-time and it is easy to implement using digital computers with A/D and D/A converters (sampler and zero order hold). The control algorithm is based on the input-output linearization and the observer design is based on the pole placement technique.

The paper is arranged into five sections: Section II presents the problem statement. Section III contains the main result of the paper and presents the combined tracking controller-observer. Section IV applies the observer-control design techniques to an autonomous quadrotor aircraft and presents some simulation results. Finally, the conclusions are given in section V.

II. PROBLEM STATEMENT

Consider an underactuated mechanical system modelled as a rigid body subject to external generalized forces. Let

1( , , ) nnq q q= ∈… denote the configuration vector of the

underactuated vehicle, kinT is the (positive semidefinite) kinetic energy and U is the potential energy. The Lagrangian of the underactuated vehicle is

1( , ) ( ) ( )2T

kinq q T U q M q q U q= − = −L (1)

where ( )M q is the (positive definite symmetric) inertia matrix. The Euler-Lagrange equation for this underactuated system is as the following

( ) ( )d

F q udt q q

∂ ∂− =

∂ ∂L L

(2)

where pu ∈ is the control input with p n< . Here p denotes the number of control inputs that is less than the number of configuration variables n and ( ) n pF q ×∈ denotes the (non-square) matrix of the external forces. The equation of motion of this underactuated mechanical system can be derived as

11-4244-0136-4/06/$20.00 '2006 IEEE

Page 2: [IEEE IECON 2006 - 32nd Annual Conference on IEEE Industrial Electronics - Paris, France (2006.11.6-2006.11.10)] IECON 2006 - 32nd Annual Conference on IEEE Industrial Electronics

( ) ( , ) ( ) ( )M q q q q q G q F q u+Γ + = (3)

where ( , )q q qΓ contains centrifugal and coriolis terms and

( )G q contains the gravity terms.

Defining the state variables 1x y q= = and 2x q= , the Lagrangian model of the underactuated vehicle can be written in the following condensed form

1 2( , , )x Ax x x u

y Cx

ϕ = + = (4)

where 1 2 2[ ]T nx x x= ∈ , 1 nx ∈ , 2 nx ∈ is the state

vector, pu ∈ is the control input, ny ∈ is the output

measurement, [ 0]nC I= , nI is the ( )n n× identity matrix,

0

0 0nI

A =

, ( )1

0

( ). ( , ) ( ) ( )M q q q q G q F q uϕ −

= −Γ − +

The discrete-time Lagrangian model of the underacted system is obtained using Euler integration method

1 2( 1) ( ) ( ) ( ( ), ( ), ( ))

( ) ( )s sx k x k T Ax k T x k x k u k

y k Cx k

ϕ + = + + =(5)

where ( ) ( )sx k x kT and ( ) ( )su k u kT , k ∈ , 0sT > is the sampling period. A. Nonlinear control design

In this subsection, we use a feedback linearization input-output technique in discrete-time for tracking a desired trajectory of an underactuated system. The problem of trajectory tracking consists in taking the same number of degrees of freedom (configuration variables) as the number of available control inputs.

Let 1[ ]T ppy y y= ∈… denote the controlled outputs.

The objective is to find a state feedback control ( , )u x vα= where v represents a new control input such that the closed loop dynamic is input-output decoupled and described by the linear system: ( 1) ( )j j jy k v k+ + = , 1,2, ,j p= … . For

each component jy of the output vector, we can associate a

finite characteristic number j for 1,2, ,j p= … such that

( )jy k + does not depend on the control input u for

0,1,2, , j= … and ( 1)j jy k + + depends explicitly on

the control input u , with 1

( ) 2p

jjp n

=+ =∑ .

Now let us consider the change of coordinates 2 2: n nΦ → , ( )z x= Φ , such that the system (5) in the

new system of coordinates can be written under the following canonical form

1

( 1) ( ) ( )

( ) ( )

j jj j j

jj

z k A z k B v k

y k z k

+ = + =

(6)

where,

0 1 0

1

0 0

jA

= … …

and

0

1

jB

=

.

The change of coordinates is defined in the following

expression 1( ) [ ]p Tz x z z= Φ = … with

1 1( ) [ ( ) ( )] [ ( ) ( )]j

j j j T Tj j jz k z k z k y k y k+= = +… …

Considering the canonical form (6), then the second state feedback ( )jv k in the new system of coordinates that allows

the tracking of a reference trajectory ( )jrefy k is given by

( ) ( ( ) ( )) ( 1)j jj j jref jrefv k L z k z k y k=− − + + + (7)

where ( ) [ ( ) ( )]j Tjref jref jrefz k y k y k= +…

The vector jL is such that the eigenvalues of the

matrix ( )j j jA B L− are strictly inside the unit disc (0,1)D

with (0,1) , 1D ν ν= ∈ <

Set ( ) ( ) ( )j j jrefk z k z kε′ = − the tracking error of subsystem

(6) for 1,2, ,j p= … . The dynamic of the tracking error is

given by ( 1) ( ) ( )j jj j jk A B L kε ε′ ′+ = − .

Let 1[ ]Tpε ε ε′ ′ ′= … , 1( , , )pA diag A A= … ,

1( , , )pB diag B B= … and 1( , , )pL diag L L= … . The matrix

L is such that the spectrum ( )sp A BL− is in (0,1)D . The closed loop dynamic of the full system (5) becomes

( 1) ( ) ( )k A BL kε ε′ ′+ = − (8) B. Observer design

The computation of the control input requires the knowledge of the configuration and the velocity of the underactuated system. In this subsection, we assume that the configuration is measured and we present a discrete-time observer for the underactuated system (5) that provides an estimation of the velocity.

In the sequel, we assume that the function 1 2( , , )x x uϕ is

global Lipschitz with respect to 2x uniformly in 1x and u . We can establish the following hypothesis Hypothesis H): Let p⊂U be a bounded domain , then there exists a constant 0µ > such that

2 2, nx x∀ ∈ , 1 nx∀ ∈ , u∀ ∈ U we have

1 2 1 2 2 2( , , ) ( , , )x x u x x u x xϕ ϕ µ− < − (9)

Using a similar procedure which is proposed in [9] and [13], an observer can be extended for system (5) by the dynamical system

1 2

1

ˆ ˆ ˆ ˆ ˆ( 1) ( ) ( ) ( ( ), ( ), ( ))

ˆ( ( ) ( ))

s sx k x k T Ax k T x k x k u k

H Cx k y k

ϕ−Θ

+ = + +

−∆ −(10)

2

Page 3: [IEEE IECON 2006 - 32nd Annual Conference on IEEE Industrial Electronics - Paris, France (2006.11.6-2006.11.10)] IECON 2006 - 32nd Annual Conference on IEEE Industrial Electronics

where 2ˆ nx ∈ is the estimated state, y and u are respectively the output and input of system (5) and

2( , )n ndiag I IΘ∆ = Θ Θ is a (2 2 )n n× diagonal matrix,

0Θ> is a parameter. The matrix , (2 )H n n× is such that

the spectrum 12( ( )) (0,1)n ssp I T A HC D−+Θ − ⊂ . 2nI is

the (2 2 )n n× identity matrix.

III. COMBINED TRACKING CONTROLLER AND OBSERVER DESIGN

In this section, we study the stability of the couple

(tracking controller-observer) when the feedback uses the estimated state for a class of underactuated autonomous vehicles. More precisely, we show that the tracking error and estimation error of the closed loop dynamics converge exponentially towards the origin using Lyapunov analysis.

Consider the discrete-time Lagrangian model of the underactuated system

1 2( 1) ( ) ( ) ( ( ), ( ), ( ))

( ) ( )s sx k x k T Ax k T x k x k u k

y k Cx k

ϕ + = + + =(11)

Now replacing in the expression of the control law the true state by the estimated state given by the observer (10). We obtain the following augmented system

1 2 ˆ( 1) ( ) ( ) ( ( ), ( ), ( ( )))s sx k x k T Ax k T x k x k u x kϕ+ = + + (12)

1 2

1

ˆ ˆ ˆ ˆ ˆ ˆ( 1) ( ) ( ) ( ( ), ( ), ( ( )))

ˆ( ( ) ( ))

s sx k x k T Ax k T x k x k u x k

H Cx k y k

ϕ−Θ

+ = + +

−∆ −(13)

We introduce the estimation error ˆ( ) ( ( ) ( ))k x k x kε Θ= ∆ −

and the new tracking error ˆ( ) ( ) ( )refk z k z kε′ = − =

ˆ( ( )) ( ( ))refx k x kΦ −Φ . Introducing again formula (8) into

(13), then the tracking error and estimation error of the closed loop dynamics (12), (13) become

1 1( 1) ( ) ( )

( 1) ( ) ( )

k A k HC k

k A k k

ε ε ε

ε ε δϕ

− −Θ Θ

′ ′ ′+ = −∆ ∆ + = +

(14)

where 12 ( )n sA I T A HC−= +Θ − and

1 2 1 2ˆ ˆ( ) [ ( ( ), ( ), ( )) ( ( ), ( ), ( ))]sk T x k x k u k x k x k u kδϕ ϕ ϕΘ= ∆ −

The matrices ( )A A BL′ = − , B and L are given in section II.

The following theorem establishes the main results. Theorem 1: Consider the closed loop dynamics of an underactuated system (14). Assume that hypothesis H) holds,

then the equilibrium ( , ) (0, 0)ε ε′ = of system (14) is globally asymptotically stable. More precisely we have

0 0T∃ > ; ] ]00,sT T∀ ∈ ; 0∀Θ > ; ] [ ] [1 5( , ) 0,1 0,1β β Θ∃ ∈ × ;

2 3 40, 0, 0β β βΘ Θ Θ∃ > ∃ > ∃ > such that for every 0k ≥

we have

2 1 1 22 31 1 50

1 1 24 1 50

[ ( ) ( )] ( )

(0) (0)

T ikk k i

ik k i ii

k k iε ε β β β β β ε

ε β β β ε

− − −Θ Θ Θ=

− − −Θ Θ=

′ ′≤ + ×

+

∑∑

Proof of theorem 1

Let the estimation error ˆ( ) ( ( ) ( ))k x k x kε Θ= ∆ − , using

the fact that 1 1A A− −Θ Θ∆ ∆ = Θ and 1 1C C− −

Θ∆ = Θ , it follows that the error dynamics can be written as

( 1) ( ) ( )k A k kε ε δϕ+ = + (15) As the choice of the matrix H is such that the spectrum of

A is in (0,1)D , then there exists a unique symmetric positive definite matrix P , such that for every symmetric positive definite matrix Q We have: TA PA P Q− =− (16)

Moreover the matrices A , H and P depend on the parameter Θ and the sapling time sT .

Now consider a positive definite quadratic function

( ) ( ( )) ( ) ( )TV k V k k P kε ε ε= , Combining the equations (15) and (16), we get:

( 1) ( ) ( ) ( ) 2 ( ) ( )

( ) ( )

T T T

T

V k V k k Q k k A P k

k P k

ε ε ε δϕ

δϕ δϕ

+ − =− +

+(17)

Using the Schwartz inequality and hypothesis H) which 2 2 2ˆ( ) ( ( ) ( )) ( )s sk T x k x k T kδϕ µ µ ε< Θ − < , 0∀Θ > ,

we obtain 2

22 2

( 1) ( ) ( ) ( ) 2 ( )

( )

Ts

s

V k V k k Q k T A P k

T P k

ε ε µ ε

µ ε

+ − ≤− +

+(18)

The inequality (18) becomes 2 2

min

max min

2( )( 1) (1 ) ( )

( ) ( )s sT A P T PQ

V k V kP P

µ µλλ λ

++ ≤ − +

(19) where min( )λ i and max( )λ i are respectively the minimum

and maximum eigenvalue of ( )i .

From (16), we have min max( ) ( )Q Pλ λ< . Now for

] ]00,sT T∈ choosing 0Θ> such that 2 2

min

min max

2 ( )( ) ( )

s sT A P T P QP P

µ µ λλ λ

+<

Therefore, 2 2

min1

max min

2( )0 1 1

( ) ( )s sT A P T PQ

P P

µ µλγ

λ λΘ

+< = − + <

The inequality (19) becomes

1( 1) ( )V k V kγ Θ+ ≤ (20)

An iterative calculation gives 0k∀ ≥ 12 2

2 1( ) (0)k

kε γ γ εΘ Θ≤ (21)

3

Page 4: [IEEE IECON 2006 - 32nd Annual Conference on IEEE Industrial Electronics - Paris, France (2006.11.6-2006.11.10)] IECON 2006 - 32nd Annual Conference on IEEE Industrial Electronics

where max2

min

( )( )PP

λγ

λΘ = . To finish the proof of the theorem 1,

it is necessary to show that ( )kε′ converges exponentially to zero. Now consider the Lyapunov function candidate for the closed loop augmented system (14) defined by:

( ) ( ( ), ( )) ( ( )) ( ( ))V k V k k V k V kε ε ε ε′ ′ ′= + (22)

where ( ) ( ) ( )TV k k P kε ε′ ′ ′ ′= The matrix L is chosen such that the spectrum

( ) (0,1)sp A D′ ⊂ , then for every symmetric positive definite

matrix Q ′ , there exists a unique symmetric positive definite

matrix P ′ such that the following Lyapunov equation holds: TA P A P Q′ ′ ′ ′ ′− = − (23)

We have: 1

1 1 1 1 1

( 1) ( ) ( ) ( ) 2 ( )

( ) ( ) ( )

T T T

T T T

V k V k k Q k k A P

HC k k C H P HC k

ε ε ε

ε ε ε

−Θ

− − − − −Θ Θ Θ Θ Θ

′ ′ ′ ′ ′ ′ ′ ′+ − = − − ∆ ×

′∆ + ∆ ∆ ∆ ∆

Using the Schwartz inequality, we obtain: 21

4 221

( 1) ( ) ( ) ( ) 2

( ) ( ) ( )

TV k V k k Q k A P

HC k k P HC k

ε ε

ε ε ε

−Θ

−Θ

′ ′ ′ ′ ′ ′ ′+ − ≤− + ∆ ×

′ ′+ ∆

(24) Therefore,

2min

21 2

( 1) ( ) ( ) ( )

( ) ( ) ( )

V k V k Q k

k k k

λ ε

ρ ε ε ρ εΘ Θ

′ ′ ′ ′+ − ≤− +

′ + (25)

Where 21

1 2 A P HCρ −Θ Θ′ ′= ∆ and

4 212 P HCρ −Θ Θ′= ∆

Using the inequality (21), we get: 1min 2 2

1 2 1max

22 2 1

( )( 1) (1 ) ( ) ( ) (0)

( )

(0)

k

k

QV k V k k

ρ γ γ ε ελ

ρ γ γ ε

Θ Θ Θ

Θ Θ Θ

′′ ′ ′+ ≤ − +

+ (26) An iterative computation provides:

1 1 12 211 12 50

1 1 22 2 1 50

( ) (0)

( ) (0) (0)i

ikk k i

ik k i

i

V k V

i

β ρ γ β β

ε ε ρ γ β β ε

− − −Θ Θ Θ=

− − −Θ Θ Θ=

′ ′≤ + ×

′ +

∑∑

(27)

where min1

max

( )1

( )QP

λβ

λ

′= −

′ and 5 1β γΘ Θ= . From (16) and

(23) it is clear to see that ] [ ] [1 5( , ) 0,1 0,1β β Θ ∈ × . Otherwise,

0k∀ ≥ we have 12 1 12 2 1max 2

1 10min min

1 1 22 2215 50min

( )( ) (0)

( ) ( )

( ) (0) (0)( )

kk k i

i

i k k i

i

i

Pk

P P

iP

ρ γλε β ε β

λ λρ γ

β ε ε β β ελ

− − −Θ Θ=

− − −Θ ΘΘ Θ=

′′ ′≤ + ×

′ ′

′ +′

∑ (28) Combining (28) and (21), we have:

2 1 1 22 31 1 50

1 1 24 1 50

[ ( ) ( )] ( )

(0) (0)

T ikk k i

ik k i ii

k k iε ε β β β β β ε

ε β β β ε

− − −Θ Θ Θ=

− − −Θ Θ=

′ ′≤ + ×

+

∑∑

where 2 2max

2 2min

( )(0) (0)

( )PP

λβ ε γ ε

λΘ Θ′

′= +′

,

12

1 23

min( )Pρ γ

βλ

Θ ΘΘ =

′ and 2 2

4min( )Pρ γ

βλ

Θ ΘΘ =

′. Using the fact that

] [ ] [1 5( , ) 0,1 0,1β β Θ ∈ × . Then it follows that ( ( ), ( ))k kε ε′ converge exponentially to zero as k → +∞ . This concludes the proof of theorem 1. Remark 1: We can prove otherwise that the closed loop dynamics system (14) is globally asymptotically stable without difficulty using another procedure. Consider the following notation

εε

ε

′ = ,

1 1

0

A HCA

A

− −Θ Θ

′ −∆ ∆ = and

0δϕ

δϕ

=

The error dynamics of the closed loop system (14) can be written as

( 1) ( ) ( )k A k kε ε δϕ+ = + (29)

Since the matrices A′ and A are stable, then A is stable

because 4 2 2det( ) det( ).det( )n n nI A I A I Aλ λ λ′− = − − ,

therefore 0P∃ > , 0Q∀ > such that TA PA P Q− = − ,

where P and Q are symmetric positive definite matrices.

Define the Lyapunov function ( ) ( ) ( )TV k k P kε ε= . In

order to prove that ( ) 0kε → exponentially as k → +∞ , it

suffices to show that ( )V k decreases to zero exponentially. By the same argument as in (17), (18), (19) and (20) it is

easily to conclude that ( 1) ( )V k V kγΘ+ ≤ , where

] [2 2

min

max min

( ) 21 0,1

( ) ( )s sQ T A P T P

P P

λ µ µγ

λ λΘ+

= − + ∈ .

Hence, it can be concluded that ( )V k decreases to zero

which implies that ( )kε converges to zero.

IV. APLLICATION TO A QUADROTOR AIRCRAFT A. modelling of the quadrotor aircraft

The quadrotor aerial robot as shown in Figure 1 is an underactuated flying vehicle with four input forces generated by four electric motors and six output coordinates. The quadrotor aircraft is assumed to be a rigid body evolving in 3D space, having six degrees of freedom and subject to external efforts. The vertical motion is controlled by collectively increasing or decreasing the speed of all motors. The horizontal motion is achieved by differentially controlling the motors generating a pitching/rolling motion of the quadrotor that inclines the collective thrust. The yaw motion is obtained by increasing (decreasing) the speed of

4

Page 5: [IEEE IECON 2006 - 32nd Annual Conference on IEEE Industrial Electronics - Paris, France (2006.11.6-2006.11.10)] IECON 2006 - 32nd Annual Conference on IEEE Industrial Electronics

the front and rear motors while decreasing (increasing) the speed of the lateral motors.

ψ

1f

2f

3f

4f 1M

2M

3M

4M

φ

1u

Gℜ

XeYe

Ze

θ

1e

2e3e

ξ mg

o

G

Oℜ

Fig. 1. Quadrotor aircraft with frames and forces.

The generalized coordinate of the quadrotor are 6( , )q ξ η= ∈ , where 3( , , )X Y Zξ = ∈ is The position

of its center of mass G relative to a fixed inertial reference

, , , O X Y ZO e e eℜ = and 3( , , )η ψ θ φ= ∈ are the Euler

angles. These angles are called the yaw ( )π ψ π− < < , the

pitch ( /2 /2)π θ π− < < and the roll ( /2 /2)π φ π− < <

respectively. The vector 31 2 3( , , )Ω = Ω Ω Ω ∈ denotes the

angular velocity of the vehicle expressed in the body fixed frame 1 2 3 , , , G G e e eℜ = . The derivative of the Euler coordinate with respect to time can be expressed by

sin 0 1

cos sin cos 0 ( )

cos cos sin 0

W

ψθ

θ φ φ θ η η

θ φ φ φ

− Ω = = −

The total thrust produced by the four rotors is given by:

1 1 2 3 4u f f f f= + + + where 2i if bω= is the thrust

generated by the rotor 1,2, 3, 4i ∈ and iω is the angular speed generated by the motor iM and 0b > is a parameter. The generalized moments acting of the four rotor aircraft result from the action of the thrust forces difference of each pair of rotors are denoted by ( , , )ψ θ φτ τ τ τ , with

2 4 1 3( )f f f fψ κτ = + − − , 3 1( )f f dθτ = − and φτ =

2 4( )f f d− where d represents the distance from the rotors to the center of gravity of the quadrotor and 0κ > is a constant.

The lagrangian is 1( , ) ( ) ( )2T

kinq q T U q M q q U q= − = −L

where 3( ) ( , ( ))M q diag mI η= J , U mgZ= , ( )η =J

( ) ( )TW Wη ηI is the inertia matrix for the rotorcraft expressed directly in terms of the generalized coordinates η ,

( , , )XX YY ZZdiag= I I II is a diagonal matrix representing the inertia matrix of the rotorcraft expressed in the body fixed frame, m is the total mass of the rotorcraft, g represents the

acceleration due to gravity. The Lagrangian model of the quadrotor aircraft is ( ) ( , ) ( ) ( )M q q q q q G q F q u+Γ + =

where 3 31

( , ) (0 , ( ) ( ( )))2

Tq q diag η η ηη×∂

Γ = −∂

J J , ( )G q =

0Zmge

,3

( ) 0( )

0ZR e

F qI

η = and

1uu τ

= . [0 0 1]TZe =

denotes the unit vector in the frame Oℜ and R denotes the

rotational matrix ( , , ) (3)R SOψ θ φ ∈ representing the orientation of the quadrotor aircraft relative to the fixed

inertial frame. Defining the state variables 1x y q= = and 2x q= , the Lagrangian model of the quadrotor aircraft can

be rewritten as (4), where 1 2 12[ ]Tx x x= ∈ , 6n = ,

4p = , 1[0 ( ).( ( , ) ( ) ( ) )]TM q q q q G q F q uϕ −= −Γ − + .

Using the fact that the matrix W is bounded and the inertia

matrix M and 1M− are bounded. Then / qϕ∂ ∂ is uniformly bounded for a bounded angular velocity η which

implies that ϕ is global Lipchitz w.r.t. q . Hence the

hypothesis H) is satisfied and the observer (10) can be

applied. In the sequel, we choose the controlled outputs as

1 2 3 4( , , , ) ( , )y y y y y ξ ψ= = . The characteristic numbers of

the outputs 1 2 3 4( , , , )y y y y y= are 1 2 3= = , and

3 4 1= = (see [14] for more detail). The control law

requires the knowledge of the configuration and the velocity of the quadrotor aircraft, we consider that the position and orientation of the rotorcraft are measurable as in [12], where the 3D tracker system (Polhemus) is used for these measures; and we apply the above observer synthesis designed in section III to estimate the linear and angular velocities. B. Simulation results

The physical parameters used for the dynamic model of the rotorcraft are: (0.0142,0.0142,0.0248)diag=I ,

9.81g = , 0.56m = , 0.21d = and 1/14sT = . The following simulations have been achieved in order to show the performances of the trajectory tracking controller combined with the observer design. The desired trajectory is chosen as a vertical helix (see Figure 2) given by:

( ) 0.2 cos(0.12 )refX k k= , ( ) 0.2 sin(0.12 )refY k k= ,

( ) 0.01 0.1refZ k k= − , ( ) / 4ref kψ π= . The initial

conditions of the quadrotor aircraft and the obsever are

0 [0 0 0 ]Tξ = , 0 [0 0 0]Tξ = , 0 [0 0 0 ]Tη = , 0 [0 0 0]Tη = ,

20 [ 10 10 10] .10Tξ −= − − , 0

ˆ [0.08 0.08 0.08]Tξ = ,

0 [8 10 10] . /180Tη π= , 0 [0.48 0.48 0.48]Tη = . The

gain vectors of the controller are set to

1 [0.0994 -0.7081 1.8917 -2.2460]L = ,

2 [0.0497 -0.4215 1.3391 -1.8900]L = ,

3 [0.6240 -1.5800]L = and 4 [0.3300 -1.1500]L = .

5

Page 6: [IEEE IECON 2006 - 32nd Annual Conference on IEEE Industrial Electronics - Paris, France (2006.11.6-2006.11.10)] IECON 2006 - 32nd Annual Conference on IEEE Industrial Electronics

The parameter of the observer is tuned at 5Θ = , the gain matrix H is chosen with the vector of poles

121 2( , )σ σ σ= ∈ , 1 (0.70,0.71,0.72,0.73,0.74,0.75)σ = ,

2 1σ σ= . From Figure 3, it is important to signal that the estimation error converges to zero rapidly with respect to the tracking error, this means that the observer reacts before the controller. Moreover, figure 4 shows the evolution of the actual linear and angular velocities of the rotorcraft compared to their estimates. In order to evaluate the robustness of the controller coupled with the estimator, the output measurements are corrupted by an additive Gaussian noise with zero mean and amplitude equivalent to 5% on the corresponding measures. We remark the good performance and robustness of the tracking controller-observer in the presence of noisy measurements.

V. CONCLUSIONS

We proposed a solution to the problem of the combined discrete-time tracking controller and observer designs for nonlinear underactuated autonomous vehicles with configuration measurements and modelled via Lagrangian approach. The coupled controller-observer was shown using Lyapunov analysis to yield global stability and exponential convergence to zero of the tracking error and the estimation error of the closed loop dynamics. An application to an autonomous quadrotor aerial robot was described to illustrate the performance of the combined observer- controller design techniques.

REFERENCES [1] A. P. Aguiar, J. P. Hespanha, "Position tracking of underactuated

vehicles," Proc. of ACC’03 - American Control Conference, Denver, CO, June 2003.

[2] R. Mahony, T. Hamel, "Robust trajectory tracking for a scale model autonomous helicopter," Int. J. Robust Nonlinear Control, 2004.

[3] E. Frazzoli, M. A. Dahleh, E. Feron, "Trajectory tracking control design for autonomous helicopters using a backstepping algorithm," Proceedings of the American Control Conference, Chicago, June 2000.

[4] L. Benvenuti, P. Di Giamberardino, L. Farina, "Trajectory tracking for a PVTOL aircraft: a comparative analysis," Proceedings of the 35th Conference on Decision and Control, Kobe, Japan. December 1996.

[5] H. Nijmejer, A. Van der schaft, Nonlinear dynamical control systems, Springer-Verlag, New York, 1995.

[6] A. Isidori, Nonlinear control systems, Springer-Verlag, London, UK, 3rd edition, 1989.

[7] M.Krstic, I.Kanellakopoulos, P.Kokotovic, Nonlinear and adaptative control design, John wiley & sons Ltd New York, 1995.

[8] A. EL Assoudi, E. EL Yaagoubi, H. Hammouri, "Nonlinear observer based on the Euler discretization," Int. J. Control, vol. 75, no. 11, 784-791, 2002.

[9] K. Busawon, Sur les observateurs des systèmes non linéaires et le principe de séparation, Thesis, Université Claude Bernard Lyon I, Lyon, France 1996.

[10] H. Hammouri, K. Busawon, "A global stabilization of a class of nonlinear systems by means of an observer," Appl. Math. Lett. Vol. 6, no. 1, pp. 31-34, 1993.

[11] A. N. Attassi, H. K. Khalil, "A separation principle for the stabilization of a class of nonlinear systems," IEEE Transaction on Automatic Control, vol 44, pp. 1672-1687, 1999.

[12] P. Castillo, A. Dzul, R. Lozano, "Real-time stabilization and tracking of a four rotor mini rotorcraft," IEEE Transaction on Control Systems Technology, vol. 12, no. 4, July 2004.

[13] M. Guisser, H. Medromi, H. Ifassiouen, J. Saadi, "Trajectory tracking via an observer in discrete-time of a four rotor mini-rotorcraft,"

International Conference on Modelling and Simulation, Marrakech, Morocco, 22-24 November, 2005.

[14] M. Guisser, H. Medromi, J. Saad and H. Ifassiouen,, "Stabilization and trajectory tracking in discrete-time of an autonomous four rotor mini-rotorcraft," Second International Symposium on Communications, Control and Signal Processing, Marrakech, Morocco, 13-15 March, 2006.

Figure 2. Desired trajectory in 3D space and XY -plane.

Fig. 3. Actual position and orientation of the quadrotor aircraft (solid line) and their estimates (dotted line) and desired rajectory (dashed line).

Fig. 4. Actual linear velocity and angular velocity of the quadrotor aircraft (solid line) and their estimates (dotted line).

6