Simultaneous Localization and Mapping (SLAM) with LeJOS

[kc123]

Hi,
I am working on a semester project for my graduate level class using Lego NXT and LeJOS. I have some pretty exciting result so far and would like to share with everyone and get some feedback.

Background:

Mapping and localization is a challenging topic in robotic, especially with the Lego Mindstorm NXT due to limited sensor and actuator (the servo motors in this case). The sonar sensor is susceptible to irregular reflection and tires tend to slips on different surfaces. All these make correctly estimating where the robot is very difficult. Almost all the NXT mapping project online uses high precision optical sensor that cost $50 dollars, unfortunately not everyone can afford one. The goal of this project is using probabilistic algorithms to deal with uncertainties (both sensor measurement and movement) to better estimate robot’s location and create an environment map at the same time.

Currently, I break down my approach to three parts:

1, Occupancy Grid Mapping

2, Localization with Monte Carlo Localization (MCL)

3, Combing MCL location back with robot’s estimated position (Kalman Filter)

More detail will follow.

-Kevin

[kc123]

Here is an example of one of the test. The robot is moving around and scan two rectangular rooms that's connected. Each green dots shows where the robots stops. When it stops, it takes eight sonar measurement in all direction. The location of the detected "obstacle" is plot on a occupancy grid map with each grid is 3cm by 3cm square.

As you can see the measurement is very noisy with some phantom measurements on the edge of the map.



In case image doesn't work here is the url http://www.freeimagehosting.net/1fd5b

[kc123]

Here is the image again (hopefully it will show up correctly this time).


The map created from row data is useless because all the noise in sensor measurement and movement errors. One way to fix this is by applying Bayesian reasoning to update the map using probabilistic rule. The Bayesian update rule I used is describe in the book "An Introduction to AI Robotics" by Murphy.

See image below for occupancy grid map updated by Bayesian rule (rendered in 3D):


http://www.freeimagehosting.net/8bb31

The image shows the same set data processed by Bayesian update rule. The black line shows the route the robot took. Again each grid represent a 3cm by 3cm square, and the height of each grid shows the probability of that grid is being occupied. The darker color a gird is the more likely the grid is being occupied.

The map created by Bayesian update rule looks much better and is useable for path planning purposes.

[Aswin]

Very impressive.

Can you give us a photo of the room as well for comparison?

[kc123]

Hi, Aswin:

The particular data I rendered is recorded from an in-class demo couple months ago, which I unfortunately didn’t take any pictures. But I can certainly try get some more new data with pictures.

Because it takes a lot debugging with these algorithms, what I did was logging all the movements and sensor data as the robot moves around. Then I have a robot “simulator” that is capable of reading recorded data and feeds them back into the program. So far I have being used recorded data for my SLAM development. I was planning on using actual NXT hardware again once I finish wrap up my algorithm development.

Anyway, I agree with you that without a picture it’s a kind hard to tell what the robot is mapping. I will try to collective some new data and have pictures of the actual environment.

-Kevin

[kc123]

Here are the new images...

Picture of the environment:


Plotting of raw data:


Bayesian updated occupancy grid map (1cm x 1cm grid):

[steveiswicked]

Hello Kevin,
That is very impressive, I tried and failed mapping a room with sonar. Naively I had no idea then what a complex problem it would turn out to be with all those dubious echoes to filter out. I asked hitechnic if they were going to make a laser range finder but they won't because they're not safe for kiddies to play with.

[kc123]

Hi,
Yeah, I agree filtering out all the noise is a hard problem. The biggest issues I have for now is the the error in robot's movement. After a while the robot's estimated location would be completely off, which would totally mess up mapping new sonar measurement. That is what I am working on right now...using SLAM technique to help correct robot's positioning.

If you are looking for more precise distance sensor "http://www.mindsensors.com/" has some high precision optical distance sensor. The latest version of LeJOS library supports those under OpticalDistanceSensor API. From their videos, the sensor seems to be pretty good. I got one this past weekend and plan to play around with it.

I am working on debugging my algorithm and integrating new sensors. Once that's done I will post some more results.

-Kevin

[steveiswicked]

Hello Kevin,

Yes, dead reckoning is a pain, I looked into it and it seemed to me that one solution is feature recognition. Once you could recognise a feature you could use it as a kind of beacon and then triangulate from it. Trouble for me was the maths, all the papers I found on noise filtering seemed intent on impressing the examiners rather than explaining it to a hobbyist, they made big assumptions about the readers prior knowledge and didn't tell the reader where to go to fill the gaps.

Steve

[ljw7630]

Hi, Kevin, is that possible to have your source code?

[kc123]

Uh...no, I don't know what you are using it for. Although I was thinking submitting some of code back to leJOS if there are enough interest.

I have a pretty good implementation of occupancy grid map, and I fixed some bugs in the Monte Carlo localization.

-Kevin