c++ - IMU数据中的重力补偿

Question

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

以下是我将原始传感器值存储到名为 sample 的 struct 的方法:

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 方向)；因此它可以用于将加速度测量从本地 body 引用系(IMU直接测量的值)旋转到本地地球引用系(x-y平面与地面相切的坐标系)。您可以使用四元数乘法来执行此操作

v' = q 。 v. q *

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

中减去该 vector

v'' = v' - u

v'' 是重力补偿/移除 vector 。但是，它不在 IMU 的本地 body 引用系中(记住我们将其旋转到本地地球引用系)。因此，要将其转换回来，您可以将其乘以逆四元数

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

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

---

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

---

*有用的资源

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

用四元数旋转 vector (如果您想查看一些代码，尤其是第三个答案):https://math.stackexchange.com/questions/40164/how-do-you-rotate-a-vector-by-a-unit-quaternion