MPU9250六轴算法

[imuncle]

前段时间我研究了MPU9250的数据读取和九轴姿态解析算法，效果还不错，详情点这里。

但是今天发现九轴算法得出来的角度有两个问题，一个是波动较大，一个是会出现下图所示的斜坡。

后来我发现，应该是陀螺仪积分出来的角度和磁力计加速度计坐标转换过来的角度不一致，导致被缓慢纠正，进一步发现pitch轴和roll轴没有这个问题，只有yaw轴有，所以是磁力计的问题。

磁力计这东西吧，问题还真挺多，到了一个新地方还要重新校准一下，而且特别容易受到干扰。我采集了一段数据后发现，加速度、角速度、磁场强度，就磁场数据波动最大，这也是角度波动大的罪魁祸首。

我尝试对磁力计进行低通滤波或者均值滤波，但效果都不是很好，最后角度数值稳定下来了，但是那个斜坡还是无法消除。

没办法，暂时抛下磁力计试试六轴算法，也就是说只使用MPU9250里面的MPU6050。

六轴算法

我参考了正点原子的飞控代码，在正式总结之前先说一声正点原子牛逼！

陀螺仪数据

六轴算法中最重要的就是陀螺仪数据的处理，因为陀螺仪数据是四元数更新的根本依据。加速度能纠正pitch轴和roll轴，所以yaw轴的稳定全靠陀螺仪数据的准确。

这里主要是采集一段陀螺仪静止的数据，找到三轴数据的静差。我采集了1024个数据，先计算他们的方差，如果方差大于阈值，则表明陀螺仪不是静止的，则不进行校准，当方差满足要求时，计算样本数据的平均值，当做陀螺仪的静差。

处理过后，才静止情况下三轴的陀螺仪数据小数点后两位都是0，效果还不错。

实现代码如下，函数较多：

#define SENSORS_NBR_OF_BIAS_SAMPLES     1024    /*计算方差的采样样本个数 */
#define GYRO_VARIANCE_BASE              4000    /* 陀螺仪零偏方差阈值 */

/*计算方差和平均值*/
static void sensorsCalculateVarianceAndMean(BiasObj* bias, struct Axisf* varOut, struct Axisf* meanOut)
{
    uint32_t i;
    int64_t sum[3] = {0};
    int64_t sumsq[3] = {0};

    for (i = 0; i < SENSORS_NBR_OF_BIAS_SAMPLES; i++)
    {
        sum[0] += bias->buffer[i].x;
        sum[1] += bias->buffer[i].y;
        sum[2] += bias->buffer[i].z;
        sumsq[0] += bias->buffer[i].x * bias->buffer[i].x;
        sumsq[1] += bias->buffer[i].y * bias->buffer[i].y;
        sumsq[2] += bias->buffer[i].z * bias->buffer[i].z;
    }

    varOut->x = (sumsq[0] - ((int64_t)sum[0] * sum[0]) / SENSORS_NBR_OF_BIAS_SAMPLES);
    varOut->y = (sumsq[1] - ((int64_t)sum[1] * sum[1]) / SENSORS_NBR_OF_BIAS_SAMPLES);
    varOut->z = (sumsq[2] - ((int64_t)sum[2] * sum[2]) / SENSORS_NBR_OF_BIAS_SAMPLES);

    meanOut->x = (float)sum[0] / SENSORS_NBR_OF_BIAS_SAMPLES;
    meanOut->y = (float)sum[1] / SENSORS_NBR_OF_BIAS_SAMPLES;
    meanOut->z = (float)sum[2] / SENSORS_NBR_OF_BIAS_SAMPLES;
}

/*传感器查找偏置值*/
static int sensorsFindBiasValue(BiasObj* bias)
{
    int foundbias = 0;

    if (bias->isBufferFilled)
    {

        struct Axisf mean;
        struct Axisf variance;
        sensorsCalculateVarianceAndMean(bias, &variance, &mean);

        if (variance.x < GYRO_VARIANCE_BASE && variance.y < GYRO_VARIANCE_BASE && variance.z < GYRO_VARIANCE_BASE)
        {
            bias->bias.x = mean.x;
            bias->bias.y = mean.y;
            bias->bias.z = mean.z;
            foundbias = 1;
            bias->isBiasValueFound= 1;
        }else
            bias->isBufferFilled=0;
    }
    return foundbias;
}

