Small angle control Conclusion

Robots Aériens et SOus-Marins (RASOM)

Contrôle (Partie 2)

Abdelhamid.Chriette@ec-nantes.fr

Option Robotique
Département Auto-Robot – École Centrale de Nantes.

23 novembre 2020

Abdelhamid Chriette (CN) RASOM 23 novembre 2020 1 / 57

Small angle control Conclusion

1 Small angle control

Rappels : modèle dynamique du Quadrotors
Proportional Derivative (PD) Control
Sliding Mode Control Control(SMC)
Backstepping Control(BSC)

2 Conclusion

Abdelhamid Chriette (CN) RASOM 23 novembre 2020 2 / 57

Small angle control Conclusion

Rappels : modèle dynamique du Quadrotors

Quadrotor Dynamics

n Dynamique de translation exprimée par rapport au repère inertiel :

   
0 0
mr̈ =  0  +W RB  0  (1)
−mg F1 + F2 + F3 + F4

n Dynamique de rotation exprimée dans le repère local du drone :

       
ṗ L(F2 − F4 ) p p
I  q̇  =  L(F3 − F1 ) − q  × I q  (2)
ṙ M2 + M4 − M1 − M3 r r

où r = [x y z]T , I = moment d’inertie, m = masse du drone, [p q r] :

composantes du vecteur vitesse de rotation du CM du drone.

Abdelhamid Chriette (CN) RASOM 23 novembre 2020 3 / 57

Small angle control Conclusion

Rappels : modèle dynamique du Quadrotors

Quadrotor Dynamics

n Vecteur entrée force :

u1 = F1 + F2 + F3 + F4

n Vecteur entrée couple (moment) :

 
L(F2 − F4 )
u2 =  L(F3 − F1 ) 
M2 + M4 − M1 − M3

Abdelhamid Chriette (CN) RASOM 23 novembre 2020 4 / 57

Small angle control Conclusion

Proportional Derivative (PD) Control

Figure 2.3: The nested control loops for position and attitude control.
Figure – Boucles de contrôle imbriquées pour le contrôle de la position et de l’attitude.

2.2 Small Angle Control

n La boucle de contrôle d’attitude interne utilise des accéléromètres et des
Each robot
gyroscopes is controlled pour
embarqués independently by nested
contrôler feedbackleloops
le roulis, as shown
tangage et inleFig. 2.3. et
lacet
fonctionne
The inneràattitude
environ 1 kHz.
control loop uses onboard accelerometers and gyros to control the roll,

n pitch, and
La boucle deyaw and runs at
contrôle deapproximately 1 kHz [34],utilise
position externe while thedes
outerestimations
position controlde
loopla
position et de laofvitesse
uses estimates du velocity
position and centreofdethemasse
center ofpour contrôler
mass to control the la trajectoire
trajectory in en
trois three
dimensions.
dimensions. Similar nesting of control loops is presented in previous works [13, 21,
30, 34, 40].
Our controllers are derived by linearizing the equations of motion and motor models
(3.3 – 2.4) at an operating point that corresponds to the nominal hover state, r = r0 ,
✓=
Abdelhamid = 0,(CN)
Chriette = , ṙ = 0, and ˙ = ✓˙ =RASOM
˙ = 0, where the roll and pitch angles are small 2020
23 novembre 5 / 57
Small angle control Conclusion

Proportional Derivative (PD) Control

Linéarisation du modèle

n Nos contrôleurs sont dérivés en linéarisant les équations des modèles de

mouvement et de moteur à un point de fonctionnement qui correspond à
l’état de vol stationnaire nominal :
r = r0 , θ = φ = 0, ψ = ψ0 , ṙ = 0 et θ̇ = φ̇ = ψ̇ = 0
où les angles de roulis et de tangage sont faibles (cφ ≈ 1, cθ ≈ 1, sφ ≈ φ et
sθ ≈ θ)
n Dans cet état de vol stationnaire, les poussées nominales des hélices doivent
satisfaire :
mg
Fi,0 =
4
et les vitesses des moteurs sont données par
r
mg
ωi,0 = ωh =
4kF

Abdelhamid Chriette (CN) RASOM 23 novembre 2020 6 / 57

Small angle control Conclusion

Proportional Derivative (PD) Control

Attitude Control

