Individual Localization and Tracking in Multi-Robot Settings with Dynamic Landmarks

1. Individual Localization and Tracking in Multi-RobotSettings with Dynamic Landmarks AnoushaMesbah PrashantDoshi University of Georgia

2. Objective Focus is on the subject robot’s localization at its own level in the presence of others who may not be cooperative Localization in the presence of dynamic landmarks whose signature is known Tracking the uncertain location of the other robot(s)

3. RelatedWork • 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

4. Background: Particle Filtering οtκ t-1 t at-1 Propagate Weight Resample P(xt|at-1,xt-1) xt-1,(n) = <x,y, θ> A technique for implementing recursive Bayesian filter using Monte Carlo sampling Tracks the variable of interest over time Represents the posterior density by a set of random particles with associated weights

5. 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:

6. Particle Filtering: Observation • Let vector Okt =(rkt,Φkt,ckt) be features of landmark k • rkt: average range of detected landmark k • Φkt:difference in orientation of the particle and bearing of the observed landmark • Ckt: 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 landmark • Ck: true color of the landmark • Weighting will be done by calculating P(otk|fk,xt,(n),m)= N(rtk-rk,σr)* N(φtk-φk,σφ)*N(ctk-ck,σc)

7. Multi-Robot Laser Tag • Modified version of Rosencrantz Laser Tag domain • Assumptions • i and j know exact locations of landmarks • Robots are not aware of their own and each other’s location • i tags j and proceeds to j’s base • j uses mixed strategy behavior until it is tagged • After tagging, j moves the nearest landmark to confound and slow i’s progress to the base

8. Our Approach: Nested Particle Filtering t-1 • i will predict j’s action in order to track it • For each of i’s hypothesized pose, i maintains set of hypothesized poses for j (nesting) • j’s location is represented by set of nested particles • On predicting j’s action i may update the possible location of moved landmark xit-1,(n) = <(x,y, θ)i, > xjt-1,(n) = <(x,y, θ) j>

9. Nested Particle Filtering • Robot i’s particle at time t-1 • xi t-1, (n) = < (x,y,Θ)i, xjt-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 particles • Robot i’s observation is two-fold: observing landmarks and observing robot j • Oit= < Oikt , Oijt> • Oijt indicative of j being spotted • Oikt indicative of landmark k being spotted

10. Visual Representation of Nested PF Οti=<oikt,oijt> t-1 at-1 Weight Resample Propagate oijt xit,(n) = <(x,y, θ)i, > t xit,(n) = <(x,y, θ)i, > xit,(n) = <(x,y, θ)i, >, wi xit-1,(n) = <(x,y, θ)i, > xjt-1,(n) = <(x,y, θ) j> xjt,(n) = <(x,y, θ) j> xjt,(n) = <(x,y, θ) j>,wj

11. 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 equation • If xit-1 (n) contains nested particle set • For each of j ’s particle belonging to xit-1,(n) • Predict action using the model of j • Propagate to get a new pose for j’s particle xjt (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

12. Nested PF: Weight • For each particle of ixit,(n) at time t • Calculate the range and bearing of the particle from observed landmark k • Tag the particle based on observation function • If the particle contains nested particle • For each of j ’s particle xjt (n) belonging to xit (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

13. Experiments • Hypothesis: By explicitly tracking j using particles and estimating pushed landmarks, our approach localizes better in comparison to others • Experiments 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 map • Wolf approach: Localization based on static landmarks only (disregard the pushed landmarks) • Nested PF approach: localization based on the updated map

14. Experiments • Each simulated robot is equipped with • 2D Laser range scanner • Tactile sensor • Camera for distinguishing colors

15. Experiments • Mean Squared Error (MSE) is measured between the i’sparticles 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 performance • Robots 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 it • Greater number of j’s particle help in better tracking j and estimating the new location of pushed landmark

16. Experiments • Performance after j has pushed the landmark is of interest • Original Map performance compares to Nested PF when Nj=20 • 20 particle for j is not sufficient to track j • By increasing Nj to 50, NestedPF outperforms Original map

17. Experiments

18. Experiments: Conclusion Nested PF consistently outperforms the other approaches once the landmark has been pushed by j Performs 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 propagate Although Nested PF is computationally extensive, it performs better in tracking another robot whose actions could impact localization of the original robot

19. 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 environment • Maintaining more information about the other robot leads to better localization • Total 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 dynamic • Since the number of particles increase exponentially heuristics methods that seek to intelligently allocate particles is of concern

20. Thank you Questions?

21. Nested PF: Propagation Propagation (xit-1 , ait-1) returnsxit 1: for xit-1 (n) xit-1 do 2: Propagate the pose (x,y,θ)it-1 ,in xit-1,(n) using propagation equation to obtain (x,y,θ)it 3: if xit-1,(n) contains nested particle set xjt-1,(n) then 4: predict action ajt-1 using model mj 5: xjt Propagate (xjt-1 , ajt-1 ) 6: xit<(x,y,θ)it,xjt > 7: end if 8: xit+xit,(n) 9: end for 10:returnxit

22. Nested PF: Weight Weight(xit , oit=<oikt,oijt>) returnsxit , w 1: forxit,(n)xitdo 2: calculate r,Φ using observation equation 3: weight xit,(n) : wi(n)Ν (rkt –r,σr )*Ν (Φkt –Φ,σΦ )* Ν (ckt –c,σc ) 4: ifxit,(n)contians a nested particle set xjt,(n)then 5: xjt ,wj Weight (xjt , oijt ) 6: xit,(n)<(x,y,θ)it,xjt ,wj> 7: end if 8: xit+xit,(n) 9: end for 10:returnxit