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

Question

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

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

我们的想法是假设从最后的已知位置和当前位置开始的当前角度。

我正在尝试在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)));

最后，您必须考虑方向，即角度是正（顺时针转动）还是负转动（逆时针转动）。您可以通过计算b a和theta = sign(b(1)*a(2)-b(2)*a(1)) * acos(dot(a,b)/(norm(a)*norm(b))); 的z分量的符号来执行此操作。如果它是正的，那么角度应该是正的。如果是负数，则角度应为负数。使用函数cross product，我们的新等式变为：

dest1

对于您的示例，上面的等式为您的三个点dest2，dest3和a提供了88.85,92.15和-88.57的角度。

注意：您需要注意的一个特例是，如果您的对象正在直接移动远离目标点，即b和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 之间的角度{{1}}是180度。在这种情况下，您将不得不选择任意转弯方向（左或右）和转弯的度数（180将是理想的;））。以下是使用函数SIGN解释此条件的一种方法：

{{1}}

Answer 2

您可以尝试使用矢量的点积。

将向量'a'和'b'定义为：

a = new - old;
b = dest - new;

并使用点积为：

的事实

a dot b = norm2(a) * norm2(b) * cos(theta)

其中theta是两个向量之间的角度，你得到：

cos(theta) = (a dot b)/ (norm2(a) * norm2(b))

计算a dot b的最佳方法，假设它们是列向量，就像这样：

a_dot_b = a'*b;

和

norm2(a) = sqrt(a'*a);

所以你得到：

cos(theta) = a'*b/(sqrt((a'*a)) * sqrt((b'*b)))

根据余弦的符号，您可以向左或向右移动

Answer 3

基本上，您有一个由点old和new定义的线，并希望确定dest是否位于该线的右侧或左侧？在这种情况下，请查看以前的question。