aim 2013 talk

Adaptive trajectory tracking for quadrotor

MAVs in presence of parameter uncertainties

and external disturbances

Gianluca Antonelli†, Filippo Arrichiello†,Stefano Chiaverini†, Paolo Robuffo Giordano⊕,

in alphabetical order

†University of Cassino and Southern Lazio, Italyhttp://webuser.unicas.it/lai/robotica

⊕CNRS at IRISA, Francehttp://www.irisa.fr/lagadic

kindly presented by prof. Bruno Siciliano

AntonelliArrichielloChiaverini Robuffo-Giordano Wollongong, 12 July 2013

Trajectory tracking control for quadrotor

Adaptive with respect to

uncertainties in total massuncertainties in Center Of Gravity (COG)presence of 6-DOF external disturbances

Assumption: closed-loop orientation dynamics faster thantranslational one

Stability analysis

Numerical simulations

Experimental results (not in the paper)


Kinematics

earth-fixed

O x

z

y

η1

body-fixed

Ob

xb

zb

yb

u, surge

w, heave

v, sway

p, roll

r, yaw

q, pitch η1 =[

x y z]T

η2 =[

φ θ ψ]T

ν1 = RBI η̇1

ν2 = Jk,o(η2)η̇2


Dynamics

Mathematical model expressed in body-fixed frame

MRBν̇ +CRB(ν)ν + τ v,W + gRB(RBI ) = τ v,

beyond the common terms, we model

τ v,W = Φv,W (RIB)γv,W =

[

RBI O3×3

O3×3 RBI

]

γv,W

whit γv,W ∈ R6 external disturbance constant in the inertial

frame, e.g., wind


Dynamics -2-

Exploiting the linearity in the parameters

Φv(ν̇ ,ν,RBI )γv = τ v

and rewriting with respect to the inertial frame while separating the xydynamics from z:

[

Φxy(η, η̇, η̈)φz(η, η̇, η̈)

]

γv = τ v

with γv ∈ R16:

mass (1 parameter)

first moment of inertia (3 p.)

inertia tensor (6 p.)

external disturbance (6 p.)

with:

τ v =

[

τ 1

τ 2

]

=

00Z

K

M

N


Thrust

xy

z

O

xbyb zb

Ob

f1

f2

f3

f41

2

3

4

ωt,1

ωt,2

ωt,3

ωt,4

τt,1

τt,2

τt,3

τt,4

l

fi = bω2t,i

τt,i = dω2t,i

τ 1 =

00

4∑

i=1

fi

τ 2 =

l(f2 − f4)l(f1 − f3)

−τt,1 + τt,2 − τt,3 + τt,4


Mapping from the angular velocities to the force-torques

Z

K

M

N

= Bv

ω2t,1

ω2t,2

ω2t,3

ω2t,4

with

Bv=

b b b b

0 b(l + rC,y) 0 −b(l − rC,y)b(l + rC,x) 0 −b(l − rC,x) 0

−d d −d d

rC,x, rC,y being the COG coordinates

COG influences the mapping from thrust generated from the

motors to the vehicle forces/moments


Inverse mapping

Any controller determines a control action[

Zc Kc Mc Nc

]Tfurther

projected onto the motor input u ∈ R4

u = B−1v

Zc

Kc

Mc

Nc

where B−1v ∈ R

4×4 is

B−1v =

l − rC,x

4bl0

1

2bl−l − rC,x

4dll − rC,y

4bl

1

2bl0

l − rC,y

4dll + rC,x

4bl0 −

1

2bl−l + rC,x

4dll + rC,y

4bl−

1

2bl0

l + rC,y

4dl


Current inverse mapping

When the COG position estimate r̂C is affected by an error, the real

mapping becomes

Z

K

M

N

= Bv|rCB−1

v

∣

∣

r̂C

Zc

Kc

Mc

Nc

=

1 0 0 0r̃C,y

21 0

br̃C,y

2dr̃C,x

20 1 −

br̃C,x

2d0 0 0 1

Zc

Kc

Mc

Nc

wrong COG estimate ⇒ a coupling from altitude and yaw control

actions onto roll and pitch dynamics


Controller block diagram

η1d

ψd

φd, θd

Zc

posor

Kc

Mc

Nc

B−1

v

umotors

w2

t,iBv

Z

K

M

N

τ v,W

η

plant

Classical MAV control architecture with adaptation wrt parametersand compensation of the COG position


Altitude controller

error

z̃ = zd − z ∈ R

sz = ˙̃z + λz z̃ ∈ R

full version

Z =1

cosφ cos θ(φzγ̂v + kpzsz)

˙̂γv = K−1γ,zφ

Tz sz

with γ̂v ∈ R16

reduced version

Z =1

cosφ cos θ(γ̂z + kpzsz)

˙̂γz = k−1γ,zsz

with γ̂z ∈ R1

the reduced version designed to compensate only for persistent

terms ⇒ null steady state error wrt a minimal set of parameters!

(λz > 0, kpz > 0,Kγ,z > O, kγ,z > 0)


Horizontal controller

error

η̃xy =[

xd − x yd − y]T

