我正在 Ubuntu 中使用 C/C++ 编写移动机器人应用程序,目前,我正在使用激光传感器扫描环境并检测机器人移动时与物体的碰撞。
该激光器的扫描区域为 270°,最大半径为 4000mm。它能够检测此范围内的物体并报告它们与传感器的距离。
每个距离都是平面坐标,因此为了获得更多可读数据,我将它们从平面坐标转换为笛卡尔坐标,然后将它们打印到文本文件中,然后在 MatLab 中绘制它们以查看激光检测到的内容。
这张图片显示了笛卡尔坐标上的典型检测。 值以米为单位,因此 0.75 是 75 厘米,2 是两米。连续的蓝点是所有检测到的物体,而(0,0)附近的点是指激光位置,必须丢弃。由于激光扫描区域为 270°,因此产生 y < 0 下的蓝点;我添加了红线正方形(1.5 x 2 米)来确定我想要实施碰撞检查的区域。因此,我想实时检测该区域内是否有点(对象),如果是,则调用一些函数。这有点棘手,因为,这个检查也应该能够检测到是否有连续的点来确定如果对象是真实的(即如果它检测到一个点,那么它应该搜索最近的点以确定它们是否构成一个对象,或者它是否只是一个可能是检测错误的点)。
这是我用来执行单次扫描的功能:
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;
}
每次扫描后,pt包含 xy 坐标中的所有检测点。
我想做这样的事情:
- 执行一次扫描,然后在最后,
- 对每个 pt.x 和 pt.y 应用碰撞检查
- 如果在内部区域找到一个点,则检查其他近点,如果是,则停止机器人;
- 如果没有或没有找到其他近点,则开始另一次扫描
我想知道如何轻松检查先前定义的区域内的对象(由多个单点组成)。
你能帮我吗?对我来说似乎很难:(