博彩问答 MPU6050陀螺仪加速度计教程基于stm32数据的获取、分析

第二次修改：更新了卡尔曼滤波的介绍
第一次修改：更新了DMP的使用介绍
另外提示大家获取数据，计算角度等过程放到中断里，dt就是中断时间。
在使用互补滤波计算时，后一次的数据和前一次有关，所以如果上电时第一次数据出错会导致后面的数据一直是错的。解决方法：上电后延时一段时间再工作，最好不要用互补滤波
以下是原文：


第一次写知乎，如有不当之处请多谅解，有错误也请大家及时指出。本文介绍一下我学习MPU6050这款传感器的使用方法和我学习过程中的一些心得，主要定位于新手入门，如果你想理解本文的内容需要提前掌握单片机的基本知识，尤其是I2C总线通信，在这里就不过多介绍。希望我的介绍是条理清晰的，使初学者易于理解。
题主在学习的过程中参考了以下几篇文章，分享给大家：
这篇文章从Arduino的角度讲解了MPU6050，是我看过觉得最详细易懂的教程
这篇文章中数据处理的部分很有帮助
正式开始之前有几点需要注意：1.首先要确认手中的MPU6050模块是可以用的（后面会介绍怎么判断数据有没有问题），我就买到过有问题的模块，读出的数据是错误的，该模块某宝价格在10元左右（也有处理好数据直接串口输出角度的模块，不过非常贵）。



2.要了解使用该传感器获取数据、设置传感器实质上是通过I2C通信读取/写入寄存器的值。
3.题主使用的微控制器是stm32f103c8t6。
好了，终于可以开始正题了。
要使用该传感器分以下四个步骤：
1.配置好stm32的GPIO、I2C等功能。
2.初始化MPU6050（写入寄存器相关值）。
3.读取原始数据（读取寄存器值）。
4.处理数据，包括滤波（互补滤波、卡尔曼滤波、读取DMP的值转换）、计算角度等等。