∈ R2

sxy = ˙̃ηxy + λxyη̃xy ∈ R2

full version

virtual input solutions of:

[

cφsθ−sφ

]

=1

ZRz (Φxyγ̂v + kp,xysxy) ,

˙̂γv = K−1γ,xyΦ

Txysxy.

with γ̂v ∈ R16

reduced version

virtual input solutions of:

[

cφsθ−sφ

]

=1

ZRz

(

γ̂xy + kp,xysxy)

˙̂γxy = k−1γ,xysxy

with γ̂xy ∈ R2

again: the reduced version compensates only for persistent terms

⇒ null steady state error wrt a minimal set of parameters!

(λxy > 0, kp,xy > 0,Kγ,xy > O, kγ,xy > 0)


Orientation controller

The inputs are the desired roll, pitch and yawThe commanded forces map onto the real ones according to

K = Kc +r̃C,y

2Zc +

br̃C,y

2dNc

M = Mc +r̃C,x

2Zc −

br̃C,x

2dNc

N = Nc

Neither the altitude nor the yaw control loop are affected by r̃C , thus both Zc

and Nc convergence to a steady state valueRoll and pitch control can be designed by considering the estimation error asan external, constant, disturbance:

K = Kc +1

2

(

Zc +b

dNc

)

r̃C,y

M = Mc +1

2

(

Zc −b

dNc

)

r̃C,x

The disturbance value is unknown and its effect may be compensated by

resorting to several adaptive control laws well known in the literature


COG estimation

PD control for roll and pitch =⇒ steady-state error because of thewrong COG estimateA simple integral action can counteract this effect resulting a zerosteady-state error

[

˙̂rC,x

˙̂rC,y

]

= −krC

[

θd − θ

φd − φ

]

, krC > 0

As a byproduct, in absence of moment disturbance, the estimates(r̂C,x, r̂C,y) are driven towards the real COG offsets (rC,x, rC,y)


Stability analysis

Altitude controller: let γ̃v = γv − γ̂v and consider the Lyapunovfunction

V (sz, γ̃v) =m

2s2z +

1

2γ̃Tv Kγ,zγ̃v

Along the system trajectories

V̇ (sz, γ̃v) = sz(

mz̈d −mz̈ +mλz ˙̃z)

− γ̃Tv Kγ,z

˙̂γv

= sz (φzγv − cosφ cos θZ)− γ̃Tv Kγ,z

˙̂γv = −kp,zs2z ≤ 0

State trajectories are boundedAsymptotic stability can be further proven by resorting to Barbalat’sLemma as in classical adaptive control schemesSimilar machinery for the horizontal controller case


Simulation results

Constant disturbance

γv,W =

0.50.60000

[N, Nm]

displacement of 1m in the 3directions x, y and z20 deg in yaw

0 2 4 6 8 10 12 14 16 18

0

0.5

1

1.5

0 2 4 6 8 10 12 14 16 18

0

5

10

15

20

norm of the 3D position error

[m]

norm of the yaw error

[deg]

time [s]

time [s]

proposed control law (blue-solid line) andits non adaptive version (red-dashed line)


Simulation results

0 2 4 6 8 10 12 14 16 18 20−10

−5

0

5

10

0 2 4 6 8 10 12 14 16 18 20−10

−5

0

5

10

rollpitch

[deg]rollpitch

[deg]

time [s]

time [s]

Roll and pitch angle; on the top the desired (gray) and real (blue)values for the adaptive version while on the bottom the desired (graydashed) and real (red dashed) values for the non adaptive simulation


Simulation results

0 0.5 1 1.5 2 2.5 3 3.5 4 4.513

14

15

16

17

18

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8−0.2

−0.1

0

0.1

0.2

time [s]

time [s]Z

[N]

τ2[N

m]

Force along zb (top) and moments (bottom) by applying the proposedcontrol law (blue-solid line) and its non adaptive version (red-dashedline)


Simulation results

0 2 4 6 8 10 12 14 16 18

14.7

14.8

14.9

0 2 4 6 8 10 12 14 16 180

0.2

0.4

0.6

0 2 4 6 8 10 12 14 16 18

−0.1

−0.05

0

0.05

0.1

time [s]

time [s]

time [s]γz

γxy

r C,x,r C

,y

Simulated parameters (gray) and estimated ones (blue)


Experimental results

additionalweight

Experiments on a mikrokopter with an unknown weight attachedTheory and numerical simulations confirmedResults on a forthcoming publication


Adaptive trajectory tracking for quadrotor

MAVs in presence of parameter uncertainties

and external disturbances

Gianluca Antonelli†, Filippo Arrichiello†,Stefano Chiaverini†, Paolo Robuffo Giordano⊕,

in alphabetical order

†University of Cassino and Southern Lazio, Italyhttp://webuser.unicas.it/lai/robotica

⊕CNRS at IRISA, Francehttp://www.irisa.fr/lagadic

kindly presented by prof. Bruno Siciliano


aim 2013 talk

Technology

x y z t

z rz xy v

version z

d d d d rc

cos cos z v

parameters v

kpzsz z

kpzsz v