Suivi de pose bas modle

pour lasservissement dun drone

Cline Teulire
CEA LIST, Fontenay-aux-Roses
celine.teuliere@cea.fr

1er avril 2010

LIST DTSI

Suivi de pose bas modle

Objectif : Estimation de la pose (position + orientation) dun drone

Hypothses :
 Environnement structur
 Modle 3D connu (contours)

Intrt :
 Utilisation des contours -> frquents et robustes
 Prcision

Centrale
inertielle
(IMU)

DTSI

Camra

LIST DTSI

1er avril 2010

Aperu

Commande
en position

Commande
en vitesse

drone

Contrleur
dassiette

Estimateur

IMU

p
Suivi de pose

Camra

Embarqu

Poste au sol
DTSI

LIST DTSI

1er avril 2010

Suivi de pose bas modle

Suivre la pose 3D de la camra par rapport lenvironnement

Modle 3D

Camra

DTSI

LIST DTSI

1er avril 2010

Principe
Scne relle

Plan image
Projection

?
Modle 3D

Projection

Pose
initiale
Nouvelle
pose ?
Dplacement

DTSI

LIST DTSI

1er avril 2010

Formulation

Exponential map

Paramtrisation avec les coordonnes

exponentielles canoniques

Vecteur de 6 paramtres

DTSI

LIST DTSI

1er avril 2010

Diffrentes approches

Mthodes de type optimisation :

 Formulation sous forme des moindres carrs [Drummond-02]

 Asservissement visuel virtuel (VVS) [Comport-06]

Projection

Calcul du vecteur
derreur

Loi de commande
virtuelle

Calcul de la nouvelle
pose

Approches Baysiennes
 Filtrage particulaire [Pupilli-Calway]

DTSI

LIST DTSI

1er avril 2010

Asservissement Visuel Virtuel

Scne relle

Plan image
Projection

Modle

Projection du
modle

DTSI

LIST DTSI

1er avril 2010

Asservissement Visuel Virtuel

Scne relle

Plan image
Projection

Modle

Projection du
modle

DTSI

Points chantillonns sur

les contours projets

LIST DTSI

1er avril 2010

Asservissement Visuel Virtuel

Scne relle

Plan image
Projection

Modle

Projection du
modle

DTSI

Points chantillonns sur

les contours projets
Recherche du plus fort
gradient le long de la
normale

LIST DTSI

1er avril 2010

10

Asservissement Visuel Virtuel

Scne relle

Plan image
Projection

Modle

Projection du
modle

Calcul du vecteur derreur

pondr par M-estimation
Points chantillonns sur les contours projets

dki
dk,i+1
dk,i+2

Recherche du plus fort gradient le long de la

normale
M-estimation

distance point/contour

DTSI

LIST DTSI

1er avril 2010

11

Asservissement Visuel Virtuel

Scne relle

Plan image
Projection

Modle

Projection du
modle

Calcul du vecteur derreur

pondr par M-estimation

Loi de commande
virtuelle

Distance point/contour:

DTSI

LIST DTSI

1er avril 2010

connu

12

Asservissement Visuel Virtuel

Scne relle

Plan image
Projection

Modle

Projection du
modle

DTSI

Calcul du vecteur derreur

pondr par M-estimation

LIST DTSI

Loi de commande
virtuelle

1er avril 2010

13

Asservissement Visuel Virtuel

Scne relle

Plan image
Projection

Modle

Projection du
modle

Calcul du vecteur derreur

pondr par M-estimation

Loi de commande
virtuelle

Calcul de la nouvelle
pose

DTSI

LIST DTSI

1er avril 2010

14

Asservissement Visuel Virtuel

Scne relle

Plan image
Projection

Modle

Projection du
modle

Calcul du vecteur derreur

pondr par M-estimation

Loi de commande
virtuelle

Calcul de la nouvelle
pose

DTSI

LIST DTSI

1er avril 2010

