判断两个yaw角度之差是否超过了90度

一. 判断两个yaw角度之差是否超过了90度

要判断两个 yaw 角度之差是否超过 90 度，你可以通过计算这两个角度的差值，并将其归一化为 [-180, 180] 的范围内。接着，只需判断该差值的绝对值是否大于 90 度。

实现步骤：

1. 计算角度差：两个角度的差值可以通过直接相减得到，但需要将结果限制在 [-180, 180] 范围内，因为角度是周期性的。
2. 归一化到 [-180, 180] 范围：如果角度差超出了这个范围，使用模运算将其归一化。
3. 判断是否超过 90 度：最终判断归一化后的角度差的绝对值是否大于 90。

代码实现：

import math

def is_yaw_difference_exceed_90(yaw1: float, yaw2: float) -> bool:
    # 计算两个角度的差值
    yaw_diff = yaw2 - yaw1

    # 将差值归一化到 [-180, 180] 之间
    yaw_diff = (yaw_diff + 180) % 360 - 180

    # 判断是否超过90度
    return abs(yaw_diff) > 90

# 示例用法
yaw1 = 30
yaw2 = 150

if is_yaw_difference_exceed_90(yaw1, yaw2):
    print("两个 yaw 角度之差超过了 90 度")
else:
    print("两个 yaw 角度之差没有超过 90 度")

二 . yaw_diff = (yaw_diff + 180) % 360 - 180这个公式时如何推导而来的

代码说明：

1. 角度差：yaw_diff = yaw2 - yaw1 计算两个 yaw 角度之间的差值。
2. 归一化：(yaw_diff + 180) % 360 - 180 将差值归一化为 [-180, 180] 范围内，保证我们处理的是最小角度差。
3. 判断：最后判断 abs(yaw_diff) > 90 来确认角度差是否超过了 90 度。

示例：

• 若 yaw1 = 30 和 yaw2 = 150，则角度差为 120，超过了 90 度。
• 若 yaw1 = 30 和 yaw2 = 80，则角度差为 50，没有超过 90 度。

公式 yaw_diff = (yaw_diff + 180) % 360 - 180 用于将角度差 yaw_diff 归一化到 [-180, 180] 的范围内。该公式的推导基于以下数学原理和角度的周期性：

1. 角度的周期性

角度是周期性的，360 度等价于 0 度，所以角度的取值可以任意增加或减少 360 的倍数，仍然表示同一个方向。例如，30 度和 390 度是等价的。

• 角度可以用模运算 (%) 来限制在某个范围内，比如 [0, 360)，即 angle % 360 可以将任何角度限制在这个范围内。

2. 为什么需要归一化到 [-180, 180]

虽然 360 度和 0 度是等价的，但在很多应用场景下（例如航向角的计算），我们更关心的是最短的角度差，而不是一个大于 180 度的角度。例如，我们希望知道从 10 度到 350 度的角度差应该是 -20 度，而不是 340 度。

为了实现这一点，我们将角度差限制在 [-180, 180] 范围内，其中负数表示逆时针旋转，正数表示顺时针旋转。

公式推导

假设我们有一个角度差 yaw_diff，其范围可能超出了 [-180, 180]。我们希望将其限制在该范围内。

1. 平移到 [0, 360) 范围：通过 yaw_diff + 180，我们将角度差平移到 [0, 360) 这个范围。

   • yaw_diff 可能是负值或者大于 180 的值，因此加上 180 度后，将范围变为 [0, 360)。

2. 取模 360：(yaw_diff + 180) % 360 可以将任何角度限制在 [0, 360) 的范围内。

   • 取模运算的结果总是返回非负值，这样可以确保所有的角度差被标准化到 [0, 360) 范围内。

3. 平移回到 [-180, 180] 范围：由于我们之前将角度平移了 180 度，现在我们需要将其平移回原来的范围，即 [-180, 180]。我们通过减去 180 来完成这一操作。

   • [(yaw_diff + 180) % 360 - 180] 将角度差规范到 [-180, 180]，确保角度差为最小角度差。

例子

假设 yaw_diff = 200，即两个角度之间的差值为 200 度：

1. 平移：yaw_diff + 180 = 200 + 180 = 380
2. 取模：380 % 360 = 20
3. 平移回去：20 - 180 = -160，最终结果为 -160 度。

这表示从一个角度到另一个角度的最小旋转角度是 -160 度（逆时针）。

再看一个负角度的例子，假设 yaw_diff = -190：

1. 平移：yaw_diff + 180 = -190 + 180 = -10
2. 取模：-10 % 360 = 350
3. 平移回去：350 - 180 = 170，最终结果为 170 度。

这表示最小旋转角度是 170 度（顺时针）。

总结

公式 yaw_diff = (yaw_diff + 180) % 360 - 180 的作用是将任何角度差转换到 [-180, 180] 范围内，从而表示最短的旋转角度。它利用了角度的周期性和模运算，将原始角度差平移到一个容易处理的范围，再平移回所需的目标范围。

三. yaw_diff = abs(yaw_diff + 180) % 360 - 180， 用不用在这里加上一个abs

在公式 yaw_diff = (yaw_diff + 180) % 360 - 180 中，不需要加上 abs，原因如下：

1. abs 的作用：

abs() 函数返回一个值的绝对值，即非负数。如果你在公式中加上 abs，会影响到结果的符号。对于计算两个角度之间的差值时，符号是很重要的，因为它表示旋转的方向（正数表示顺时针，负数表示逆时针）。

2. 现有公式的作用：

• yaw_diff = (yaw_diff + 180) % 360 - 180 的作用是将角度差规范到 [-180, 180] 的范围内。
  • 这样可以正确处理角度的周期性，确保角度差是最小的旋转角度，并且保留旋转的方向。

如果你在这里加上 abs，如 yaw_diff = abs((yaw_diff + 180) % 360 - 180)，会导致结果总是正值，无法区分顺时针还是逆时针旋转，这在某些情况下是不符合需求的。例如：

• 角度差 +170（顺时针） 和 -170（逆时针） 都会被计算成 170，丢失了方向信息。

3. 正确的使用场景：

如果你的需求是只关心角度差的大小，而不关心旋转方向（例如，只想知道两个角度是否相差超过了某个度数），这时可以使用 abs，但不是在公式内部加，而是在外部加。

# 当你只关心角度差的绝对值
yaw_diff = (yaw_diff + 180) % 360 - 180
yaw_diff = abs(yaw_diff)  # 只关心角度差的大小

总结：

• 如果你需要知道两个角度的最小差值和旋转方向，不需要加 abs，公式 yaw_diff = (yaw_diff + 180) % 360 - 180 已经正确处理了这一点。
• 如果你只关心角度差的大小，不在乎方向，可以在计算完成之后加 abs，例如 yaw_diff = abs(yaw_diff)。