Planification de trajectoires robustes 3D pour drones en

Planification de trajectoires robustes 3D pour drones
en environnement dynamique
T. Youssef, Ch. Inarn, P. Melchior et A. Oustaloup
IMS laboratory
(Université Bordeaux 1 - IPB/Enseirb-Matmeca)
[email protected]
Journées Robotique et Automatique (JRA’12), IRCCyN, Nantes, 23-24 Octobre 2012
Introduction : Contexte, Problématique
Collaboration avec FlyFly-N-Sense
http
http://www.fly
://www.fly--n-sense.com/
et dans le cadre du Cluster Aquitain AETOS
« Services et Systèmes de Drones» http://www.aetos-aquitaine.fr/
Problématique du 3D Sense & Avoid
2
Introduction : Principales idées
Contexte
Planification de trajectoire pour atteindre une cible mobile
Potentiels artificiels
Problématique
3D Sense & Avoid en environnement dynamique (obstacles fixes ou mobiles)
Résultats précédents
Outil : potentiels artificiels attractifs et répulsifs fractionnaires
Evitement d’obstacles fixes ou mobiles
Caractérisation par un ordre fractionnaire du danger que présente chaque
obstacle
Détermination de la trajectoire avec prise en compte du danger que présente
chaque obstacle
Objectif
Environnement dynamique : la méthode a été étendue en considérant des
potentiels attractifs (comme Ge and Cui) prenant en compte la position et la
vitesse de l’UAV, de la cible et des obstacles
Prise en compte des variations paramétriques du robot
Extension au cas 3D pour obtenir une planification de trajectoire robuste
3
pour UAV
Introduction : Schéma général
Planification de trajectoire
Objectif:
Atteindre
une cible
Génération
de la
trajectoire





ψ





z ref

(lacet ) 
x ref
y ref
ref
Commande
+_
Génération
de
mouvement
+_
+_
+_
ε 1 (t )
ε 2 (t )
ε 3 (t )
ε 4 (t )
C1(p)
u 1 (t )
actionneur
correcteur
C2(p)
correteur
C3(p)
u 2 (t )
u 3 (t )
correcteur
θ
G2(p)
actionneur
ψ
G3(p)
actionneur
correcteur
C4(p)
φ
G1(p)
u 4 (t )
z
G4(p)
actionneur
4
Plan
Introduction
Contexte
Problématique
35
m =110
rob
30
Planification de trajectoires
z rob (m )
25
20
m =400
15
Potentiels attractifs
Potentiels répulsifs
rob
10
0
x (m)200
rob
400
20
40
60
Modèle dynamique d’un N
N--rotor et linéarisation du modèle
80
100 y (m) 120
rob
160
140
180
Commande
Simulations
Targe
t
Vidéos
Conclusion et Perspectives
5
Planification de trajectoires : Potentiels
Méthode basée sur le champ de potentiel artificiel.
Elle consiste à :
• Modéliser les obstacles avec des champs de potentiels répulsifs
• Modéliser la cible par un champ de potentiel attractif.
r
r
r
U total ( M ) = U att ( M ) + ∑ U repk ( M )
k
Avec :
r
U total (M ) désigne le champ de potentiel artificiel global
r
U att (M ) désigne le champ de potentiel attractif de la cible
r
U repk (M ) désigne le champ de potentiel répulsif associé à l' obstacle k
 x
r  
M =  y  la position du drone dans son plan de travail
 z 