Nous souhaitons concevoir un contrôleur d’attitude pour suivre les trajectoires en

SO(3) pour un vol proche de l’état de vol stationnaire nominal où les angles de
roulis et de tangage sont faibles.

Si l’on fait l’hypothèse que les produits d’inertie sont faibles (idéalement, ils sont
nuls car les axes sont proches des axes principaux) et Ixx ≈ Iyy à cause de la
symétrie alors :

Ixx ṗ = u2 − qr(Izz − Iyy ) (3)

Iyy q̇ = u3 − pr(Ixx − Izz ) (4)
Izz ṙ = u4 (5)

Abdelhamid Chriette (CN) RASOM 23 novembre 2020 7 / 57

Small angle control Conclusion

Proportional Derivative (PD) Control

Attitude Control

Nous pouvons également supposer que la composante de la vitesse angulaire dans

la direction zB , r, est faible, de sorte que les termes les plus à droite dans (3) et
(4) qui sont des produits impliquant r sont faibles par rapport aux autres termes.

Pour ces raisons, nous pouvons utiliser des lois de contrôle PD simples qui
prennent la forme par rapport aux autres termes. On note que près de l’état de vol
stationnaire nominal φ̇ ≈ p, θ̇ ≈ q et ψ̇ ≈ r

u2,des = kp,φ (φdes − φ) + kd,φ (pdes − p) (6)

des des
u3,des = kp,θ (θ − θ) + kd,θ (q − q) (7)
des des
u4,des = kp,ψ (ψ − ψ) + kd,ψ (r − r) (8)

Abdelhamid Chriette (CN) RASOM 23 novembre 2020 8 / 57

Small angle control Conclusion

Proportional Derivative (PD) Control

Attitude Control

Le vecteur des vitesses de rotor souhaitées peut être trouvé à partir de la force
u1,des , et des moments u2,des , u3,des et u4,des désirés en inversant :

   Ω2 
kF kF kF kF 1,des
 0   Ω2 
lkF 0 −lkf   2,des 
udes =
 −lkF  
0 lkF 0   Ω23,des 
kM −kM kM −kM Ω24,des

Abdelhamid Chriette (CN) RASOM 23 novembre 2020 9 / 57

Small angle control Conclusion

Proportional Derivative (PD) Control

Position Control

Nous présentons ici deux méthodes représentatives de contrôle de position qui

utilisent les angles de roulis et de tangage comme données d’entrée via une
méthode similaire au backstepping.

- La première, un contrôleur de vol stationnaire (Hover Controller), est

utilisée pour maintenir la position à un endroit souhaité en x, y et z.
- La deuxième (3D Trajectory Control) suit une trajectoire en trois
dimensions.

Abdelhamid Chriette (CN) RASOM 23 novembre 2020 10 / 57

Small angle control Conclusion

Proportional Derivative (PD) Control

Position Control

n Hover Controller

Ici, nous utilisons l’angle de tangage et de roulis pour contrôler la position dans les
plans xW et yW , u4 pour contrôler angle de lacet, et u1 pour contrôler la position
le long de zW a . rT (t) et ψT (t) sont, respectivement, la trajectoire et l’angle de
lacet que le drone essayera de suivre (Rappel : ψT (t) = ψ0 en vol stationnaire).
a. W : World

Les accélérations désirées, r̈des

i , sont calculées à partir du retour PID de l’erreur
de position ei = (ri,T − ri ), comme suit :
Z
des
(r̈i,T − r̈i ) + kd,i (ṙi,T − ṙi ) + kp,i (ri,T − ri ) + (ri,T − ri ) = 0

où ṙi = r̈i = 0 dans les conditions de vol stationnaire.

Abdelhamid Chriette (CN) RASOM 23 novembre 2020 11 / 57

Small angle control Conclusion

Proportional Derivative (PD) Control

Position Control

Hover Controller
La linéarisation de la dynamique de translation nous permettra d’obtenir la relation entre les
accélérations souhaitées et les angles de roulis et de tangage comme suit :
r̈1des = g(θdes cos ψT + φdes sin ψT )
r̈2des = g(θdes sin ψT − φdes cos ψT )
u1,des
r̈3des =
m
Ces relations sont inversées pour calculer les angles de roulis et de tangage souhaités pour le
contrôleur d’attitude, à partir des accélérations souhaitées, ainsi que de u1,des :
1 des
φdes = (r̈ sin ψT − r̈2des cos ψT )
g 1
1 des
θdes = (r̈ cos ψT + r̈2des sin ψT )
g 1
u1,des = mr̈3des