15

Limitations

Bas niveau (traitement
dimage) :
 Mauvaise localisation des
points : plusieurs forts
gradients le long de la
normale ?

Haut niveau (dplacement

de la camra) :
 Mauvaise localisation de
la camra : recherche
partir dune position
prdite ?

DTSI

LIST DTSI

1er avril 2010

16

Approche multi-hypothses

Plusieurs points de forts
gradients sont mmoriss

Les hypothses sur les

points sont converties en
hypothses sur les lignes
 Classification par k-mean

Pose
Projection
Tracking
Dtection de
lignes

DTSI

LIST DTSI

1er avril 2010

17

Approche multi-hypothses
Pose
Projection
Tracking
Dtection de lignes

Tirage pondr parmi les
lignes candidates et calcul
de la pose correspondante

Tirage pondr

Feature Set

Feature Set

Feature Set

VVS

VVS

VVS

Pose

Pose

Pose

Plusieurs hypothses pour

la pose de la camra

On conserve la pose qui donne le plus

petit rsidu lors de la minimisation

DTSI

LIST DTSI

1er avril 2010

18

Exemples

Simulation

DTSI

LIST DTSI

1er avril 2010

19

Exemples

Squence relle

DTSI

LIST DTSI

1er avril 2010

20

Scnes complexes

DTSI

LIST DTSI

1er avril 2010

21

Estimation de vitesse

Commande
en position

Commande
en vitesse

drone

Contrleur
dassiette

Estimateur

IMU

p
Suivi de pose

Camra

Embarqu

Poste au sol
DTSI

LIST DTSI

1er avril 2010

22

Modle du quadri-rotor

Systme non linaire sous actionn

Repres utiliss



I : Repre inertiel (terrestre)

C : Repre fix au centre de masse du
vhicule (suppos concider avec la
camra)

Translation

Oc

Rotation

xc

yc

zc

Etat :
 p : position du centre de masse dans I
 : vitesse du centre de masse dans I
 R : rotation de C vers I
 : vitesse de rotation dans C

ex

Entres :

ey

ez

 T : Pousse dans C
 : Couple dans C

DTSI

LIST DTSI

1er avril 2010

23

Estimation de vitesse

EKF (Extended Kalman Filter)
 Etat :

Hypothses simplificatrices :





Vitesses de translations en x et y dcouples

Petits angles
Coefficient de frottement constant
Pousse constante

Modle dvolution :

DTSI

LIST DTSI

1er avril 2010

24

Estimation de vitesse

appris (ici par un algorithme gntique)

EKF :

Observation : position obtenue par suivi visuel

DTSI

LIST DTSI

1er avril 2010

25

Commande

Commande
en position

Commande
en vitesse

drone

Contrleur
dassiette

Estimateur

IMU

p
Suivi de pose

Camra

Embarqu

Poste au sol
DTSI

LIST DTSI

1er avril 2010

26

Commande

Lacet
 Vitesse de rotation contrle en embarqu laide des gyro

Translation
 Commande hirarchique : PI en vitesse, P en position

Consigne dassiette :

Assiette contrle en embarqu

DTSI

LIST DTSI

1er avril 2010

27

Dispositif experimental
Camra:
- Focus : 2.1mm

Drone :
- Contrle dassiette
embarqu (donnes
IMU) 166Hz

Transmission
numrique :
- Transmission de
lassiette dsire vers
le drone
- Transmission des
donnes inertielles au
poste au sol

Scne

DTSI

Transmission analogique :
- Transmission 2.4GHz
- Une nouvelle image est
disponible toutes les 50 ms

Poste au sol :
Algorithmes de
vision et calcul de
la commande
(20Hz)

LIST DTSI

1er avril 2010

28

Dispositif exprimental

Scne : objets planaires / objets 3D

Scne

Modle

Tche : points de passage successifs

(diffrentes consignes de position)

La cible est utilise uniquement pour
linitialisation

