代码之家 › 专栏 › 技术社区 › Jakub Hampl

如何正确计算移动对象的方向?

trigonometry matlab

Jakub Hampl · 技术社区 · 14 年前

我正在解决以下问题:我有一个物体,我知道它现在的位置和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 是目的地。

编辑

这里有一张图片,通过几个例子来说明这个问题:

Illustration of the problem

在上面的图像中,有一些点被绘制和注释。黑线表示我们估计的物体当前的方位。

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

3 回复 | 直到 14 年前

Community CDub 8 年前

首先,您需要注意上面显示的示例图上的比例。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 属于 乙 和 一 . 如果是正的,角度应该是正的。如果是负的,角度应该是负的。使用函数 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 .

注: 你需要注意的一个特殊情况是你的物体是否在移动 直接地 远离目的地,即如果 一 和 乙 是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

Nathan Fellman 14 年前

可以尝试使用向量的点积。

将向量“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)))

根据余弦的符号,你可以向左或向右

Community CDub 8 年前

实际上你有一条由点定义的线 old 和 new 并希望确定 dest 在那条线的左边还是右边?在这种情况下,看看前面的 question .