Individual Localization and Tracking in Multi-Robot Settings with Dynamic Landmarks Anousha Mesbah Prashant Doshi 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 cooperative Localization in the presence of dynamic landmarks whose signature is known Tracking the uncertain location of the other robot(s)

RelatedWork 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 sampling Tracks the variable of interest over time Represents the posterior density by a set of random particles with associated weights a t-1 t-1 x t-1,(n) = οtκοtκ t PropagateWeightResample P(x t |a t-1,x t-1 )

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

Particle Filtering: Observation Let vector O k t =(r k t,Φ k t,c k t ) be features of landmark k r k t : average range of detected landmark k Φ k t : difference in orientation of the particle and bearing of the observed landmark C k t : observed color of the landmark Let O k =(r k,Φ k,c k ) be the true features of the landmark k r k : true range based on exact location of the landmark Φ k : difference in orientation of the particle and bearing of the true location of landmark C k : true color of the landmark Weighting will be done by calculating P(o t k |f k,x t,(n),m)= N(r t k -r k,σ r )* N(φ t k -φ k,σ φ )*N(c t k -c k,σ c )

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

Our Approach: Nested Particle Filtering 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 t-1 x i t-1,(n) = x j t-1,(n) =

Nested Particle Filtering Robot i’s particle at time t-1 x i t-1, (n) = where x j t-1,(n) is set of j’s particles at time t-1 x j t-1,(n) = representing j’s pose M j = Δ(A j ) 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 O i t = O ij t indicative of j being spotted O ik t indicative of landmark k being spotted

Visual Representation of Nested PF t Propagate Weight Resample a t-1 t-1 x i t-1,(n) = x j t-1,(n) = x i t,(n) = Ο t i = x i t,(n) =, w i o ij t x j t,(n) =,w j x i t,(n) = x j t,(n) =

Nested PF: Propagation For each particle at time t-1 x i t-1 (n) Propagate the new pose for i based on action performed using the propagation equation If x i t-1 (n) contains nested particle set For each of j ’s particle belonging to x i t-1,(n) Predict action using the model of j Propagate to get a new pose for j’s particle x j t (n) and add it to the list of j’s particle for x i t (n) Add x i t,(n) to the list of particles at time t

Nested PF: Weight For each particle of i x i t,(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 x j t (n) belonging to x i t (n) Weight the particle based on +ve /-ve observation of j Add particle x j t (n) and its weight to the list of j’s particle for x i t (n) Add x i t (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 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

Experiments Each simulated robot is equipped with 2D Laser range scanner Tactile sensor Camera 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 (N i ) and j (N j ) 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 N i =250,N j =50 MSE=3.0 N i =500,N j =50 MSE = 2.43 N i =1,000,N j =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

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

Experiments

Experiments: Conclusion Nested PF consistently outperforms the other approaches once the landmark has been pushed by j Performs the best when N i =1,000 and N j =50 Takes 3s to perform propagation and 3s to perform Weighting when N i =500 and N j =50 When increased N i =1,000 and N j =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

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

Thank you

Nested PF: Propagation Propagation (x i t-1, a i t-1 ) returns x i t 1 : for x i t-1 (n) x i t-1 do 2: Propagate the pose (x,y,θ) i t-1, in x i t-1,(n) using propagation equation to obtain (x,y,θ) i t 3: if x i t-1,(n) contains nested particle set x j t-1,(n) then 4:predict action a j t-1 using model m j 5:x j t  Propagate (x j t-1, a j t-1 ) 6:x i t  7: end if 8: x i t  + x i t,(n) 9: end for 10:return x i t

Nested PF: Weight Weight(x i t, o i t = ) returns x i t, w 1 : for x i t,(n) x i t do 2: calculate r,Φ using observation equation 3: weight x i t,(n) : w i (n)  Ν (r k t –r,σ r )*Ν (Φ k t –Φ,σ Φ )* Ν (c k t –c,σ c ) 4: if x i t,(n) contians a nested particle set x j t,(n) then 5:x j t, w j  Weight (x j t, o ij t ) 6:x i t,(n)  7: end if 8: x i t  + x i t,(n) 9: end for 10:return x i t