6
Potentiel attractif
Champ potentiel attractif (S.S.Ge et Y.J.Cui)
On définit la force attractive de la manière suivante :
r
Fatt = α p (Pcible - Pdrone ) + α v (Vcible - Vdrone )
αp et αv : coefficients positifs
Minimise la distance drone-cible
Fait converger la vitesse du drone vers celle de la cible
Champ potentiel attractif fractionnaire
Remplacer la vitesse par la dérivée non entière de la position relative
drone-cible
r
d n (Pcible - Pdrone )
FattCRONE = α p (Pcible - Pdrone ) + α v
dt n
7
Peut-être interprété comme un schéma de commande classique où αp et αv sont
les paramètres d’un régulateur PD :
8
Fonction de transfert en boucle ouverte β(s) :
Fonction de transfert en boucle fermée H(s) :
où:
Le facteur d’amortissement dépend de la masse du robot mrob.
Donc la planification de trajectoire n’est pas robuste vis-à-vis des
variations de masse du robot.
9
où αp and αv sont les paramètres d’un PD fractionnaire.
10
Le facteur de résonance et le facteur d’amortissement s’en
déduisent :
Le facteur d’amortissement est indépendant de la masse du robot
Robustesse de la planification de trajectoire.
11
Comparaison avec même paramètres αp, αv
Ge and Cui
mrob is varying from 110 to 400 kg
ωcg from 0.0077 to 0.0027 rad/s
phase margin from 72◦ to 48◦
ξ from 0.85 to 0.45.
Fractional with same αp, αv
mrob is varying from 110 to 400 kg
ωcg from 0.023 to 0.0086 rad/s
phase margin from 61◦ to 59◦
ξ = 0.7.
12
Comparaison avec le même ωcg que Ge and Cui pour la masse nominale du robot
mrob=150 kg, ωcg =0.0058 rad/s
Ge and Cui
Fractional with same ωcg
αp=0.002x0.22
αv= 0.8x0.22
mrob is varying from 110 to 400 kg,
ωcg from 0.0077 to 0.0027 rad/s
phase margin from 72◦ to 48◦
ξ from 0.85 to 0.45.
mrob is varying from 110 to 400 kg
ωcg from 0.0073 to 0.0028 rad/s
phase margin from 59◦ to 54◦
ξ = 0.7.
13
Application en 3D
Simulation sans obstacle
Paramètres
mrob = [110, 150, 190, 250, 400]
Masse nominale de 150 kg
Paramètres Ge and Cui : αp=0.002 et αv= 0.8
Ordre fractionnaire n = 0.7
14
Application en 3D
30
35
28
mrob=110
26
30
24
mrob=400
25
mrob=110
20
zrob (m )
z ro b (m )
22
18
20
mrob=400
16
14
15
12
10
0
10
0
200
400
20
40
60
80
yrob(m)
100
120
140
400
20
80
60
40
yrob(m)
160
180
35
m =110
rob
30
25
z rob (m )
xrob(m)
xrob(m)
Ge and Cui
200
Fractional
140
100with
120 same
α160p, αv180
Planification
de
trajectoire
robuste et plus rapide malgré les
variations de masse du drone
20
mrob=400
15
10
0
xrob(m)200
400
20
40
60
80
Fractional
160
140
15
with same ωcg
100 y (m) 120
rob
180
Potentiel répulsif
Potentiel fractionnaire répulsif normalisé
Basée sur la définition de Weyl :
Niveau du danger
100%
rmin
rmax
y
0%
16
x
Cible fixe, 1 obstacle fixe et 2 mobiles
Planification
de
trajectoire
robuste et plus rapide malgré les
variations de masse du drone
Même αp, αv
17
Cible mobile, 1 obstacle fixe et 2 mobiles
Même αp, αv
Target
Planification
de
trajectoire
robuste et plus rapide malgré les
variations de masse du drone
18
Champ potentiel total
A une position z donnée, avec la superposition de tous les champs répulsifs,
des différents obstacles existant, et le champ de potentiel attractif de la cible,
on aura le champ de potentiel total de l’espace de travail.
Le potentiel attractif forme un puits attractif.
Le potentiel répulsif forme une colline répulsive.
Le drone va atteindre la cible en suivant le flux du gradient négatif de ce
potentiel total.
potentiel répulsif
potentiel total
+
potentiel attractif
19
Modélisation dynamique d’un NN-rotors
Hypothèses:
N-rotor parfaitement symétrique (N paire)
Moteur (1) nez du N-rotor
 u1 → commande altitude
 u → commande roulis
