29
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

Planification de trajectoires robustes 3D pour drones …jra2012.irccyn.ec-nantes.fr/presentations/JRA2012-session6... · Planification de trajectoires robustes 3D pour drones en

Embed Size (px)

Citation preview

Page 1: Planification de trajectoires robustes 3D pour drones …jra2012.irccyn.ec-nantes.fr/presentations/JRA2012-session6... · Planification de trajectoires robustes 3D pour drones en

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)

[email protected]

Journées Robotique et Automatique (JRA’12), IRCCyN, Nantes, 23-24 Octobre 2012

Page 2: Planification de trajectoires robustes 3D pour drones …jra2012.irccyn.ec-nantes.fr/presentations/JRA2012-session6... · Planification de trajectoires robustes 3D pour drones en

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

Page 3: Planification de trajectoires robustes 3D pour drones …jra2012.irccyn.ec-nantes.fr/presentations/JRA2012-session6... · Planification de trajectoires robustes 3D pour drones en

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

Page 4: Planification de trajectoires robustes 3D pour drones …jra2012.irccyn.ec-nantes.fr/presentations/JRA2012-session6... · Planification de trajectoires robustes 3D pour drones en

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

Page 5: Planification de trajectoires robustes 3D pour drones …jra2012.irccyn.ec-nantes.fr/presentations/JRA2012-session6... · Planification de trajectoires robustes 3D pour drones en

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)

Page 6: Planification de trajectoires robustes 3D pour drones …jra2012.irccyn.ec-nantes.fr/presentations/JRA2012-session6... · Planification de trajectoires robustes 3D pour drones en

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

Page 7: Planification de trajectoires robustes 3D pour drones …jra2012.irccyn.ec-nantes.fr/presentations/JRA2012-session6... · Planification de trajectoires robustes 3D pour drones en

) 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

Page 8: Planification de trajectoires robustes 3D pour drones …jra2012.irccyn.ec-nantes.fr/presentations/JRA2012-session6... · Planification de trajectoires robustes 3D pour drones en

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 :

Page 9: Planification de trajectoires robustes 3D pour drones …jra2012.irccyn.ec-nantes.fr/presentations/JRA2012-session6... · Planification de trajectoires robustes 3D pour drones en

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.

Page 10: Planification de trajectoires robustes 3D pour drones …jra2012.irccyn.ec-nantes.fr/presentations/JRA2012-session6... · Planification de trajectoires robustes 3D pour drones en

où ααααp and ααααv sont les paramètresd’un PD fractionnaire.

10

Page 11: Planification de trajectoires robustes 3D pour drones …jra2012.irccyn.ec-nantes.fr/presentations/JRA2012-session6... · Planification de trajectoires robustes 3D pour drones en

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.

Page 12: Planification de trajectoires robustes 3D pour drones …jra2012.irccyn.ec-nantes.fr/presentations/JRA2012-session6... · Planification de trajectoires robustes 3D pour drones en

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

Page 13: Planification de trajectoires robustes 3D pour drones …jra2012.irccyn.ec-nantes.fr/presentations/JRA2012-session6... · Planification de trajectoires robustes 3D pour drones en

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

Page 14: Planification de trajectoires robustes 3D pour drones …jra2012.irccyn.ec-nantes.fr/presentations/JRA2012-session6... · Planification de trajectoires robustes 3D pour drones en

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

Page 15: Planification de trajectoires robustes 3D pour drones …jra2012.irccyn.ec-nantes.fr/presentations/JRA2012-session6... · Planification de trajectoires robustes 3D pour drones en

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

Page 16: Planification de trajectoires robustes 3D pour drones …jra2012.irccyn.ec-nantes.fr/presentations/JRA2012-session6... · Planification de trajectoires robustes 3D pour drones en

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

Page 17: Planification de trajectoires robustes 3D pour drones …jra2012.irccyn.ec-nantes.fr/presentations/JRA2012-session6... · Planification de trajectoires robustes 3D pour drones en

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

Page 18: Planification de trajectoires robustes 3D pour drones …jra2012.irccyn.ec-nantes.fr/presentations/JRA2012-session6... · Planification de trajectoires robustes 3D pour drones en

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

Page 19: Planification de trajectoires robustes 3D pour drones …jra2012.irccyn.ec-nantes.fr/presentations/JRA2012-session6... · Planification de trajectoires robustes 3D pour drones en

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

Page 20: Planification de trajectoires robustes 3D pour drones …jra2012.irccyn.ec-nantes.fr/presentations/JRA2012-session6... · Planification de trajectoires robustes 3D pour drones en

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é

Page 21: Planification de trajectoires robustes 3D pour drones …jra2012.irccyn.ec-nantes.fr/presentations/JRA2012-session6... · Planification de trajectoires robustes 3D pour drones en

( ) ( )

( ) ( )

( )

−+

Ω−=

−+

Ω−+

Ω

−+Ω+Ω−=

−+

Ω−+

Ω

−−=

∑∑

∑∑

=

=

+

+≠=+

=

++

+≠=

φθψ

ψφφθ

ψθθφ

&&&&

&&&&&

&&&&&

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:

Page 22: Planification de trajectoires robustes 3D pour drones …jra2012.irccyn.ec-nantes.fr/presentations/JRA2012-session6... · Planification de trajectoires robustes 3D pour drones en

( )( )( )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):

Page 23: Planification de trajectoires robustes 3D pour drones …jra2012.irccyn.ec-nantes.fr/presentations/JRA2012-session6... · Planification de trajectoires robustes 3D pour drones en

( )( )( )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 Ω+Ω−Ω+Ω−=Ω

Page 24: Planification de trajectoires robustes 3D pour drones …jra2012.irccyn.ec-nantes.fr/presentations/JRA2012-session6... · Planification de trajectoires robustes 3D pour drones en

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

θ

Page 25: Planification de trajectoires robustes 3D pour drones …jra2012.irccyn.ec-nantes.fr/presentations/JRA2012-session6... · Planification de trajectoires robustes 3D pour drones en

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

Page 26: Planification de trajectoires robustes 3D pour drones …jra2012.irccyn.ec-nantes.fr/presentations/JRA2012-session6... · Planification de trajectoires robustes 3D pour drones en

Simulation:

Allure de la sortie pour 1 radians pour les trois sorties (Roulis, Tangage, Lacet) et 1 mètre en

consigne:

26

Page 27: Planification de trajectoires robustes 3D pour drones …jra2012.irccyn.ec-nantes.fr/presentations/JRA2012-session6... · Planification de trajectoires robustes 3D pour drones en

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

Page 28: Planification de trajectoires robustes 3D pour drones …jra2012.irccyn.ec-nantes.fr/presentations/JRA2012-session6... · Planification de trajectoires robustes 3D pour drones en

Vidéo d’une simulation Python

28

Page 29: Planification de trajectoires robustes 3D pour drones …jra2012.irccyn.ec-nantes.fr/presentations/JRA2012-session6... · Planification de trajectoires robustes 3D pour drones en

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