LMICSE: Lego Mindstorms in Computer Science Education

The Problem

Imagine a long hallway with a set of open doors along one side. The doors are distributed unevenly along it, and the doors are not all of the same width. Your robot can move back and forth along the hallway, and at anytime it is either in front of one of the doors or is along a wall segment. The situation might look like this:

The robot's task is move to a predefined goal point along the hallway. But it doesn't know its starting point. It does have a sonar unit that is aimed at the wall/doors. The unit is reasonably reliable, and can be used to determine whether the robot is currently in front of a wall or a door. To solve this problem, the robot repeated moves forward a fixed distance and takes a sonar reading. If it thinks it has reached the end of the hallway, it starts backing up instead of moving forward. It may need to move back and forth along the hallway a few times before it accurately knows where it is, but usually it will be able to reliably determine its location in one pass back and forth.

Not only does the robot not know its starting position, but there are two additional problems that need to be taken into consideration:

• The sonar reading may be wrong (e.g., the reading indicates an open door when in front of a wall segment).
• The distance the robot attempts to move may not the actual distance it moved (e.g. when it attempts to move 2 inches forward it really just moves 1.9 inches).

So you will need to develop probabilistic models for both of these sources of error.

Materials

To complete this project you will need a robot that

• can reliably move back and forth along a linear path.
• can reliably move a certain distance (within a tolerance).
• has a sonar unit aimed perpendicularly to the axis of movement (side facing sonar).
• can receive command from a base computer (move forward or backward, return sonar reading).

Here are two pictures of the robot we used to solve this problem. It uses rotation sensors for odometry, and a worm-gear drive you you can't see. The design of your robot is up to you.

Here's a picture of our overall setup:

The Basic Approach of MCL

The Monte Carlo approach to localization is based on a collection of samples (which are also known as particles). Each sample consists of a possible location the robot may currently occupy, along with a value which represents the probability that the robot is currently at that location. Strictly speaking these are not probabilities, so they are often referred to as importance weights, and we will use that terminology here.

How many samples should be used? The more you have, the more quickly the program will converge to a correct solution, but at the cost of more computational time and hence slower robot movement.

When the program begins the robot does not know where it is, so the current samples (really the locations the samples contain) are evenly distributed over the range of possible locations, and the importance weights are all the same. Over time the samples near the actual current position should become more likely, and those further away less likely. If we created a line graph which plotted the samples using location verses importance weight, the graph should spike over the actual location. Think of this curve as representing the robot's belief state about the robot's location. In the beginning it is a flat line (since the robot has no idea where it is), but over time it should show humps over likely locations, with (hopefully) a large spike over the actual location.

The general idea is as follows:

1. Initialize the set of samples (the current samples) so that their locations are evenly distributed and their importance weights are equal.
2. Repeat until done with the current set of samples:
   1. Move the robot a fixed distance and then take a sensor reading.
   2. Update the location of each of the samples (using the movement model)..
   3. Assign the importance weights of each sample to the likelihood of that sensor reading given that new location (using the sensor model).

Remember that the actual distance moved may not be the expected distance. So model that by adding (or subtracting) a random error factor. Similarly, if the sensor reading seems to indicate a door, remember that is might be wrong, and take that into consideration. These are called, respectively, the movement and sensor models. One could stop refining the algorithm here - over time the importance weights would tend towards a solution.

However, the samples with low importance weights add little to figuring out the robot's current location. That fact leads us to the answer of the next question: "Where does the Monte Carlo part come in - doesn't that name suggest games of chance?" Yes, it does. And this is where the big improvement comes in, making the algorithm converge to a solution more quickly. In the loop above, we now add the following steps:

4. Create a new collection of samples by sampling (with replacement) from the current set of samples based on their importance weights.
5. Let this be new collection become the current set of samples.

In step four, samples with a high importance weight are more likely to be chosen than those with low probability, and some sample may be repeatedly chosen. So we (usually) end up with a set of samples with a higher cumulative importance weight that those we had before. This is similar to the approach genetic algorithms take - survival of the fittest!

It is now possible that more than one sample may be at the same location, and clearly samples will cluster around likely locations. So where is the robot likely to be? At the location (or area) with the most samples.

The Basic Monte Carlo Localization Algorithm

Here is the MCL algorithm (based on the one presented by Dieter Fox in his paper listed below) for generating at time t the next set of samples S_t+1 from the current set S_t. The x_t are the locations and the w_t the probabilities - so an (x_t, w_t) pair represents a sample. The distance traveled is u_t, and the sensor reading is z_t. We use superscripts to indicate the individual samples, so x_t⁽ⁱ⁾ is the location of sample i at time t. n is the number of samples.

1. inputs: Distance u_t, sensor reading z_t., sample set S_t  =  { (x_t⁽ⁱ⁾, w_t⁽ⁱ⁾) | i = 1,...,n}
   
2. for i  =  1 to n do            // First update the current set of samples
   1.  x_t  = updateDist(x_t, u_t)         // Compute new location
   2.  w_t⁽ⁱ⁾ = prob( z_t | x_t⁽ⁱ⁾  )           // Compute new probability
      
3. S_t+1  =  null                      // Then resample to get the next generation of samples
4. for i  =  1 to n do
   1.  Sample an index j from the distribution given by the weights in S_t
   2.  Add (x_t^(j), w_t^(j)) to S_t+1           // Add sample j to the set of new samples
      
