EEPW论坛

传感器数据融合

MPU6050有了驱动之后就可以考虑数据融合了。简单来说，陀螺仪精度高，但时间长了会有漂移，加速度动态精度差，但没有长期漂移。综合利用陀螺仪和加速度计的特点，优势互补获得准确的姿态角度，方法就是用卡尔曼滤波做数据融合。

Snake0301使用的是MPU6050内置的DMP，直接输出姿态数据，使用方法网上有例程，可见这里：

https://gist.github.com/jannson/11280208

但是很多人说用DMP后Z轴会有漂移，Snake0301也有反映，且DMP的输出速率不是很高，最快200Hz，所以准备舍弃这种方法，还是中规中矩的用滤波吧。

今天看了"让四轴飞"网友的工程，发现程序还真是简洁，基本上一个模块一个.c文件即可实现，总共才那么六七个模块，重新移植修改起来就很方便了。反观crazyfliie的工程，代码量多了好几倍，又加入了操作系统FreeRTOS，真担心MCU的处理能力，不过看他们的视频，还是很有信心的。

附链接地址：

http://v.youku.com/v_show/id_XNTEwODM4MzEy/v.swf.html

上面分析了Crazyflie的acc采样数值的滤波，接着分析其他函数吧。上代码

void imu6Read(Axis3f* gyroOut, Axis3f* accOut)
{
  mpu6050GetMotion6(&accelMpu.x, &accelMpu.y, &accelMpu.z, &gyroMpu.x, &gyroMpu.y, &gyroMpu.z);

  imuAddBiasValue(&gyroBias, &gyroMpu);
  if (!accelBias.isBiasValueFound)
    imuAddBiasValue(&accelBias, &accelMpu);
  if (!gyroBias.isBiasValueFound)
    imuFindBiasValue(&gyroBias);

#ifdef IMU_TAKE_ACCEL_BIAS
  if (gyroBias.isBiasValueFound &&
      !accelBias.isBiasValueFound)
  {
    Axis3i32 mean;

    imuCalculateBiasMean(&accelBias, &mean);
    accelBias.bias.x = mean.x;
    accelBias.bias.y = mean.y;
    accelBias.bias.z = mean.z - IMU_1G_RAW;
    accelBias.isBiasValueFound = TRUE;
//    uartPrintf("Accel bias: %i, %i, %i\n",
//                accelBias.bias.x, accelBias.bias.y, accelBias.bias.z);
  }
#endif

  imuAccIIRLPFilter(&accelMpu, &accelLPF, &accelStoredFilterValues,
                    (int32_t)imuAccLpfAttFactor);

  imuAccAlignToGravity(&accelLPF, &accelLPFAligned);

  // Re-map outputs
  gyroOut->x = (gyroMpu.x - gyroBias.bias.x) * IMU_DEG_PER_LSB_CFG;
  gyroOut->y = (gyroMpu.y - gyroBias.bias.y) * IMU_DEG_PER_LSB_CFG;
  gyroOut->z = (gyroMpu.z - gyroBias.bias.z) * IMU_DEG_PER_LSB_CFG;
  accOut->x = (accelLPFAligned.x - accelBias.bias.x) * IMU_G_PER_LSB_CFG;
  accOut->y = (accelLPFAligned.y - accelBias.bias.y) * IMU_G_PER_LSB_CFG;
  accOut->z = (accelLPFAligned.z - accelBias.bias.z) * IMU_G_PER_LSB_CFG;
//  uartSendData(sizeof(Axis3f), (uint8_t*)gyroOut);
//  uartSendData(sizeof(Axis3f), (uint8_t*)accOut);
}

typedef struct
{
  int16_t x;
  int16_t y;
  int16_t z;
} Axis3i16;

typedef struct
{
  int32_t x;
  int32_t y;
  int32_t z;
} Axis3i32;

typedef struct
{
  float x;
  float y;
  float z;
} Axis3f;

typedef struct
{
  Axis3i16   bias;
  bool       isBiasValueFound;
  bool       isBufferFilled;
  Axis3i16*  bufHead;
  Axis3i16   buffer[IMU_NBR_OF_BIAS_SAMPLES];
} BiasObj;

BiasObj    gyroBias;
BiasObj    accelBias;
int32_t    varianceSampleTime;
Axis3i16   gyroMpu;
Axis3i16   accelMpu;
Axis3i16   accelLPF;
Axis3i16   accelLPFAligned;
Axis3i16   mag;
Axis3i32   accelStoredFilterValues;
uint8_t    imuAccLpfAttFactor;

  mpu6050GetMotion6(&accelMpu.x, &accelMpu.y, &accelMpu.z, &gyroMpu.x, &gyroMpu.y, &gyroMpu.z);

从MPU6050读取6个数值，不表。

2、imuAddBiasValue(&gyroBias, &gyroMpu);

函数原型：

/**
 * Adds a new value to the variance buffer and if it is full
 * replaces the oldest one. Thus a circular buffer.
 */
