5.9 slam – simultaneous localization and mapping€¦ · localization and mapping. einleitung...
TRANSCRIPT
![Page 1: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/1.jpg)
9 SLAM – Simultaneous Localization and Mapping
![Page 2: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/2.jpg)
Einleitung
• eines der aktivsten Forschungsgebiete innerhalb der Robotik• Roboterlokalisierung bei gegebener Karte (Kap. 8) und Karte aus
Sensordaten bei bekannter Position (Kap. 7) sind relativ einfach• beide Probleme aber gleichzeitig zu lösen ist schwierig• aktuelle Algorithmen verwenden probabilistische Ansätze, um
Unsicherheiten explizit zu modellieren• deterministische Ansätze sind meist schlechter
![Page 3: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/3.jpg)
Einleitung
• Eingabe:– Sensordaten– Kontrollkommandos des Roboters (Fahren)
• Gesucht:– Schätzung der Karte– Pfad des Roboters (alle bisherigen
Positionen)
![Page 4: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/4.jpg)
Probleme
• Sowohl Positionen der Landmarken als auch Roboterweg sind unbekannt
• Die Zuordnung von Sensordaten zu Landmarken sind in der Regel unbekannt
• Roboter muss entscheiden, ob Sensordaten zu einer bereits beobachteten Landmarke zugeordnet werden können oder zu einer noch nicht gesehenen Landmarke
• Zuordnungsproblematik wird durch Fehler im Roboterweg verstärkt
• Fehler kann prinzipiell unbegrenzt wachsen
![Page 5: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/5.jpg)
SLAM – Graph – 1
A
B
C
D
E
Rob
Roboter sieht die Landmarken A und B.
Fehler
B
![Page 6: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/6.jpg)
SLAM – Graph – 2
A
B
C
D
Rob
E
Nach Bewegung des Roboters nimmt dieGenauigkeit der Lokalisierung ab
Die Unsicherheit der Positionsschätzungender Landmarken C und D steigt
Fehler
![Page 7: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/7.jpg)
SLAM – Graph – 3
A
B
C
D
Rob
E
Roboter erkennt eine bereitsgesehene Landmarke wieder
![Page 8: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/8.jpg)
SLAM – Graph – 4
A
B
C
D
Rob
E
wird genauer
rekursive Verbesserung (Optimierung)
![Page 9: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/9.jpg)
Problem
• Datenassoziation• Sobald Landmarken nicht eindeutig unterscheidbar
sind, muss die Frage beantwortet werden, in welchen Fällen zwei gleich aussehende Landmarken tatsächlich dieselbe Landmarke waren
• Macht der Roboter hier Fehler, kann das starke Auswirkungen auf die Lösung (Karte) haben
![Page 10: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/10.jpg)
Lösung des SLAM – Problems
• Aufbau des Graphen
• Optimierung
![Page 11: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/11.jpg)
SLAM – Simultaneous Localization and Mapping
p ( x t ,m∣z1: t , u1: t )Online SLAM Problem(inkrementelles SLAM)
Full SLAM Problem(vollständiges SLAM)
p ( x1: t , m∣z1: t , u1: t )
p ( x t ,m , c t∣z1: t , u1: t )
p ( x1: t , m , c1 : t∣z1 : t , u1 : t )
![Page 12: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/12.jpg)
Full SLAM
![Page 13: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/13.jpg)
Online SLAM
![Page 14: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/14.jpg)
Verfahren
• EKF SLAM– verwendet Extended Kalman Filter– Online
• Fast SLAM– verwendet Partikel Filter
• Graph SLAM– vollständiges SLAM
![Page 15: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/15.jpg)
Verfahren – Niko Sünderhauf
![Page 16: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/16.jpg)
9.1 Simple SLAM – Einfaches Modell
![Page 17: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/17.jpg)
Simple SLAM
N Landmarken (Positionen):
Roboterposition ( xr , yr )
( x l1, y l
1)
( x l2, y l
2)
( x lN, y l
N)
…
![Page 18: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/18.jpg)
Simple SLAM - Zustand
x t=( xr ( t ) , yr ( t ) , x l1 , y l1 ,⋯, x li , y li ,⋯x lN , y lN )T
u t=(v x( t ) , v y( t ))T
Omnidirektionales Bewegungsmodell(Antrieb in alle Richtungen)(Roboterfront zeigt immer in gleiche Richtung)
Bewegung
![Page 19: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/19.jpg)
Simple SLAM – Bewegungsgleichung
x t=At⋅x t−1+Bt⋅u t+εt
At=(1 0 ⋯ 00 1 ⋯ 0⋮ ⋮ ⋱ ⋮0 0 ⋯ 1
) Bt=(1 00 10 00 0⋮ ⋮
0 0)
ε t=(ε rx( t ) , εry( t ) ,0,0 ,⋯,0 )T
(εrx ( t ) , ε ry( t ))T→N (0,V rt )
ε t→N (0,V t )
V t=(V rt 0 ⋯ 0
0 0 ⋯ 0⋮ ⋮ ⋱ ⋮0 0 ⋯ 0
)Landmarken bewegen sich nicht
![Page 20: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/20.jpg)
Simple SLAM – Sensordaten
Landmarken seien eindeutig identifizierbar, für jede Landmarke wird ihre Position relativ zum Roboter gemessen
zt=(z1( t )
⋮
z i( t )
⋮
zN ( t ))=(
x l1−xr ( t )
y l1− yr( t )
⋮xl N−xr( t )
y lN−yr (t ))
![Page 21: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/21.jpg)
Simple SLAM - Messgleichung
zt=H t⋅x t+δt
H t=(H 1( t )
H 2( t )
⋮
H N ( t )) H i=(−1 0 0 ⋯ 0 1 0 0 ⋯ 0
0 −1 0 ⋯ 0 0 1 0 ⋯ 0)
(2i+1)-te Spalte
![Page 22: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/22.jpg)
Simple SLAM – Messgleichung
zt=H t⋅x t+δt
δ t→N (0,W t )
δ t=(δ1 ( t ) , δ2( t ) ,⋯, δN ( t ))T
W t=(W 1t 0 ⋯ 0
0 W 2t ⋯ 0
⋮ ⋮ ⋱ ⋮
0 0 ⋯ W Nt)
δi ( t )→N (0,W it )
![Page 23: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/23.jpg)
Simple SLAM – Kalmanfilter
• Damit sind alle alle Systemparameter (lineares System!) festgelegt und der lineare Kalman-Filter-Algorithmus kann direkt verwendet werden.
![Page 24: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/24.jpg)
Simple SLAM – Erweiterungen
• Das Bewegungsmodel kann erweitert werden. Zur Roboterposition kommt dann noch die Orientierung dazu.
• Statt bei jeder Landmarke die relative Position zu messen, kann auch Abstand und Orientierung gemessen werden.
• Zusätzlich kann noch das Problem der Zuordnung der Messdaten zu den Landmarken-Nummern dazukommen.
• EKF SLAM
![Page 25: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/25.jpg)
9.2 EKF SLAM
![Page 26: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/26.jpg)
EKF – SLAM
• erster SLAM Algorithmus
• verwendet erweiterten Kalmanfilter
• Online SLAM
• bekannte Korrespondenzen zu den Landmarken
• unbekannte Korrespondenzen zu den Landmarken
![Page 27: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/27.jpg)
Beispiel
Hier sieht der Roboter die erste Landmarke erneut, sämtliche Unsicherheiten werden erheblichkleiner.
Roboterposition
Landmarken (8)
![Page 28: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/28.jpg)
EKF SLAM
yt=(x tm)
zu berechnen: p ( y t∣z1: t , u1: t )
y t=( x , y ,θ ,m1, x ,m1, y , s1 ,⋯,mN , x ,mN , y , sN )T
![Page 29: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/29.jpg)
EKF SLAM
μ0=(0,0 ,⋯,0 )T 3N+3 - Vektor
Σ 0=(0 0 0 0 ⋯ 00 0 0 0 ⋯ 00 0 0 0 ⋯ 00 0 0 ∞ ⋯ 0⋮ ⋮ ⋮ ⋮ ⋱ ⋮
0 0 0 0 ⋯ ∞) (3N+3)×(3N+3) - Matrix
![Page 30: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/30.jpg)
EKF SLAM
y t= y t−1+F xT⋅(−v tw t⋅sinθ+
v tw t⋅sin (θ+w t⋅Δt )
v tw t⋅cosθ−
vtwt⋅cos(θ+wt⋅Δt )
w t⋅Δt)+N (0, F x
T⋅Rt⋅F x)
yt=g (u t , y t−1 )+N (0, F xT⋅Rt⋅F x )
F x=(1 0 0 0 ⋯ 00 1 0 0 ⋯ 00 0 1 0 ⋯ 0)
3⋅N
![Page 31: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/31.jpg)
EKF SLAM
yt=g (u t , y t−1 )+N (0, F xT⋅Rt⋅F x )
g (u t , y t−1 )≈g (ut , μt−1 )+G t⋅( y t−1−μ t−1 )
Gt= I+F xT⋅g t⋅F x
g t=(0 0
v tw t(−cos( μt−1,θ )+cos ( μ t−1, θ+wt Δt ))
0 0vtw t(−sin( μt−1,θ )+sin ( μ t−1,θ+wt Δt ))
0 0 0)
![Page 32: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/32.jpg)
EKF SLAM
μ̄t=g ( ut , μt−1 )=μ t−1+F xT⋅(−v tw t⋅sin θ+
v tw t⋅sin (θ+w t⋅Δt )
v twt⋅cosθ−
v tw t⋅cos(θ+wt⋅Δt )
wt⋅Δt) θ=μt−1,θ
Σ̄ t=Gt⋅Σ t−1⋅G tT+F x
T⋅Rt⋅F x
![Page 33: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/33.jpg)
Sensorbeobachtungen
![Page 34: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/34.jpg)
EKF SLAM
Landmarke j wird zum ersten Mal gesehen:
(μ̄ j , xμ̄ j , yμ̄ j , s)=(
μ̄t , xμ̄ t , ysti )+(
rti⋅cos (φt
i+ μ̄ t , θ )
r ti⋅sin (φt
i+ μ̄ t , θ )
0 )
δ=(δ xδ y)=(μ̄ j , x− μ̄ t , xμ̄ j , y− μ̄ t , y) q=δT⋅δ
![Page 35: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/35.jpg)
EKF SLAM
zti=(
r ti
φti
sti )=( √(m j , x− x )
2+(m j , y− y )
2
atan 2(m j , y− y ,m j , x−x )−θ
m j , s)+N (0,Qt )
m={m1 ,⋯,mN } m j=(m j , x ,m j , y ,m j , s )T j=c t
i
zti=h( y t , j )+N (0,Qt )
Qt=(σ r
2 0 0
0 σφ2 0
0 0 σs2)h ( y t , j)≈h( μ̄t , j)+H t
i( y t− μ̄ t )
![Page 36: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/36.jpg)
EKF SLAM
h ( y t , j)≈h( μ̄t , j)+H ti( y t− μ̄ t )
H ti=h t
i⋅F x , j
H ti=
1q(√q⋅δ x −√q⋅δ y 0 −√q⋅δ x √q⋅δ y 0
δ y δ x −1 −δ y −δ x 0
0 0 0 0 0 1)⋅F x , j
![Page 37: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/37.jpg)
EKF SLAM
F x , j=(1 0 0 0 ⋯ 0 0 0 0 0 ⋯ 00 1 0 0 ⋯ 0 0 0 0 0 ⋯ 00 0 1 0 ⋯ 0 0 0 0 0 ⋯ 00 0 0 0 ⋯ 0 1 0 0 0 ⋯ 00 0 0 0 ⋯ 0 0 1 0 0 ⋯ 00 0 0 0 ⋯ 0 0 0 1 0 ⋯ 0
)H ti=h t
i⋅F x , j
3⋅ j−3 3⋅N−3⋅j
![Page 38: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/38.jpg)
EKF SLAM
for all features zti : S t
i=H t
i⋅Σ̄ t⋅(H t
i)T+Q t
K ti=Σ̄ t⋅(H t
i )T⋅(S ti )
−1
μ̄t= μ̄t+K ti ( z t
i−(√q
atan 2(δ y , δ x )− μ̄t , θμ̄ j , s
))Σ̄ t=( I−K t
i⋅H t
i)⋅Σ̄ t
μt= μ̄t Σ t=Σ̄ t
![Page 39: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/39.jpg)
9.3 Graph SLAM
![Page 40: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/40.jpg)
9.3.1 Pose Graph SLAM Problem
![Page 41: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/41.jpg)
Idee• Karte – alle Roboterpositionen, keine
Landmarken
• Graph (Pose Graph) zur Repräsentation des Problems
• Knoten – Roboterpositionen
• Kante – Constraint zwischen 2 Knoten (Odometrie Messung)
• Konstruiere den Graphen und finde eine Knotenmenge, die einen Fehler über alle Constraints minimiert
![Page 42: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/42.jpg)
Karte und Pose Graph
x=( x , y ,θ)T2D Welt:
Roboterpositionen: x0 , x1 ,⋯
x0 x1 x 2 x 3 x 4Pose Graph:
Odometrie – Constraints
![Page 43: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/43.jpg)
Weg des Roboters
![Page 44: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/44.jpg)
Roboter sieht Positionen erneut
![Page 45: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/45.jpg)
Kanten – Constraints
x0 x1 x 2 x 3 x 4
Odometrie:
x i+1∼N (g ( x i ,u i) , Σi)
x i+1=g ( x i ,u i)+w i
w i∼N (0 ,Σi)Normalverteilung
Bewegungsmodell
![Page 46: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/46.jpg)
Kanten – Constraints
Loop closure constraints:
x0 x1 x 2 x 3 x 4
x j∼N (g (x i ,ui , j) ,Λ ij)
Der Roboter sieht einePosition erneut.
![Page 47: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/47.jpg)
Graph SLAM – Optimierung
Gegeben: ui ,uij∈U
Gesucht: X *
X *=argmaxX
P (X |U )
x i∈X
![Page 48: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/48.jpg)
Graph SLAM – Optimierung
X *=argmaxX
P (X |U )
P (X |U )=α∏i
P (x i+1 | x i ,u i)∏i , j
P (x j | x i ,u i , j)
Odometry Constraints Loop Closure Constraints
![Page 49: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/49.jpg)
Graph SLAM – Optimierung
P (X |U )=α∏i
P (x i+1 | x i ,u i)∏i , j
P (x j | x i ,u i , j)
∥a−b∥Σ2=(a−b)T Σ−1
(a−b)Squared Mahalanobis distance:
P (x i+1 | x i ,u i)=ηe−
12∥g (x i ,ui )−xi+1∥Σi
2
P (x j | x i ,u ij)=ηe−
12∥g (x i , uij)−x j∥Λi, j
2
![Page 50: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/50.jpg)
Graph SLAM – Optimierung
P (X |U )=η∏i
e−
12∥g ( xi ,u i)−x i+1∥Σ i
2
∏i , j
e−
12∥g ( xi ,u i , j)−x j∥Λij
2
−log (P(X |U ))=η(∑i
∥g (x i ,ui)−x i+1∥Σi2+∑i , j
∥g ( x i ,u i , j)−x j∥Λij2)
X *=argmaxX
P (X |U )
X *=argminX
−log P(X |U )
![Page 51: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/51.jpg)
Graph SLAM – Optimierung
X *=argminX
−log P(X |U )
X *=argminX
(∑i
∥g ( x i ,u i)−x i+ 1∥Σi2+∑i , j
∥g ( x i ,u i , j)−x j∥Λij
2)
(weighted) least squares optimization problem
![Page 52: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/52.jpg)
Least Squares Optimization Problem
F (x )=12∑i=1
n
f i( x)2
f i : Rm→R x∈Rm x=( x1,⋯ , xm)
T
f (x )=( f 1(x ) , f 2(x ) ,⋯, f n( x))T
F (x )=12f ( x)T⋅ f (x )
Optimierungsproblem:
x *=argminx
F (x )=argminx
12f (x )T⋅ f ( x)
![Page 53: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/53.jpg)
Weighted Least Squares Optimization Problem
x *=argminx
F (x )=argminx
12f (x )TΩ f (x )
F=12∑i
∥ f i∥Σi2=
12f T Σ−1 f =
12f TΩ f
covariance matrix information matrix
![Page 54: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/54.jpg)
Lösung Gauss – Newton
J TΩ J Δ x=−J TΩT f (x )
x→ x+Δ x
x *=argminx
F (x )=argminx
12f (x )TΩ f (x )
Iteration: x0
J=∂ f∂ x
![Page 55: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/55.jpg)
Beispiel
x0 x1 x2 x3 x4
u1u0 u2 u3
u0,4
prior constraint
odometry constraintsloop closure constraints
![Page 56: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/56.jpg)
Beispiel
x0 x1 x2 x3 x4
u1u0 u2 u3
u0,4
exakter Wert Messung
u0
1 1.1
u1
1 1.0
u2
1 1.1
u3
-3 -2.7
u0,4
0 0
![Page 57: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/57.jpg)
Beispiel
x0 x1 x2 x3 x4
u1u0 u2 u3
u0,4
exakter Wert Messung
x0
0 0.0
x1
1 1.1
x2
2 2.1
x3
3 3.2
x4
0 0.5
![Page 58: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/58.jpg)
Beispiel
x0 x1 x2 x3 x4
u1u0 u2 u3
u0,4
Constraints: ∥g (x i ,ui)−x i+1∥Σi2=∥x i+u i− xi+1∥Σi
2
i=0,1,2 ,3
∥g (x0 ,u0,4)−x4∥Λ0,4
2=∥x0+u0,4−x4∥Λ0,4
2
∥x0−0∥Π2
prior constraint
![Page 59: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/59.jpg)
Beispiel
x0 x1 x2 x3 x4
u1u0 u2 u3
u0,4
x *=argminx
F (x )=argminx
12f (x )TΩ f (x )
f (x )=(x0+u0− x1
x1+u1−x2
x2+u2− x3
x3+u3− x4
x0+u0,4−x4
x0−0) Ω=(
0.01 0 0 0 0 00 0.01 0 0 0 00 0 0.01 0 0 00 0 0 0.01 0 00 0 0 0 0.01 00 0 0 0 0 0.001
)−1
![Page 60: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/60.jpg)
Beispiel
x0 x1 x2 x3 x4
u1u0 u2 u3
u0,4
x *=argminx
F (x )=argminx
12f (x )TΩ f (x )
f (x )=(x0+u0− x1
x1+u1−x2
x2+u2− x3
x3+u3− x4
x0+u0,4−x4
x0−0) Ω=(
100 0 0 0 0 00 100 0 0 0 00 0 100 0 0 00 0 0 100 0 00 0 0 0 100 00 0 0 0 0 1000
)
![Page 61: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/61.jpg)
Beispiel
x *=argminx
F (x )=argminx
12f (x )TΩ f (x )
f (x )=(x0+u0− x1
x1+u1−x2
x2+u2− x3
x3+u3− x4
x0+u0,4−x4
x0−0) Ω=(
100 0 0 0 0 00 100 0 0 0 00 0 100 0 0 00 0 0 100 0 00 0 0 0 100 00 0 0 0 0 1000
)J=∂ f∂ x=(∂ f∂ x0
∂ f∂ x1
∂ f∂ x2
∂ f∂ x3
∂ f∂ x4)=(
1 −1 0 0 00 1 −1 0 00 0 1 −1 00 0 0 1 −11 0 0 0 −11 0 0 0 0
)
![Page 62: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/62.jpg)
Beispiel – Iteration
J TΩ J Δ x=−J TΩT f (x )
x→ x+Δ x
x0=(0 1.1 2.1 3.2 0.5)T
![Page 63: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/63.jpg)
9.3.2 Graph SLAM mit Landmarken
![Page 64: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/64.jpg)
Graph – SLAM mit Landmarken
• Full SLAM
• Knoten sind Roboterpositionen und Landmarken
• Kanten beschreiben Bewegungen und Sensorbeobachtungen
• Kanten sind nichtlineare Constraints zugeordnet
![Page 65: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/65.jpg)
Graph – SLAM mit Landmarken
wird minimiert
![Page 66: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/66.jpg)
Konstruktion
![Page 67: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/67.jpg)
Konstruktion
![Page 68: 5.9 SLAM – Simultaneous Localization and Mapping€¦ · Localization and Mapping. Einleitung •eines der aktivsten Forschungsgebiete innerhalb der Robotik •Roboterlokalisierung](https://reader037.vdokument.com/reader037/viewer/2022100214/5f071d0a7e708231d41b5f58/html5/thumbnails/68.jpg)
Konstruktion