Individual Localization and Tracking in Multi-Robot

Settings with Dynamic Landmarks

Anousha Mesbah Prashant Doshi

University of Georgia

Objective

Focus is on the subject robot’s localization at its own level in the presence of others who may not be cooperativeLocalization in the presence of dynamic landmarks whose signature is knownTracking the uncertain location of the other robot(s)

Related Work

Cooperative positioning localization and mapping in which other agents act as landmarks (Kurazume & Nagata94)

Leap frog path planning uses cooperative positioning method (Tully et al. 10)

Localization using stationary objects in dynamic environment (Fox et al.99, Wolf et al. 05)

Separating out possible transient objects and use only stationary objects for localization

Simultaneous Localization and People tracking (Fox et al.99)

People are modeled using Brownian noise

Background: Particle Filtering

A technique for implementing recursive Bayesian filter using Monte Carlo samplingTracks the variable of interest over timeRepresents the posterior density by a set of random particles with associated weights

at-1 t-1

xt-1,(n) = <x,y, θ>

οtκ t

Propagate Weight Resample

P(xt|at-1,xt-1)

Particle Filtering: Propagation

Each particle, x(n) : n = 1…N, represents a possible pose of the robot, x(n) =<x,y,Θ>Given an action a with a commanded velocity ν and rotational velocity ω; and being actual translational and rotational velocity, motion model propagates each particle:

)cos(cos

)sin(sin

vv

vv

y

x

y

x

v

Particle Filtering: ObservationLet vector Ok

t =(rkt,Φk

t,ckt) be features of

landmark krk

t: average range of detected landmark k

Φkt:difference in orientation of the particle and bearing

of the observed landmarkCk

t: observed color of the landmark

Let Ok =(rk,Φk,ck) be the true features of the landmark k

rk: true range based on exact location of the landmark

Φk:difference in orientation of the particle and bearing of the true location of landmarkCk: true color of the landmark

Weighting will be done by calculating P(ot

k|fk,xt,(n),m)= N(rtk-rk,σr)*

N(φtk-φk,σφ)*N(ct

k-ck,σc)

Multi-Robot Laser Tag

Modified version of Rosencrantz Laser Tag domainAssumptions

i and j know exact locations of landmarksRobots are not aware of their own and each other’s location

i tags j and proceeds to j’s basej uses mixed strategy behavior until it is taggedAfter tagging, j moves the nearest landmark to confound and slow i’s progress to the base

Our Approach: Nested Particle Filtering

i will predict j’s action in order to track itFor each of i’s hypothesized pose, i maintains set of hypothesized poses for j (nesting)j’s location is represented by set of nested particlesOn predicting j’s action i may update the possible location of moved landmark

t-1

xit-1,(n) = <(x,y, θ)i , >

xjt-1,(n) = <(x,y, θ) j>

Nested Particle Filtering

Robot i’s particle at time t-1xi

t-1, (n) = < (x,y,Θ)i, xj t-1 > where xj

t-1 ,(n) is set of j’s particles at time t-1

xj t-1,(n)= <x,y,Θ> representing j’s pose

Mj = Δ(Aj) mixed strategy behavior i assumes for j

In propagation step, i samples among j’s possible actions to propagate j’s particlesRobot i’s observation is two-fold: observing landmarks and observing robot j

Oit = < Oik

t , Oijt>

Oijt indicative of j being spotted

Oikt indicative of landmark k being spotted

Visual Representation of Nested PFt

PropagateWeight Resample

at-1 t-1

xit-1,(n) = <(x,y, θ)i , >

xjt-1,(n) = <(x,y, θ) j>

xit,(n) = <(x,y, θ)i , >

Οti=<oik

t,oijt>

xit,(n) = <(x,y, θ)i , >, wi

oijt

xjt,(n) = <(x,y, θ) j>,wj

xit,(n) = <(x,y, θ)i , >

xjt,(n) = <(x,y, θ) j>

Nested PF: Propagation

For each particle at time t-1 xit-1 (n)

Propagate the new pose for i based on action performed using the propagation equationIf xi

t-1 (n) contains nested particle set

For each of j ’s particle belonging to xit-1,(n)

Predict action using the model of jPropagate to get a new pose for j’s particle xj

t

(n) and add it to the list of j’s particle for xit (n)

Add xit,(n) to the list of particles at time t

Nested PF: Weight

For each particle of i xit,(n) at time t

Calculate the range and bearing of the particle from observed landmark k Tag the particle based on observation functionIf the particle contains nested particle

For each of j ’s particle xjt (n) belonging to xi

t (n)

Weight the particle based on +ve /-ve observation of j

Add particle xjt (n) and its weight to the list of j’s particle

for xit (n)

Add xit (n) and its weight to the list of particles at

time t

Experiments

Hypothesis: By explicitly tracking j using particles and estimating pushed landmarks, our approach localizes better in comparison to othersExperiments were run in the Microsoft Robotics Simulated Environment comparing three different methods

Original map: Localization based on the original map disregarding any changes in the mapWolf approach: Localization based on static landmarks only (disregard the pushed landmarks) Nested PF approach: localization based on the updated map

Experiments

Each simulated robot is equipped with2D Laser range scannerTactile sensorCamera for distinguishing colors

Experiments

Mean Squared Error (MSE) is measured between the i’s particles and its actual pose at different time steps with different number of particles for i (Ni) and j (Nj) over 3 runs

Lower MSE indicates better performanceRobots move asynchronously and continuously (Real time simulation)As number of i’s particle increases, the averaged MSE for i’s localization decreases at each time step

Ni=250,Nj=50 MSE=3.0

Ni=500,Nj=50 MSE = 2.43

Ni=1,000,Nj=50 MSE = 2.15

As number of j’s particle increases, the averaged MSE decreases when landmark has been pushed and i has observed itGreater number of j’s particle help in better tracking j and estimating the new location of pushed landmark

Experiments

Performance after j has pushed the landmark is of interestOriginal Map performance compares to Nested PF when Nj=20

20 particle for j is not sufficient to track jBy increasing Nj to 50, NestedPF outperforms Original map

Experiments

Experiments: Conclusion

Nested PF consistently outperforms the other approaches once the landmark has been pushed by jPerforms the best when Ni=1,000 and Nj=50

Takes 3s to perform propagation and 3s to perform Weighting when Ni=500 and Nj=50

When increased Ni=1,000 and Nj=50, it takes 10s to propagateAlthough Nested PF is computationally extensive, it performs better in tracking another robot whose actions could impact localization of the original robot

Conclusion and Future Work

Recursive localization approach based on particle filtering particles for the other robot are nested within the particles of the subject robot

Allow tracking of the other robot explicitly in comparison to implicit approaches that marginalize the other robots as noise in the environmentMaintaining more information about the other robot leads to better localizationTotal number of particles increases exponentially

Requires more computational resources

Future work: augment the localization with simultaneous mapping, which would be useful when the position of the landmarks is not known a priori, and assuming the environment is dynamicSince the number of particles increase exponentially heuristics methods that seek to intelligently allocate particles is of concern

Thank you

Questions?

Nested PF: Propagation

Propagation (xit-1 , ai

t-1) returns xit

1: for xit-1 (n) xi

t-1 do

2: Propagate the pose (x,y,θ)it-1 , in xi

t-1,(n) using

propagation equation to obtain (x,y,θ)it

3: if xit-1,(n) contains nested particle set xj

t-1,(n) then

4: predict action ajt-1 using model mj

5: xjt Propagate (xj

t-1 , ajt-1 )

6: xit < (x,y,θ)i

t, xjt >

7: end if8: xi

t + xit,(n)

9: end for10:return xi

t

Nested PF: Weight

Weight(xit , oi

t=<oikt,oij

t>) returns xit , w

1: for xit,(n) xi

t do

2: calculate r,Φ using observation equation3: weight xi

t,(n) : wi(n) Ν (rk

t –r,σr )*Ν (Φkt –Φ,σΦ )*

Ν (ckt –c,σc )

4: if xit,(n) contians a nested particle set xj

t,(n) then

5: xjt , wj Weight (xj

t , oijt )

6: xit,(n) < (x,y,θ)i

t, xjt , wj >

7: end if8: xi

t + xit,(n)

9: end for10:return xi

t