Abdelhamid Chriette (CN) RASOM 23 novembre 2020 12 / 57

Small angle control Conclusion

Sliding Mode Control Control(SMC)

SMC

Non Linear Robust Control Technique.

Variable Structure Control(VSC).
Controller output is discontinous, high frequency switching controller.

Abdelhamid Chriette (CN) RASOM 23 novembre 2020 13 / 57

Small angle control Conclusion

Sliding Mode Control Control(SMC)

SMC

The Sliding Mode Controller is designed only for the attitude control part.
PD control is used for altitude control owing to simplicity of the altitude
control equation.

Abdelhamid Chriette (CN) RASOM 23 novembre 2020 14 / 57

Small angle control Conclusion

Sliding Mode Control Control(SMC)

SMC

Approximating with near hover conditions : p ≈ φ̇, q ≈ θ̇ and r ≈ ψ̇.

The dynamics of attitude is given as :

Iω̈ = u2 − ω̇ × Iω̇ ⇒ ω̈ = −I−1 ω̇ × Iω̇ + I−1 u2 (9)

 T
Where, ω = φ θ ψ
The error e(t) is defined as, e(t) = ω − ωc . Since the relative degree of the system
is 2, the sliding variable can be defined as :

s = ė + λe

Abdelhamid Chriette (CN) RASOM 23 novembre 2020 15 / 57

Small angle control Conclusion

Sliding Mode Control Control(SMC)

SMC

Differentiating once,

ṡ = ë + λė
ṡ = ω̈ − ω̈c + λ(ω̇ − ω̇c )
ṡ = −I−1 ω̇ × Iω̇ + I−1 u2 − ω̈c + λ(ω̇ − ω̇c ) (10)
−1 −1
ṡ = −I ω̇ × Iω̇ − ω̈c + λ(ω̇ − ω̇c ) + I u2
ṡ = αSM + βSM u2

Where αSM = −I−1 ω̇ × Iω̇ − ω̈c + λ(ω̇ − ω̇c ) and βSM = I−1 .
ωc can be calculated from Equation 9 and 10.

Abdelhamid Chriette (CN) RASOM 23 novembre 2020 16 / 57

Small angle control Conclusion

Sliding Mode Control Control(SMC)

SMC

For nominal control, the control law will be chosen as :

−1
u2 = βSM (−αSM + v)

For sliding mode, v has to be chosen as :

v = −K ∗ sign(s)

Here, K is the sliding mode gain matrix. Using the above equations, the control
input for controlling the altitude is :
−1
u2 = βSM (−αSM − K ∗ sign(s)) (11)

Abdelhamid Chriette (CN) RASOM 23 novembre 2020 17 / 57

Small angle control Conclusion

Sliding Mode Control Control(SMC)

SMC

λ and K are gain matrices.

 
λ1 0 0
λ =  0 λ2 0 
0 0 λ3
 
K1 0 0
K =  0 K2 0 
0 0 K3

Abdelhamid Chriette (CN) RASOM 23 novembre 2020 18 / 57

Small angle control Conclusion

Backstepping Control(BSC)

BSC

Recursive control algorithm

Works by designing intermediate control laws for some of the state variables.
Does not cancel the non-linearities in the system, since some of the non linear
terms can contribute to the stability of the system.

Abdelhamid Chriette (CN) RASOM 23 novembre 2020 19 / 57

Small angle control Conclusion

Backstepping Control(BSC)

BSC

The dynamic model is re-written for the sake of convenience. The state vairables
are renamed as follows : x1 = φ, x2 = ẋ1 , x3 = θ, x4 = ẋ2 , x5 = ψ, x6 = ẋ5 ,
x7 = z, x8 = ż, x9 = x, x10 = ẋ9 , x11 = y and x12 = ẋ11 .
The control inputs are :
1. Thrust=u1
2. Rolling Input=u21
Pitching Input=u22
Yawing Input=u23
Iyy −Ixx Izz −Ixx Ixx −Iyy
The following parameters are used : a1 = Ixx , a2 = Iyy , a3 = Izz ,
1 1 1
b1 = Ixx , b2 = Iyy and b3 = Izz .

