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

[poster]

我正在解决以下问题：我有一个对象，并且现在知道它的位置，并且在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 = [xy]; ）， old是300ms的旧位置， dest是目的地。

编辑

这是一张图片，通过一些示例来说明问题：



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

如果目的地是dest1我期望的角度约为88°。
如果目的地是dest2我期望的角度约为110°。
如果目的地是dest3我希望角度约为-80°。



回答：

首先，您需要注意上面显示的样本图上的比例。 x轴刻度线以1的步长移动，而y轴刻度线以20的步长移动。两个轴经过适当缩放（如命令axis equal ）的图片将比您要窄得多，因此您期望得到的角度不正确。预期角度将接近直角，与90度仅相差几度。

Nathan得出的方程对于列向量输入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))); 最后，您必须考虑方向，即角度应为正（顺时针旋转）还是负（逆时针旋转）。您可以通过计算b和a的叉积的z分量的符号来实现。如果为正，则角度应为正。如果为负，则角度应为负。使用函数SIGN ，我们的新公式变为：

theta = sign(b(1)*a(2)-b(2)*a(1)) * acos(dot(a,b)/(norm(a)*norm(b))); 对于您的示例，上述等式给出了您的三个点dest1 ， dest2和dest3的角度为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

更多&回答...