static void imuAddBiasValue(BiasObj* bias, Axis3i16* dVal)
{
  bias->bufHead->x = dVal->x;
  bias->bufHead->y = dVal->y;
  bias->bufHead->z = dVal->z;
  bias->bufHead++;

  if (bias->bufHead >= &bias->buffer[IMU_NBR_OF_BIAS_SAMPLES])
  {
    bias->bufHead = bias->buffer;
    bias->isBufferFilled = TRUE;
  }
}

函数原型：

/**
 * Checks if the variances is below the predefined thresholds.
 * The bias value should have been added before calling this.
 * @param bias  The bias object
 */
static bool imuFindBiasValue(BiasObj* bias)
{
  bool foundBias = FALSE;

  if (bias->isBufferFilled)
  {
    Axis3i32 variance;
    Axis3i32 mean;

    imuCalculateVarianceAndMean(bias, &variance, &mean);

    //uartSendData(sizeof(variance), (uint8_t*)&variance);
    //uartSendData(sizeof(mean), (uint8_t*)&mean);
    //uartPrintf("%i, %i, %i", variance.x, variance.y, variance.z);
    //uartPrintf("%i, %i, %i\n", mean.x, mean.y, mean.z);

    if (variance.x < GYRO_VARIANCE_THRESHOLD_X &&
        variance.y < GYRO_VARIANCE_THRESHOLD_Y &&
        variance.z < GYRO_VARIANCE_THRESHOLD_Z &&
        (varianceSampleTime + GYRO_MIN_BIAS_TIMEOUT_MS < xTaskGetTickCount()))
    {
      varianceSampleTime = xTaskGetTickCount();
      bias->bias.x = mean.x;
      bias->bias.y = mean.y;
      bias->bias.z = mean.z;
      foundBias = TRUE;
      bias->isBiasValueFound = TRUE;
    }
  }

  return foundBias;
}

在填充完128个值后做如下处理：

imuCalculateVarianceAndMean(bias, &variance, &mean);

函数原型：

/**
 * Calculates the variance and mean for the bias buffer.
 */
static void imuCalculateVarianceAndMean(BiasObj* bias, Axis3i32* varOut, Axis3i32* meanOut)
{
  uint32_t i;
  int32_t sum[GYRO_NBR_OF_AXES] = {0};
  int64_t sumSq[GYRO_NBR_OF_AXES] = {0};

  for (i = 0; i < IMU_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]) / IMU_NBR_OF_BIAS_SAMPLES);
  varOut->y = (sumSq[1] - ((int64_t)sum[1] * sum[1]) / IMU_NBR_OF_BIAS_SAMPLES);
  varOut->z = (sumSq[2] - ((int64_t)sum[2] * sum[2]) / IMU_NBR_OF_BIAS_SAMPLES);

  meanOut->x = sum[0] / IMU_NBR_OF_BIAS_SAMPLES;
  meanOut->y = sum[1] / IMU_NBR_OF_BIAS_SAMPLES;
  meanOut->z = sum[2] / IMU_NBR_OF_BIAS_SAMPLES;

  isInit = TRUE;
}

可以看出varOut的每一项(x,y,z)为该项的方差

。

meanOut为128个数的均值。

若计算出的3个方差小于设定值，且当前时间距上次采样时间小于1ms，则更新3个偏移值，并更新BiasObj的isBiasValueFound成员。

该函数Calculates the mean for the bias buffer.

5、  imuAccIIRLPFilter(&accelMpu, &accelLPF, &accelStoredFilterValues,
                    (int32_t)imuAccLpfAttFactor);

该函数上楼已经分析了，pass。

6、imuAccAlignToGravity(&accelLPF, &accelLPFAligned);

// Pre-calculated values for accelerometer alignment
float cosPitch;
float sinPitch;
float cosRoll;
float sinRoll;

	

/**
 * Compensate for a miss-aligned accelerometer. It uses the trim
 * data gathered from the UI and written in the config-block to
 * rotate the accelerometer to be aligned with gravity.
 */
static void imuAccAlignToGravity(Axis3i16* in, Axis3i16* out)
{
  Axis3i16 rx;
  Axis3i16 ry;

  // Rotate around x-axis
  rx.x = in->x;
  rx.y = in->y * cosRoll - in->z * sinRoll;
  rx.z = in->y * sinRoll + in->z * cosRoll;

  // Rotate around y-axis
  ry.x = rx.x * cosPitch - rx.z * sinPitch;
  ry.y = rx.y;
  ry.z = -rx.x * sinPitch + rx.z * cosPitch;

  out->x = ry.x;
  out->y = ry.y;
  out->z = ry.z;
}

7、最终结果

  // Re-map outputs
  gyroOut->x = (gyroMpu.x - gyroBias.bias.x) * IMU_DEG_PER_LSB_CFG;
  gyroOut->y = (gyroMpu.y - gyroBias.bias.y) * IMU_DEG_PER_LSB_CFG;
  gyroOut->z = (gyroMpu.z - gyroBias.bias.z) * IMU_DEG_PER_LSB_CFG;
  accOut->x = (accelLPFAligned.x - accelBias.bias.x) * IMU_G_PER_LSB_CFG;
  accOut->y = (accelLPFAligned.y - accelBias.bias.y) * IMU_G_PER_LSB_CFG;
  accOut->z = (accelLPFAligned.z - accelBias.bias.z) * IMU_G_PER_LSB_CFG;

