Summary

The purpose of this lab is to introduce you to basic localization using sonars and vision. The first task will set up a simple scenario with the robot using two walls to localize itself within an open workspace. The second task will use visual markers to allow the robot to localize itself and build a map of its environment.

Setup

On the Nserver, generate a map that consists of two walls at 90 degrees to one another and touching at a corner. The robot should always start out somewhere it is able to sense at least one of two walls.

If you have not already done so, put your basic robot motion code into the mod-skeleton format so that the navigation module is a standalone program that can take commands from other programs via IPC. You can download the mod-skeleton and GCM code from the engin CVS repository. IPC is already installed on the robots and shadowfax. You will also need to have a command line function to send commands to the robot. Simple examples are the gcmquit.c and gcmangle.c files in the mod-skeleton directory, which sends a GCM command to all modules tell them to quit immediately. The mod-skeleton code is now capable of working with the Nomad simulator if you include the NOMADIC flag in the makefile.

Follow good coding principles and software organization for this lab. Your particle filter or Kalman filter, for example, will likely end up as part of a separate mapping module and not part of the navigation module. Therefore, design your particle filter system to be modular. There should be an initialization function that creates the particles or initializes the Kalman filter, then a function that updates the system state based on a new set of measurements. You will want to define a structure to hold the particle/Kalman system state. Note that the filter update routine needs to be flexible enough to execute an update with no measurements. Then, whether the filter is in your navigation module or a separate module, all it cares about it updating itself based on a set of measurements (or not).

The measurements may arrive via IPC messages, or through internal variables in the navigation module. It might make the most sense to have the filter take as input the GCM data message associated with range data. Then the data is in the proper form no matter where the particle system is located.

Task 1: 2D Localization

The first task is to implement either a Kalman filter or a particle filter for localization using the sonars. In either case, you can ignore the robot's orientation and just track (x, y) position by assuming the robot will always stay within 4m of both walls, so it will always have a reading from each. If you take the two closest readings that are 90 degrees apart, that will give you an approximate (x, y) coordinate pair.

An alternative, more sophisticated approach, would be to use multiple sonar readings and fit lines that are 90 degrees apart to the readings. Using that appraoch, you could also try tracking orientation as part of your state vector.

Once you have a localization method implemented, have your robot move between a series of waypoints (minimum 4) and then return to it's original starting position. You can assume there are no obstacles in the environment except the walls.

If you wish, you may use the following generic particle filter skeletons. The code is somewhat documented, and was used several years ago during a summer project for tracking fruit flies in video.

Task 2: Visual localization

In the third week we will look at the Swarthmore Vision Module and how it can be used to identify unique markers in the environment. You will again use either a particle filter or a Kalman filter to track the robot's state, but measurements-based updates to the state will, in general, be less common as they will occur only when the vision module detects a marker.

Once you have a localization method implemented, have your robot move between a series of waypoints (minimum 4) and then return to it's original starting position. You can assume there are no obstacles in the environment except the walls.

Example Particle Filter Localization Code