EEPW论坛

赞一个

一个就够了，

赞一个

传感器数据融合

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

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

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

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

条理清晰，加油。

今天手贱，想把link的几个母头用502粘到一起，结果胶水多了，把插口都塞满了，暂时用不了了，找机会重新做插头了。

昨晚上花了两个小时才把st-link的插口重新做好，第一次做完了后发现母头比以前的大一点点，如果以前的是2.54的话，第一次做的可能就是2.6的了，总之大了那么一点，只好重新剪掉重新做了。真是郁闷，发现做这个挺麻烦的，不知道工厂里是怎么大批量做的。

今天看了"让四轴飞"网友的工程，发现程序还真是简洁，基本上一个模块一个.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;

1、第一个函数

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; } }

其中IMU_NBR_OF_BIAS_SAMPLES = 128，此函数易读，即向一个bias结构体的长128数组中填充数据，并更新下一个待填充数据的地址。第一次填充完128个数据后，isBufferFilled成员即更改为TRUE。

3、 imuFindBiasValue(&gyroBias);

函数原型：

/** * 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; }

该函数在BiasObj结构体的128长的数组没填充完时不处理即返回False

在填充完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; }

其中GYRO_NBR_OF_AXES = 3，IMU_NBR_OF_BIAS_SAMPLES = 128。

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

。

meanOut为128个数的均值。

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

4、 imuCalculateBiasMean(&accelBias, &mean);

该函数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; }

看函数注释，函数对滤波后的加速度值做X轴和Y轴方向上进行补偿。

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。

综合上面几个步骤，该函数imu6Read的功能为对从mpu6050读取到的数值进行融合处理。从中可以看出，Crazyflie对数据的处理还是比较周到的，包括加速度的iir滤波，对加速度和角速度偏移的求取，加速度和角速度的修正，角速度对加速度的反馈等，而匿名四轴的程序暂时只看到对加速度进行了滑动窗口形式的滤波。