Particle Filter for Robot Localization ECE 478 Homework #1

Transcription

1 Particle Filter for Robot Localization ECE 478 Homework #1 Phil Lamb November 15,

2 Contents 1 Introduction 3 2 Implementation Assumptions and Simplifications Simulation Variables Possible Improvements Results PC = PC = Noise = A Python Source Code 7 List of Figures 1 Simulation 1(P C = 2000, σ M = 0.5, S N = 5) Simulation 1(P C = 2000, σ M = 0.5, S N = 5) Simulation 1(P C = 500, σ M = 0.5, S N = 250)

3 1 Introduction This report summarizes the implementation of and results from a simulation of robot localization in 2D space using a particle filter. 1.1 Particle Filters In general a particle filter is a statistical model used to track the evolution of a variable with possibly non-gausian distribution [?]. The particle filter maintains many copies (particles) of the variable, each particle having a weight indicating the quality of that particle [?]. In robotics the particle filter is often used to localize a robot, or estimate its location given inputs from sensors and a model of the robots motion. The particle filter has been successfully applied to solve both general localization tasks as well as the more difficult kidnapped robot problem where a robot is moved to an entirely new location and is able to re-establish its location [?]. The particle filter is continuously iterated to improve the localization estimate and update localization after the robot moves. This happens in three steps: Prediction, Update, and Resample[?]. More details on the implementation of these steps are given in section??. In the prediction step each particle is modified based on the robot s motion model. The motion model takes into account movement and odometry errors (e.g. if commanded to move forward 5m a robot may not travel in a perfectly straight line and may not travel exactly 5m). This prediction step is described by equation?? where p is the previous position of the particle, p + is the new position of the particle, function M represents the motion model, and x is the commanded motion of the robot. p + = p + M( x) (1) In the update step the weight of each particle is modified based on the likelyhood that that particle represents the correct position value from the robot s sensor readings. The particle weighting assumes a gaussian distribution (gaussian error on sensor measurements). After all particles have been updated the weights are normalized so that the sum of all particles weight is one. The final step is to resample the particles. In this step all previous particles are discarded and new particles are chosen from a weighted distribution of the previous particles. Particles from the previous iteration with a higher weight are more likely to be chosen for the succeeding iteration. This discards low-probability particles and preserves high-probability cycles for the next iteration of the filter. If the particles are not resampled the population becomes depelted with low-probability particles not contributing to the solution [?]. Resampling does not necessarially need to be done on every iteration. 3

4 2 Implementation The particle filter simulation was implemented in Python. The program simulates a simple robot that can move and localize itself in 2D space. Source code for the simulation can be found in appendix??. 2.1 Assumptions and Simplifications A number of simplifications and assumptions about the robot were made in order to simplify the simulation. Even with these simplifications the simulation demonstrates the application of particle filters to robot localization. This simulation assumes that the robot is in a square room of variable size and can measure its location from each wall (determine its (X, Y ) position). For the purposes of this simulation all error in the motion model is assumed to be gaussian where µ = 0 and σ = σ M (see section??). During the update step of the particle filter the weight of the particle is determined by its difference from the measured position of the robot. This measurement is the actual value of the robot with added error. The measurement error (e) is assumed to be uniform where N S e N S (see section??). The particle resampling step is accomplished by associating elements of two lists. The first list contains particles, the second contains a cumulative sum of the particles weights which sum to one (the cdf of the particle distribution). A random value is chosen from a uniform distribution between zero and one and the index in the weight list is chosen by locating the index in the array where the randomly chosen value would be inserted. This is accomplished using Python s array bisection (binary search) algorithm (bisect) [?]. The particle in the matching index in the particle array is chosen for the next iteration. In this simulation resampling happens after every iteration. 2.2 Simulation Variables The simulation may be modified using a number of variables: WORLDSIZE - This defines the size of the robot s world or room. This value is the maximum X and Y value of the room. ROBOTSPEED (RS) - This value gives the maximum forward speed of the robot (the maximum speed is actually ROBOTSPEED 2 due to implementation of the motion simulation). PARTICLECOUNT (P C) - This specifies the number of particles to be used. WEIGHTSIGMADIVISOR (σ w ) - This is used to determine the width (σ) of the normal distribution used to determine particle weight during the prediction phase. The weight is determined by: d 2 w = e 2 σw 2 where w is the particle weight and d is the difference between the particle and the measured location of the robot. 4

5 MOTIONMODELSIGMA (σ M ) - This value is used to determine the width of the normal distribution used in the motion model. The σ of the normal distribution is equal to σ M RS. MEASNOISE (N S ) - Maximum magnitude of noise added to a simulated sensor measurement. 2.3 Possible Improvements The simulation could be improved in the future by using non-gaussian distributions for the motion model and measurement noise. This may better highlight the strengths of the particle filter over similar Kalman filter approaches. The simulation may also be improved by creating a more complex environment for the robot to navigate. This would be a more realistic example of particle filter usage. 3 Results All simulations are run in a 500x500 pixel world with the robot starting at the center. The position of the initial particles (x, y) are chosen randomly from a uniform distribution where 0 x 500 and 0 y 500. The robot s motion is generated by adjusting the heading of the robot by a randomly chosen value h from a uniform distribution where π 8 h < π 8. The robot is then moved distance x in the direction of its heading where 0 x < 2 ROBOTSPEED. The robot s position is updated on every iteration of the simulation. The following sections contain results and analysis of three simulations. 3.1 Simulation 1 (Baseline) The first simulation is used as a baseline for comparison of other simulations. In this simulation 2000 particles are generated. The measurement error is randomly selected from a uniform distribution [ 5, 5] (N S = 5). The motion model noise is randomly selected from a gaussian distribution where σ = 0.5 ROBOTSPEED (σ M = 0.5). As seen in figure?? the localization estimate quickly reaches a reasonably accurate location fix. The minimum size of the particle cloud is limited by the error in the motion model. 3.2 Simulation 2 (Few Particles) The second simulation uses the same sensor and motion model errors as the first simulation (N S = 5, σ M = 0.5). However this simulation uses a much smaller number of particles (P C = 50). The results can be seen in figure??. 5

6 Figure 1: Simulation 1 (P C = 2000, σ M = 0.5, S N = 5) 6

7 Figure 2: Simulation 2 (P C = 50, σ M = 0.5, S N = 5) 7

8 Figure 3: Simulation 3 (P C = 500, σ M = 0.5, S N = 250) 3.3 Simulation 3 (High Sensor Noise) The third simulation shows an interesting property of the particle filter: even with very high sensor noise the robot can eventually be localized. For this simulation the motion model noise remains the same as the previous simulations (σ M = 0.5) however the sensor noise is increased drastically to S N = 250. This is half the size of the robot s world! As can be seen in figure?? the localization takes much longer than in previous simulations but eventually reaches a result similar to that of the previous simulations. References [1] I. M. Rekleitis, A particle filter tutorial for mobile robot localization, tech. rep., Centre for Intelligent Machines, McGill University, [2] S. Thrun, Particle filters in robotics, in Uncertainty in AI (UAI), [3] H. d.-s. Kaijen Hsiao and J. Miller, Particle filters and their applications, [4] M. J. Laubachf, particle filter demo. 8

9 A Simulator Source Code [linenos=true,stepnumber=10]python../python/particlefilter.py 9