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