Rotorcraft trajectory tracking by supervised NLI control

(1)

HAL Id: hal-00938523

https://hal-enac.archives-ouvertes.fr/hal-00938523

Submitted on 29 Apr 2014

HAL is a multi-disciplinary open access archive for the deposit and dissemination of sci- entific research documents, whether they are pub- lished or not. The documents may come from teaching and research institutions in France or abroad, or from public or private research centers.

L’archive ouverte pluridisciplinaire HAL, est destinée au dépôt et à la diffusion de documents scientifiques de niveau recherche, publiés ou non, émanant des établissements d’enseignement et de recherche français ou étrangers, des laboratoires publics ou privés.

Antoine Drouin, Omar Lengerke, Alexandre Carlos Brandao-Ramos, Felix Mora-Camino

To cite this version:

Antoine Drouin, Omar Lengerke, Alexandre Carlos Brandao-Ramos, Felix Mora-Camino. Rotorcraft trajectory tracking by supervised NLI control. IMECS 2012, International MultiConference of Engi- neers and Computer Scientists 2012, Mar 2012, Hong Kong, China. pp 883-887. �hal-00938523�

(2)

Rotorcraft Trajectory Tracking by Supervised NLI Control

A. Drouin, O. Lengerke, A.B. Ramos and F. Mora-Camino

Abstract— The purpose of this communication is to present a new nonlinear control structure for trajectory tracking taking explicitly into account actuators saturation. Here trajectory tracking by a four rotor aircraft is considered. After introducing the flight dynamics equations for the four rotor aircraft, a trajectory tracking control structure based on a two layer non linear inverse approach is adopted and a supervision layer is introduced to take into account the possible actuators saturation.

Index Terms— Rotorcraft, nonlinear inverse control, saturation supervision, trajectory tracking.

I. INTRODUCTION

n the last years a large interest has risen for the four rotor concept since it appears to present simultaneously hovering, orientation and trajectory tracking capabilities of interest in many practical applications [1].

The flight mechanics of four rotor aircarft are highly non linear and different control approaches (integral LQR techniques, integral sliding mode control [2]) have been considered with little success to achieve not only autonomous hovering and orientation, but also trajectory tracking In this paper, some simplifying assumptions are adopted and the flight dynamics equations for a four rotor aircraft with fixed pitch blades, or rotorcraft, are considered.

One important limitation to perform automatic guidance for a rotorcraft is related with the one way effect of rotors and its saturation levels. Then the purpose of this study is to introduce a supervision layer in a non linear inverse control structure to improve maneuverability and trajectory tracking effectiveness by this class of rotorcraft. This approach has been already considered in the case of aircraft trajectory tracking by different authors [3,4, 5].

It appears that the flight dynamics of the considered rotorcraft present a two level input affine structure which is made apparent when a new set of equivalent inputs is defined. This allows the development of a non linear inverse control approach with two time scales, one devoted to attitude control and one devoted to orientation and trajectory tracking. However this is done in general without

considering actuators saturations and when these occur, trajectory tracking capability can be largely affected.

A. Drouin is with MAIAA Laboratory, Automation Research Group at ENAC, Toulouse, France, [email protected].

O. Lenguerke is with Mechatronic and Robotic Laboratory, Universidad Autonoma de Bucaramanga-UNAB, Bucaramanga, Colombia, [email protected]

A.B. Ramos is with Instituto de Ciências Exatas, UNIFEI, Itajubá, Brazil,, [email protected]

F. Mora-Camino is with MAIAA Laboratory, Automation Research Group at ENAC, Toulouse, France, Tel.+33562174358, fax.

+33562174403, [email protected]

II. ROTORCRAFT FLIGHT DYNAMICS

The considered system is shown in figure 1 where rotors one and three are clockwise while rotors two and four are counter clockwise. In appendix the dynamics of the rotors are briefly characterized.

The main simplifying assumptions adopted with respect to flight dynamics in this study are a rigid cross structure, no wind, negligible aerodynamic contributions resulting from translational speed, no ground effect as well as negligible air density effects and very small rotor response times. It is then possible to write simplified rotorcraft flight equations [6].

