如何使用陀螺仪的原始数据 (°/s) 计算 3D 旋转？

Question

我的问题可能看起来微不足道，但我读得越多就越困惑...... 我开始了一个小项目，想要大致跟踪旋转物体的运动。（确切地说是篮球） 我有一个三轴加速度计（低通滤波）和一个测量°/s的三轴陀螺仪。 我知道陀螺仪存在的问题，但由于测量只会持续几秒钟，而角度往往很大-我现在不关心漂移和万向节。

我的陀螺仪给我提供了所有三个轴的旋转速度。由于我想要对加速度进行两次积分以获取每个时间步骤的位置，因此我想要将传感器坐标系转换为地球坐标系。 对于第一次尝试，我想保持简单，所以我决定使用大型标准旋转矩阵。 但由于我的结果非常糟糕，我想知道这是否是正确的方法。如果我理解正确-该矩阵只是按特定顺序相乘的3个矩阵。由于篮球的旋转没有任何“自然”顺序，这可能不是一个好主意。我的传感器同时测量3个角速度。如果我逐步将它们放入我的系统中，“步骤”将不正确，因为我的第二个矩阵计算围绕“新y轴”的旋转，但实际上我的传感器测量的是围绕“旧y轴”的角速度。到目前为止正确吗？

那么我该如何正确计算3D旋转？ 我需要使用四元数吗？但是我如何从3个不同的旋转中得到一个？并且我在这里是否再次遇到同样的问题？

我从一个单位矩阵（（1,0,0）（0,1,0）（0,0,1））开始，将其与加速向量相乘以给出第一次移动。 然后我想使用旋转矩阵找出下一个加速度的实际方向，以便可以简单地将加速度相加。

但是现在我太困惑了，找不到合适的方法。

有什么建议吗？ 顺便说一句，对不起，我的英语很差，我很累，（显然）不是母语为英语的人；）

谢谢， Alex

- Alex

Answer 1

简短回答

是的，使用四元数并使用旋转的一阶线性化来计算方向的变化。这可以简化为以下伪代码：

    float pose_initial[4]; // quaternion describing original orientation
    float g_x, g_y, g_z; // gyro rates
    float dt; // time step. The smaller the better.

    // quaternion with "pose increment", calculated from the first-order 
    // linearization of continuous rotation formula
    delta_quat = {1, 0.5*dt*g_x, 0.5*dt*g_y, 0.5*dt*g_z};
    // final orientation at start time + dt
    pose_final = quaternion_hamilton_product(pose_initial, delta_quat);

这种解决方案用于PixHawk的EKF导航滤波器（它是开源的，可以在这里查看公式）。它简单、廉价、稳定且足够准确。

单位矩阵（描述“零”旋转）等同于四元数[1 0 0 0]。您可以使用适当的转换公式获得描述其他姿势的四元数（例如，如果您有欧拉角，可以使用这个）。

注：

• 四元数遵循[w，i，j，k]表示法。
• 这些方程假设角速度以SI单位给出，即每秒弧度。

长答案

陀螺仪描述了一个物体的旋转速度，将其分解为绕正交局部轴XYZ的三个旋转速度。但是，您可以等价地将旋转速度描述为围绕某个轴的单一速率 - 在旋转体本地参考系或全局参考系中。

这三个旋转速度同时影响物体，不断改变旋转轴。

我们在此面临从连续时间实际世界切换到更简单的离散时间公式的问题，这可以通过计算机轻松解决。离散化时，我们总会引入误差。有些方法会导致更大的误差，而另一些方法则会更准确。

您的方法是将三个同时绕正交轴旋转的方法合并，对于小的积分步骤（假设小于1/1000 s，尽管这取决于应用程序），这样您就可以模拟旋转轴的连续变化。但是，这样做计算成本很高，并且随着时间步长的增加，误差会增加。

作为一种一阶线性化的替代方案，您可以将姿态增量计算为角速度梯度的小增量（还使用四元数表示）：

    quat_gyro = {0, g_x, g_y, g_z};
    q_grad    = 0.5 * quaternion_product(pose_initial, quat_gyro);
    // Important to normalize result to get unit quaternion!
    pose_final = quaternion_normalize(pose_initial + q_grad*dt); 

这种技术被用于Madgwick旋转滤波器(这里有一个实现)，对我来说效果不错。

- dunadar

---

抱歉我的数学不好。这个四元数积是哈密顿积吗？谢谢。 - Sakamoto Poteko

经过测试，是哈密尔顿积。 - Sakamoto Poteko