filtre à particules - université lavalpgiguere/cours/introrobotique/notes/...–apollo guidance...
TRANSCRIPT
![Page 1: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/1.jpg)
Philippe Giguère
Filtres à particules
ICP
Automne 2019
![Page 2: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/2.jpg)
GLO-7021 : Présentations orales
• Durée 15 minutes
• Critères d’évaluations sur le site• http://www2.ift.ulaval.ca/~pgiguere/cours/IntroRobotique/notes/Grille%20%C3%A9valuation%20Oraux.pdf
– Éviter trop d’équations, texte
– Privilégier figures, explication haut-niveau
– Take-home message
• Ce vendredi, de 8h30 à 12h30
• Local VCH-00212
GLO-4001/7021 Introduction à la robotique mobile 2
![Page 3: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/3.jpg)
Feuille de route
3
Pose X
carte m
planification
actionneurs
capteurs
Commandes u
Mesures z
![Page 4: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/4.jpg)
Estimation d’état :filtres à particules
GLO-4001/7021 Introduction à la robotique mobile 4
![Page 5: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/5.jpg)
Estimation d’état : filtre Kalman
• Bruit gaussien
• Distribution unimodale
• Système linéaire :
– filtre Kalman
• Système non linéaire :
– filtre Kalman étendu (EKF)
– filtre Kalman non-parfumé (UKF)
• EKF: rapide à effectuer
– Apollo Guidance Computer (AGC)
– 2.048 MHz
– 2 048 x 16 RAM
– 36 864 x 16 ROM 5
Interface DSKY du AGC
ROM tressé à la main: core rope memory
![Page 6: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/6.jpg)
Méthodes Monte-Carlo
• Estimation de fonctions par méthodes aléatoires, par échantillonnage au hasard
• Développées durant le projet Manhattan, années 1940.
• Pour problèmes multi-variables ou sans solutions analytiques
• Exemple : calcul d’intégrale définie
6
http://hackaday.com/2015/09/11/fermiac-the-computer-that-advanced-the-manhattan-project/
![Page 7: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/7.jpg)
Filtrage Bayésien (rappel)
8
Algorithme FiltreBayes(bel(xt-1), ut, zt)
for all xt do
endfor
return bel(xt)
bel( ) : belief ou croyance (raccourci de notation)
ut : commande au temps t
zt : mesure au temps t, après la commande ut
xt : pose du robot au temps t, après ut et zt
h : facteur de normalisation
Prédiction
Mise-à-jour
Difficile à implémenter exactement car pas toujours de solution analytique de l’intégrale (1)/produit (2)
(1)
(2)| ))( (()t pre tt t dp z xbel x bel xh
1 1 1( | , )( ) ( )pre t tt t ttdbel x belp x xx xu d
distribution
![Page 8: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/8.jpg)
Méthodes Monte-Carlo
GLO-4001/7021 Introduction à la robotique mobile 9
y
x
10 « pas » vers l’avant
100 simulations == 100 particules
distributionnon-gaussienne
Bruits sur angle et vitesse
une gaussiennene capture pasla forme de cettedistribution
![Page 9: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/9.jpg)
Monte-Carlo filtre à particules
• Chaque particule représente une hypothèse
• Chaque particule a un poids associé
• Le poids wi représente en quelque sorte la « confiance » sur cette hypothèse
• Filtre utilise (en général) un nombre C fixe de particules
10
T
iX x y
iw
(pour un robot 2D)ième particule :
![Page 10: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/10.jpg)
Exemple : distribution unimodale
GLO-4001/7021 Introduction à la robotique mobile 11
ww
distribution à représenter
représentation 1
représentation 2
• On pige une valeur x au hasard• Le poids w est proportionnel à la hauteur de la distribution
x
![Page 11: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/11.jpg)
Exemple : distribution unimodale
GLO-4001/7021 Introduction à la robotique mobile 12
ww
distribution à représenter
représentation 1
représentation 2
![Page 12: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/12.jpg)
Exemple : distribution multimodale
GLO-4001/7021 Introduction à la robotique mobile 13
distribution à représenter
représentation 1
représentation 2
Non-gaussienne
pas
sé à
cô
té :
rien
n’e
st p
arfa
it…
![Page 13: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/13.jpg)
Variance étendu distribution
• On ne calcule plus la covariance P explicitement comme dans Kalman
– Implicite par la distribution des particules
• Permet de représenter des distributions arbitrairement complexes
14
Comment associer une seule variance ici ???
![Page 14: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/14.jpg)
Filtre à particule
GLO-4001/7021 Introduction à la robotique mobile 15
u(t) z(t)
Intègre l’information en 2 étapes, tout comme les filtres de Kalman
![Page 15: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/15.jpg)
while (explore)
for i=1:C
end
z(k+1) = mesure();
for i=1:C
end
for i=1:C
end
Neff = 1/
if (Neff < Nseuil)
Xi(k+1) = resample(Xi(k+1) , wi(k+1));
wi(k+1) = 1/C
end
k=k+1
end
Filtre à particules : aperçu
16
( 1) ( , ( ) )( ) ,i iXf u kX k X k
( 1)( |( 1 ( 1))) ( )i iiXzw k w kk kp
( 1)(
{1)
1 }( )
ii
jj
w kw k
w k
Prédiction
Mise-à-jour
Normalisation
Ré-échantillonnage
2
1
N
i
i
w
on simule avec le bruit!
![Page 16: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/16.jpg)
while (explore)
for i=1:C
end
z(k+1) = mesure();
for i=1:C
end
for i=1:C
end
Neff = 1/
if (Neff < Nseuil)
Xi(k+1) = resample(Xi(k+1) , wi(k+1));
wi(k+1) = 1/C
end
k=k+1
end
Filtre à particules : aperçu
17
( 1) ( , ( ) )( ) ,i iXf u kX k X k
( 1)( |( 1 ( 1))) ( )i iiXzw k w kk kp
( 1)(
{1)
1 }( )
ii
jj
w kw k
w k
Prédiction
Mise-à-jour
Normalisation
Ré-échantillonnage
2
1
N
i
i
w
on simule avec le bruit!
![Page 17: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/17.jpg)
Étape 1 : prédiction du F. P.
• On déplace la particule selon :
– le modèle du système,
– les commandes u(k),
– le bruit v
• On répète pour chaque particule i=1:C
• Bref on déplace les C particules en simulant le robot avec le bruit
18
( 1) ( ( ), ( ), )i X i VX k f X k u k
(0,( )
cos
sin
(
)
)(0, )
x
y
Vd V t
f x d
f y d
f
N
N vt
Pour notre robot 2D
pas besoin d’avoir fX() linéaire…
![Page 18: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/18.jpg)
Étape 1 : prédiction, Kalman vs. F.P.
• Dans Kalman (étendu ou pas), on ne propage pas le bruit sur la pose
• Dans filtre à particule, on propage avec du bruit
• Et il n’y a pas de matrice de covariance P
GLO-4001/7021 Introduction à la robotique mobile 19
(ˆ ˆ( , ( )| ) )1 ( )XX k k X kf u k ˆ ˆ( 1| ) ( )) (x k k x k u k
Kalman Kalman étendu
( 1) ( , ( ) )( ) ,i iX Vf u kX k X k
( 1| ) ( )( ) ( ) ( )T
vP k k P kk k C k
![Page 19: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/19.jpg)
Filtre à particules : exemple prédiction
• Deux particules, X1 et X2
• Vitesse constante V
• Léger bruit sur angle
• Léger bruit sur vitesse
GLO-4001/7021 Introduction à la robotique mobile 20
( 1) ( , ( ), ,( ))X i ViX k X kf u k
fX peut être arbitrairement compliqué (en général, besoin
de plus de particules pour les distributions complexes)
![Page 20: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/20.jpg)
Autre exemple bruit pour un pas
• Rotation : bruit gaussien
• Translation : bruit gaussien
• Un pas:
– erreur de rotation avant et après translation
– erreur translation proportionnelle à la distance parcourue
GLO-4001/7021 Introduction à la robotique mobile 21
(permet de mieux décorrélerx, y et car autant de sources
de bruits que de degrés de liberté)
![Page 21: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/21.jpg)
Modèle bruit déplacement
GLO-4001/7021 Introduction à la robotique mobile 23
trans = 5 angle =1
![Page 22: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/22.jpg)
Modèle bruit déplacement
GLO-4001/7021 Introduction à la robotique mobile 24
trans = 1 angle =5
![Page 23: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/23.jpg)
Modèle bruit déplacement
GLO-4001/7021 Introduction à la robotique mobile 25
trans = 10 angle =10
![Page 24: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/24.jpg)
Bien modéliser le bruit de déplacement
• Au pire, on surestime le bruit
– sous-estimer est néfaste… (divergence du filtre à particule)
GLO-4001/7021 Introduction à la robotique mobile 26
Au départ :
sous-estime le bruit
temp
s
aucune particule près de la position réelle
t
t+1
t+2
surestime le bruit
t
t+1
t+2
particulerobot
![Page 25: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/25.jpg)
Étape 2 : Mise-à-jour
• Incorporer l’information de capteurs extéroceptifs
• Réduire l’incertitude
• Utiliser la distribution de probabilité des mesures hz(X) pour ajuster le poids wi de chaque particule
GLO-4001/7021 Introduction à la robotique mobile 28
![Page 26: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/26.jpg)
Exemple de filtre, fonction non-bijective
29
hz(x)
x
fonction de capteur
X1 X3 X5 X4 X2 X6
z1
z3z5
z4
z2
z6
Particule
Pré
dir
e la
mes
ure
att
end
ue
po
ur
chaq
ue
par
ticu
le X
i
mise-à-jour
ˆprédict on i z
particules Xi un peu partout : je suis perdu!
![Page 27: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/27.jpg)
GLO-4001/7021 Introduction à la robotique mobile 30
hz(x)
x
fonction de capteur
X1 X3 X5 X4 X2 X6
z1
z3z5
z4
z2
z6
mesure zt
w3=w3*0.00
w5=w5*0.00
w4=w4*0.33
w1=w1*0.37
w2=w2*0.02
w6=w6*0.00
Particule
On prend une mesure réelle zt avec le capteur
On connait la variance 2 sur notre mesure
mettre à jour les poids wi
Exemple de filtre, fonction non-bijective
mise-à-jour
![Page 28: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/28.jpg)
GLO-4001/7021 Introduction à la robotique mobile 31
hz(x)
x
fonction de capteur
X1 X3 X5 X4 X2 X6
z1
z3z5
z4
z2
z6
mesure zt
w3=w3*0.00
w5=w5*0.00
w4=w4*0.33
w1=w1*0.37
w2=w2*0.02
w6=w6*0.00
Exemple de filtre, fonction non-bijective
mise-à-jour
prédictions similaires pour des positions très différentes : capteur non-bijectif
Ici, la plupart des particules obtiennent un poids quasi nul
![Page 29: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/29.jpg)
Exemple de filtre, fonction non-bijective
GLO-4001/7021 Introduction à la robotique mobile 32
hz(x)
xNote : par simplicité je n’affiche plus les autres particules ayant des wi faibles
X1 X4
hz(x)
commande u(t)
d’avancer
prédiction
Les particules ont avancé de distances légèrement différentes (bruit)
![Page 30: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/30.jpg)
Exemple de filtre, fonction non-bijective
33
hz(x)
xX1 X4
hz(x)
z4
z1 mesure ztcommande u(t)
d’avancer
mise-à-jour
Le filtre à particule a donc réussi à isoler la bonne hypothèse dans cet exemple, après un seul pas!
![Page 31: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/31.jpg)
Appauvrissement avec le temps
• Avec les mises-à-jour, certaines particules finissent par avoir un poids
• Perte du pouvoir de représentation
GLO-4001/7021 Introduction à la robotique mobile 34
0iw
mesure laser
![Page 32: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/32.jpg)
Sample Importance Resampling : SIR
• Critère d’appauvrissement :
GLO-4001/7021 Introduction à la robotique mobile 35
2
1
1eff C
i
i
N
w
(Neff = nombre effectif de particules)
1 2 3 4
1 1 1 1, , ,
4 4 4 4w w w w
2 44
11
1 1 1 164
41 4116164
![Page 33: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/33.jpg)
Sample Importance Resampling : SIR
• Critère d’appauvrissement :
GLO-4001/7021 Introduction à la robotique mobile 36
2
1
1eff C
i
i
N
w
1 2 3 4
1 1 1 1, , ,
4 4 4 4w w w w
2 44
11
1 1 1 16
41 4116164
4
(Neff = nombre effectif de particules)
![Page 34: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/34.jpg)
Sample Importance Resampling : SIR
• Critère d’appauvrissement :
GLO-4001/7021 Introduction à la robotique mobile 37
1 2 3 4
1 1 1 1, , ,
4 4 4 4w w w w
2 44
11
1 1 1 16
41 4116164
4
1 2 3 40.85, 0.05, 0, 0.1w w w w Neff = 1.3605
2
1
1eff C
i
i
N
w
(Neff = nombre effectif de particules)
![Page 35: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/35.jpg)
Sample Importance Resampling : SIR
• Si Neff < Nseuil , il y a appauvrissement
– peu de particules avec un poids wi non-négligeable
• Normal, car à la longue l’état Xi diverge
• Doit donc rétablir une nouvelle population de particules
• Comment? En échantillonnant avec remplacement dans la population actuelle, avec probabilité wi
GLO-4001/7021 Introduction à la robotique mobile 38
![Page 36: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/36.jpg)
Sample Importance Resampling
• Exemple pour C=10 particules
GLO-4001/7021 Introduction à la robotique mobile 39
wi
![Page 37: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/37.jpg)
Sample Importance Resampling
• Exemple pour C=10 particules
GLO-4001/7021 Introduction à la robotique mobile 40
wi
déco
mpte
![Page 38: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/38.jpg)
Sample Importance Resampling
• Exemple pour C=10 particules
GLO-4001/7021 Introduction à la robotique mobile 41
wi
déco
mpte
déco
mpte
![Page 39: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/39.jpg)
Sample Importance Resampling
• Exemple pour C=10 particules
GLO-4001/7021 Introduction à la robotique mobile 42
wi
déco
mpte
déco
mpte
déco
mpte
![Page 40: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/40.jpg)
Sample Importance Resampling
• Exemple pour C=10 particules
43
wi
Particule 9 jamais pigée, car w9 est très faibleNote : on ne fait qu’un seul tirage dans le filtre
déco
mpte
déco
mpte
déco
mpte
![Page 41: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/41.jpg)
Sample Importance Resampling : SIR
• Algorithme efficace de ré-échantillonnage :
44tiré de « A particle filter tutorial for mobile robot localization », I. Rekleitis
![Page 42: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/42.jpg)
Prédiction + mise-à-jour
• Même mécanique de base que Kalman
GLO-4001/7021 Introduction à la robotique mobile 45
( 1) ( , ( ) )( ) ,i iXf u kX k X k
( 1)( |( 1 ( 1))) ( )i iiXzw k w kk kp
Resampling, si Neff < Nseuil
Normaliser les wi
![Page 43: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/43.jpg)
while (explore)
for i=1:C
end
z(k+1) = mesure();
for i=1:C
end
for i=1:C
end
if (Neff < Nseuil)
Xi(k+1) = resample(Xi(k+1) , wi(k+1));
wi(k+1) = 1/C
end
k=k+1
end
Filtre à particules
46
( 1) ( , ( ) )( ) ,i iXf u kX k X k
( 1)( |( 1 ( 1))) ( )i iiXzw k w kk kp
( 1)(
{1)
1 }( )
ii
jj
w kw k
w k
Prédiction
Mise-à-jour
Normalisation
Ré-échantillonnage
2
1
1eff seuilN
i
i
N N
w
![Page 44: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/44.jpg)
Exemple 1 : particules + trilatération
47
Initialisation de C=50 particules Xi=[x y ]T, position initiale x,y connue, angles au hasard.Le système arrive à trouver un estimé de .
direction réelle
mesures z1, z2 et z3
z3
Après 1 déplacement
2
2
( )
2( | )iib X z
i ezp X
b2
b3
b1
![Page 45: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/45.jpg)
Exemple 1 : particules + trilatération
• Deuxième déplacement : presque toutes les particules avec le mauvais sont disparues
GLO-4001/7021 Introduction à la robotique mobile 48
direction réelle
![Page 46: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/46.jpg)
Exemple 1 : particules + trilatération
• 5ème déplacement
GLO-4001/7021 Introduction à la robotique mobile 49
direction réelle
![Page 47: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/47.jpg)
Exemple 2 : monde rectangle (similarité)
• Robot avec deux mesures laser : z1 et z2
• Pas de compas
• Carte connue
GLO-4001/7021 Introduction à la robotique mobile 50
![Page 48: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/48.jpg)
Exemple 2 : monde rectangle (similarité)
• Initialisé avec C=400 particules au hasard
– position : uniformément
– angles : 0, 90, 180 ou 270 degrés
• Ligne rouge : mesures laser
GLO-4001/7021 Introduction à la robotique mobile 51
(Problème de localisation globale)
![Page 49: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/49.jpg)
Exemple 2 : monde rectangle (similarité)
GLO-4001/7021 Introduction à la robotique mobile 52
• Symétrie + absence de compas = ambiguïté
Position réelle du robot
![Page 50: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/50.jpg)
Exemple 3 : monde asymétrique
GLO-4001/7021 Introduction à la robotique mobile 53
• Asymétrie de la carte = levée de l’ambiguïté
![Page 51: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/51.jpg)
Exemple 3 : monde asymétrique
GLO-4001/7021 Introduction à la robotique mobile 54
• Asymétrie de la carte = levée de l’ambiguïté
mesure estimée zmesure réelle z
^
![Page 52: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/52.jpg)
Exemple 3 : monde asymétrique
GLO-4001/7021 Introduction à la robotique mobile 55
• Asymétrie de la carte = levée de l’ambiguïté
![Page 53: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/53.jpg)
Effet corridor
• Perte de précision le long de grands corridors
– portée du LiDAR plus courte que la longueur du corridor
• Exemple, portée LiDAR 2 m :
GLO-4001/7021 Introduction à la robotique mobile 56
Position réelle du robot
![Page 54: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/54.jpg)
Bien adapté à la localisation globale
57
Quand on ne sait pas du tout où est la position de départ…
Trajectoire parcourue
tiré de Probabilistic Robotics, par Thrun, Burgard et Fox
![Page 55: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/55.jpg)
Bien adapté à la localisation globale
58
Trajectoire parcourue
tiré de Probabilistic Robotics, par Thrun, Burgard et Fox
![Page 56: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/56.jpg)
Bien adapté à la localisation globale
59
Trajectoire parcourue
tiré de Probabilistic Robotics, par Thrun, Burgard et Fox
![Page 57: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/57.jpg)
Localisation globale locale
60
On initialise un filtre de Kalman (étendu) : pistage (tracking)
On détecte que le robot s’est localisé
![Page 58: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/58.jpg)
Exemple 2: Fusion odom + gyro + GPS + carte!
• Bruit sur GPS : = 50 m
• Bruit sur déplacement = 2 m
• Odométrie + rotations du véhicule
• Heuristique : si la position n’est pas sur une route, réduire le poids de la particule de 90% :
GLO-4001/7021 Introduction à la robotique mobile 61
( ( ( ))
( ) 0.1 ( )
if PasSurRoute x i
w i w i
end
![Page 59: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/59.jpg)
Gardez à l’esprit…
• Le nombre de particule C influence la précision
• Plus la dimensionnalité de l’état est grande, plus C doit être grand.
GLO-4001/7021 Introduction à la robotique mobile 62
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1-1
-0.8
-0.6
-0.4
-0.2
0
0.2
0.4
0.6
0.8
1
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
00.2
0.40.6
0.81
0
0.5
10
0.2
0.4
0.6
0.8
1
00.2
0.40.6
0.81
0
0.5
10
0.2
0.4
0.6
0.8
1
00.2
0.40.6
0.81
0
0.5
10
0.2
0.4
0.6
0.8
1
![Page 60: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/60.jpg)
Problème si capteur trop précis…• Peu de particules se
trouvent dans la région couverte par la mesure
• Perte de variété (deprivation)
• Solutions:
GLO-4001/7021 Introduction à la robotique mobile 63
distribution mesure p(z|x)
prédiction p(xt|xt-1,u)
particule
seule particule à survivre
– Placer des particules à l’endroit de la mesureLenser, Veloso. Sensor resetting localization for poorly modelled mobile robots. ICRA, 2000.
– Piger des particules dans une meilleure distribution combinant l’estimé de position et la mesure
• gmapping, FASTSLAM 2.0.
– Utiliser un bruit de capteur plus grand que la réalité
![Page 61: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/61.jpg)
Variations pour filtres à particules
• Ajouter des particules uniformément après la mise-à-jour
– Augmente robustesse, survit au kidnapping
• Ajouter des particules près de l’endroit indiqué par les capteurs
– prévient le problème de capteurs trop précis
– difficile si l’inversion de la fonction de capteur ne nous donne pas une position
• Augmenter ou diminuer le nombre C de particules en fonction du temps
– Adapting the sample size in particle filters through KLD-Sampling, D. Fox, IJRR, 2003.
64KLD : Kullback-Leibler divergence
![Page 62: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/62.jpg)
Rao Blackwellized Particle Filter(RBPF)
GLO-4001/7021 Introduction à la robotique mobile 65
![Page 63: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/63.jpg)
Filtre à particules Rao-Blackwellized
• FP : Si le nombre de variables d’état est grand, le nombre C de particules nécessaires est énorme
– C est exponentiel en # variables
– Fléau de la dimension
• Filtre particule Rao-Blackwellized :
– Combiner FP + Kalman
– Pour réduire le temps de calcul
– Formulation exploite la structure présente dans le problème
GLO-4001/7021 Introduction à la robotique mobile 66
![Page 64: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/64.jpg)
Filtre à particules Rao-Blackwellized
• FP : Peut représenter n’importe quel type de distribution (unimodale, multimodale)
• Kalman : la distribution à représenter doit être unimodale, idéalement gaussienne
GLO-4001/7021 Introduction à la robotique mobile 67
unimodale
pro
ba
bil
ité
multimodale
pro
ba
bil
itéunimodale
pro
ba
bil
ité
(pas efficace)
![Page 65: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/65.jpg)
Exemple RBPF pour localisation
GLO-4001/7021 Introduction à la robotique mobile 68
( , , ) ( | , ) ( , )
( | , ) ( , )
( | , ) ( , )
p x y p x y p y
p y x p x
p x y p x y
( , , )p x y
Y a-t-il une factorisation « meilleure » que les autres, en
fonction de notre problème?
Si on fait l’hypothèse que sachant x, y, l’orientation est dist. unimodale, oui!
Les particules ont à peu près la même orientation, pour une région x,y donnée
Trois factorisations :
![Page 66: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/66.jpg)
Factoriser en deux problèmes
GLO-4001/7021 Introduction à la robotique mobile 69
( , , ) ( | , ) ( , )p x y p x y p x y
Distribution multimodale : FP
Distribution unimodale : Kalman
Chaque particule utilisera donc un filtre de Kalman!
(On y reviendra +tard pour FASTSLAM)
![Page 67: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/67.jpg)
Bien visualiser le concept
GLO-4001/7021 Introduction à la robotique mobile 70
( | , ) ( , )p x y p x y
Filtre à particuleKalman
![Page 68: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/68.jpg)
Bien visualiser le concept
GLO-4001/7021 Introduction à la robotique mobile 71
( | , ) ( , )p x y p x y
Filtre à particuleKalman
![Page 69: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/69.jpg)
Commandes et mesures désynchronisées
72
![Page 70: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/70.jpg)
Filtres : mesures à différents rythmes?
• Commandes aux 100 ms
• Capteur 1 aux 300 ms
• Capteur 2 aux 500 ms
73
Propage commande
Mise-à-jour mesure z1
Propage commande
Propage commande
Propage commande
Mise-à-jour mesure z1
Propage commande
Propage commande
Mise-à-jour mesure z2
Propage commande
Mise-à-jour mesure z1
Propage commande
Propage commande
Propage commande
Mise-à-jour mesure z1
Propage commande
Mise-à-jour mesure z2
Propage commande
Propage commande
Mise-à-jour mesure z1
Autant pour les filtres à particules que les filtres de Kalman
![Page 71: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/71.jpg)
Types de problème de localisation
74
![Page 72: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/72.jpg)
Problèmes de localisation
• Localisation faible
– « Ai-je déjà été ici ?»
– marqueurs déposés au sol
– algorithmes de planification bug0, bug1, bug2
– reconnaissance de lieu (place recognition)
• Localisation forte
– « Où suis-je par rapport à un repère global ?»
– p(xt|z1:t,u1:t)
• xt : pose actuelle du robot
• z1:t : mesures des capteurs et u1:t : commandes
75
![Page 73: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/73.jpg)
Localisation forte
• Pistage/Tracking (unimodale)
– je connais ma position initiale x0
– implémenté par filtre Kalman
• Globale (multimodale)
– je ne connais pas ma position initiale x0
– implémenté par filtre à particule, etc.
• Kidnapping
– téléportation possible du robot sans l’avertir
– détecte algorithme qui diverge/échoue
– globale tracking globale tracking76
difficu
lté croissan
te
détection
![Page 74: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/74.jpg)
Iterative Closest Point : ICP
77
![Page 75: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/75.jpg)
Positionnement relatif par scan 2D• Utiliser directement les données Z d’un capteur de distance
pour estimer la position relative entre deux scans.
• Scénario :
79
Monde m +position du robot
Scan laser
coordonnées globales coordonnées locales (robot)
![Page 76: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/76.jpg)
Opération de calage de points
80
• Deux scans lidar 2D/3D, A et B, pris à des endroits différents mais proches
• Problème : trouver R et T entre A et B
• Similaire à odométrie visuelle
• Sauf que les points lidar sont anonymes …
• et les mesures seront rarement pour une même partie de surface
![Page 77: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/77.jpg)
Problème de calage de point (simple)
• Ici on assume que l’appariement entre points A et B est exact et connu :
• Cherche :
• Se résout en 10 lignes matlab, sans itération (SVD)
81
2
,
argmin || ( ) ||T
iR
i
i
A BT R
A Method for Registration of 3-D Shapes, Besl and McKay, 1992
13
4
5
2
1
35
2
4
![Page 78: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/78.jpg)
Plus proche voisin pour calcul coût
• Les points n’ont pas de signatures
• Apparier points par critère du plus proche voisin PPV(i).
GLO-4001/7021 Introduction à la robotique mobile 82
2
,( )argmin || ||i PPV
R i A
iT
TRBA
![Page 79: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/79.jpg)
Plus proche voisin pour calcul coût
GLO-4001/7021 Introduction à la robotique mobile 83
Ai
BPPV(i)
coût pour Ai
2
,( )argmin || ||i PPV
R i A
iT
TRBA
![Page 80: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/80.jpg)
ICP : Iterative Closest Point
• Fonctionne par ajustement itératif avec T et R
• Boucler jusqu’au critère de convergence :
– Faire l’appariement PPV(i)
– Estimer T et R qui minimisent* l’erreur :
– Applique la transformation : BT+RB
• Attention! ne fonctionne que pour des petites transformations T et R.
– sorte de descente de gradient
– sujet aux minimums locaux, si grands déplacements T et R
84*solution approximative car dépend appariement, d’où le besoin de faire de multiples itérations
( )
2
,
argmin || ( ) ||i
Ai
PPVT
iR
T BRA
![Page 81: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/81.jpg)
ICP : Iterative Closest Point
• Fonctionne par ajustement itératif avec T et R
• Boucler jusqu’au critère de convergence :
– Faire l’appariement PPV(i)
– Estimer T et R qui minimisent* l’erreur :
– Applique la transformation : BT+RB
• Attention! ne fonctionne que pour des petites transformations T et R.
– sorte de descente de gradient
– sujet aux minimums locaux, si grands déplacements T et R
85*solution approximative car dépend appariement, d’où le besoin de faire de multiples itérations
( )
2
,
argmin || ( ) ||i
Ai
PPVT
iR
T BRA
![Page 82: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/82.jpg)
Position de départ de recherche ICP
GLO-4001/7021 Introduction à la robotique mobile 86
Odométrie + gyroscope pour trouver une position initiale
Avant ajustement odométrie
Proprioceptif
![Page 83: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/83.jpg)
Position de départ de recherche ICP
GLO-4001/7021 Introduction à la robotique mobile 87
Avec ajustement odométrie
Proprioceptif
Odométrie + gyroscope pour trouver une position initiale
![Page 84: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/84.jpg)
Position de départ de recherche ICP
• Applique ensuite ICP, car on est proche du minimum local
GLO-4001/7021 Introduction à la robotique mobile 88
Extéroceptif
![Page 85: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/85.jpg)
Position de départ de recherche ICP
• Applique ensuite ICP, car on est proche du minimum global : raffine la position
GLO-4001/7021 Introduction à la robotique mobile 89
Extéroceptif
![Page 86: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/86.jpg)
Exemple ICP
• Fonction icp.m de Per Bergström
(http://www.mathworks.com/matlabcentral/fileexchange/12627)
GLO-4001/7021 Introduction à la robotique mobile 90
Laboratoire de ce vendredi
![Page 87: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/87.jpg)
ICP : Problème des données aberrantes
• Quant il y a des occlusions dans la scène
– pièce n’est pas convexe
• Certains points ne peuvent pas être apparié
• Présence de données aberrantes (outliers).
GLO-4001/7021 Introduction à la robotique mobile 91
![Page 88: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/88.jpg)
Données aberrantes : fonctions robustes
• Utiliser d’autres fonctions d’erreurs
92
),
(argmin ( )AR
i VT i
PP ie T BRA
e( )
• Retirer % des erreurs les plus grandes
P. Babin, P. Giguère, F. Pomerleau, Analysis of Robust Functions for Registration Algorithms, ICRA 2019.
Erreur
![Page 89: Filtre à Particules - Université Lavalpgiguere/cours/IntroRobotique/notes/...–Apollo Guidance Computer (AGC) –2.048 MHz –2 048 x 16 RAM –36 864 x 16 ROM 5 Interface DSKY](https://reader031.vdocuments.net/reader031/viewer/2022011908/5f56c87a10bd9e66c85b2466/html5/thumbnails/89.jpg)
Considération : # points
• Possible d’accélérer les performances en décimant le nombre de points
93
Tracking a Depth Camera: Parameter Exploration for Fast ICP,
François Pomerleau et al., IROS 2011.