Universität Bremen
Stochastische Selbstlokalisation I
Thomas Röfer
Unsicherheit
Probabilistischer Ansatz
Modelle für Bewegung und WahrnehmungGitterkarten
Monte Carlo Methode
Kogn itive Robotik II – Stochastische Selbstlokalisation I 2
Universität Bremen
Rückblick „Architektur der Rollstühle“
Verteilt
OdometryOdometry
StopSensorStopSensor
AdaptiveSteeringAdaptiveSteering
BasicBehaviorsBasicBehaviors
ScannerScanner
MarkMapMarkMap
RouteNavigationRouteNavigation
setPosition
SimChairSimChair SimChairSimChair
OdometryOdometry
StopSensorStopSensor
AdaptiveSteeringAdaptiveSteering
BasicBehaviorsBasicBehaviors
ScannerScanner
MarkMapMarkMap
RouteNavigationRouteNavigation
setSpeed
ModifikationMitteilung
Intramodulkommunikation
Fahrkommandovom Joystick
Ultraschalldaten
der Umgebung
FahrkommandoSicherheits-
modu lSicherheits-
modu l
Sicheres
Sicherer Rollstuhl
Sensorik & AktuatorikModul (SAM)
Sensorik & AktuatorikModul (SAM)
EchtzeitanwendungEchtzeitanwendung
NetzwerkNetzwerk
AsynchroneAnwendung
AsynchroneAnwendung
RollstuhlRollstuhl
32 ms
Architektur
Sender<id,T>Sender<id,T>
RelayRelay
Receiver<id,T>Receiver<id,T>
RelayRelayRelayRelay
Netzwerk
Rea
litä
t/Sim
ula
tion
Interrupt<3>Interrupt<3>
Serial<3>Serial<3>
ControlPort<3>ControlPort<3>
MeyraMeyra
OdometryOdometry
ObstacleMapObstacleMap
ObstacleDistanceObstacleDistance
StopInTimeStopInTime
Interrupt<4>Interrupt<4>
Serial<4>Serial<4>
ServicePort<4>ServicePort<4>
SonarsSonars
SmoothMotionC.SmoothMotionC.
RollandRolland
RelayRelay
NetNet
TablesTables
ObstacleAvoidanceObstacleAvoidance
SAMSAM
PlatformPlatform
Sick<5>Sick<5>
Interrupt<5>Interrupt<5>
Serial<5>Serial<5>
Simulation/Realität
Kogn itive Robotik II – Stochastische Selbstlokalisation I 3
Universität Bremen
Ansätze zur Selbstlokalisation
Ohne bekannte Startposition
224cm, 75° , 799cm, -83° , 880cm, -87° , 260cm
Weiterführen einer bekannten Position
Kogn itive Robotik II – Stochastische Selbstlokalisation I 4
Universität Bremen
Unsicherheit – Bewegung
Kogn itive Robotik II – Stochastische Selbstlokalisation I 5
Universität Bremen
Unsicherheit – Messungen
Ultraschall Laser
Kogn itive Robotik II – Stochastische Selbstlokalisation I 6
Universität Bremen
Probabilistischer Ansatz
� Aktueller Aufenthaltsort st hängt ab von
� Wahrgenommene Daten dt
� Dem Modell der Umwelt m (z.B. Karte + Modell des Roboters)
� Die Wahrscheinlichkeit p, an st zu sein, wenn d0 ... dt und m gegeben sind, lautet
�
� Die zur Verfügung stehenden Daten sind
� Beobachtungen der Umgebung ot
� Wissen über durchgeführte Aktionen at
�
� „ Vertrauen“ in aktuelle Position st
� bt(st )
Kogn itive Robotik II – Stochastische Selbstlokalisation I 7
Universität Bremen
Rekursive Definition des Belief State
∑=k
kk BPBAPAP )()|()(
Kogn itive Robotik II – Stochastische Selbstlokalisation I 8
Universität Bremen
Benötigte Modelle
� Auswirkung en von Bewegungen auf die Aufenthaltswahrscheinlichkeitsverteilung
� Auswirkung en der Umgebungswahrnehmung auf die Aufenthaltswahrscheinlichkeitsverteilung
Kogn itive Robotik II – Stochastische Selbstlokalisation I 9
Universität Bremen
Wahrnehmungsmodell – Ultraschall
Kogn itive Robotik II – Stochastische Selbstlokalisation I 10
Universität Bremen
Wahrnehmungsmodell – Karte ausles.
� Messwert aus Karte auslesen
� posglobal = posrobot + possensor
� (x,y) = scan(posglobal.x, posglobal.y,Math.abs(cos(posglobal .θ)),Math.abs(sin(posglobal .θ)))
� Messwert ist Abstand von (x,y) und posglobal
� scan(x,y,dx,dy)
� sx = dx < 0 ? -1 : dx > 0 ? 1 : 0,sy = dy < 0 ? -1 : dy > 0 ? 1 : 0;if(dx > dy) {
int sum = dx / 2;while(map[x][y] == EMPTY) {sum += dy;if(sum >= dx) {sum -= dx;y1 += sy;
}x1 += sx;
}}else // ...
Kogn itive Robotik II – Stochastische Selbstlokalisation I 11
Universität Bremen
Wahrnehmungsmodell – Laser
Kogn itive Robotik II – Stochastische Selbstlokalisation I 12
Universität Bremen
Bewegungsmodell
posneu = posalt + (posversatz.x (1 + gauss σ�),posversatz.y (1 + gauss σ�‘),posversatz.θ
�
(1 + gauss� ))
Kogn itive Robotik II – Stochastische Selbstlokalisation I 13
Universität Bremen
Gitterkarte
� Nachteile
� Großer Speicherverbrauch
Z.B. 250 x 250 x 180 = 11.250.000 Zellen für eine Etage des MZH bei 20 x 20 cm2
Zellen und 180 Rotationen
� Sehr rechenaufwendig (d.h. langsam)
� Ungenau
x
y
θ
Kogn itive Robotik II – Stochastische Selbstlokalisation I 14
Universität Bremen
Gitterkarte –Aufenthaltswahrscheinlichkeiten
Kogn itive Robotik II – Stochastische Selbstlokalisation I 15
Universität Bremen
Gitterkarte – Aktualisierung
Anhand von Odometrie verschieben
Anhand von Odometrie verschieben
Sensormesswerteintegrieren
Sensormesswerteintegrieren
Von Hindernissen belegteBereiche ausschließen
Von Hindernissen belegteBereiche ausschließen
KarteKarte
NormierungNormierung
Kogn itive Robotik II – Stochastische Selbstlokalisation I 16
Universität Bremen
Rotation
Gitterkarte – Odometrie
Translation
Kogn itive Robotik II – Stochastische Selbstlokalisation I 17
Universität Bremen
Gitterkarten – Odometriefehler
2 4 24 8 4
2 4 2
1 2 12 4 2
1 2 1
1 2 12 4 2
1 2 1
*
Kogn itive Robotik II – Stochastische Selbstlokalisation I 18
Universität Bremen
Gitterkarten – Sensormesswerte
Für jeden neuen Sensormesswert o des Sensors i
� Für alle xFür alle y
Für alle θp[x][y][θ] *= sensorModell(o,kartenMesswert(x,y,θ,i))
Optimierung d er Integration d er Sensormesswerte
� Jeden Sensormesswert für jede Zelle im Voraus berechnen
� Aber: Speicherplatz = Größe der Karte x Anzahl der Sensoren!
Generelle Optimierung en
� Trackingmodus
� Zerlegung des Gitters als OctreeNeuberechnung nur in Unterquadern, deren Gesamtwahrscheinlichkeit nicht zu klein ist (kompliziert wg. Bewegungsintegration)
Kogn itive Robotik II – Stochastische Selbstlokalisation I 19
Universität Bremen
Gitterkarten – Ergebnis
Kogn itive Robotik II – Stochastische Selbstlokalisation I 20
Universität Bremen
Monte Carlo – Grundidee
� Beispiel: Berechnung von PI
int all = 0,inside = 0;
while(true) {double x = Math.random(),
y = Math.random();++all;if(x*x + y*y <= 1)
++inside;System.out.println(4.0 * inside / all);
}
100
1
y
x
2
2
r
F
rF
=
=
π
π
Kogn itive Robotik II – Stochastische Selbstlokalisation I 21
Universität Bremen
Monte Carlo – Modell
� Samples
� Position posi
� Wahrscheinlichkeit pi
� Bewegung saktualisierung
� posi := posi + ( posversatz.x (1 + gaussσ),posversatz.y (1 + gaussσ‘),posversatz.θ (1 + gaussσ‘‘))
� Wahrnehmung saktualisierung
� pi = sensorModell(o,kartenMesswert(x,y,θ,i))
Kogn itive Robotik II – Stochastische Selbstlokalisation I 22
Universität Bremen
Monte Carlo – Resampling
� Wichtigkeit
� Für 1% der Samples
� posmin := posmaxpmin := pmax
wobei min = minargj pjmax = maxargj pj
� Unsicherheit
� Für wenige Samples
� posmin := (random,random,random)pmin := Σj pj / #Samples
wobei min = minargj pj
Kogn itive Robotik II – Stochastische Selbstlokalisation I 23
Universität Bremen
Monte Carlo – Ergebnisse