/**
 * 计算陀螺方差
 */
int processGyroBias(int16_t gx, int16_t gy, int16_t gz, struct Axisf *gyroBiasOut)
{
    static int count = 0;
    gyroBiasRunning.buffer[count].x = gx;
    gyroBiasRunning.buffer[count].y = gy;
    gyroBiasRunning.buffer[count].z = gz;
    count++;
    if(count == 1024)
    {
        count = 0;
        gyroBiasRunning.isBufferFilled = 1;
    }

    if (!gyroBiasRunning.isBiasValueFound)
    {
        sensorsFindBiasValue(&gyroBiasRunning);
    }

    gyroBiasOut->x = gyroBiasRunning.bias.x;
    gyroBiasOut->y = gyroBiasRunning.bias.y;
    gyroBiasOut->z = gyroBiasRunning.bias.z;

    return gyroBiasRunning.isBiasValueFound;
}

void imuDataHandle(void)
{
    //....
    gyroBiasFound = processGyroBias(gyrox, gyroy, gyroz, &gyroBias);

    fgyrox = -(float)(gyrox-gyroBias.x)/gyro_sensitivity;
    fgyroy = (float)(gyroy-gyroBias.y)/gyro_sensitivity;
    fgyroz = (float)(gyroz-gyroBias.z)/gyro_sensitivity;
    //...
}

这里的方差计算与正常的方差计算公式有些不同，算是近似了一下吧，比较方便写程序一点。

加速度数据

加速度数据处理起来就简单很多，主要是计算一个缩放因子，直接贴上代码吧：

/**
 * 根据样本计算重力加速度缩放因子
 */
int processAccScale(int16_t ax, int16_t ay, int16_t az)
{
    static int accBiasFound = 0;
    static uint32_t accScaleSumCount = 0;

    if (!accBiasFound)
    {
        accScaleSum += sqrtf(powf((float)ax/8192, 2) + powf((float)ay/8192, 2) + powf((float)az/8192, 2));
        accScaleSumCount++;

        if (accScaleSumCount == SENSORS_ACC_SCALE_SAMPLES)
        {
            accScale = accScaleSum / SENSORS_ACC_SCALE_SAMPLES;
            accBiasFound = 1;
        }
    }

    return accBiasFound;
}

void imuDataHandle(void)
{
    //....
    if (gyroBiasFound)
    {
        processAccScale(accx, accy, accz);  /*计算accScale*/
    }

    faccx = -(float)(accx)/acc_sensitivity/accScale;
    faccy = (float)(accy)/acc_sensitivity/accScale;
    faccz = (float)(accz)/acc_sensitivity/accScale;
    //...
}

数据融合

融合算法和九轴的一样，只是少了磁力计一项，代码如下：

