Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16) University of Rome “La Sapienza” Master in Artificial Intelligence and Robotics Learning in Autonomous Systems Proff. Luca Iocchi, Giorgio Grisetti Course web site: http://www.dis.uniroma1.it/ ∼ iocchi/Teaching/las/ Giorgio Grisetti Particle Filters For Localization 1 / 20
Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16) Overview Definition of the problem Characterizationof the state space and the control space Characterization of the control noise Sampled Transition Model Likelihood Efficient computation Giorgio Grisetti Particle Filters For Localization 2 / 20
Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16) Problem Definition we want to design a particle filter for 2D robot localization out robot will be moving on a plane and have an orientation we know the map of the environment as a 2D grid, where each cell of the grid can be occupied, free or unknown we will use a laser scanner as a sensor, that reports the distance measured at fixed angular intervals Giorgio Grisetti Particle Filters For Localization 3 / 20
Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16) Problem Definition: state The state of our system x t ∈ SE 2 is a point with orientation It can be represented as a triplet x t x t = (1) y t θ t or in matrix form cos θ t − sin θ t x t X t = sin θ t cos θ t y t c (2) 0 0 1 It is clearly possible to pass from one representation to the other. To this extent we introduce the t 2 v and v 2 t functions. X t = v 2 t ( x t ) (3) x t = t 2 v ( X t ) (4) Giorgio Grisetti Particle Filters For Localization 4 / 20
Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16) Problem definition: measurements The laser returns a set of distances measured at fixed angle intervals: z t = { z j } , thus our laser measurement will consist of a set of range measurements. To simplify the further calculations we will consider the endpoints of each beam (wich is a 2D point). � cos α j � z cart = z j (5) j sin α j where α j is known from the laser specs. Giorgio Grisetti Particle Filters For Localization 5 / 20
Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16) Problem definition: controls and transitions Controls of our system come as offsets measured through the odometry. Thus u t ∈ SE 2 lives in the same space as the state x t . If my robot sits in a certain position x t and receives the control u t he will move to the next position x t +1 . This can be expressed using the matrix form as X t +1 = X t U t (6) or in vector form as x t +1 = ( x t , u t ) (7) = t 2 v ( v 2 t ( x t ) v 2 t ( u t )) (8) This is our noise-free transition function that predicts the next state from the previous one Giorgio Grisetti Particle Filters For Localization 6 / 20
Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16) Prediction Realizing the predict step in a particle filter requires to define a sampler for the transition model x ( i ) t +1 ∼ p ( x t +1 | x ( i ) t , u t ) (9) To this extent we might consider the transitions perfect, and only the controls affected by noise x ( i ) t +1 ∼ p ( x t +1 | x ( i ) t , u t + n t ) (10) the idea is then to draw a sample n ( i ) ∼ p ( n t ) and compute the next t sample according to 8 as x ( i ) t +1 = t 2 v ( v 2 t ( x ( i ) t ) v 2 t ( u t + n ( i ) t )) (11) Giorgio Grisetti Particle Filters For Localization 7 / 20
Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16) Characterizing the noise the odometry noise grows proportionally with the distance traveled a reasonable implementation would be to draw noise from a Gaussian distribution whose covariance grows with the traveled distance let u t ∼ N ( 0 | Σ t ) (12) � � σ 2 x , σ 2 y , σ 2 where Σ t = diag θ higher covariances will correspond to higher noise, thus more “spread” samples Our goal is to find a heuristic that correlates the control u t with the covariance coefficients σ 2 x , σ 2 y , σ 2 θ . If the robot does not move, the covariance is zero. Giorgio Grisetti Particle Filters For Localization 8 / 20
Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16) Characterizing the noise A simplistic implementation σ 2 a 11 a 12 a 13 || u x || x = σ 2 || u y || a 21 a 22 a 23 (13) y σ 2 || u θ || a 31 a 32 a 33 θ � �� � A Here the covariances are obtained as a linear combination of the magnitude of the motion of the system, through the matrix A . once the covariances are generated (see method prepareSampling ), we can generate samples and perturbate our particle vector(see method sample ) by applying Eq. 11. Giorgio Grisetti Particle Filters For Localization 9 / 20
Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16) Characterizing the control noise the odometry noise grows proportionally with the distance traveled a reasonable implementation would be to draw noise from a Gaussian distribution whose covariance deviation grows with the traveled distance let u t ∼ N ( 0 | Σ t ) (14) � � σ 2 x , σ 2 y , σ 2 where Σ t = diag θ higher covariances will correspond to higher noise, thus more “spread” samples Our goal is to find a heuristic that correlates the control u t with the covariance coefficients σ 2 x , σ 2 y , σ 2 θ . If the robot does not move, the covariance is zero. Giorgio Grisetti Particle Filters For Localization 10 / 20
Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16) The predict step becomes compute σ 2 x , σ 2 y , σ 2 θ using eq 13 (see function prepareSampling σ 2 || u x || x σ 2 = A || u y || y σ 2 || u θ || θ for each particle, compute its successor sample u ( i ) ∼ N ( 0 | Σ t ) t compute x ( i ) t +1 ∼ p ( x t +1 | x ( i ) t , u t + n ( i ) t ) Giorgio Grisetti Particle Filters For Localization 11 / 20
Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16) Update define a prediction function ˆ z = h ( x ) (15) that computes the predicted laser scan ˆ z if the robot was in position x . The prediction could be obtained for instance through ray-tracing. consider a noise distribution for the indivdual range scan (e.g a gaussian centered around the prediction). regarding the individual beams as independant � p ( z | x ) = p ( z j | x ) (16) i � ( z j − ˆ � z j ) 2 � ∝ exp − (17) σ 2 i j z j ) 2 ( z j − ˆ � = exp − (18) σ 2 j j Giorgio Grisetti Particle Filters For Localization 12 / 20
Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16) Update: Issues computing the prediction for one beam requires ray-casting (expensive) this operation has to be repeated for each beam and each particle the weights obtained by the Eq 18 are highly selective Beware of the sigma : they are much larger than what you think. Giorgio Grisetti Particle Filters For Localization 13 / 20
Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16) Distance Map we can easily calculate a lookup grid that stores in each cell u , v the distance from the closest occupied cell u ′ , v ′ in the grid map. u ′ , v ′ || ( u , v ) − ( u ′ , v ′ ) || d u , v = min (19) this data structure is called distance-map . the calculation of the distance-map can be done by executing a breadth first visit of the grid where the occupied cells are marked. the idea is to expand the search frontier along regions that have the same “distance”. see the file distancemap.h determining the distance between a point and the closest obstacle requires just a lookup in the distance map . Giorgio Grisetti Particle Filters For Localization 14 / 20
Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16) Update: Distance Map Based Given a candidate robot pose x we can easily calculate the predicted endopoints of the laser scan z in the world frame z map = X z cart (20) j considering a single endpoint in the map frame, we can compute the distance between this point and the closest occupied point in the map k ( || z map d j = min − p k || ) (21) j here { p k } are all occupied points in the map if the distance is small the remapped endpoint is close to an occupied point, and its distance is small. Otherwise the distance is big. Giorgio Grisetti Particle Filters For Localization 15 / 20
Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16) Update: Distance Map Based exploiting the grid discretization we can compute d j through a simple lookup as � u � world2grid ( X z cart = ) (22) j v ≃ d j d u , v (23) This is an O (1) operation Giorgio Grisetti Particle Filters For Localization 16 / 20
Recommend
More recommend
