IMU数据中的重力补偿

Question

我正在尝试对我的加速度计数据进行重力补偿。给定一个具有 6 DOF（加速计、陀螺仪）的加速计，我想消除/补偿加速计读数中重力的影响（加速计可以自由旋转）。

struct以下是我将原始传感器值存储到被调用的方法sample：

uint8_t *p=data; // p is a pointer to the sensor data

    int i;
    for(i=0; i<4; i++)  // quaternion
    {
        sample.quaternion[i]=((float)get_int32(p))/(1<<29);
        len+=snprintf(s+len, sizeof(line)-len, "\t%9.6f", sample.quaternion[i]);
        p+=4;
    }

    for(i=0; i<3; i++) // euler213_degrees
    {
        sample.euler213_degrees[i]=get_int16(p);
        len+=snprintf(s+len, sizeof(line)-len, "\t%d", sample.euler213_degrees[i]);
        p+=2;
    }

    for(i=0; i<3; i++) // euler123_degrees
    {
        sample.euler123_degrees[i]=get_int16(p);
        len+=snprintf(s+len, sizeof(line)-len, "\t%d", sample.euler123_degrees[i]);
        p+=2;
    }

    for(i=0; i<3; i++) // acceleration_g
    {
        sample.acceleration_g[i]=(2.0*get_int16(p))/(1<<15);
        len+=snprintf(s+len, sizeof(line)-len, "\t%6.3f", sample.acceleration_g[i]);
        p+=2;
    }

    for(i=0; i<3; i++) // gyroscope_dps
    {
        sample.gyroscope_dps[i]=(2000.0*get_int16(p))/(1<<15);
        len+=snprintf(s+len, sizeof(line)-len, "\t%6.1f", sample.gyroscope_dps[i]);
        p+=2;
    }

您能告诉我一种获取重力补偿加速度计数据的方法吗？

Answer 1

IMU（6 DOF 设备）计算的四元数基本上是设备相对于本地地球参考系的姿态（3D 方向）；因此它可以用于将加速度测量从本地身体参考系（IMU直接测量的值）旋转到本地地球参考系（xy平面与地面相切的坐标系）。您可以使用四元数乘法来执行此操作

\n

v\' = q 。v. q *

\n

其中q是四元数，v是加速度向量（请查找四元数向量乘法以了解更多详细信息*）。因为我们知道重力是本地地球参考系中的向量u = ( 0,0,g ) （其中 g \xe2\x89\x88 9.81 m/s^2，假设 IMU 测量值也以 m/s 为单位） ^2)，然后我们可以从v\'中减去这个向量

\n

v\'\' = v\' - u

\n

v\'\'是重力补偿/移除矢量。但是，它不在 IMU 的本地身体参考系中（记住我们将其旋转到本地地球参考系）。因此，要将其转换回来，您可以将其乘以逆四元数

\n

v\'\'\' = q-1 。v\'\' 。q-1 *

\n

v\'\'\'将是原始加速度测量值减去重力。为了确认结果正确，您可以在设备静止时进行测量。无论设备如何旋转，加速度测量值都应接近 ( 0,0,0 )。

\n\n

如果 IMU 不提供四元数测量，则可以通过加速度计和陀螺仪读数的融合来计算（这正是 IMU 正在做的事情）。如果您对此感兴趣，我建议查找 Madgwick、Mahony 或扩展卡尔曼滤波器算法来进行姿态估计。

\n\n

*有用的资源

\n

四元数乘法：https://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/arithmetic/index.htm

\n

用四元数旋转向量（如果您想查看一些代码，尤其是第三个答案）： https: //math.stackexchange.com/questions/40164/how-do-you-rotate-a-vector-by -a-单位四元数

\n