# Determining position from a 2D map and LIDAR

We need to determine the 2D position of my robot. To do so, we have a LIDAR at a known high, with an horizontal plane, which gives us the distance to the nearest point for each angular degree (so 360 points for one rotation). The environment is known, so I have a map of the position of every object that the LIDAR is susceptible to hit.

My question is, based on the scatter plot that the LIDAR is returning, how can we retrieve the position of my robot in the map ? We would need the x, y position in the map frame and also the theta angle from the map frame to my robot frame.

We have tried to match objects on map with groups of points based on their distance between each other and by identifying those objects and the way the LIDAR "sees" them to retrieve the robot position. But it is still unsuccessful.

To put it in a nutshell, we want to make SLAM without the mapping part. How is it possible, and with what kind of algorithms ?

A first step could be to stop the robot while acquiring data if it seems easier to process.

• Is the robot moving while taking the LIDAR sensor reading? If so, did you take this into account? – Bending Unit 22 Aug 23 '16 at 11:30
• Yes, the robot is moving, but a first step is to make it stop during the data acquiring. Furthermore, if the LIDAR rotation speed is greater than the displacement speed, we think we could process the data as if the robot is stopped. – EngelOfChipolata Aug 23 '16 at 11:41
• "The environment is known" if this is the case then you don't need SLAM. It is localization problem which is simpler than SLAM. – CroCo Aug 23 '16 at 12:25
• Exactly, this is what I intended to say using the last but one sentence. But even if it seems simpler, we can't figure out how to do it properly. I completely agree that in SLAM you don't have to "match" to a known environment, so I did not put the SLAM tag. – EngelOfChipolata Aug 23 '16 at 13:03

Your problem is called localization. A simple approach would be to use a point set registration algorithm such as ICP with a map represented as a point cloud, where the real-world position of every point is known. To localize, you would then run the point set registration algorithm to find the best match for the LIDAR scan you just acquired in the map. The algorithm will return a 2D transform that will give you the position and orientation of your robot (strictly speaking your lidar, assuming it is at the origin of your point cloud).

• The thing is, my map will not be entirely discovered, and so, the LIDAR points cloud will overlap just a part of the map. Is the ICP still working in that case ? Furthermore, it is possible to generate a point cloud that could be matched from every position of the robot (e.g. more points on an obstacle if the robot is close to it, but the number of points in the map will stay the same) ? – EngelOfChipolata Aug 23 '16 at 18:49
• Don't quote me on that, but I think the "basic" ICP algorithm works with partial matches. I do know that not all ICP variants are robust to noise (which is what the varying point density would look like to the registration algorithm), but there certainly are some variants that work for your case. Maybe have a look at this paper by Rusinkiewicz and Levoy that presents and compares variants of ICP. Also, you can consider other algorithms than ICP. The wikipedia page I linked in my response is a good starting point. – user12985 Aug 23 '16 at 19:11
• By reading a bit about it, it seems that since ICP is least square based, perfect matching is not needed. If the noise or the non matching parts are not similar to another part of the map (luck...), the good place should be the lowest least square value. Thank you so much ! – EngelOfChipolata Aug 24 '16 at 11:29
• To deal with multiple potential matches (what the lidar sees is not enough to uniquely identify your position), I would look into using a particle filter. This filter will allow you to keep track of the multiple possible positions and, as new information gets available (e.g. through movement), improve your position estimate. – user12985 Aug 24 '16 at 20:00
• I think we will make the matching search space smaller than the whole map, because we already have a good estimation of the position through odometry, but as we are not that good at mechanics, it drifts a lot... – EngelOfChipolata Aug 25 '16 at 6:17

1- Of course you don't have perfect matches

2- ICP is not used for localization, it is used to calculate the transformation the robot's pose has undergone (T+R)

3- To localize your robot, you would have to use a particle filter as lucab has said

I just wanted add on to both user12895 and AL-ROBOT's answers.

Based on experience:

• What you need is an (Iterative Point Cloud) ICP algorithm.

• Do not worry, if the robot cannot detect the entire map when it scans with the lidar, you can just match what ever data you capture to a part of the map depending on your current position.

• Great work on getting position working quite well with odometry at the moment, along with ICP it will reduce the error in the pose estimation very well.

• Not sure what sort of hardware you are using to do all the processing. Because the execution speed of the processing is what matters when it comes to whether you can process the ICP while moving. In the past I have done this with LabVIEW on a myRIO1900 by NI, and the robot was moving at a decent speed. I have heard from my peers that if you write the ICP in c++ you could run it very fast. I have actually witnessed very fast mobile robots doing ICP very reliably. In both cases mentioned above the LIDARS used were Hokuyo Lidars which are decent. Although they only have a FOV of 120 degrees, so if you cannot find one that has a FOV of 360 degrees. I recommend using two, one at the front and one at the back HOKUYO Lidars.