Abdelhamid Chriette (CN) RASOM 23 novembre 2020 20 / 57

Small angle control Conclusion

Backstepping Control(BSC)

Using these state variables and the parameters, the dynamic model can be written
as :    
ẋ1 x2
 ẋ2   a 1 4 6 + b1 u21
x x 
   
 ẋ3   x4 
   
 ẋ4   a 2 2 6 + b2 u22
x x 
   
 ẋ5   x6 
   
 ẋ6   a 3 2 4 + b3 u23
x x 
    (12)
 ẋ7  =  x8 
   
 ẋ8   −g + b (cos x1 cos x3 )u1 
   4 
 ẋ9   x 
   10 
 ẋ10   b4 (cos x5 sin x3 + sin x5 cos x3 sin x1 )u1 
   
 ẋ11   x12 
ẋ12 b4 (sin x5 sin x3 − cos x5 cos x3 sin x1 )u1

Abdelhamid Chriette (CN) RASOM 23 novembre 2020 21 / 57

Small angle control Conclusion

Backstepping Control(BSC)

BSC : Roll Controller

The states x1 and x2 are the roll and its rate of change.
   
ẋ1 x2
= (13)
ẋ2 a1 x4 x6 + b1 u21

A simple positive definite Lyapunov Function is picked :

1 2
V1 = z (14)
2 1
where : z1 = x1c − x1

V̇1 = z1 ż1 = z1 (ẋ1c − ẋ1 ) = z1 (ẋ1c − x2 ) (15)

A positive definite bounding function is picked which is an bound on V̇1 as given

in Equation 19

V̇1 = z1 (ẋ1c − x2 ) ≤ −c1 z12 (16)

Abdelhamid Chriette (CN) RASOM 23 novembre 2020 22 / 57

Small angle control Conclusion

Backstepping Control(BSC)

BSC : Roll Controller

c1 is a positive constant. To satisfy inequality 19 the virtual control input is
chosen to be :

(x2 )desired = ẋ1c + c1 z1 (17)

Rewriting Equation 18

V̇1 = z1 ż1 = z1 (ẋ1d − x2 ) = z1 (ẋ1d − (z2 + ẋ1d + c1 z1 ))

(18)
⇒ V̇1 = −z1 z2 − c1 z12

The next step is to augment the first Lyapunov function V1 with a quadratic term
in the second variable z2 to get a positive definite V2 .
1
V2 = V1 + z22
2
⇒ V̇2 = V̇1 + z2 ż2 (19)

⇒ V̇2 = −z1 z2 − c1 z12 + z2 (ẋ2 − ẍ1c − c1 ż1 )

Abdelhamid Chriette (CN) RASOM 23 novembre 2020 23 / 57

Small angle control Conclusion

Backstepping Control(BSC)

BSC : Roll Controller

Choosing a positive definite bounding function and substituing the model (ẋ2 )
into 22 leads to the following :

−z1 z2 − c1 z12 + z2 (a1 x4 x6 + b1 u21 − ẍ1d − c1 ż1 ) ≤ −c1 z12 − c2 z22 (20)

Using the equality case of Equation 23, we get :

1
u21 = (ẍ1d + c1 ż1 − a1 x4 x6 + z1 − c2 z2 ) (21)
b1
Equation 24 is the controller output for tracking the commanded roll angle.
The required pitch, yaw and the altitude can also be tracked using the same
design methodology that is followed for the roll angle. The controller out outs are
presented in the next slide.

Abdelhamid Chriette (CN) RASOM 23 novembre 2020 24 / 57

Small angle control Conclusion

Backstepping Control(BSC)

BSC : Pitch, Yaw and Altitude Control Laws

Pitch Control :
1
u22 = (ẍ3d + c3 ż3 − a2 x2 x6 + z3 − c4 z4 ) (22)
b2

Yaw Control :
1
u23 = (ẍ5d + c5 ż5 − a3 x2 x4 + z5 − c6 z6 ) (23)
b3

Altitude Control :
1
u1 = (ẍ7d + c7 ż7 − c8 z8 + g + z7 ) (24)
b4 cos x1 cos x3

Abdelhamid Chriette (CN) RASOM 23 novembre 2020 25 / 57