ball tracking in robocup cse-571 probabilistic robotics · 2007-11-12 · 1 cse-571 probabilistic...

1

CSE-571Probabilistic Robotics

Particle filters for tracking

Ball Tracking in RoboCup

Extremely noisy (nonlinear) motion of observer

Inaccurate sensing, limited processing power

Interactions between target and environment

Interactions between robot(s) and target

Goal: Unified framework for modeling the ball and its interactions.

Dieter Fox 2CSE-571: Probabilistic Robotics

Tracking Techniques

Kalman Filter

Highly efficient, robust (even for nonlinear)

Uni-modal, limited handling of nonlinearities

Particle Filter

Less efficient, highly robust

Multi-modal, nonlinear, non-Gaussian

Rao-Blackwellised Particle Filter, MHT

Combines PF with KF

Multi-modal, highly efficient


Dynamic Bayes Network for Ball Tracking

k-2b k-1b kb

r k-2 r k-1 r k

zk-2 zk-1 z k

u k-2 u k-1

zk-1 zkzk-2

kmk-1mk-2m

Ball observation

Ball location and velocity

Ball motion mode

Map and robot location

Robot control

Landmark detection

Bal

l tr

ack

ing

Ro

bo

t lo

cali

zati

on

l l l

b b b


2

Robot Location

k-2b k-1b kb

r k-2 r k-1 r k

zk-2 zk-1 z k

u k-2 u k-1

zk-1 zkzk-2

kmk-1mk-2m

Ball observation


Ball motion mode


Robot control

Landmark detection

Bal

l tr

ack

ing

Ro

bo

t lo

cali

zati

on

l l l

b b b


Robot and Ball Location (and velocity)

k-2b k-1b kb

r k-2 r k-1 r k

zk-2 zk-1 z k

u k-2 u k-1

zk-1 zkzk-2

kmk-1mk-2m

Ball observation


Ball motion mode


Robot control

Landmark detection

Bal

l tr

ack

ing

Ro

bo

t lo

cali

zati

on

l l l

b b b


Ball-Environment Interactions

None Grabbed

BouncedKicked

Deflected


Ball-Environment Interactions

None Grabbed

BouncedKicked

(0.8)

Robot loses grab

(0.2)

Ro

bo

tkic

ks

ball

(0.9

)

Within grab range

and robot grabs

(prob. from model )

Kick fails (0.1)

Deflected

(residual prob .)


3

Integrating Discrete Ball Motion Mode

k-2b k-1b kb

r k-2 r k-1 r k

zk-2 zk-1 z k

u k-2 u k-1

zk-1 zkzk-2

kmk-1mk-2m

Ball observation


Ball motion mode


Robot control

Landmark detection

Bal

l tr

ack

ing

Ro

bo

t lo

cali

zati

on

l l l

b b b


Grab Example (1)

k-2b k-1b kb

r k-2 r k-1 r k

zk-2 zk-1 z k

u k-2 u k-1

zk-1 zkzk-2

kmk-1mk-2m

Ball observation


Ball motion mode


Robot control

Landmark detection

Bal

l tr

ack

ing

Ro

bo

t lo

cali

zati

on

l l l

b b b


Grab Example (2)

k-2b k-1b kb

r k-2 r k-1 r k

zk-2 zk-1 z k

u k-2 u k-1

zk-1 zkzk-2

kmk-1mk-2m

Ball observation


Ball motion mode


Robot control

Landmark detection

Bal

l tr

ack

ing

Ro

bo

t lo

cali

zati

on

l l l

b b b


Inference: Posterior Estimation

k-2b k-1b kb

r k-2 r k-1 r k

zk-2 zk-1 z k

u k-2 u k-1

zk-1 zkzk-2

kmk-1mk-2m

Ball observation


Ball motion mode


Robot control

Landmark detection

Bal

l tr

ack

ing

Ro

bo

t lo

cali

zati

