Solved 16-833 Homework 1 Robot Localization using Particle Filters

Overview The goal of this homework is to become familiar with robot localization using particle filters, also known as Monte Carlo Localization. In particular, you will be implementing a global localization filter for a lost indoor mobile robot (global meaning that you do not know the initial pose of the robot). Your lost robot is operating in Wean Hall with nothing but odometry and a laser rangefinder. Fortunately, you have a map of Wean Hall and a deep understanding of particle filtering to help it localize. As you saw in class, particle filters are non-parametric variants of the recursive Bayes filter with a resampling step. The Prediction Step of the Bayes filter involves sampling particles from a proposal distribution, while the Correction Step computes importance weights for each particle as the ratio of target and proposal distributions. The Resampling Step resamples particles with probabilities proportional to their importance weights. When applying particle filters for robot localization, each particle represents a robot pose hypothesis which for a 2D localization case includes the (x, y) position and orientation θ of the robot. The Prediction and Correction Steps are derived from robot motion and sensor models respectively. This can be summarized as an iterative process involving three major steps: 1. Prediction Step: Updating particle poses by sampling particles from the motion model, that is x [m] t ∼ p(xt |ut , x [m] t−1 ). The proposal distribution here is the motion model, p(xt |ut , xt−1). 1 2. Correction Step: Computing an importance weight w [m] t for each particle as the ratio of target and proposal distributions. This reduces to computing weights using the sensor model, that is w [m] t = p(zt |x [m] t ,M). 3. Resampling Step: Resampling particles for the next time step with probabilities proportial to their importance weights. Here, m is the particle index, t is the current time step, and M is the occupancy map. x [m] t is the robot pose of particle m at time t, and w [m] t is the importance weight for particle m at time t. 2 Monte Carlo Localization Monte Carlo Localization (MCL), a popular localization algorithm, is essentially the application of particle filtering for mobile robot localization. You can refer to Section 4.3 of [1] for details on the MCL algorithm. Algorithm 1, taken from [1], describes the particle filter algorithm applied for robot localization. Algorithm 1 Particle Filter for Robot Localization 1: X¯ t = Xt = φ 2: for m = 1 to M do 3: sample x [m] t ∼ p(xt | ut , x [m] t−1 ) . Motion model 4: w [m] t = p(zt | x [m] t ,M) . Sensor model 5: X¯ t = X¯ t + D x [m] t , w [m] t E 6: end for 7: for m = 1 to M do 8: draw i with probability ∝ w [i] t . Resampling 9: add x [i] t to Xt 10: end for 11: return Xt As you can see, the MCL algorithm requires knowledge of the robot motion and sensor models, and also of the resampling process to be used. You will need to implement these three components of the algorithm. We briefly describe these three components and point you to resources with more details and pseudo-codes. Motion Model The motion model p(xt |ut , xt−1) is needed as part of the prediction step for updating particle poses from the previous time step using odometry readings. Chapter 5 of [1] details two types of motion models, the Odometry Motion Model and the Velocity Motion Model. You can use either model for sampling particles according to x [m] t ∼ p(xt |ut , x [m] t−1 ). The Odometry Motion Model might be more straightforward to implement since it uses odometry measurements directly as a basis for computing posteriors over the robot poses. Sensor Model The sensor model p(zt |xt , m) is needed as part of the correction step for computing importance weights (proportional to observation likelihood) for each particle. Since 2 the robot is equipped with a laser range finder sensor, we’ll be using a beam measurement model of range finders. Section 6.3 of [1] details this beam measurement model p(zt |xt , m) as a mixture of four probability densities, each modeling a different type of measurement error. You’ll have to play around with parameters for these densities based on the sensor data logs that you have. You are also free to go beyond a mixture of these four probability densities and use a measurement model that you think describes the observed laser scans better. Additionally, as part of this beam measurement model, you’ll be performing ray-casting on the occupancy map so as to compute true range readings z k∗ t from individual particle positions (shifted to laser position). Hint: The book specifies that the sensor model needs to be a normalized probability distribution, however, we have found in practice it is easier to debug when the mixture weights (and thus the distribution) are unnormalized as the particle weights are later normalized. Resampling As part of the resampling process, particles for the next time step are drawn based on their weights in the current time step. A straightforward resampling procedure would be sampling from a multinomial distribution constructed using importance weights of all particles. However, repetitive resampling using such a technique may cause the variance of the particle set (as an estimator of the true belief) to increase. One strategy for reducing the variance in particle filtering is using a resampling process known as low variance sampling. Another strategy is to reduce the frequency at which resampling takes place. Refer to the Resampling subsection under Section 4.3.4 of [1] for more details on variance reduction and using low variance resampling for particle filters. 3 Implementation Resources You may use any programming language for implementation. There is no requirement that your implementation run in real-time, although it is advisable to use something faster. Feel free to utilize the techniques that we have discussed in class as well as extensions discussed in [1] or elsewhere. You will be performing global localization for a lost indoor mobile robot in Wean Hall given a map, odometry readings and laser scans. The data directory that you received with this handout (courtesy of Mike Montemerlo) has the following files: ˆ data/instruct.txt – Format description for the map and the data logs. ˆ data/log/robotdataN.log – Five data logs (odometry and laser data). ˆ data/map/wean.dat – Map of Wean Hall to use for localization. ˆ data/map/bee-map.c – Example map reader in C from BeeSoft that you may use. Note we also provide a Python map reader. ˆ assets/wean.gif – Image of map (for reference). ˆ assets/robotmovie1.gif – Animation of data log 1 (for reference). 3 We have also provided you with helper code (in Python) that reads in the occupancy map, parses robot sensor logs and implements the outer loop of the particle filter algorithm illustrated in Algorithm 1. The motion model, sensor model, and resampling implementations are left as an exercise for you. ˆ main.py – Parses sensor logs (robotdata1.log) and implements outer loop of the particle filter algorithm shown in Algorithm 1. Relies on SensorModel, MotionModel and Resampling classes for returning appropriate values. ˆ map reader.py – Reads in the Wean Hall map (wean.dat) and computes and displays corresponding occupancy grid map. ˆ motion model.py, sensor model.py, resampling.py - Provides class interfaces for expected input/output arguments. Implementation of corresponding algorithms are left as an exercise for you. You are free to use the helper code directly or purely for reference purposes. To utilize the framework, please start with a Python 3 enviroment. We recommend creating a virtual enviroment using e.g. conda, and pip install -r requirements.txt (located in the code directory) for the basic dependencies. A short tutorial for creating a virtual enviroment can be found at here. Improving Efficiency Although there is no requirement that your code run in real-time, the faster your code, the more particles you will be able to use feasibly and the faster your parameter tuning iterations will go. You’ll most probably have to apply some implementation ‘hacks’ to improve performance, for instance, ˆ Intializing particles in completely unoccupied areas instead of uniformly everywhere on the map. ˆ Subsampling the laser scans to say, every 5 degrees, instead of considering all 180 range measurements. ˆ When computing importance weights based on the sensor model, be cognizant of numerical stability issues that may arise when multiplying together likelihood probabilities of all range measurements within a scan. You might want to numerically scale the weights or replace the multiplication of likelihood probabilities with a summation of log likelihoods. ˆ Since motion model and sensor model computations are independent for all particles, parallelizing your code would make it much faster. ˆ You’ll observe that operations like ray-casting are one of the most computationally expensive operations. Think of approaches to make this faster, for instance using coarser discrete sampling along the ray or possibly even precomputing a look-up table for the raycasts. ˆ Lastly, if you use Python, apply vectorization as much as possible; if you’re comfortable with C++, consider using the OpenMP backend (which is a oneliner) to accelerate. 4 Debugging For easier debugging, ensure that you visualize and test individual modules like the motion model, sensor model or the resampling separately. Some ideas for doing that are: ˆ Test your motion model separately by using a single particle and plotting its trajectory on the occupancy map. The odometry will cause the particle position to drift over time globally, but locally the motion should still make sense when comparing with given animation of datalog 1 (robotmovie1.gif). ˆ Cross-check your sensor model mixture probability distribution by plotting the p(zt |z ∗ t ) graph for some set of values of z ∗ t . ˆ Test your ray-casting algorithm outputs by drawing robot position, laser scan ranges and the ray casting outputs on the occupancy map for multiple time steps. 4 What to turn in You should generate a visualization (video) of your robot localizing on robotdata1.log and another log of your choice. Don’t worry—your implementation may not work all the time—but should perform most of the time for a reasonable number of particles. Hyperlinks to the videos must be in the report—we prefer unlisted Youtube videos or Google Drive links. Please ensure proper viewing permissions are enabled before sharing the links. Please speed-up videos to ensure each log is under 2 minutes, and mention the speed multiplier in the video or report. The report must describe your approach, implementation, description of performance, robustness, repeatability, and results. Make sure you describe your motion and sensor models (including your ray casting process), your resampling procedure, as well as the parameters you had to tune (and their values). Include some future work/improvement ideas in your report as well. Turn in your report and code on Gradescope by the due date. Do not upload the data/ folder or any other data. Only one group member needs to submit, and should list all group members on the title page as well as via Gradescope (see instructions here). Score breakdown ˆ (10 points) Motion Model: implementation correctness, description ˆ (20 points) Sensor Model: implementation correctness, description ˆ (10 points) Resampling Process: implementation correctness, description ˆ (10 points) Discussion of parameter tuning ˆ (30 points) Performance of your implementation ˆ (10 points) Discussion of performance and future work ˆ (10 points) Write-up quality, video quality, readability ˆ (Extra Credit: 10 points) Kidnapped robot problem 5 ˆ (Extra Credit: 10 points) Adaptive number of particles ˆ (Extra Credit: 5 points) Vectorized Python or fast C++ implementation 5 Extra credit Focus on getting your particle filter to work well before attacking the extra credit. Points will be given for an implementation of the kidnapped robot problem and adaptive number of particles. Please answer the corresponding questions below in your write up. i. Kidnapped robot problem: The kidnapped robot problem commonly refers to a situation where an autonomous robot in operation is carried to an arbitrary location. You can simulate such a situation by either fusing two of the log files or removing a chunk of readings from one log. How would your localization algorithm deal with the uncertainity created in a kidnapped robot problem scenario? Can you make improvements to your algorithm to better address this problem? ii. Adaptive number of particles: Can you think of a method that is more efficient to run, based on reducing the number of particles over timesteps? Describe the metric you use for choosing the number of particles at any time step. You will also receive bonus credits provided your implementation is optimized, either with vectorization in Python or acceleration in C++. 6 6 Advice The performance of your algorithm is dependent on (i) parameter tuning and (ii) number of particles. While increasing the number of particles gives you better performance, it also leads to increased computational time. An ideal implementation has a reasonable number of particles, while also not being terribly slow. Consider these factors while deciding your language of choice—e.g. choosing between a faster implementation in C++ or vectorized optimization in Python vs. using the raw Python skeleton code. References [1] Sebastian Thrun, Wolfram Burgard, and Dieter Fox. Probabilistic robotics. MIT press, 2005. 7