The moment equations can be written as:

) (

) / (

) ( ) / (

3 4 1 2

4 3 1

2 2 4

F F F F I k r

r p k F F I a q

r q k F F I a p

zz yy xx

− +

−

=

+

−

== − +

&

(1) where p, q, r are the components of the body angular

Fig 1. Four rotor aircraft

speed, with _k₂₍_I_zz₋_I_yy₎_/_I_xx and _k₄₌₍_I_xx₋_I_zz₎_/_I_yy, Ixx, Iyy

and Izz being the moments of inertia in body-axis and m the total mass of the rotorcraft. The Euler equations are given by:

r q

p

)) cos(

/ ) (cos(

)) cos(

/ ) ((sin(

) sin(

) cos(

) tan(

) sin(

) tan(

θ φ θ

φ

ψθ φ φ

φ θ φ

θ φ

+

= −

=

+ +

=

&

(2)

where θ, φ, and ψ are respectively the pitch, bank and heading angles.

I

F4 F1 p

F2

F3

r q

(3)

The acceleration equations written directly in the local Earth reference system are such as:

F M

g z

F m

y

F m

x

) cos(

) / 1 (

)) sin(

) cos(

) sin(

) )(sin(

/ 1 (

)) sin(

) sin(

) cos(

) sin(

) )(cos(

/ 1 (

φ

θθ φ ψ φ

ψψ θ φ ψ φ

+

−

=

−

=

+

=

&

(3)

where x, y and z are the centre of gravity coordinates and where :

4 3 2

1 F F F

F

F= + + + (4) and with the constraints:

{¹^,²^,³^,⁴

0≤F ≤F_max i∈

ii } (5) III. NLICONTROL APPROACH FOR TRAJECTORY TRACKING

Here we are interested in controlling the four rotor aircraft so that its centre of gravity follows a given path with a given heading ψ while attitude angles θ and φ remain small. Many potential applications require not only the centre of gravity of the device to follow a given trajectory but also the rotorcraft to present a given orientation.

A. Attitude Control

From equations (1) it appears that the effectiveness of the rotor actuators is much larger with respect to the roll and pitch axis than with respect to the yaw axis. Then it is considered here that attitude control is involved with controlling the θ and φ angles. In equations (1) the effect of rotor forces appears as differences so, we define new attitude inputs u1 and u2 as:

3 1

1 F F

u = − u₂=F₂−F₄ (6.1) In the heading and position dynamics, the effects of rotor forces and moments appear as sums, so we define new guidance inputs v1 and v2 as:

3 1

1 F F

v = + v₂ =F₂+F₄ (6.2) It is supposed that u1 and u2 can be made to vary significantly while v1 and v2 can remain constant.

Then the attitude dynamics can be rewritten under the affine input form:

U X g V X f

X&= ( , )+ ( ) (7.1)

Y'=(θ,φ) (7.2) with

) , , , (

' p qθ φ

X = , U'=(u₁,u₂) andV'=(v₁,v₂) (8) Then, taking profit of non linear inverse control theory, it appears that all the attitude angles have relative degrees equal to one and that there is no internal dynamics while the output equations can be rewritten as:

) ( ) ( )

(Y U N X V P X

M

Y&&= + + (9)

with

⎥⎦

⎢ ⎤

⎣

⎡

= −

xx yy

yy

I a I tg

a

I Y a

M sin / /

0 /

) cos

( θ φφ (10)

⎥⎦⎤

⎢⎣⎡

−

= −

zz zz

zz

I tg k I tg k

I k I

X k

N cos / cos /

/ sin /

) sin

( θφ φ θ φφ (11)

]' , [ ) (X P₁ P₂

P =

where:

)) cos sin