void imuUpdate(struct Axisf acc, struct Axisf gyro)
{
    float q0q0 = q0 * q0;
    float q1q1 = q1 * q1;
    float q2q2 = q2 * q2;
    float q3q3 = q3 * q3;

    float q0q1 = q0 * q1;
    float q0q2 = q0 * q2;
    float q0q3 = q0 * q3;
    float q1q2 = q1 * q2;
    float q1q3 = q1 * q3;
    float q2q3 = q2 * q3;

    float normalise;
    float ex, ey, ez;
    float halfT;
    float vx, vy, vz;

    now_update = HAL_GetTick(); //单位ms
    halfT = ((float)(now_update - last_update) / 2000.0f);
    last_update = now_update;

    gyro.x *= DEG2RAD;  /*度转弧度*/
    gyro.y *= DEG2RAD;
    gyro.z *= DEG2RAD;

    /* 对加速度计数据进行归一化处理 */
    if(acc.x != 0 || acc.y != 0 || acc.z != 0)
    {
        normalise = sqrt(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
        acc.x /= normalise;
        acc.y /= normalise;
        acc.z /= normalise;
    }

    /* 计算加速度计投影到物体坐标上的各个分量 */
    vx = 2.0f*(q1q3 - q0q2);
    vy = 2.0f*(q0q1 + q2q3);
    vz = q0q0 - q1q1 - q2q2 + q3q3;

    /* 叉积误差累计，用以修正陀螺仪数据 */
    ex = (acc.y*vz - acc.z*vy);
    ey = (acc.z*vx - acc.x*vz);
    ez = (acc.x*vy - acc.y*vx);

    /* 互补滤波 PI */
    exInt += ex * Ki * halfT;
    eyInt += ey * Ki * halfT;   
    ezInt += ez * Ki * halfT;
    gyro.x += Kp*ex + exInt;
    gyro.y += Kp*ey + eyInt;
    gyro.z += Kp*ez + ezInt;

    /* 使用一阶龙格库塔更新四元数 */
    q0 += (-q1 * gyro.x - q2 * gyro.y - q3 * gyro.z) * halfT;
    q1 += ( q0 * gyro.x + q2 * gyro.z - q3 * gyro.y) * halfT;
    q2 += ( q0 * gyro.y - q1 * gyro.z + q3 * gyro.x) * halfT;
    q3 += ( q0 * gyro.z + q1 * gyro.y - q2 * gyro.x) * halfT;

    /* 对四元数进行归一化处理 */
    normalise = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 /= normalise;
    q1 /= normalise;
    q2 /= normalise;
    q3 /= normalise;

    /* 由四元数求解欧拉角 */
    mpu9250.attitude.x = -asinf(-2*q1*q3 + 2*q0*q2) * RAD2DEG;  //pitch
    mpu9250.attitude.y = atan2f(2*q2*q3 + 2*q0*q1, -2*q1*q1 - 2*q2*q2 + 1) * RAD2DEG;   //roll
    mpu9250.attitude.z = atan2f(2*q1*q2 + 2*q0*q3, -2*q2*q2 - 2*q3*q3 + 1) * RAD2DEG;   //yaw
}

最终效果

后话

后来我兴致冲冲地把六轴算法移植到步兵车上，结果云台还是响应速度慢，摆尾动作云台稳不住。

[imuncle]

找到问题所在了，我的云台速度环反馈值还是电机的转速，但其实应该使用MPU9250的陀螺仪数据，这两者是不一样的。

更正反馈值之后云台就老稳了！

[Benjemin1010]

我六轴计算出来的yaw角都会有漂移，不管怎么滤波都会有，请问您遇见过吗，做了什么样的处理呢

[imuncle]

@Benjemin1010 yaw轴漂移主要是因为角速度有误差，静置的时候角速度值不为0。我每次上电后将陀螺仪静置几秒钟，让陀螺仪采集静态时三轴角速度的静差，然后把这个静差减掉。最后结果看来基本没有漂移。

如果还有漂移可以考虑给角速度设置一个死区，比如：

if(gyro.x < 0.1 && gyro.x > -0.1)
    gyro.x = 0;

这样可以缓解零漂问题

[Benjemin1010]

我现在是采用的陀螺仪静置十几秒采集一个静差，在机体静止的情况下确实漂移很小，但是一旦长时间运动再加上温漂yaw角就会漂移较大，还在摸索解决方案中。。。不过设置角速度死区确实没有想到，先去尝试一下，非常感谢

[Jason0-0]

老板同问....最近C板的陀螺仪也有这个问题，主要体现在往一个方向转的时候陀螺仪给出的角位移有时候甚至超出了设定值，导致它在某个时候会停转并且往回转，严重的时候甚至会疯转。这个是应该往pid的方向想还是陀螺仪的问题呢？

[imuncle]

@Jason0-0 不太清楚你这个问题的描述，QQ细聊

[huoxingdawang]

你好，为什么我的陀螺仪平放在桌子上转yaw轴还挺稳定的，然后拿起来晃一晃再放回去yaw轴就开始疯狂漂移，请问您遇到过这种情况吗？

[imuncle]

@huoxingdawang 遇到过，这个是真没办法，本质上还是陀螺仪太菜了，晃了之后陀螺仪的噪声方差就变了

[huoxingdawang]

啊这，有不那么菜的陀螺仪推荐吗？或者上9轴用磁力计修正？

[1198171877]

最近我发现一个问题，yaw轴会在一个很大的值收敛，话说这个是我pid的问题还是我用的问题，求大佬解答