matrix - 如何使用原始陀螺仪数据°/s来计算3D旋转？

Question

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

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

那么如何正确计算 3D 旋转呢？ 我需要吃四元吗？但如何从 3 个不同的轮换中获得一个呢？我这里不是又遇到同样的问题了吗？

我从单位矩阵 ((1, 0, 0)(0, 1, 0)(0, 0, 1)) 乘以加速度向量开始，得到第一个运动。 然后我想使用旋转矩阵来找出下一个加速度的真正方向，这样我就可以简单地将加速度加在一起。

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

有什么建议吗？ 顺便提一句。抱歉我的英语很差，我很累而且(显然)不是母语人士；)

谢谢， 亚历克斯

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's EKF navigation filter (它是开源的，check out formulation here)。它简单、便宜、稳定且足够准确。

单位矩阵(描述“空”旋转)相当于四元数[1 0 0 0]。您可以使用合适的转换公式获得描述其他姿势的四元数(例如，如果您有欧拉角，则可以使用 this one )。

注释:

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

长答案

陀螺仪将物体的旋转速度描述为绕正交局部轴 XYZ 的三个旋转速度的分解。但是，您可以等效地将旋转速度描述为围绕某个轴的单个速率 - 无论是在旋转体的本地引用系中还是在全局引用系中。

三种旋转速度同时作用于 body ，不断改变旋转轴。

这里我们面临从连续时间现实世​​界切换到可以使用计算机轻松解决的更简单的离散时间公式的问题。离散化时，我们总是会引入错误。有些方法会导致更大的错误，而另一些方法会明显更准确。

围绕正交轴连接三个同时旋转的方法可以在较小的积分步长(假设小于 1/1000 秒，尽管这取决于应用程序)的情况下工作得相当好，因此您可以模拟旋转轴的连续变化。然而，这在计算上是昂贵的，并且随着时间步长的增大，错误也会增加。

作为一阶线性化的替代方法，您可以将位姿增量计算为角速度梯度的小增量(也使用四元数表示):

    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 rotation filter ( here an implementation )，对我来说效果很好。