on

l l l

b b b

),,|,,(1:1:1:1 k

l

k

b

kkkkuzzrmbp


4

Particle Filter for Robot Localization


Rao-Blackwellised PF for Inference

Represent posterior by random samples

Each sample

contains robot location, ball mode, ball Kalman

filter

Generate individual components of a particle

stepwise using the factorization

iiiiiiimyxbmrs ,,,,,,,

),|(),,|(),,,|(

),|,,(

1:1:1:11:1:1:1:11:1:1:1:1

1:1:1:1:1

kkkkkkkkkkkk

kkkkk

uzrpuzrmpuzrmbp

uzrmbp


Rao-Blackwellised Particle Filter for Inference

k-1b kb

r k-1 r k

kmk-1m


Ball motion mode


Bal

l tr

ack

ing

Ro

bo

t lo

cali

zati

on

)(

1

)(

1

)(

1,,

i

k

i

k

i

kbmr

Draw a sample from the previous sample set:


Generate Robot Location

r k-1 r k

u k-1

zk


Robot control

Landmark detection

Ro

bo

t lo

cali

zati

on

l

k-1b

k-1m


Ball motion mode

Bal

l tr

ack

ing

kb

km

__,,),,,,|(~)(

1

)(

1

)(

1

)(

1

)( i

kkk

i

k

i

k

i

kk

i

kruzbmrrpr


5

Generate Ball Motion Model

k-1b

r k-1 r k

u k-1

zk

kmk-1m


Ball motion mode


Robot control

Landmark detection

Bal

l tr

ack

ing

Ro

bo

t lo

cali

zati

on

l

kb

_,,),,,,|(~)()(

1

)(

1

)(

1

)()( i

k

i

kkk

i

k

i

k

i

kk

i

kmruzbmrmpm


Update Ball Location and Velocity

k-1b

r k-1 r k

u k-1

zk

kmk-1m


Ball motion mode


Robot control

Landmark detection

Bal

l tr

ack

ing

Ro

bo

t lo

cali

zati

on

l

kb

z kb

)()()()(

1

)()()(,,),,,|(~

i

k

i

k

i

kk

i

k

i

k

i

kk

i

kbmrzbmrbpb


Importance Resampling

Weight sample by

if observation is landmark detection and by

if observation is ball detection.

Resample

)|()()( i

k

l

k

i

krzpw

k

i

k

i

k

i

k

i

k

i

k

i

k

i

k

b

k

i

k

i

k

i

k

b

k

i

k

bbrmbpbrmzp

brmzpw

d),,|(),,|(

),,|(

)(

1

)()()()()()(

)(

1

)()()(


Ball-Environment Interaction


6

Ball-Environment Interaction


Tracking and Finding the Ball

Cluster ball samples by discretizing pan / tilt angles

Uses negative information


Experiment: Real Robot

0

0.05

0.1

0.15

0.2

0.25

0.3

0.35

0.4

0.45

0.5

0 5 10 15 20 25 30 35 40 45 50

Pe

rce

nta

ge

of b

all

lost

Number of ball samples

With MapWithout Map

Robot kicks ball 100 times, tries to find it afterwards

Finds ball in 1.5 seconds on average


ReferenceObservations

Reference* Observations

Simulation Runs


7

Comparison to KF* (optimized for straight

motion)

RBPFKF*


RBPFKF*



RBPFKF’


RBPFKF’


Comparison to KF’ (inflated prediction noise)


Error vs. Prediction Time

0

10

20

30

40

50

60

70

80

0 0.5 1 1.5 2

RM

S E

rror

[cm

]

Prediction time [sec]

RBPFKF'KF*


Orientation Errors

0

20

40

60

80

100

120

140

160

180

2 3 4 5 6 7 8 9 10 11

Orienta

tion E

rror

[degre

es]

Time [sec]

RBPFKF*KF'


8

STREET MAPSource: Tiger / Line data

BUS ROUTES / STOPSSource: Metro GIS

Dieter Fox CSE-571: Probabilistic Robotics 29

RESTAURANTS / STORESSource: MS MapPoint Given data stream from a wearable GPS unit

Infer the user’s location and mode of transportation (foot, car, bus, bike, ...)

Predict where user will go

Detect novel behavior / user errors


[Liao-Fox-Kautz: AAAI-04, AIJ-07]

Dead and semi-dead zones near buildings, trees, etc.

Sparse measurements inside vehicles, especially bus

Multi-path propagation

Inaccurate street map

…


Map is directed graph

Location: Edge e Distance d from start of edge

Prediction: Move along edges according to velocity model

Correction: Update estimate based on GPS reading


9

Problem: Predicted location is multi-modal


e1

e2

e3

k-1x

e0

Problem: GPS reading is not on the graph


e1

e2

e3

zk

kx

Probabilistically “snap” GPS reading to the graph

Perform A* search to compute innovation


e1

e2

e3

zk

kx

if =e1

Probabilistically “snap” GPS reading to the graph

Perform A* search to compute innovation


e1

e2

e3

zkkx

if =e2

10

Rao-Blackwellised particle filter represents posterior by sets of weighted particles:

Each particle contains Kalman filter for location:


},...,1,,{)()(

niwsSii

k

),(,,,2)()()()()( iiiii

Nves

Edge transitions, velocities, edge

associations

Gaussian for position


GPS measurements

Particles (Kalman filters)

Encode prior knowledge into the model

Modes have different velocity distributions

Buses run on bus routes

Get on/off the bus near bus stops

Switch to car near car location

Dieter Fox CSE-571: Probabilistic Robotics 39 Dieter Fox CSE-571: Probabilistic Robotics 40

GPS readingzk-1 zk

Edge, velocity, positionxk-1 xk

Time k-1 Time k

mk-1 mk Transportation mode

( ) ( ) ( ) ( )( ) ( ) 2, , , , ( , )

i i i ii is e v NmParticles:

11


MeasurementsProjections

Bus modeCar modeFoot mode

GreenRedBlue

Goal (destination):

workplace (home, friends, restaurant, ...)

Trip segments: <start, end, transportation>

Home to Bus stop A on Foot

Bus stop A to Bus stop B on Bus

Bus stop B to workplace on Foot


BA Workplace


GPS readingzk-1 zk


Time k-1 Time k


tk-1 tk Trip segment

gk-1 gk Goal

(( ) ( ) ( ) ( ) () ) ( ) 2, , ,, , , ( , )

i i i i i ii

s m eg v NtParticles:

Key to goal / path prediction and error detection Customized model for each user Unsupervised model learning

Learn variable domains (goals, trip segments)

Learn transition parameters (goals, trips, edges)

Training data

30 days GPS readings of one user, logged every second (when outdoors)


12


Predicted goal

Predicted path

GOING TO THE WORKPLACE GOING HOME


High probability transitions: bus car foot


GPS readingzk-1 zk


Time k-1 Time k


tk-1 tk Trip segment

gk-1 gk Goal

bk-1 bk Behavior modenormal / unknown / error

[Patterson-Liao-etAl: Ubicomp-04]


13

Dieter Fox CSE-571: Probabilistic Robotics 49 Dieter Fox CSE-571: Probabilistic Robotics 50

Discussion

Particle filters are intuitive and simple

Support point-wise thinking (reduced uncertainty)

It’s an art to make them work

Good for test implementation if system behavior is not well known

Inefficient compared to Kalman filter

Rao-Blackwellization

Only sample discrete / highly non-linear parts of state space

Solve remaining part analytically (KF,discrete)Dieter Fox 51CSE-571: Probabilistic Robotics

ball tracking in robocup cse-571 probabilistic robotics · 2007-11-12 · 1 cse-571 probabilistic...

Documents