DTSI

LIST DTSI

3
4

1
6

1er avril 2010

29

Rsultats exprimentaux

DTSI

LIST DTSI

1er avril 2010

30

Rsultats exprimentaux
Erreur (m)
Position error
2.5
X axis
Y axis
2

1.5

0.5

Changements de consigne
0.5

1.5

2.5

20

40

60

80

Erreur (m/s)1

100
Velocity error

120

140

160

180

Temps (s)

X axis
Y axis

0.8

0.6

0.4

Frquence : 20 Hz

0.2

0.2

0.4

0.6

0.8

DTSI

20

40

60

80

LIST DTSI

100

120

140

160

180

Temps (s)

1er avril 2010

31

Rsultats exprimentaux

Mesure de la prcision
 Vrit terrain fournie par un tracker
Laser Leica
 Frquence dacquisition : 1000Hz
 Mesure au micromtre

Miroir

DTSI

LIST DTSI

1er avril 2010

32

Rsultats exprimentaux
(m)

Temps (s)

Vue camra

DTSI

LIST DTSI

1er avril 2010

33

Rsultats exprimentaux
(m)

Temps (s)

Bonne

prcision

 Erreur max : 10cm

 Due limprcision du modle :

DTSI

LIST DTSI

1er avril 2010

34

Rsultats exprimentaux : estimation de vitesse

Vitesse (m/s)

Temps (s)

Temps (s)

DTSI

LIST DTSI

1er avril 2010

35

Conclusion

Suivi bas modle :
 Pas la mode
 Mais ne pas ngliger
 Prcision
 Robustesse (amliore par lapproche multi-hypothse)
 Adapt des environnements structurs

La fusion avec les donnes inertielles permet une bonne

estimation de vitesse

DTSI

LIST DTSI

1er avril 2010

36

DTSI

LIST DTSI

1er avril 2010

37

DTSI

LIST DTSI

1er avril 2010

38

DTSI

LIST DTSI

1er avril 2010

39

Publications

DTSI

LIST DTSI

1er avril 2010

40

DTSI

LIST DTSI

1er avril 2010

41

Modle du quadri-rotor

The system is nonlinear and under actuated.
T

Definition of used frames.
 I : Inertial frame attached to the
earth

 B : body-fixed frame attached to the

vehicles center of mass.

Vehicle state:
 : position of the center of mass in I
 v : speed of the center of mass in I
R : orientation matrix from B to I

Dynamic Model used :
 : rotational velocity in B

Inputs:
T : Global thrust in B
Translational
dynamics
 : Control torque in B

F=f(T,R)

Constant
forces

Rotational
Dynamics

High gain controller

DTSI

LIST SRI-LIST
DTSI

Already controlled

1er avril 2010

42

Particle filtering approach

Prediction step: Evolution of the particles according to a motion model

State:
(canonical exponential coordinates)

Set of particles: sampling of the pose distribution
Update step: For each particle
Projection of the model according to the pose parametrised by
Sampling of the edges
Tracking of the points
Distance computation
Deduce particles weight

Resampling

DTSI

LIST DTSI

1er avril 2010

43

Precaution when filtering on SE(3)

When designing distributions on a Lie group

 Gaussian distributions ?
 On the canonical exponential coordinates, and then
exponential map

Output considered?
 Usually the weighted mean No meaning here

Wh
Singular Value
 Mean
rotation is projected
orthogonally
on SO(3)
ere
Decomposition
DTSI

LIST DTSI

1er avril 2010

44

Use of the dynamics

Evolution
model

Precision of the inertial measurements of

the UAV?

No video yet
DTSI

LIST DTSI

1er avril 2010

45

Comparison toward a combination

Particle
filtering

MH Model-based
tracking

Time consuming 

Work in progress:
 Combine Top-down / Bottom-up
informations?
 Use a small set of hypothesis
DTSI

LIST DTSI

1er avril 2010

46