( ( ) cos sin (

4cos

1 θ φ θ φ

φ

φ φ

tg r q tg p r

q

r p k P

+ +

+

−

=

(12.1)

dt tg

d r dt tg

d q

tg r p k r q k P

/ ) cos ( / ) sin (

4 sin 2

2θ φ θθ φφ

+ +

+

= (2.2)

Then, while φ≠±π/2, the attitude dynamics given by (9) are invertible. Then it appears opportune to adopt as a partial control objective to assign to the attitude angles second order linear dynamics towards their current reference values:

⎥⎥

⎦

⎤

⎢⎢

⎣

⎡

−

− − −

= −

⎥=

⎦

⎢ ⎤

⎣

⎡

) ( 2

2 2

c c d

d

d Y

φ φ ω φ ω

ζ ω θ ω θ θ φ ζ

θ

φ φ φ

θ θ

θ &

&

(13)

where ζ_θ^,ζ_φ^,ω_θ^,ω_φ are respectively damping and natural frequency parameters while θc and φc are reference values for the attitude angles. Then the resulting non linear inverse attitude control law is given by:

) ) ( ) ( ( )

(Y ¹ N X V P X Y_d M

U=− ⁻ + −&& (14)

B. Guidance Control Law

Considering that the attitude dynamics are stable and much faster than the guidance dynamics, the guidance equations can be approximated by the control affine form:

⎥⎥

⎦

⎤

⎢⎢

⎣

⎡

+ +

− −+ ++

−

=

⎥⎥

⎥

⎦

⎤

⎢⎢

⎢

⎣

⎡

) ( ) cos(

) cos(

) / 1 (

) ( )) sin(

) cos(

) sin(

) )(sin(

/ 1 (

) ( )) sin(

) sin(

) cos(

) sin(

) )(cos(

/ 1 (

) cos (

cos

2 1

2 1 1

2

v v m

g

v v m

v I v

k

z y x

c c

c

c c

c c zz

c

φ

θ ψ φ

φ θ

ψψ θ φθφ ψ φ

ψ

&

(15)

Here also, the outputs of the guidance dynamics present relative degrees equal to 1 while the internal dynamics, which are concerned with the attitude angles are supposed already stabilized. Then, considering that second order linear dynamics are also of interest for the guidance variables, we can define desired accelerations by:

⎥⎥

⎥

⎦

⎤

⎢⎢

⎢

⎣

⎡

−

− − −

−

=

⎥⎥

⎦

⎤

⎢⎢

⎣

⎡

) ( 2

2 2 2 2

c z z z

c y

y y

c x x x

c

c c c c

z z z

y y y

x x x

z y x

ω ω ζ ω ω

ζζ ωωψ ωω ψ ψ

ψ ζ_ψ _ψ _ψ

&

(16)

where _ζ_ψ_,_ζ_x_,_ζ_y_,_ζ_z_,_ω_ψ_,_ω_x_,_ω_y_,_ω_zare respectively damping and natural frequency parameters while ψ^c, xc, yc and zc are reference values for the attitude angles. Of course, many other schemes can be proposed to define desired accelerations at the guidance level.

Once desired accelerations are made available, the inversion of the guidance dynamics brings nominal the solution:

(4)

cos ) ) cos (

2(

1 2 2 ₂

1 c

c c zz c

c

c k

g I z y x m

v ₌ _&_& ₊_&_& ₊ _&_& ₊ ₋ φθ ψ_&_& (17.1)

cos ) ) cos (

2(

1 2 2 ₂

2 c

c c zz c

c

c k

g I z y x m

v ψ

φθ _&_&

&

& + + + +

= (17.2)

with

)) /(

) sin cos

((x y z g

arctg _c _c _c

c= && ψ+&& ψ && +

θ ^(18.1)

and

2 2

2 ( )

cos arcsin( sin

g z y x

y x

d d d

d c

c + + +

= −

&

& ψ ψ

φ (18.2)

p, q, θ, φ

ψ,x,y,z

Fig 2. Proposed control structure

Then, returning to the expression of the attitude control law , it happens that the centre of gravity acceleration terms compensate each others and the law becomes simply:

) ) ( ) ( ( )

(Y ¹ N X P X Y_d M

U =− ⁻ + −&& (19)

with

c c c

c c

N X N

N ψ

φφ θθ θ

φφ θ

&

⎥⎥

⎦

⎤

⎢⎢

⎣

=⎡ −

⎥⎦

⎢ ⎤

⎣

=⎡

cos cos cos sin cos

cos cos sin )

(

2

1 (20)

The whole proposed control structure is given in the above figure 2.

IV. FLIGHT CONTROL SUPERVISION

Since the above control approach does not consider explicitly the input level constraints, we introduce here a supervision layer whose function is to avoid the generation

of unfeasible reference values for the inputs by modifying, as less as possible, the current control objectives. According to (5), (6) and (7), the control signals should be such as:

2 ,

max 1

max≤ ≤ =

−F u_i F i (21.1) and ₀_≤_v_i_≤₂_F_max _i₌₁_,₂ (21.2)

Conditions (21.1) implies for the desired attitude accelerations to satisfy the following conditions:

max

min φ φ

φ^&^& ≤^&^&c ≤^&^& (22.1)

with

max 2

2 min

cos F I P a N

yy

φ&& = + − φ (22.2)

and

max 2

2 max

cos F I P a N

yy

φ&& = + + φ (22.3)

and condition:

max

min θ θφ θ

θ&& ≤ &&c−tg &&c ≤ && (23.1)

with

) ( ) (

/ ₁ ₁ ₂ ₂

max

min =−aF I_xx+ N +P −tgθ N +P

θ^&^& ^(23.2)

and

) ( ) (

/ ₁ ₁ ₂ ₂

max

max=aF I_xx+ N +P −tgθ N +P

θ^&^& (23.3)

Then, reference values for instant attitude angles accelerations can be obtained from the solution of the following linear –quadratic optimization problem:

2

2 ( )

) (

min θ α φ β

β

α _c− + _c−

r

&

(24.1) with

max

min β φ

φ^&^& ≤ ≤ ^&^& (24.2)

max

min α θ β θ

θ^&^& ≤ −tg ≤ ^&^& (24.3)

Observe that the solution of this problem is equal to ( ) if it is feasible with respect to constraints (24.2) and (24.3), otherwise the solution will be on the border of the convex feasible set.

c cφ θ^&^&^,^&^&

Then if _α^*and _β^*are solution of this problem, u1 and u2 are given by:

) ) ( ) ( ( )

( *

* 1

2

1 ⎥

⎦

⎢ ⎤

⎣

−⎡ +

−

⎥⎦=

⎢⎣ ⎤

⎡ −

β X α P V X N Y u M

u (25)

In the case of v1 and v2 (relations (21.2)) and considering the expressions of θ^c and φ^c the above approach leads to the consideration of an intricate non convex optimization problem. A different approach is proposed here. Let λ be such as:

) ( ,

,y y z g z g

x

x&_r = &&_c &&_r = &&_c &&_r+ = &&_c+

& λ λ λ ⁽²⁶⁾

then according to (18.1) and (18.2):

c

r θ

θ = and φ_r =φ_c (27) Feasible reference values for &x&_r,&y&_r,&z&_r and ψ^&^&_r^{can be}

obtained from the solution of the following linear –quadratic optimization problem:

² ² ² (28.1)

, ( 1) ( )

min λ η μ ψ_c

μ

λ − + − &&

with Attitude

dynamics

Guidance dynamics NLI attitude

control NLI guidance

control

z y x z y x

r,ψ&,&,&,&, , ,

θc,φc

c c c

c x&& &y& &z&

&

& , , ,

ψ

u1,u2

v1 ,v2, θ^c^,φ^c θ, φ

Attitude control loop

Guidance control loop

(5)

max 2 2

2 ) 4

cos ( cos ) ) ( (

0 F

k g I z y x m

c c zz c

c

c + + + − ≤

≤ ^&^& ^&^& ^&^& λ φθ μ (28.2)

max 2 2

2 ) 4

cos ( cos ) ) ( (

0 F

k g I z y x m

c c zz c

c

c + + + + ≤

≤ ^&^& ^&^& ^&^& λ φθ μ (28.3)

where η is here a time constant. Let and be the solution of the above problem, then the control inputs can be taken as:

λ* μ^*

cos ) ) cos ( 2(

1 * 2 2 2 *

1 μ

φθ λ

c c c zz

c

c k

g I z y x m

v = && +&& + && + − (29.1)

cos ) ) cos ( 2(

1 _* ₂ ₂ ₂ _*

2 λ φθ μ

c c c zz

c

c k

g I z y x m

v = && +&& + && + + (29.2)

Then, F₁,F₂,F₃,F₄ given by:

2 / ) ( ₁ ₁

1 u v

F = + F₂ =(u₂ +v₂)/2 (30.1) 2

/ ) ( ₁ ₁

3 v u

F = − F₄ =(v₂ −u₂)/2 (30.1) satisfy condition (5).

V. CASE STUDIES

Here we considered two cases: one where the objective is to hover at an initial position of coordinates x0, y0, z0

while acquiring a new orientation ψ¹, and one where the rotorcraft is tracking the helicoïdal trajectory of equations:

t t

x_c⁽⁾=ρ^cosν t

t

y_c⁽⁾=ρ^sinν t

z_c =δ+γ (31) 2

/ )

( ν π

ψ_c t = t+

where ρ is a constant radius and γ is a constant path angle.

A. Heading control at hover

In this case we get the guidance control laws:

) 2(

1

1 zz c

k g I m

v = − ψ&& ⁽ ⁾

2 1

2 zz c

k g I m

v = + ψ^&^& (32)

with the following reference values for the attitude angles:

=0

θc and φc=⁰ (33) Here the heading acceleration is given by:

) ( 2ζ ω ω ²ψ ψ₁

ψ&&_c =− ψ ψr− ψ − (34)

Starting from an horizontal attitude ( _θ(0)=0, φ(0)=0), attitude inputs u1 and u2 given by relation (14) remain equal to zero. Then, figures 3 and 4 display some simulation results:

⁰ ^{0 . 5} ¹ ^{1 . 5} ² ^{2 . 5} ³ ^{3 . 5} ⁴ ^{4 . 5} ⁵

0 5 1 0 1 5

Fig 3. Hover control inputs

⁰ ^{0 . 5} ¹ ^{1 . 5} ² ^{2 . 5} ³ ^{3 . 5} ⁴ ^{4 . 5} ⁵

0 0 . 2 0 . 4 0 . 6 0 . 8 1 1 . 2 1 . 4 1 . 6

ψ-ψ^c

time Fig 4. Heading response during hover

B. Trajectory tracking case

In this case we get the guidance control laws:

) 2(

1 ₂ ₂ ₂

2

1 v m g

v = = ρν + (38) Here the permanent reference values for the attitude angles are such as:

=0

θc (39.1) and

2 4 2

2

sin

g

c=− +

ν ρ

ν

φ ρ (39.2) and the desired guidance and orientation accelerations are given by:

0 , 0

) sin(

) cos(

2 2

=

=−

=

−

=

c c

z

t y

t x

ψ ν ν ρν ν ρ

&

(40) Attitude inputs are given by relation (14) where now:

⎥⎦

⎢ ⎤

⎣

⎡ −

− =

a I a I tg tg

a M I

xx xx

yy

/ /

0 ) cos 1 /(

φ

θ φ (41)

and ^N⁽^X^)'⁼[⁰ ⁰] (41) In figures 5 to 7 simulation results are displayed where at initial time the rotorcraft is hovering:

y(t

-2 . 5 -2 -1 . 5 -1 -0 . 5 0 0 . 5 1 1 . 5 2 2 . 5

x(t)

Fig 4. Evolution of rotorcraft horizontal track

F1,F3

0 0 . 5 1 1 . 5 2 2 . 5 3 3 . 5 4 4 . 5 5

0 0 . 0 2 0 . 0 4 0 . 0 6 0 . 0 8 0 . 1 0 . 1 2 0 . 1 4

z(t

F2,F4

time

Fig 5. Evolution of rotorcraft altitude time