第一步：初始化GPIO、I2C
这个部分不详细讲，这是使用stm32的基本功。
void I2C_config(void) { //定义结构体 GPIO_InitTypeDef GPIO_InitStructure; I2C_InitTypeDef I2C_InitStructure; //开启时钟 RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1,ENABLE); //初始化GPIO GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD; GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_Init(GPIOB, &GPIO_InitStructure); //初始化I2C，开启I2C1 I2C_InitStructure.I2C_Mode = I2C_Mode_I2C ; I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2; I2C_InitStructure.I2C_OwnAddress1 = SlaveAddress; I2C_InitStructure.I2C_Ack = I2C_Ack_Enable; I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; I2C_InitStructure.I2C_ClockSpeed = 100000; I2C_Init(I2C1, &I2C_InitStructure); I2C_Cmd(I2C1, ENABLE); I2C_AcknowledgeConfig(I2C1, ENABLE); }这里也给出通过读写MPU6050寄存器的函数：
其中SlaveAddress是I2C从设备的地址，后面通过宏定义定义为MPU6050的地址。
void I2C_WriteByte(uint8_t REG_Address,uint8_t REG_data) { I2C_GenerateSTART(I2C1,ENABLE); while(!I2C_CheckEvent(I2C1,I2C_EVENT_MASTER_MODE_SELECT)); I2C_Send7bitAddress(I2C1,SlaveAddress,I2C_Direction_Transmitter); while(!I2C_CheckEvent(I2C1,I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)); I2C_SendData(I2C1,REG_Address); while(!I2C_CheckEvent(I2C1,I2C_EVENT_MASTER_BYTE_TRANSMITTED)); I2C_SendData(I2C1,REG_data); while(!I2C_CheckEvent(I2C1,I2C_EVENT_MASTER_BYTE_TRANSMITTED)); I2C_GenerateSTOP(I2C1,ENABLE); } uint8_t I2C_ReadByte(uint8_t REG_Address) { uint8_t REG_data; while(I2C_GetFlagStatus(I2C1,I2C_FLAG_BUSY)); I2C_GenerateSTART(I2C1,ENABLE);//起始信号 while(!I2C_CheckEvent(I2C1,I2C_EVENT_MASTER_MODE_SELECT)); I2C_Send7bitAddress(I2C1,SlaveAddress,I2C_Direction_Transmitter);//发送设备地址+写信号 while(!I2C_CheckEvent(I2C1,I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));// I2C_Cmd(I2C1,ENABLE); I2C_SendData(I2C1,REG_Address);//发送存储单元地址，从0开始 while(!I2C_CheckEvent(I2C1,I2C_EVENT_MASTER_BYTE_TRANSMITTED)); I2C_GenerateSTART(I2C1,ENABLE);//起始信号 while(!I2C_CheckEvent(I2C1,I2C_EVENT_MASTER_MODE_SELECT)); I2C_Send7bitAddress(I2C1,SlaveAddress,I2C_Direction_Receiver);//发送设备地址+读信号 while(!I2C_CheckEvent(I2C1,I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED)); I2C_AcknowledgeConfig(I2C1,DISABLE); I2C_GenerateSTOP(I2C1,ENABLE); while(!(I2C_CheckEvent(I2C1,I2C_EVENT_MASTER_BYTE_RECEIVED))); REG_data=I2C_ReceiveData(I2C1);//读出寄存器数据 return REG_data; }

第二步：初始化MPU6050
之前介绍过，使用MPU6050实际上就是配置寄存器，这里给出常见的寄存器地址：
#define SMPLRT_DIV 0x19 //陀螺仪输出率的分频，典型值0x07,(1kHz) #define CONFIG 0x1A //低通滤波频率,一般0x01~0x05,数值越大带宽越小延时越长 #define GYRO_CONFIG 0x1B #define ACCEL_CONFIG 0x1C #define ACCEL_XOUT_H 0x3B //加速度计X轴数据高位 #define ACCEL_XOUT_L 0x3C //加速度计X轴数据低位 #define ACCEL_YOUT_H 0x3D //以此类推 #define ACCEL_YOUT_L 0x3E #define ACCEL_ZOUT_H 0x3F #define ACCEL_ZOUT_L 0x40 #define TEMP_OUT_H 0x41 //温度传感器数据 #define TEMP_OUT_L 0x42 #define GYRO_XOUT_H 0x43 //陀螺仪X轴数据高位 #define GYRO_XOUT_L 0x44 #define GYRO_YOUT_H 0x45 #define GYRO_YOUT_L 0x46 #define GYRO_ZOUT_H 0x47 #define GYRO_ZOUT_L 0x48 #define PWR_MGMT_1 0x6B //电源管理，典型值：0x00(正常启用) #define WHO_AM_I 0x75 //IIC地址寄存器(默认数值0x68，只读) #define SlaveAddress 0xD0 //MPU6050模块AD0引脚接低电平时的地址GYRO_CONFIG寄存器：陀螺仪自检及测量范围，一般0x18(不自检，量程2000度/s)，其他量程分别为250度/s博彩问答，500度/s博彩问答，1000度/s，对应的值分别为0x00，0x08，0x10。
ACCEL_CONFIG寄存器：加速计自检、测量范围，一般不自检，四种量程2g，4g，8g，16g，对应的值分别为0x00，0x08，0x10，0x18。
量程越大，测量的范围越大，精度越低。
0x3B到0x48这14个寄存器里存储的数据就是我们做关心的传感器测量值，会实时更新，我们只需要定时读取其中的数据就行。前6个为加速度计的测量值，后6个为陀螺仪的测量值，中间两个为温度测量值，每个数据有两个字节组成，读取完数据后需要我们合成。下面给出读取数据以及合成数据的函数：
unsigned int GetData(unsigned char REG_Address) { char H,L; H=I2C_ReadByte(REG_Address); L=I2C_ReadByte(REG_Address+1); return (H<<8)+L; //合成数据 }使用MPU6050前要初始化MPU，下面给出初始化函数：
void InitMPU6050(void) { I2C_WriteByte(PWR_MGMT_1,0x00); //解除休眠状态 I2C_WriteByte(SMPLRT_DIV,0x07); //陀螺仪采样率1kHz I2C_WriteByte(CONFIG,0x02); //设置低通滤波器 I2C_WriteByte(GYRO_CONFIG,0x18); //陀螺仪量程2000deg/s I2C_WriteByte(ACCEL_CONFIG,0x08); //加速度量程4g }更多寄存器的地址和功能见数据手册，这里初学暂时用不到。
第三步：读取原始数据
上面已经给出了读取函数，只要定义一个数组，调用GetData函数就可以了。
int16_t acc1[3]={0}; int16_t gyr1[3]={0}; void GetAccGyro(void)//读取6轴数据 { acc1[0] = GetData(ACCEL_XOUT_H); acc1[1] = GetData(ACCEL_YOUT_H); acc1[2] = GetData(ACCEL_ZOUT_H); gyr1[0] = GetData(GYRO_XOUT_H); gyr1[1] = GetData(GYRO_YOUT_H); gyr1[2] = GetData(GYRO_ZOUT_H); }我们可以通过串口打印的方式查看这些数据，这里不详细介绍。下面重点说一下怎么判断原始数据是否正确。
我手中的这个陀螺仪模块输出的数据是 signed int16的格式，也就是每个数据的范围是-32768 ~ +32767。以acc1[0]的数据为例，若加速度量程初始化为4g，若acc1[0]=0，说明X轴方向的加速度为0，若acc1[0]=32767，则X轴方向的加速度为4g，若acc1[0]=-32768，则X轴负方向的加速度为4g，若acc1[0]=16384，则X轴方向的加速度为2g，以此类推。所以我们可以算出该量程下加速度计的精度为32768/4g = 8192 LSB/g。这个数据后面要用到，其他量程的计算公式类似。类似的，若gyr1[0] = 0，说明沿X轴转动的角速度为0。陀螺仪在2000度/s的量程下精度为32768/2000 = 16.384 LSB/°/s。
所以，在传感器水平静止的放在桌面上时，陀螺仪角速度的值应该为零，加速度计的X轴，Y轴的值应该为0，Z轴的值应该为8192（重力加速度g，1/4量程）。当然我们不能保证传感器的位置是完全水平的，传感器测量的数据也有一定的噪声，一般来说，水平静止时的值在理论值±几百之内是合理的，说明传感器没有问题。接下来可以简单尝试动态测量，向X轴正方向加速运动，加速度计X轴的值增大，沿X轴加速旋转，陀螺仪X轴的值增大，以此类推。
需要注意的是，acc1和gyr1两个数组一点要定义成16位有符号整数的格式，因为库函数读出的数据是无符号整数，在赋值的时候被转换成有符号整数。



第四步：处理数据
我们使用MPU6050主要是想计算出各方向的角度，这里就不得不说一下原理，在文章开头推荐的Arduino那篇文章里有详细介绍，这里简单说明。
一、先说加速度计，加速度计测量的是三个方向的加速度，我们测量角度是在假设传感器静止（原地旋转）的情况下计算的角度，在传感器运动过程中测量的数据是有误差的，所以我们需要融合陀螺仪和加速度计的数据得到较准确的角度。我们知道不管在什么地方都是有重力加速度的，所以只要计算重力加速度与各个轴的夹角即可算出当前角度。


假设我们要计算
，

计算机里计算反三角函数是比较麻烦的，如果实际应用中摆动角度小可以用别的函数近似替代，另外，推荐使用反正弦或反正切，反余弦需要多考虑正负的问题。
还有一个姿态角的定义需要介绍一下，见下图

由于重力加速度永远是竖直向下的，所以以g作为参考向量可以计算Pitch角和Roll角，无法计算Yaw角，若要计算Yaw，需要使用磁力计，以地磁作为参考向量计算Yaw。
二、接下来说陀螺仪：
陀螺仪的原理就是测量各方向的角速度，角速度积分就是角度，即
，这里的
是当前计算角度，
是上次计算的角度，
是当前测量的角速度，dt是积分时间（中断时间）。


原理就讲这么多，接下来说数据处理，
第一步要校准数据（零点漂移）。传感器安装在设备上总有一个初始的角度，我们设这个角度为0度，那我们每一次的数据都要减去这个初始数据，得到一个相对的角度。为了保证数据的准确性，我们在初始静止的状态测量大量数据（我取了2000个），用电脑算出平均值，然后以后测量的数据都要减去这个平均值。
但是加速度的校准不能简单考虑减去平均值，因为静止不动时加速度计的值不一定为0。以四轴飞行器为例，把飞行器放置到水平地面上，且传感器水平安装在飞行器上，此时我们认为加速度计的X轴和Y轴的理论值为0，Z轴理论值为8192（根据初始化的量程确定）。但实际安装肯定有误差，我们就将加速的Z轴的值减去测量平均值再加上8192。假设传感器竖直安装，X轴朝上，我们给X轴加8192即可。
还有一种方法是不对原始值进行数据校准，而是在计算出角度后对最终的角度值进行校准，原理和方法类似，大家可以进行尝试。
这种矫正方法只针对系统平衡状态下传感器是水平或竖直安装的。对于复杂的变化的系统并不适用。
第二步要把测量值换算成相应的单位（g、°/s）。换算公式为：加速度 = 测量值 / 精度（单位：g，
）。角速度 = 测量值 / 精度（单位：度每秒）。
下面给出这两步的代码：
void RectifyData(void)//校准偏差、换算单位 { u8 i; acc[0] = (acc1[0] - a); acc[1] = (acc1[1] - b); acc[2] = (acc1[2] - c + 8192); gyr[0] = (gyr1[0] - d); gyr[1] = (gyr1[1] - e); gyr[2] = (gyr1[2] - f); //a,b,c,d,e,f为校准偏差时测得的平均值，写程序的时候直接带入即可 for(i=0; i<3; i++) { acc[i] /= 8192.0f; gyr[i] /= 16.384f; } }第三步需要进行滤波和数据融合，算出角度。目前常见的方法有三种：互补滤波、卡尔曼滤波、硬件DMP解算四元数。主要介绍最简单第一种，第二种过段时间更新。MPU6050可以通过内部的DMP（数字运动处理器）解算出四元数，通过公式转化为欧拉角，得到的角度数据较可靠，但移植过程较复杂，我目前还不会，以后有机会更新。
一、互补滤波
由于加速度计有高频噪声，陀螺仪有低频噪声，可以通过互补滤波融合得到较可靠的角度值，公式如下：
。
为融合后的角度，
为陀螺仪计算的角度，
为加速度计计算的角度，R为滤波器系数，一般取较小的数，接近0。相当于对陀螺仪数据高通滤波，对加速度计数据低通滤波。基本思想为使用加速度计的数据修正陀螺仪的漂移。R取值过大，角度收敛慢，动态性能降低，R过小，角度波动较大，滤波效果降低。下面给出代码：
const float fRad2Deg = 57.295779513f; //弧度换算角度乘的系数 const float dt = 0.005; //时间周期 float angle[3] = {0}; float R = 0.98f; void ImuCalculate_Complementary(void)//计算角度 { u8 i; static float angle_last[3]={0}; float temp[3] = {0}; temp[0] = sqrt(acc[1]*acc[1]+acc[2]*acc[2]); temp[1] = sqrt(acc[0]*acc[0]+acc[2]*acc[2]); for(i = 0; i < 2; i++)//pitch and roll { angle[i] = R*(angle_last[i]+gyro[i]*dt) + (1-R)*fRad2Deg*atan(acc[i]/temp[i]); angle_last[i] = angle[i]; } angle[2] = angle_last[2]+gyro[2]*dt;//yaw angle_last[2] = angle[2]; }二、卡尔曼滤波
卡尔曼滤波（Kalman filtering）一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。简单来说就是一种基于概率的优化系统（滤波）的方法。原理可以参考此文：
下面给出代码：
void kalman_filter(float angle_m, float gyro_m, float *angle_f, float *angle_dot_f) { //------------------------------ static float angle, angle_dot; const float Q_angle = 0.000001, Q_gyro = 0.0001, R_angle = 0.5, dt = 0.002; static float P[2][2]={ { 1, 0 }, { 0, 1 } }; static float Pdot[4] = {0, 0, 0, 0}; const uint8 C_0 = 1; static float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1; //------------------------------ angle += (gyro_m - q_bias) * dt; Pdot[0] =Q_angle - P[0][1] - P[1][0]; Pdot[1] = -P[1][1]; Pdot[2] = -P[1][1]; Pdot[3] = Q_gyro; P[0][0] += Pdot[0] * dt; P[0][1] += Pdot[1] * dt; P[1][0] += Pdot[2] * dt; P[1][1] += Pdot[3] * dt; angle_err = angle_m - angle; PCt_0=C_0 * P[0][0]; PCt_1=C_0 * P[1][0]; E = R_angle + C_0 * PCt_0; K_0 = PCt_0 / E; K_1 = PCt_1 / E; t_0 = PCt_0; t_1 = C_0 * P[0][1]; P[0][0] -= K_0 * t_0; P[0][1] -= K_0 * t_1; P[1][0] -= K_1 * t_0; P[1][1] -= K_1 * t_1; angle += K_0 * angle_err; q_bias += K_1 * angle_err; angle_dot = gyro_m - q_bias; *angle_f = angle; *angle_dot_f = angle_dot; }输入参数：float angle_m 加速度计计算的角度，float gyro_m陀螺仪角速度，float *angle_f融合后的角度，float *angle_dot_f融合后的角速度
输出参数：滤波后的角度及角速度(float *angle_f融合后的角度，float *angle_dot_f融合后的角速度)
在滤波融合算法设计过程中，主要对协方差Q和R的取值进行设计（Q_angle, Q_gyro, R_angle），R取值越小，滤波响应和收敛越迅速；Q取值越小，抑制滤除噪声的能力越强。因此，具体取值也需要反复实际调试进行权衡确定。dt是采样周期。


三、DPM移植
前段时间尝试了DMP，效果很好，虽然更新频率最多200Hz，但是数据很稳定，噪声小。具体的移植方法参考开头推荐的第三篇文章，这里提示一些技巧。
我移植的是圆点博士的DMP库，主函数初始化调用了下面这两个函数
ANBT_I2C_Configuration(); AnBT_DMP_MPU6050_Init();中断里调用了下面的代码，四元数转换欧拉角网上也有很多介绍
dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more); if ( sensors & INV_WXYZ_QUAT ) { q0=quat[0] / q30; q1=quat[1] / q30; q2=quat[2] / q30; q3=quat[3] / q30; Pitch = asin(-2*q1*q3 + 2*q0*q2)* fRad2Deg; Roll = atan2(2*q2*q3 + 2*q0*q1, -2*q1*q1 - 2*q2*q2 + 1)* fRad2Deg; Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * fRad2Deg; }刚开始移植的时候编译器会报一大堆错，不要着急，分析一下圆点博士的代码，错误一般就是一些变量、函数的定义、声明问题，主要有些常见的名字不要和自己的代码冲突。按照编译器的提示一点一点解决就好了。


最后的话
写了好长时间终于写完了，全文手打，这篇文章结合了多篇教程和我自己学习的经验，喜欢就点个赞吧。对了，上面虽然没有给出main函数，但我相信有了上面的基本步骤你已经可以自己写main函数了（调用初始化函数，中断里调用读取、校准、处理数据的函数），还有部分头文件的代码也没有给出，这些小问题就交给大家自己解决了。
ps.我在学习的过程中阅读了《ARM Cortex-M3体系结构与编程（冯新宇）》、《四旋翼无人飞行器设计（冯新宇、 范红刚）》，推荐给大家。