如何从车辆到目标找出方位角/俯仰角？

Question

我正在尝试计算从移动的飞行器模拟到地面某一点的方位角和高度角。我拥有车辆位置矢量P和它的方向四元数vehQ。我有目标位置T，并且通过从P中减去T建立了差值矢量dPT。

如何计算到目标的方位角/高度角呢？由于我不熟悉三维数学，希望能得到一些解释。

谢谢。

- Dave Drake

Answer 1

首先，找到从车辆到目标的相对位置向量 dPT：

worldspace target vector dPT = T - P

由于车辆位置 P 和目标位置 T 都是世界坐标系下的，所以得到的向量 dPT 也将用世界坐标系表示。因此，您必须使用车辆方向四元数将 dPT 从世界坐标系旋转到车辆坐标系。正确的方法取决于生成四元数的约定，但可能是以下公式之一：

vehicle target vector U = vector_part( vehQ * quaternion(0,dPT) * conjugate(vehQ) )
                 or
                      U = vector_part( conjugate(vehQ) * quaternion(0,dPT) * vehQ )

由于您没有提供任何关于四元数约定的信息，我无法知道哪一个是适用于您应用的正确约定。但是，很有可能您的四元数源还提供了使用其四元数来旋转向量的函数或方法。因此，您实际上应该查找这些函数，阅读它们的文档，并在自己编写之前尝试使用它们。

一旦您将目标向量转换为车辆坐标系，您可以使用标准公式来找到方位角和俯仰角。这取决于您的坐标约定，但作为示例，如果您的坐标约定是以 Z 轴为“向上”方向的右手坐标系：

azimuth = atan2(U.y, U.x)
elevation = atan2(U.z, sqrt(U.x^2 + U.y^2))

这应该计算出一个以弧度为单位的高程，范围从-PI/2到+PI/2，以及一个方位角，范围从-PI到+PI，其中0沿着+x轴，逆时针增加（只要高程不是垂直的...）。

- comingstorm