u 2→ commande tan gage
 3
 u 4 → commande lacet
Procédé
x
φ

θ ↔ y
ψ
z

r
Roulis : φ Rotation autour de l’axe (O,x), -π <φ <π .
r
Tangage : θ Rotation autour de l’axe (O, y), -π 2 <θ <π 2 .
r
Lacet : Ψ Rotation autour de l’axe (O,z), -π <ψ <π .
20
Les équations du mouvement selon les axes de Roulis, Tangage et Lacet:

 2 N +1


360
bl
I rotor & N
 Iz − Iy &


k +1

2
&
&
sin
1
*
.
1
.
=
−
(
k
−
)
Ω
+
(
−
)
Ω
φ
θ
θψ&


 k
∑
 ∑
k +

I x  k = 2,k ≠ N +1
N
I
I

 
 k =1

x
x

2





2 N +1


 I rotor & N
360
bl
 I −I


2
2
2
&
&
φ  ∑ (− 1)k .Ω k  + z x φ&ψ&
.Ω k  +
θ =  − Ω1 + Ω N +1 + ∑ cos (k − 1)*
Iy 
N   I y  k =1
Iy
N


2

k = 2 , k ≠ +1
2



d  N
 Ix − Iy & &
k

&
&
ψ
θφ
=  ∑ (− 1) .Ω k2  +

I z  k =1
Iz



Les équations des accélérations selon les 3 axes de l’espace:
(
cosψ sin θ cos φ + sinψ sin φ ) N

bΩ i2
∑
 &x& =
m
i =1

(
sinψ sin θ cos φ − cosψ sin φ ) N

bΩi2
 &y& =
∑
m
i =1

N
cos θ cos φ

2
&
&
z
=
−
g
+
b
Ω
∑
i

m
i =1

Avec :
I rotor : moment d' inertie du moteur ( kg.m 2 ),
b : coefficient de poussée ( kg.m.rad -2 ),
l : demi - envergure du drone (en m),
d : coefficient de trainée ( kg.m 2 .rad -2 )
I x = ∫ y 2 + z 2 dm, I y = ∫ x 2 + z 2 dm et I z = ∫ y 2 + x 2 dm : moments d' inertie selon les axes X, Y et Z.
Ωi : vitesse de rotation d' un moteur " i ".
(
)
(
)
(
)
21
Linéarisation du modèle d’un quadriquadri-rotors
Exemple Quadri-rotor: Linéarisation du modèle
On considère les entrées du modèle dynamique comme suit :
4
U1 = ∑ bΩ i2
i =1
(
)
= b(Ω − Ω )
= d (− Ω + Ω
U 2 = b Ω 24 − Ω 22
U3
U4
2
3
2
1
2
1
2
2
− Ω 32 + Ω 24
)
Et la perturbation suivante :
Ω = −Ω1 + Ω 2 − Ω 3 + Ω 4
On cherche une représentation d' état sous la forme :
X& = f ( X , U )
Avec :
X = (x1 K x12 ) ∈ ℜ12
X = [x1 = φ
x2 = x&1
x3 = θ
x4 = x&3
x5 = ψ
x6 = x&5
x7 = z
x8 = x&7
x9 = x
x10 = x&9
x11 = y
x12 = x&11 ]
Recherche du point de fonctionnement (Point d’équilibre):
f ( X 0 , U 0 ) = 0 ↔ [0 0 0 0 0 0 0 U10 = mg 0 0 0 0]
En vol stationnaire, la
portance U1 compense
le poids du drone.
Validation du modèle.
22
Exemple Quadri-rotor: Linéarisation du modèle
On considère les entrées du modèle dynamique comme suit :
4
U1 = ∑ bΩ i2
i =1
(
)
= b(Ω − Ω )
= d (− Ω + Ω
U 2 = b Ω 24 − Ω 22
U3
U4
2
3
2
1
2
1
2
2
− Ω 32 + Ω 24
)
Et la perturbation suivante :
Ω = −Ω1 + Ω 2 − Ω 3 + Ω 4
Ce qui mène au modèle dynamique linéarisé du quadri-rotor:

Iz − I y &
bl 2
I
θψ&
Ω 4 − Ω 22 + rotor θ&(Ω1 − Ω 2 + Ω 3 − Ω 4 ) +
 φ&& =
Ix
Ix
Ix

 && bl 2
I
I −I
Ω1 − Ω 32 + rotor φ&(− Ω1 + Ω 2 − Ω 3 + Ω 4 ) + z x φ&ψ&
θ =
Iy
Iy
Iy

Ix − Iy & &

d
2
2
2
2
&
&
=
−
Ω
+
Ω
−
Ω
+
Ω
+
ψ
θφ

1
2
3
4
I
I

z
z
(
)
(
)
(
)

1 4
2
 &x& = m ∑ bΩ i
i =1

−1 4

&
&
y
=
bΩ i2

∑
m i =1

4
&z& = − g + 1
2
b
Ω
∑
i

m i =1

23
Commande
Planification de trajectoire
Objectif:
Atteindre
une cible
Génération
de la
trajectoire





ψ





z ref

( lacet ) 
x ref
y ref
ref
Contrôle/Commande
+_
Génération
du
mouvement
+_
+_
+_
ε 1 (t )
ε 2 (t )
ε 3 (t )
ε 4 (t )
C1(p)
u 1 (t )
actionneur
correcteur
C2(p)
correteur
C3(p)
correcteur
C4(p)
correcteur
φ
G1(p)
u 2 (t )
u 3 (t )
θ
G2(p)
actionneur
ψ
G3(p)
actionneur
u 4 (t )
z
G4(p)
actionneur
24
Découplage des sorties par analyse RGA :
Réseau de découplage (RGA)
Procédé 2x2
u1
G*c,11(p)
G11(p)
G*c,12(p)
G12(p)
G*c,21(p)
G21(p)
G*c,22(p)
G22(p)
y1
u2
u1
G .G c* = matrice RGA
y2
u2
 Si λ ij < 0 . 5 λ ij = 0
avec 
λ ij = λ ij
 Sinon
y1
Résultat du découplage →
y2
25
Simulation:
Allure de la sortie pour 1 radians pour les trois sorties (Roulis, Tangage, Lacet) et 1 mètre en
consigne:
26
Correcteur
e1
e2
+
_
+_
C1(p)
C2(p)
Réseau de découplage
u1
u2
Procédé 2x2
y1
G*c,11(p)
G11(p)
G*c,12(p)
G12(p)
G*c,12
c,21(p)
G*
(p)
G21(p)
y2
G*c,22(p)
Correcteur :

CRONE 2 nd génération :
m'

p 


1+

ω

b 

, avec m ' < 1
C ( p ) = C 0 
p 

1+ ω 

h 

G22(p)
PIDF :
Ou



τd p 
1

C ( p ) = C 0 1 +
+
τi p 1+ τd p 



N


27
Vidéo d’une simulation Python
28
Conclusion : Principales contributions
3D Sense & Avoid
Environnement dynamique
35
m =110
rob
30
25
z rob (m )
Potentiels attractifs/répulsifs
fractionnaires
20
m =400
15
Prise en compte du danger des obstacles
rob
10
0
x (m)200
rob
Modèle dynamique d’un UAV NN-rotors
400
20
40
60
80
100 y (m) 120
rob
140
Variations paramétriques
de l’UAV
> Planification 3D robuste
> Commande robuste
Targe
t
Simulations & Vidéos (Python)
Perspectives : implantation
160
29
180