5. return S_t+1

Program Requirements

• To begin, the robot is placed in an arbitrary position along a series of wall and door segments, with the IR tower in an appropriate position for communication. The program on the robot is started, and then that on the base computer.
• At startup, the base computer program should read in a text file containing a series of integers. These integers represent the lengths of alternating wall and door segments, beginning with a wall segment. These should correspond to the actual environment the robot is placed in, running from right to left. It should initialize its set of samples.
• The base computer program should then repeatedly cause the robot to move and sense, and employ the MCL algorithm each time. It should eventually stop the robot at the exact midpoint of the wall/door series. It is assumed that movement to the left is forward.
• Your program should employ reasonable movement and sensor models.
• Your solution should reliably move to the center of the course on multiple runs.

The Video

We have created split-screen videos that show our robot solving the localization problem using MCL. The top pane shows the robot moving linearly in front of the wall/door series, while the bottom pane shows a simple visualization from the base computer. The Java class for this visualization is provided (see the next section). A key to the visualization:

• Red rectangles: wall segments.
• Blue line: location of the robot.
• Green line: goal location.
• White histogram: indicates the number of samples in that location.

The videos:

• Video 1: A fairly unambiguous world, so the robot localizes quickly (17.8 mb).
• Video 2: The same world, different starting point. Again localizes quickly (6.4 mb).
• Video 3: A more ambiguous world, and the robot takes a while to properly localize (45.6 mb).

The Provided Code

We provide in this zip three classes on which you are to base your solution:

1. RemoteRCX: This class runs on the robot and need to be modified and completed. As provided, it simply reads values sent to it by the base computer by IR, shows that value on its LCD, and echoes it back to the base computer. You need to handle commands from the base computer to move and to return its current sonar reading.
2. MCL: This is the main class for the base computer, and needs modification and a lot of extension. Right now it causes a visualization to appear with some hard coded data, and then demonstrates communication with the robot by sending it a series of values and sending to System.out the values the RCX returns.
3. Visualize: This class does not need modification, although you can modify it if you want to jazz the visualization up. Its constructor receives the wall/door series information as an array of integers and the goal location. It has an update method that receives the estimated robot location and an array of doubles representing the locations of the current sample set and causes the visualization to update.

A well structured solution would have some additional classes, but this is not required of your solution.

Hints and Things to Think About

• The robot needs to reverse direction when it reaches an end of the course. To handle this, reverse course when the computed location of the robot gets within a certain distance of either end.
• When the robot has moved, the updated sample may have a location which is now outside of the "playing" area. Just set its new probability to 0, so it won't be sampled into the next generation.
• If you have never used sonar before, consider doing the LMICSE sonar activity to get a sense of the capabilities of your particular sonar sensor. (Sorry if the activity is not yet online. It will be soon!)
• How to get your robot to reliably move a certain distance? The low tech way is just have it move for a fixed amount of time, doing experiments to see what the distribution of actually moved distances is. Or you can used the Lejos timing navigator class. But if you have access to rotation sensors (Lego calls them angle sensors) you can use the Lejos rotation navigation class to get more accurate movement. Using this class will also help keep you robot moving in a straight line. There is a discussion of these navigator classes in the LeJOS tutorial.
• Remember that Java's random class has a nextGaussian method, which returns a random variable drawn from the standard distribution centered on 0 with standard distribution 1. This can be used in your movement and sensor models.

Acknowledgements

This lab is based on an example given by Dieter Fox in the second paper listed in the references. Lloyd Greenwald has used this problem in his robotics classes at Drexel University, and with his student Babak Shirmohammadi developed a problem statement and solution. Lloyd presented this project at one of the LMICSE project workshops, and we use one of his PowerPoint images here. He also discusses his approach the paper listed in the references.

This writeup is by Myles McNally and was inspired by Lloyd's presentation. An early version of the provided code was by Babak Shirmohammadi, but much of the current version is by Myles McNally.

References

Basic probabilistic algorithms are covered in most AI textbooks, and there are a number of robotics texts which discuss the localization problem in general. To learn more about Monte Carlo Localization in particular, we recommend the following:

• "Monte Carlo Localization: Efficient Position Estimation of Mobile Robots," by Dieter Fox, Wolfram Burgard, Frank Dellaert, and Sebastian Thrun, Proceeding of the 1999 National Conference on Artificial Intelligence (AAAI), and also online. This a good place to begin.
• "Adapting the Sample Size in Particle Filters Through KLD-Sampling," by Dieter Fox, International Journal of Robotics Research, v. 22, n. 12 (Dec, 2003), pp.985-1003, and also online. This article uses the one-dimension problem addressed in this project as an example, and also gives a good general discussion of Monte Carlo Localization in the context of other approaches.

A discussion of the basic approach behind this project can be found in the following:

• "Using educational robotics to motivate complete AI solutions", by Lloyd Greenwald, Donovan Artz, Yogi Mehta, and Babak Shirmohammadi. AI Magazine: Special issue on robots and robotics in undergraduate AI education. American Association for Artificial Intelligence Press, 27(1), 2006, pp. 83-95.

A website devoted to the use of Monte Carlo Methods can be found here. There is a variety of information on the site and there are lots of links to research projects and papers.