Planification de trajectoires robustes 3D pour drones...
Transcript of Planification de trajectoires robustes 3D pour drones...
Planification de trajectoires robustes 3D pour drones en environnement dynamiqueen environnement dynamique
T. Youssef, Ch. Inarn, P. Melchioret A. Oustaloup
IMS laboratory(Université Bordeaux 1 - IPB/Enseirb-Matmeca)
Journées Robotique et Automatique (JRA’12), IRCCyN, Nantes, 23-24 Octobre 2012
Collaboration avec FlyCollaboration avec Fly--NN--SenseSense httphttp://www.fly://www.fly--nn--sense.com/sense.com/
et dans le cadre du Cluster et dans le cadre du Cluster Aquitain Aquitain AETOS AETOS «« Services et Systèmes de Drones» http://www.aetos-aquitaine.fr/
IntroductionIntroduction : Contexte, Problématique: Contexte, Problématique
2
Problématique du 3D Problématique du 3D SenseSense & & AvoidAvoid
3D Sense & Avoid en environnement dynamique (obstacles fixes ou mobiles) Résultats précédents
Outil : potentiels artificiels attractifs et répulsifs fractionnaires
Problématique
Contexte Planification de trajectoire pour atteindre une cible mobile Potentiels artificiels
IntroductionIntroduction : Principales idées: Principales idées
Evitementd’obstaclesfixesoumobiles
3
Caractérisation par un ordre fractionnaire du danger que présente chaqueobstacleDétermination de la trajectoire avecprise en compte du danger que présentechaque obstacle
Environnement dynamique: la méthode a été étendue en considérant despotentiels attractifs (comme Ge and Cui) prenant en compte laposition et lavitesse de l’UAV, de la cible et des obstacles
Prise en compte des variations paramétriques du robotObjectif
Extension au cas3D pour obtenir une planification de trajectoire robustepour UAV
Evitementd’obstaclesfixesoumobiles
IntroductionIntroduction : Schéma général: Schéma général
4
CommandePlanification de trajectoire
Objectif:
Atteindre
une cible Génération
de la
trajectoire ψ
z
θ
φ
)(lacet
z
y
x
ref
ref
ref
ref
ψGénération
de
mouvement
+_( )t1ε
C1(p) G1(p)( )tu1
correcteur actionneur
+_( )t2ε
C2(p) G2(p)( )tu2
correteur actionneur
+_( )t3ε
C3(p) G3(p)( )tu3
correcteur actionneur
+_( )t4ε
C4(p) G4(p)( )tu4
correcteur actionneur
Introduction Introduction
Planification de trajectoiresPlanification de trajectoires
Potentiels attractifsPotentiels attractifs
Potentiels répulsifsPotentiels répulsifs
ContexteContexte
ProblématiqueProblématique
0
200
10
15
20
25
30
35
xrob
(m)
z rob(m
)
mrob
=400
mrob
=110
PlanPlan
Modèle Modèle dynamique d’un Ndynamique d’un N--rotor et linéarisation du modèlerotor et linéarisation du modèle
CommandeCommande
SimulationsSimulations
VidéosVidéos
Conclusion et PerspectivesConclusion et Perspectives5
Potentiels répulsifsPotentiels répulsifs
Target
200
40020 40 60 80 100 120 140 160 180
yrob
(m)
xrob
(m)
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.
∑+= MUMUMU )()()(rrr
Planification de trajectoires : PotentielsPlanification de trajectoires : Potentiels
6
Avec :
∑+=k
repkatttotal MUMUMU )()()(
global artificiel potentiel de champ le désigne )(MU total
r
cible la de attractif potentiel de champ le désigne )(MUatt
r
k obstaclel' à associé répulsif potentiel de champ le désigne )(MU repk
r
travaildeplan son dans dronedu position la
=z
y
x
Mr
) V-(V+) P-(P =F droneciblevdroneciblepatt ααr
On définit la force attractive de la manière suivante :
Minimise la distance drone-cible
Champ potentiel attractif (S.S.Ge et Y.J.Cui)
positifs tscoefficien : et vp αα
Potentiel attractif Potentiel attractif
7
ndronecible
n
vdroneciblepattCRONE dt
) P-(P d+) P-(P =F αα
r
Minimise la distance drone-cibleFait 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
Peut-être interprété comme un schéma de commande classique où αααα et αααα sont
8
Peut-être interprété comme un schéma de commande classique où ααααp et ααααv sont
les paramètres d’un régulateur PD :
Fonction de transfert en boucle ouverteβ(s) :
Fonction de transfert en boucle ferméeH(s) :
9
où:
Le facteur d’amortissement dépend de la masse du robot mrob.Donc la planification de trajectoiren’est pas robustevis-à-vis desvariations de masse du robot.
où ααααp and ααααv sont les paramètresd’un PD fractionnaire.
10
Le facteur de résonance et le facteur d’amortissement s’endéduisent :
11
Le facteur d’amortissement estindépendant de la masse du robot
Robustesse de la planification de trajectoire.
Comparaison avec même paramètresααααp, ααααv
12
mrob is varying from 110 to 400 kgωωωωcg from 0.0077 to 0.0027 rad/sphase margin from 72 to 48ξξξξ from 0.85 to 0.45.
mrob is varying from 110 to 400 kgωωωωcg from 0.023 to 0.0086 rad/sphase margin from 61 to 59ξξξξ = 0.7.
Fractional with same ααααp, ααααvGe and Cui
Comparaison avec le mêmeωωωωcg que Ge and Cui pour la masse nominale du robot
mrob=150 kg,ωωωωcg =0.0058 rad/s
13
ααααp=0.002x0.22ααααv= 0.8x0.22mrob is varying from 110 to 400 kgωωωωcg from 0.0073 to 0.0028 rad/sphase margin from 59 to 54ξξξξ = 0.7.
Fractional with same ωωωωcg
mrob is varying from 110 to 400 kg,ωωωωcg from 0.0077 to 0.0027 rad/sphase margin from 72 to 48ξξξξ from 0.85 to 0.45.
Ge and Cui
Paramètresmrob = [110, 150, 190, 250, 400]
Massenominale de150kg
Simulation sans obstacle
Application en 3D
14
Massenominale de150kg
Paramètres Ge and Cui :ααααp=0.002 etααααv= 0.8
Ordre fractionnaire n = 0.7
10
15
20
25
30
35
z rob(m
)
mrob=400
mrob
=110
12
14
16
18
20
22
24
26
28
30z ro
b(m) m
rob=110
mrob
=400
Fractional
Application en 3D
15
0
200
40020 40 60 80 100 120 140 160 180
10
15
20
25
30
35
yrob
(m)
xrob
(m)
z rob(m
)
mrob
=400
mrob
=110
0200
400 20 40 60 80 100 120 140 160 180
10
yrob
(m)
xrob
(m)0
200
40020 40 60 80 100 120 140 160 180
10
yrob
(m)
xrob
(m)
Planification de trajectoirerobuste et plus rapide malgré lesvariations de masse du drone
Ge and Cui
Fractionalwith sameααααp, ααααv
Fractionalwith sameωωωωcg
Potentiel fractionnaire répulsif normalisé
Basée sur la définition de Weyl :
Potentiel répulsif Potentiel répulsif
16
rmin
rmax
x
y
0%
100%
Niveau du danger
Cible fixe, 1 obstacle fixe et 2 mobiles Même ααααp, ααααv
17
Planification de trajectoirerobuste et plus rapide malgré lesvariations de masse du drone
Target
Cible mobile, 1 obstacle fixe et 2 mobiles Même ααααp, ααααv
18
Planification de trajectoirerobuste et plus rapide malgré lesvariations de masse du drone
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.
Champ potentiel total
19
potentiel total.
+
potentiel attractifpotentiel attractif
potentiel répulsifpotentiel répulsif
potentiel totalpotentiel total
Hypothèses:
N-rotor parfaitement symétrique (N paire)
Moteur (1) nez du N-rotor
Modélisation dynamique d’un NModélisation dynamique d’un N--rotorsrotors
20
( ) πφπ <<-,,xOr
( ) 22-,, πθπ <<yOr
( ) πψπ <<-,,zOr
Roulis : φ Rotation autour de l’axe .
Tangage : θ Rotation autour de l’axe .
Lacet : Ψ Rotation autour de l’axe .
→→
→→
lacetcommandeu
gagecommandeurouliscommandeu
altitudecommandeu
4
3
2
1
tan
↔z
y
x
ψθφ
Procédé
( ) ( )
( ) ( )
( )
−+
Ω−=
−+
Ω−+
Ω
−+Ω+Ω−=
−+
Ω−+
Ω
−−=
∑
∑∑
∑∑
=
=
+
+≠=+
=
++
+≠=
φθψ
ψφφθ
ψθθφ
&&&&
&&&&&
&&&&&
z
yxN
kk
k
z
y
xzN
kk
k
y
rotorN
Nkk
kNy
x
yzN
kk
k
x
rotorN
Nkk
kx
I
II
I
d
I
II
I
I
Nk
I
bl
I
II
I
I
Nk
I
bl
1
2
1
12
12
,2
22
12
21
1
112
12
,2
2
.1
.1.360
*1cos
.1.360
*1sin
Les équations du mouvement selon les axes de Roulis, Tangage et Lacet:
21
( )
( )
( ) ( ) ( )". i "moteur un d'rotation de vitesse:
et Z. Y X, axes lesselon inertied' moments :dmy=Iet dmx=I dm,y=I).radkg.m ( trainéedet coefficien: d
m),(en dronedu envergure-demi: l),kg.m.rad ( poussée det coefficien :b
), kg.m (moteur du inertied'moment :I:Avec
coscos
sincoscossinsin
sinsincossincos
22z
22y
22x
2-2
2-
2rotor
1
2
1
2
1
2
i
N
ii
N
ii
N
ii
xzz
bm
gz
bm
y
bm
x
Ω+++
Ω+−=
Ω−=
Ω+=
∫∫∫
∑
∑
∑
=
=
=
φθ
φψφθψ
φψφθψ
&&
&&
&&
Les équations des accélérations selon les 3 axes de l’espace:
( )( )( )2
423
22
214
21
233
22
242
4
1i
21
:suivanteon perturbati laEt
U
U
U
U
:suit comme dynamique modèledu entrées les considèreOn
Ω+Ω−Ω+Ω−=Ω
Ω+Ω−Ω+Ω−=
Ω−Ω=
Ω−Ω=
Ω=∑=
d
b
b
b i
Exemple Quadri-rotor: Linéarisation du modèle
Linéarisation du modèle d’un quadriLinéarisation du modèle d’un quadri--rotorsrotors
22
4321 Ω+Ω−Ω+Ω−=Ω
( )[ ]1112119109787565343121
12121
:),(
:forme la sousétat d'tion représenta une chercheOn
xxyxxxxxxxzxxxxxxxxxxX
xxXAvec
UXfX
&&&&&&
K
&
=============
ℜ∈=
=
ψθφ
[ ]000000000000),( 1000 mgUUXf =↔=
En vol stationnaire, la
portance U1 compense
le poids du drone.
Validation du modèle.
Recherche du point de fonctionnement (Point d’équilibre):
( )( )( )2
423
22
214
21
233
22
242
4
1i
21
:suivanteon perturbati laEt
U
U
U
U
:suit comme dynamique modèledu entrées les considèreOn
Ω+Ω−Ω+Ω−=
Ω−Ω=
Ω−Ω=
Ω=∑=
d
b
b
b i
Exemple Quadri-rotor: Linéarisation du modèle
23
( ) ( )
( ) ( )
( )
Ω+−=
Ω−=
Ω=
−+Ω+Ω−Ω+Ω−=
−+Ω+Ω−Ω+Ω−+Ω−Ω=
−+Ω−Ω+Ω−Ω+Ω−Ω=
∑
∑
∑
=
=
=
4
1
2
4
1
2
4
1
2
24
23
22
21
432123
21
432122
24
1
1
1
ii
ii
ii
z
yx
z
y
xz
y
rotor
y
x
yz
x
rotor
x
bm
gz
bm
y
bm
x
I
II
I
d
I
II
I
I
I
blI
II
I
I
I
bl
&&
&&
&&
&&&&
&&&&&
&&&&&
φθψ
ψφφθ
ψθθφ
Ce qui mène au modèle dynamique linéarisé du quadri-rotor:
4321 Ω+Ω−Ω+Ω−=Ω
Contrôle/CommandePlanification de trajectoire
Objectif:
Atteindre
une cible
+_( )t1ε
C1(p) G1(p)( )tu1
correcteur actionneur
z
y
x
ref
ref
ref φ
CommandeCommande
24
Génération
de la
trajectoire
Atteindre
une cible Génération
du
mouvement
+_( )t2ε
C2(p) G2(p)( )tu2
correteur actionneur
+_( )t3ε
C3(p) G3(p)( )tu3
correcteur actionneur
+_( )t4ε
C4(p) G4(p)( )tu4
correcteur actionneur
)(lacetrefψ
ψ
z
θ
Procédé 2x2Réseau de découplage (RGA)
1yG11(p)
G12(p)
G21(p)
G*c,11(p)
G*c,12(p)
G*c,21(p)
1u
Découplage des sorties par analyse RGA :
25
2yG22(p)G*c,22(p)2u
==<
=ijij
ijijc Sinon
SiavecRGAmatriceGG
λλλλ 05.0
. *
Résultat du découplage →
y1
y2
u1 u2
Simulation:
Allure de la sortie pour 1 radians pour les trois sorties (Roulis, Tangage, Lacet) et 1 mètre en
consigne:
26
G*c,12(p)
1y1u
Procédé 2x2Réseau de découplage
G11(p)
G12(p)
G21(p)
Correcteur
C1(p) G*c,11(p)
G*c,21(p)
+_1e
G*c,12(p)
27
G*c,12(p)
+++=<
+
+=
pN
p
pCpC
PIDF
Oumavec
p
p
CpC
générationCRONECorrecteur
d
d
i
m
h
b
nd
ττ
τω
ω1
11)(
:
1',1
1
)(
:2:
0
'
0
2y2u
G21(p)
G22(p)C2(p)
G* (p)
G*c,22(p)+_2e
Vidéo d’une simulation Python
28
3D 3D SenseSense & & AvoidAvoid
Environnement dynamiqueEnvironnement dynamique
Potentiels attractifs/répulsifsfractionnaires
Prise en compte du Prise en compte du danger des obstaclesdanger des obstacles
0
10
15
20
25
30
35
z rob(m
)
mrob
=400
mrob
=110
ConclusionConclusion: Principales contributions: Principales contributions
Modèle dynamique d’un UAV NModèle dynamique d’un UAV N--rotorsrotors
Variations paramétriquesde l’UAV> Planification 3D robuste> Planification 3D robuste
> Commande robuste> Commande robuste
29
Simulations & Simulations & Vidéos (Python)Vidéos (Python)
Target
200
40020 40 60 80 100 120 140 160 180
yrob
(m)
xrob
(m)
Perspectives : implantationPerspectives : implantation