对陀螺仪的数据去偏差后赋值给gyroOut单位为度/s，对补偿后的加速度值赋值给accOut，单位为G。

前面介绍了crazyflie的数据融合的处理方法，这里对mpu6050的数据按照这种方法处理，得到以下的运行结果。

刚启动时的结果：

稳态建立后的结果为：

上面的数据为加速度的数据，单位为G。可以看到处理后的数据精度比较高，在千分之2左右，效果还是比较好的。

Mahony的算法的解读

    ------本文参考于http://www.eepw.com.cn/article/247809.htm

    本帖目的是通过分析函数，学习四元数的算法，就是新闻里说的“消化吸收再创新”，主要是前两个，至于再创新那就不一定有这个本事了。

    本文将分析一种常见的四轴飞行器姿态解算方法，Mahony的互补滤波法。此法简单有效，希望能给学习四轴飞行器的朋友们带来帮助。关于姿态解算和滤波的理论知识，推荐秦永元的两本书，一是《惯性导航》，目前已出到第二版了;二是《卡尔曼滤波与组合导航原理》。程序中的理论基础，可在书中寻找。

经常在算法中碰到IMU和AHRS这两个词，关于这两个词的具体含义与区别，这个地址有讨论：http://www.douban.com/note/75915865/，不懂得人可以稍微了解一下。不说别的，直奔主题。

下面开始进入正题：


　　先定义Kp，Ki，以及halfT 。
   　Kp，Ki，控制加速度计修正陀螺仪积分姿态的速度，halfT ，姿态解算时间的一半。此处解算姿态速度为500HZ，因此halfT 为0.001

　　#define Kp 2.0f
　　#define Ki 0.002f
　　#define halfT 0.001f

　　初始化四元数
　　float q0 = 1, q1 = 0, q2 = 0, q3 = 0;

　　定义姿态解算误差的积分
　　float exInt = 0, eyInt = 0, ezInt = 0;

　　以下为姿态解算函数。
　　参数gx，gy，gz分别对应三个轴的角速度，单位是弧度/秒;
　　参数ax，ay，az分别对应三个轴的加速度原始数据
　　由于加速度的噪声较大，此处应采用滤波后的数据

　　void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
　　{
　　float norm;
　　float vx, vy, vz;
　　float ex, ey, ez;

　　将加速度的原始数据，归一化，得到单位加速度
　　norm = sqrt(ax*ax + ay*ay + az*az);
　　ax = ax / norm;
　　ay = ay / norm;
　　az = az / norm;

　　把四元数换算成“方向余弦矩阵”中的第三列的三个元素。根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系，正好是这三个元素。所以这里的vx、vy、vz，其实就是当前的机体坐标参照系上，换算出来的重力单位向量。(用表示机体姿态的四元数进行换算)

　　vx = 2*(q1*q3 - q0*q2);
　　vy = 2*(q0*q1 + q2*q3);
　　vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

　　这里说明一点，加速度计由于噪声比较大，而且在飞行过程中，受机体振动影响比陀螺仪明显，短时间内的可靠性不高。陀螺仪噪声小，但是由于积分是离散的，长时间的积分会出现漂移的情况，因此需要将用加速度计求得的姿态来矫正陀螺仪积分姿态的漂移。

　　在机体坐标参照系上，加速度计测出来的重力向量是ax、ay、az;陀螺积分后的姿态来推算出的重力向量是vx、vy、vz;它们之间的误差向量，就是陀螺积分后的姿态和加速度计测出来的姿态之间的误差。

　　向量间的误差，可以用向量积(也叫外积、叉乘)来表示，ex、ey、ez就是两个重力向量的叉积。这个叉积向量仍旧是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系，而且叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。由于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。

　　叉乘是数学基础，维基百科里有详细解释。
　　ex = (ay*vz - az*vy);
　　ey = (az*vx - ax*vz);
　　ez = (ax*vy - ay*vx);

　　将叉乘误差进行积分
　　exInt = exInt + ex*Ki;
　　eyInt = eyInt + ey*Ki;
　　ezInt = ezInt + ez*Ki;

　　用叉乘误差来做PI修正陀螺零偏，通过调节Kp，Ki两个参数，可以控制加速度计修正陀螺仪积分姿态的速度
　　gx = gx + Kp*ex + exInt;
　　gy = gy + Kp*ey + eyInt;
　　gz = gz + Kp*ez + ezInt;

　　四元数微分方程，没啥好说的了，看上面推荐的书吧，都是理论的东西，自个琢磨琢磨

　　实在琢磨不明白，那就把指定的参数传进这个函数，再得到相应的四元数，最后转化成欧拉角即可了。不过建议还是把理论弄清楚一点。

   　q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
　　q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
　　q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
　　q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;

　　四元数单位化
　　norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
　　q0 = q0 / norm;
　　q1 = q1 / norm;
　　q2 = q2 / norm;
　　q3 = q3 / norm;
　　}

　　姿态解算后，就得到了表示姿态的四元数。