ji r i sa 8.1 · 1 pro v sechny sousedy j vrcholu i spo c tej odhad (x0 ji;y 0 ji) pozice vrcholu...
TRANSCRIPT
Rastrove mapySLAM
Uvod do mobilnı robotiky – NAIL028Rastrove mapy; SLAM
Jirı Isa
Matematicko-fyzikalnı fakulta Univerzity Karlovy v Praze
8.1.2009
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
Obsah
1 Rastrove mapy
2 SLAM
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
Rastrova mapaHierarchicka mapaPlanovanı cestyRozsırenıPravdepodobnostnı mrızka
Rastrova mapa
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
Rastrova mapaHierarchicka mapaPlanovanı cestyRozsırenıPravdepodobnostnı mrızka
Hierarchicka mapa
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
Rastrova mapaHierarchicka mapaPlanovanı cestyRozsırenıPravdepodobnostnı mrızka
Jaka cesta?
Nejkratsı (drıve grafy viditelnosti)
Nejbezpecnejsı (drıve Voronoi diagramy)
Kompromis (drıve lichobeznıkova dekompozice)
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
Rastrova mapaHierarchicka mapaPlanovanı cestyRozsırenıPravdepodobnostnı mrızka
Nejkratsı cesta
A∗
Potencialove pole
FloodFill / BFS
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
Rastrova mapaHierarchicka mapaPlanovanı cestyRozsırenıPravdepodobnostnı mrızka
Nejkratsı cesta
A∗
Potencialove pole
FloodFill / BFS
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
Rastrova mapaHierarchicka mapaPlanovanı cestyRozsırenıPravdepodobnostnı mrızka
Nejkratsı cesta
A∗
Potencialove pole
FloodFill / BFS
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
Rastrova mapaHierarchicka mapaPlanovanı cestyRozsırenıPravdepodobnostnı mrızka
Nejkratsı cesta
A∗
Potencialove pole
FloodFill / BFS
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
Rastrova mapaHierarchicka mapaPlanovanı cestyRozsırenıPravdepodobnostnı mrızka
Nejbezpecnejsı cesta
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
Rastrova mapaHierarchicka mapaPlanovanı cestyRozsırenıPravdepodobnostnı mrızka
Nejbezpecnejsı cesta
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
Rastrova mapaHierarchicka mapaPlanovanı cestyRozsırenıPravdepodobnostnı mrızka
Kompromis – rychle a bezpecne
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
Rastrova mapaHierarchicka mapaPlanovanı cestyRozsırenıPravdepodobnostnı mrızka
Kompromis – rychle a bezpecne
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
Rastrova mapaHierarchicka mapaPlanovanı cestyRozsırenıPravdepodobnostnı mrızka
Kompromis – rychle a bezpecne
Chceme:
U(i , j) =U(i − 1, j) + U(i + 1, j) + U(i , j − 1) + U(i , j + 1)
4
Mozne resenı (1): Soustava rovnic
Mozne resenı (2): Gauss-Siedelova iterativnı metoda:
Ut+1 :=Ut(i − 1, j) + Ut(i + 1, j) + Ut(i , j − 1) + Ut(i , j + 1)
4
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
Rastrova mapaHierarchicka mapaPlanovanı cestyRozsırenıPravdepodobnostnı mrızka
Kompromis – rychle a bezpecne
Chceme:
U(i , j) =U(i − 1, j) + U(i + 1, j) + U(i , j − 1) + U(i , j + 1)
4
Mozne resenı (1): Soustava rovnic
Mozne resenı (2): Gauss-Siedelova iterativnı metoda:
Ut+1 :=Ut(i − 1, j) + Ut(i + 1, j) + Ut(i , j − 1) + Ut(i , j + 1)
4
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
Rastrova mapaHierarchicka mapaPlanovanı cestyRozsırenıPravdepodobnostnı mrızka
Kompromis – rychle a bezpecne
Chceme:
U(i , j) =U(i − 1, j) + U(i + 1, j) + U(i , j − 1) + U(i , j + 1)
4
Mozne resenı (1): Soustava rovnic
Mozne resenı (2): Gauss-Siedelova iterativnı metoda:
Ut+1 :=Ut(i − 1, j) + Ut(i + 1, j) + Ut(i , j − 1) + Ut(i , j + 1)
4
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
Rastrova mapaHierarchicka mapaPlanovanı cestyRozsırenıPravdepodobnostnı mrızka
Ruzny teren
Chceme:
U(i , j) =M(i , j)
∑k U(k)
4
Gauss-Siedelova iterace:
Ut+1 :=M(i , j)
∑k U(k)
4
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
Rastrova mapaHierarchicka mapaPlanovanı cestyRozsırenıPravdepodobnostnı mrızka
Pravdepodobnostnı mrızka
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
UlohaExpectation-maximization algoritmus (EM)RelaxaceZobecnena relaxaceReference
Simultannı lokalizacea mapovanı (SLAM)
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
UlohaExpectation-maximization algoritmus (EM)RelaxaceZobecnena relaxaceReference
Mapa
Mnozina vyznacnych prvku
Umoznuje rozhodnout, kudy semuze robot pohybovat
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
UlohaExpectation-maximization algoritmus (EM)RelaxaceZobecnena relaxaceReference
Uloha
Cıl
argmaxm
P(−→o |−→u , m)
m . . . mapa
o1, . . . , ot . . . pozorovanı
u1, . . . , ut . . . akce
t . . . cas
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
UlohaExpectation-maximization algoritmus (EM)RelaxaceZobecnena relaxaceReference
Ukazka spatne mapy
(demo)
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
UlohaExpectation-maximization algoritmus (EM)RelaxaceZobecnena relaxaceReference
Ukazka dobre mapy
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
UlohaExpectation-maximization algoritmus (EM)RelaxaceZobecnena relaxaceReference
EM algoritmus – kroky
1 Mam mapu m(t)
2 E-krok – odhad historie pozic:
Pt(ξ|o, u,mt)
3 M-krok – odhad nove mapy:
mt+1 ← argmaxm
P(o|u, ξt)
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
UlohaExpectation-maximization algoritmus (EM)RelaxaceZobecnena relaxaceReference
EM algoritmus – kroky
1 Mam mapu m(t)
2 E-krok – odhad historie pozic:
Pt(ξ|o, u,mt)
3 M-krok – odhad nove mapy:
mt+1 ← argmaxm
P(o|u, ξt)
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
UlohaExpectation-maximization algoritmus (EM)RelaxaceZobecnena relaxaceReference
EM algoritmus – kroky
1 Mam mapu m(t)
2 E-krok – odhad historie pozic:
Pt(ξ|o, u,mt)
3 M-krok – odhad nove mapy:
mt+1 ← argmaxm
P(o|u, ξt)
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
UlohaExpectation-maximization algoritmus (EM)RelaxaceZobecnena relaxaceReference
EM algoritmus – prıklad
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
UlohaExpectation-maximization algoritmus (EM)RelaxaceZobecnena relaxaceReference
EM algoritmus – prıklad
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
UlohaExpectation-maximization algoritmus (EM)RelaxaceZobecnena relaxaceReference
EM algoritmus – prıklad
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
UlohaExpectation-maximization algoritmus (EM)RelaxaceZobecnena relaxaceReference
EM algoritmus – prıklad
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
UlohaExpectation-maximization algoritmus (EM)RelaxaceZobecnena relaxaceReference
EM algoritmus – vypocet
E-krok:
P ′(ξt |d ,m) = αtξ ∗ βt
ξ
αtξ = P(ξt |o1,...,t , u1,...,t−1,m)
βtξ = P(ξt |ot+1,...,T , ut,...,T−1,m)
M-krok:
P(mxy = l |d) =# of times l was observed at < x , y >
# of times something was at < x , y >
=
∑Tt=1
∫P(mxy = l |ot , ξt)p(ξt |d ,m) dξt∑T
t=1
∑l ′∈L
∫P(mxy = l ′|ot , ξt)p(ξt |d ,m) dξt
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
UlohaExpectation-maximization algoritmus (EM)RelaxaceZobecnena relaxaceReference
EM algoritmus – vypocet
E-krok:
P ′(ξt |d ,m) = αtξ ∗ βt
ξ
αtξ = P(ξt |o1,...,t , u1,...,t−1,m)
βtξ = P(ξt |ot+1,...,T , ut,...,T−1,m)
M-krok:
P(mxy = l |d) =# of times l was observed at < x , y >
# of times something was at < x , y >
=
∑Tt=1
∫P(mxy = l |ot , ξt)p(ξt |d ,m) dξt∑T
t=1
∑l ′∈L
∫P(mxy = l ′|ot , ξt)p(ξt |d ,m) dξt
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
UlohaExpectation-maximization algoritmus (EM)RelaxaceZobecnena relaxaceReference
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
UlohaExpectation-maximization algoritmus (EM)RelaxaceZobecnena relaxaceReference
Algoritmus
Pro kazdy vrchol i
1 Pro vsechny sousedy j vrcholu i spocıtej odhad (x ′ji , y′ji ) pozice
vrcholu i :x ′ji = xj + dji cos θjiy ′ji = yj + dji sin θji
a jeho rozptyl:vji = vj + uji
2 Zkombinuj vsechny odhady pozice vrcholu i :
1
vi=
∑j
1
vjixi =
∑j
x ′jivi
vjiyi =
∑j
y ′jivi
vji
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
UlohaExpectation-maximization algoritmus (EM)RelaxaceZobecnena relaxaceReference
Zobecnena relaxace
Bez kompasu
Obdobne jako predchozı
Vyznacne body jsou jiz nejen landmarky, ale take pozicerobota v case → vzdalenosti se merı mezi pozicemi robota alandmarky
Lokalnı optimum
Jirı Isa Uvod do mobilnı robotiky – NAIL028
Rastrove mapySLAM
UlohaExpectation-maximization algoritmus (EM)RelaxaceZobecnena relaxaceReference
Reference
1 Sebastian Thrun, Wolfram Burgard, Dieter Fox:Probabilistic Mapping Of An Environment By A MobileRobot
2 Tom Duckett, Stephen Marsland, Jonathan Shapiro:Learning Globally Consistent Maps by Relaxation (ICRA2000)
3 Andrew Howard, Maja J. Mataric, Gaurav Sukhatme:Relaxation on a Mesh: a Formalism for GeneralizedLocalization (IROS 2001)
Jirı Isa Uvod do mobilnı robotiky – NAIL028