当矢量速度和检测点之间的角度为90

Question

我有传感器检测到的点，我有速度矢量，传感器位置，无人机位置，传感器和检测点之间的距离。 我尝试了几天没有解决方案。 我怎样才能绕过90度的障碍物？与障碍物平行。

我试图找到速度和检测到的点之间的角度，然后用这个角度旋转矢量速度，不起作用。或物体点和检测点，并乘以速度的大小。

请任何解决方案!!

Answer 1

1. 积分数

   您无法通过单次测量完成此操作！您需要沿途采样至少3个点/距离。然后将它们转换为全局世界坐标（或任何常见参考框架），让它们称为p0,p1,p2。

2. <强>壁

   首先检查3点是否在同一行

   u = p1-p0
   v = p2-p1
   | u/|u| - v/|v| | <= zero_threshold
   

   如果不是，那么墙不是平面的，或者你遇到了多个障碍物。在任何此类情况下，您都无法从中推断任何内容，因此您应该再次测量或转向安全...

3. <强>角度

   如果你的无人机在同一高度飞行或者不断变化（轨迹是直线），那么我们可以将其视为 2D 情况。 （在 3D 中，您还需要进行高度测量...并使用墙壁的 3D 法线，因此＃2 看起来会非常不同。因此，计算墙和速度之间的角度：

   dangle = acos( dot_product((p2-p0),velocity_vector) / (|p2-p0|*|velocity_vector|) )
   

   dangle的符号由距离

   确定

   if (d0>d1>d2) turn away
   if (d0<d1<d2) turn in
   if (d0~=d1~=d2) no turn as you are parallel
   

   不知道你的坐标系属性很难说清楚。但您可以简单地检查旋转的速度矢量是否会导致dangle接近零。如果没有将标志更改为减号。