如何将偏航、俯仰、横滚和加速度值转换为笛卡尔坐标系？

Question

我正在读取偏航、俯仰、翻滚、旋转矩阵、四元数和加速度。这些读数的频率为20次/秒。它们是从移动设备收集的，该设备在3D空间中从一个位置移动到另一个位置。

我通过将逆矩阵乘以起始位置来设置参考平面。其余的读数是通过将第一个读数视为参考读数来获取的。现在我想将这些读数转换为3D笛卡尔系统。

如何转换？有人可以帮忙吗？

- Apurv

---

这似乎不是一个编程问题。更适合数学论坛。 - scottb

Answer 1

基本上，偏航角、俯仰角和翻滚角是欧拉角，它们可以帮助你获得旋转矩阵。四元数与之等效，使用它们也可以计算所需的旋转矩阵。

如果你有每个时刻i在l=20秒间隔内的旋转矩阵R_i。那么这些旋转是相对于应用在R_(i-1)的旋转，你可以计算它们相对于第一个位置的旋转。所以A_i = R_1*...*R_i但最终你也可以只保存新的行进方向（节省计算）。

假设一开始的行进方向是d_0 = (1,0,0)。你可以通过公式d_i = R_i*d_(i-1)计算下一个方向（始终规范化d_(i-1)，因为它可能由于误差而变小或变大）。第一个位置是p，起始速度是v_0 = (0,0,0)，最后的加速度是a_i。你需要为每个时刻计算矢量速度v_i：

v_i = v_(i-1) + l*a_i*A_i*d_0 = v_(i-1) + l*a_i*d_i

现在，你基本上知道自己移动的位置和速度了，因此你在当前时刻i的位置p_i可以表示为：

`p_i = p_0 + l * ( v_1 + v_2 + ... + v_i)`

对于单位：

a_i = [m/s^2]^3
v_i = [m/s]^3
p_i = [m]^3  

 

精度

现在我们来谈一下您位置计算的精度（只有在您想知道它能够工作得多好时才需要了解）。假设您的误差为e>= ||R_i*v-w||（其中w是正确的向量），这样您就可以使用计算出的旋转矩阵。由于误差会自我相乘，因此您在第i个时刻的误差为e_i <= e^i。
然后因为您将其应用到l和a_i上，所以它变成了：

f_i <= l*a_i*e^i

但是当你把速度相加时，也会累计误差，所以现在是 g_i <= f_1+...+f_i。对于位置也是一样的（两个求和符号都是关于 i）：

h_i <= g_1+...+g_i = ΣΣ (l*a_i*e^i) = l* ΣΣ (a_i*e^i)

所以，这基本上是您的位置到正确位置的最大差异（||p_i - w|| <= h_i）。仍未考虑到您无法从设备获得正确的加速度（我不知道他们通常如何做到这一点），因为正确的方式应该是：

a_i = ||∫a_i(t) dt||  (where a_i(t) is vectorial now)

您需要计算方向的差异（旋转矩阵），如下：

 Δd_i = (∫a_i(t) dt)/a_i   (again a_i(t) is vectorial)

除了来自设备旋转错误（以及浮点运算）的错误之外，您的加速度也存在误差。我现在不会计算它，但您可以替换 a_i = a_i + b_i。

所以我很确定它会远离真实位置。你甚至需要考虑你的速度可能是非零的，而实际上应该是零！

但是话说回来，我真的想知道您在实施后获得的精度，这总是让我不敢尝试的原因。

- mjb4

---

感谢您详细的回答。给你点赞。我会尝试上面的代码并告诉您结果。 - Apurv