How to check for collisions on the xy axis

Question

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

This laser has a scanning area of ​​270 ° and a maximum radius of 4000 mm. He is able to detect an object in this range and report his distance from the sensor.

Each distance is in plane coordinates, so to get more reliable data, I convert them from planar to Cartesian coordinates , and then print them in a text file, and then I draw them in MatLab to see what the laser detected.

This figure shows a typical detection in Cartesian coordinates. The values ​​are in meters, therefore 0.75 - 75 cm and 2 - two meters. The adjacent blue dots are all detected objects, and the dots near (0,0) belong to the laser position and should be discarded . Blue dots below y <0, since the laser scanning area is 270 °; I added a red square (1.5 x 2 meters) to determine the area inside. I want to implement collision checking . So, I would like to detect in real time if there are points (objects) inside this area , and if so, call some functions. This is a bit complicated because this check should also detect if there are continuous points to determine if the object is real or not (i.e. if it detects a point, then it should look for the closest point to determine whether to make up the object or if this is only a point that may be a detection error).

This is the function that 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 individual scan, pt contains all the detected points in xy coordinates.

I would like to do something like this:

• perform one scan, then at the end
• apply collisions on each pt.x and pt.y
• if you find a point in the inner area, then check other close points; if so, stop the robot;
• if there are no other nearby points or not, run another scan

I would like to know how easy it is to scan objects (consisting of several points) inside the previous specific area.

Can you help me please It seems very difficult to me :(

Answer 1

I do not think I can give a complete answer, but a few thoughts on where to go.

What do you mean in real time? How long can it take to execute any given algorithm? And which processor works in your program?

Filtering out points within your detection area should be pretty simple if you check to see if abs(x) < 0.75 and y< 2 && y > 0 . In addition, you should only consider points that are far enough from 0, so x^2 + y^2 > d . But that should be the trivial part.

More interestingly, it will detect groups of points. DBSCAN turned out to be a pretty good clustering algorithm for detecting two-dimensional groups of points. The critical question here is that DBSCAN is fast enough for real-time applications. If not, you might have to consider optimizing the algorithm (you can overlay it on n * log (n) with some smart indexing structures).

In addition, it may be worth considering how you can use the knowledge that you have from your last iteration (assuming a high frequency, data points should not change much).

Perhaps it’s worth looking at other projects in the field of robotics - I could imagine that the problem of interpreting these sensors to create information about the environment is quite common.

UPDATE

It’s quite difficult to give you good advice without knowing where you come across DBSCAN for your problem. But let me try to give you a step-by-step guide on how the algorithm works:

• For each data you receive, you check whether it is in the region that you want to observe. (The conditions mentioned above should work).
• If the datapoint is in a region, you save it in some list
• After reading all the data points, you check to see if the list is empty. If so, all is well. Otherwise, we need to check if there are more groups of data points that you need to move.

Now comes the more difficult part. You throw DBSCAN at these points and try to find groups of points. What parameters will work for an algorithm that I do not know is a must try. After that you should have several clusters of points. I'm not quite sure what you will do with the groups - the idea would be to find the points of each group having a minimum and maximum degree in polar coordinates. This way you can decide how far you have to turn your car. Particular care must be taken if the two groups are so close that it will not be possible to bridge the gap between them.

To implement DBSCAN, you can here or just ask Google to ask for help. This is a fairly common algorithm that has been encrypted thousands of times. For further optimizations regarding speed, it may be useful to create your own implementation. However, if one of the implementations you implemented seems to be suitable for use, I would try to do this before I go all the way and implement it myself.

If you encounter certain problems when implementing the algorithm, I would suggest creating a new question, since it is far from this, and you can get more people who are ready to help you.

Hopefully now everything is a little clear. If not, indicate the exact point you are in doubt about.