IMU 数据中的重力补偿

Question

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

以下是我将原始传感器值存储到名为 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 。诉。 q *

其中 q 是四元数，v 是加速度向量（请查看 quaternion-vector 乘法以获取更多详细信息* ).由于我们知道重力是局部地球参考系中的矢量 u = (0,0,g)（其中 g ≈ 9.81 m/s^2，假设IMU测量值也在m/s^2)，然后我们可以从v'

中减去这个向量

v'' = v' - u

v'' 是重力 compensated/removed 矢量。但是，它不在 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

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

IMU 数据中的重力补偿

Gravity Compensation in IMU Data

c++

quaternions