matlab - 如何正确计算移动物体的方向？

Question

我正在解决以下问题:我有一个对象，我知道它现在的位置和 300 毫秒前的位置。我假设物体在移动。我有一个要点，我希望对象到达。

我需要的是以我知道是左转还是右转的格式获取从当前对象到目标点的角度。

这个想法是假设当前位置与最后已知位置和当前位置之间的角度。

我试图在 MATLAB 中解决这个问题。我已经尝试使用 atan2 的多种变体，但要么在某些情况下我得到了错误的角度(比如当我的对象在绕圈时)，要么我在所有情况下都得到了错误的角度。

搞砸的代码示例:

a = new - old;
b = dest - new;
alpha = atan2(a(2) - b(2), a(1) - b(1);

其中 new 是当前位置(例如 x = 40; y = 60; new = [x y];)，old是 300 毫秒的旧位置，dest 是目标点。

编辑

这是一张图片，用几个例子来说明这个问题:

在上图中，绘制并注释了一些点。黑线表示我们估计的物体当前朝向。

如果目标点是 dest1，我希望角度大约为 88°。
如果目标点是 dest2，我希望角度大约为 110°。
如果目标点是 dest3，我希望角度约为 -80°。

Answer 1

首先，您需要注意上面显示的示例图上的比例。 x 轴刻度以 1 为步长移动，y 轴刻度以 20 为步长移动。两个轴适当缩放的图片(如使用命令 axis equal )会比你的窄很多，所以您期望获得的角度不正确。预期角度将接近直角，与 90 度相差几度。

方程 Nathan derives对列向量输入有效 a和 b :

theta = acos(a'*b/(sqrt(a'*a) * sqrt(b'*b)));

如果您想更改此方程以使用行向量，则必须在点积和范数的计算中切换转置运算符，如下所示:

theta = acos(a*b'/(sqrt(a*a') * sqrt(b*b')));

作为替代方案，您可以只使用函数 DOT和 NORM :

theta = acos(dot(a,b)/(norm(a)*norm(b)));

最后，您必须考虑方向，即角度应该为正(顺时针旋转)还是负(逆时针旋转)。您可以通过计算 cross product 的 z 分量的符号来做到这一点。的 b和 a .如果它是正的，角度应该是正的。如果为负，则角度应为负。使用函数 SIGN ，我们的新方程变为:

theta = sign(b(1)*a(2)-b(2)*a(1)) * acos(dot(a,b)/(norm(a)*norm(b)));

对于您的示例，上述等式给出的三个点的角度分别为 88.85、92.15 和 -88.57 dest1 , dest2 , 和 dest3 .

注意:您需要注意的一种特殊情况是，如果您的对象直接移动远离目标点，即如果 a 之间的角度和 b是180度。在这种情况下，您将不得不选择一个任意的转弯方向(左或右)和转弯的度数(180 度是理想的 ;))。这是您可以使用函数 EPS 来解释这种情况的一种方法。 :

theta = acos(dot(a,b)/(norm(a)*norm(b)));   %# Compute theta
if abs(theta-pi) < eps  %# Check if theta is within some tolerance of pi
  %# Pick your own turn direction and amount here
else
  theta = sign(b(1)*a(2)-b(2)*a(1))*theta;  %# Find turn direction
end