c++ - 如何检查 x-y 轴上的碰撞

Question

我正在 Ubuntu 中使用 C/C++ 编写移动机器人应用程序，目前，我正在使用激光传感器扫描环境并检测机器人移动时与物体的碰撞。

此激光器的扫描区域为 270°，最大半径为 4000 毫米。它能够检测到此范围内的物体并报告它们与传感器的距离。

每个距离都在平面坐标中，因此为了获得更多可读数据，我将它们从平面坐标转换为笛卡尔坐标，然后将它们打印在文本文件中，然后在 MatLab 中绘制它们以查看激光检测到了什么。

这张图展示了笛卡尔坐标上的典型检测。 值以米为单位，因此 0.75 是 75 厘米，2 是两米。连续的蓝色点是所有检测到的对象，而 (0,0) 附近的点指的是激光位置，必须丢弃。由于激光扫描区域为 270°，因此会产生 y < 0 下的蓝点； 我添加了红线方 block (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 包含 x-y 坐标中的所有检测点。

我想做这样的事情:

• 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

我想知道如何轻松检查先前定义区域内的对象(由多个单点组成)。

你能帮帮我吗？这对我来说似乎很难:(

Answer 1

我不认为我可以给出一个完整的答案，但有一些关于可能去哪里的想法。

实时是什么意思？运行任何给定算法可能需要多长时间？您的程序在什么处理器上运行？

只需检查 abs(x) < 0.75 是否可以过滤检测范围内的点即可。和 y< 2 && y > 0 .此外，您应该只考虑距离 0 足够远的点，因此 x^2 + y^2 > d .但这应该是微不足道的部分。

更有趣的是，它将检测点组。 DBSCAN已被证明是一种用于检测二维点组的相当不错的聚类算法。这里的关键问题是 DBSCAN 是否足够快以用于实时应用程序。如果不是，您可能需要考虑优化算法(您可以使用一些巧妙的索引结构将其复杂度压缩到 n*log(n))。

此外，可能值得考虑如何整合从上次迭代中获得的知识(假设频率很高，数据点的变化应该不会太大)。

可能值得研究其他机器人项目 - 我可以想象解释传感器数据以构建周围环境信息的问题是一个相当普遍的问题。

更新

在不知道您在什么地方应用 DBSCAN 解决您的问题的情况下，很难给您好的建议。但是，让我尝试逐步指导您如何使用算法:

• 对于您收到的每个数据点，您检查它是否在您想要观察的区域中。 (我上面给出的条件应该有效)。
• 如果数据点在区域内，则将其保存到某种列表中
• 读取所有数据点后，检查列表是否为空。如果是这样，一切都很好。否则，我们必须检查是否有更大的数据点组需要您浏览。

现在是更困难的部分。您将 DBSCAN 放在这些点上并尝试查找点组。哪些参数适用于我不知道的算法 - 必须尝试。之后你应该有一些点簇。我不完全确定您将如何处理这些组 - 一个想法是检测每个组中在极坐标中具有最小和最大度数的点。这样你就可以决定你必须把你的车转多远。如果两个群体靠得太近以至于无法穿过两者之间的缝隙，则必须特别小心。

对于 DBSCAN 的实现，您可以 here或者只是向谷歌寻求帮助。这是一种相当常见的算法，已经被编码了数千次。为了进一步优化速度，创建自己的实现可能会有所帮助。但是，如果您发现其中一个实现似乎可用，我会先尝试一下，然后再完全自行实现。

如果您在实现算法时遇到了特定问题，我建议您创建一个新问题，因为它与这个问题相去甚远，您可能会得到更多愿意帮助您的人。

我希望现在事情变得更清楚了。如果不是，请给出您有疑问的确切点。