通过偏航校正计算 x 和 y 轴上两个三角形的对边

Calculating the opposite side of two triangles on the x and y axis with a correction for yaw

对于一个项目，我想用 C++ 做一个非常简单的毕达哥拉斯计算。一个物体配备了一个 IMU 传感器，它可以提供四元数旋转或欧拉角。我想知道的是物体下面三角形的对边。:

我想知道 X 轴和 Y 轴（黑色箭头）的三角形的这些边：

除了对象可以旋转之外，这非常简单。当物体旋转时，我仍然想在世界中使用 X 和 Y 轴 space（黑色箭头），但是当偏航时，IMU 的欧拉角为我提供俯仰和滚动，它们在局部 space（红色箭头）：

我在偏航时仍然可以通过什么方式获得世界 space 角度（黑色箭头），以便能够计算我简单的毕达哥拉斯计算？如果我不能得到它们，有没有办法使用四元数计算我想要的对边？

我们可以通过按以下顺序考虑欧拉角来进行计算 -

  1. 音高

首先，当你改变传感器的 roll 时，传感器 "ray" 扫描出一个倾斜于 horizon 角度 pitch 的平面。我们需要首先计算 (i) 飞机与地面的交线与 (ii) 地面上传感器正下方的点之间的最近距离。这是由 d = h * tan(pitch).

给出的

  1. 滚动

接下来我们需要做另一个三角步骤。和之前一样 roll 扫过平面。沿垂直于连接 (i) 和 (ii) 的线的轴的偏移距离由 f = h / cos(pitch) * tan(roll) 给出。这使得地面上的交点为 (d, f)

  1. 偏航

之前，我们考虑了 yaw 为零的帧。我们现在需要将这个交点围绕 Z-axis 旋转 yaw。因此，最终交点由 (x, y) = (d * cos(yaw) - f * sin(yaw), d * sin(yaw) + f * cos(yaw)) 给出。您可以通过取 atan2(y, x).

来计算您想要的 "space angle"