How to check collisions on x-y axis

Question

I'm writing a mobile robotics application in C/C++ in Ubuntu and, at the moment, I'm using a laser sensor to scan the environment and detect collisions with objects when the robot moves.

This laser has a scan area of 270° and a maximum radius of 4000mm. It is able to detect an object within this range and to report their distance from the sensor.

Each distance is in planar coordinates, so to get more readeable data, I convert them from planar to cartesian coordinates and then I print them in a text file and then I plot them in MatLab to see what the laser had detected.

This picture shows a typical detection on cartesian coordinates. Values are in meters, so 0.75 are 75 centimeters and 2 are two meters. Contiguous blue points are all the detected objects, while the points near (0,0) refer to the laser position and must be discarded. Blue points under y < 0 are produced since laser scan area is 270°; I added the red line square (1.5 x 2 meters) to determine the region within I want to implement the collisions check. So, I would like to detect in realtime if there are points (objects) inside that area and, if yes, call some functions. This is a little bit tricky, because, this check should be able to detect also if there are contiguous points to determine if the object is real or not (i.e. if it detects a point, then it should search the nearest point to determine if they compose an object or if it's only a point which may be a detection error).

This is the function I use to perform a single scan:

struct point pt[limit*URG_POINTS];
//..
 for(i = 0; i < limit; i++){
 for(j = 0; j < URG_POINTS; j++){
  ang2 = kDeg2Rad*((j*240/(double)URG_POINTS)-120);
  offset = 0.03;     //it depends on sensor module [m]

  dis = (double) dist[cnt] / 1000.0;
  //THRESHOLD of RANGE
  //      if(dis > MAX_RANGE) dis = 0;  //MAX RANGE = 4[m]
  //      if(dis < MIN_RANGE) dis = 0;
  pt[cnt].x = dis * cos(ang2) * cos(ang1) + (offset*sin(ang1)); // <-- X POINTS
  pt[cnt].y = dis * sin(ang2); // <-- Y POINTS
 // pt[cnt].z = dis * cos(ang2) * sin(ang1) - (offset*cos(ang1)); <- I disabled 3D mapping at the moment
  cnt++;
}
ang1 += diff;
}

After each single scan, pt contains all the detected points in x-y coordinates.

I'd like to do something like this:

• perform a single scan, then at the end,
• apply collisions check on each pt.x and pt.y
• if you find a point in the inner region, then check for other near points, if yes, stop the robot;
• if not or if no other near points are found, start another scan

I'd like to know how to easy check objects (composed by more than one single point) inner the previous defined region.

Can you help me, please? It seems very difficult for me :(

Answer 1

I don't think I can give a complete answer, but a few thoughts on where it might be possible to go.

What do you mean with realtime? How long may it take for any given algorithm to run? And what processor does your program run at?

Filtering the points that are within your area of detection should be quite easy just by checking if abs(x) < 0.75 and y< 2 && y > 0. Furthermore, you should only consider points that are far enough away from 0, so x^2 + y^2 > d. But that should be the trivial part.

More interesting it will get to detect groups of points. DBSCAN has proven to be a fairly good clustering algorithm for detecting 2-dimensional groups of points. The critical question here is if DBSCAN is fast enough for real-time applications. If not, you might have to think about optimizing the algorithm (You can press it's complexity to n*log(n) using some clever indexing structures).

Furthermore, it might be worth thinking about how you can incorporate the knowledge you have from your last iteration (assuming a high frequency, the data points should not change to much).

It might be worth looking at other robotics projects - I could imagine the problem of interpreting sensor data to construct information of the surroundings is a rather common one.

UPDATE

It is fairly difficult to give you good advice without knowing where you stumble on applying DBSCAN on your problem. But let me try to give you a step-by-step-guide how an algorithm may work:

• For each datapoint you receive you check whether it is in the region you want to have observed. (The conditions I have given above should work).
• If the datapoint is within the region you save it to some sort of list
• After reading all data points you check if the list is empty. If so, everything is good. Otherwise we have to check if there are bigger groups of data points that you have to navigate around.

Now comes the more difficult part. You throw DBSCAN on that points and try to find groups of points. Which parameters will work for the algorithm I do not know - that has to be tried. After that you should have some clusters of points. I'm not totally sure what you will do with the groups - an idea would be to detect the points of each group that have the minimum and maximum degree in polar coordinates. That way you could decide how far you have to turn your vehicle. Special care would have to be taken if two groups are so close that it will not be possible to navigate through the gap between.

For the implementation of DBSCAN you could here or just ask google for help. It is a fairly common algorithm that has been coded thousands of times. For further optimizations concerning speed it might be helpful to create an own implementation. However, if one of the implementations you find seems to be usable, I would try that first before going all the way and implementing it on my own.

If you stumble on specific problems while implementing the algorithm I would suggest creating a new question, as it is far away from this one and you might get more people that are willing to help you.

I hope things are a bit clearer now. If not please give the exact point that you have doubts about.