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