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

我正在尝试对加速度计数据进行重力补偿。给定一个具有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;
    }

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

最佳答案

由IMU(6自由度设备)计算的四元数基本上是相对于本地地球引用系的设备姿态(3D方向)。因此，它可用于将加速度测量值从本地 body 引用系(IMU直接测量的值)旋转到本地地球引用系(x-y平面与地面相切的坐标系)。您可以使用四元数乘法来执行此操作

v'= q。 v。 *

其中q是四元数，v是加速度 vector (请查看四元数 vector 乘法以获取更多详细信息*)。由于我们知道重力是局部地球引用系中的 vector u =(0,0，g)(其中，假设IMU测量值也以m / s ^ 2表示，其中g≈9.81 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或Extended Kalman滤波器算法以进行姿态估计。

*有用的资源

四元数乘法: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

imu

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