平衡车入门—MPU6050陀螺仪的使用

一.MPU6050简介

  MPU6050是一款陀螺仪模块，不过这个模块可不简单，它可以测量X、Y、Z三轴的角速度和加速度，还带有温度传感器和数字运动处理器(DMP)。假如我们要制作平衡车、四轴、空中鼠标，那么MPU6050就真的是派上大用场了。

二.学习MPU6050的步骤

1、先学习I2C协议，因为MPU6050是通过I2C协议进行驱动的，配置寄存器和获取数据都需要通过I2C协议去实现单片机与MPU6050之间的通信，所以I2C协议必须学习。
2、了解MPU6050的相关寄存器，可以看中文文档MPU6050的datasheet，再配合MPU6050的驱动库函数，了解库函数为什么要这样配置MPU6050的寄存器。
3、把获取到的原始数据进行处理，如通过互补滤波融合得到角度。要知道只有对原始数据进行处理才能够使用，才能发挥MPU6050的价值。

三.I2C协议简介

  I2C协议是一种在单片机开发中非常常用的一个通信协议，它是通过数据总线SDA和时钟总线SCL去完成单片机与一些传感器模块的通信。SCL和SDA线根据I2C的协议的标准进行一系列高低电平的变化(时序)就可以完成信息的传输。
  I2C协议还分为硬件I2C和软件I2C，硬件I2C就是通过硬件电路去实现的I2C协议，软件I2C就是通过在单片机上找两个IO口去充当SCL和SDA线，再通过人为编写软件去控制SCL和SDA线的高低电平变化去模拟I2C协议。
  两者的区别是硬件I2C使用起来比较简单，执行速度比较快，耗时短，但是毕竟是硬件电路，稳定性不一定好，容易出现一些奇怪的问题。而软件I2C虽然是通过软件模拟的，执行速度不如硬件I2C快，有一定的耗时，不过稳定性就比硬件I2C好多了。智能车我们非常注重稳定性，所以推荐大家还是用软件I2C。

四.MPU6050硬件介绍

  我们先来认识下MPU6050的硬件，这是MPU6050模块的图片，注意是模块，中间那个才是MPU6050，不过只有MPU6050是不够的，它还需要一些外围电路才能正常工作，我们可以类比一下51单片机和51单片机的最小系统的区别。
  从MPU6050模块正面上我们还可以看到上面标注了X、Y轴的坐标系，那个就是MPU6050自身的坐标系，如最右图所示。
  以下是MPU6050的相关管脚，不过平时我们使用MPU6050时其实只需要用到VCC、GND、SCL和SDA这四个管脚。不过我们要注意一点，就是AD0管脚的作用，我们知道I2C通信中从机是要有的地址的，以区别多个从机。当AD0管脚接低电平时，从机地址是0xD0。从MPU6050的寄存器中我们可以得到答案，MPU6050作为一个I2C从机设备的时候，有8位地址，高7位的地址是固定的，就是WHO AM I 寄存器中的默认值—0x68，最低一位是由AD0的连线决定的。


  读取MPU6050原始数据这个过程中一个很重要的思路就是一步一步，确保每步都正确后就很容易读出正确的数据。我们对MPU6050进行读写传感器数据就是对MPU6050的寄存器用I2C协议进行读写。对此我们还要了解MPU6050的寄存器，这个过程跟学习51单片机差不多，就是配置寄存器，读取相关数据。

五.MPU6050的几个重要寄存器

1、SMPLRT_DIV寄存器，寄存器地址为0x19。

2、CONFIG寄存器，寄存器地址为0x1A。


3、GYRO_CONFIG寄存器，寄存器地址为0x1B。

4、ACCEL_CONFIG寄存器，寄存器地址为0x1C。

5、三轴加速度计的相关寄存器
ACCEL_XOUT_H(0x3B)、ACCEL_XOUT_L(0x3C)、
ACCEL_YOUT_H(0x3D)、ACCEL_YOUT_L(0x3E)、
ACCEL_ZOUT_H(0x3F)、ACCEL_ZOUT_H(0x40)。


6、三轴陀螺仪的相关寄存器
GYRO_XOUT_H(0x43)、GYRO_XOUT_L(0x44)、
GYRO_YOUT_H(0x45)、GYRO_YOUT_L(0x46)、
GYRO_ZOUT_H(0x47)、GYRO_ZOUT_H(0x48)

。

7、温度传感器相关的寄存器
TEMP_OUT_H(0x41)和TEMP_OUT_L(0x42)。
8、PWR_MGMT_1寄存器，寄存器地址为0x6B。

9、WHO_AM_I寄存器，寄存器的地址为0x75。

10、初始化MPU6050的常用寄存器配置：

// //初始化MPU6050 // void InitMPU6050() { 
    Single_WriteI2C(PWR_MGMT_1, 0x00);//解除休眠状态 Single_WriteI2C(SMPLRT_DIV, 0x07);//陀螺仪采样率为1K/(1+0x07)=125Hz Single_WriteI2C(CONFIG, 0x06); //低通滤波器的截止频率为1K,带宽为5Hz Single_WriteI2C(GYRO_CONFIG, 0x18);//配置陀螺仪量程为2000deg/s,不自检 Single_WriteI2C(ACCEL_CONFIG, 0x00);//配置加速度计量程为2g，不自检 } 

六.原始数据的单位换算

七.角度换算(滤波算法)

void Get_Balance_Angle(void) { 
    uint8 gyro_offset = 1;//静置时角速度的偏移量 float gyro_dt = 0.004f;//陀螺仪角速度积分系数,增长缓慢就增加 float Filter_Weight = 0.02;//滤波权重 Get_GyroData();//获取原始三轴角速度 Get_AccData();//获取原始三轴加速度 g_fBalance_Gyro = mpu_gyro_y - gyro_offset;//原始角速度减去零偏值得到实际角速度 g_fAccel_Angle = (float)atan2(mpu_acc_x,mpu_acc_z) * 57.296;//两轴加速度求反三角得到加速度角度，乘以57.296,是把弧度转化为度 //一阶互补滤波核心公式,得到融合角度 g_fBalance_Angle = Filter_Weight * g_fAccel_Angle + (1-Filter_Weight) * (g_fBalance_Angle - g_fBalance_Gyro * gyro_dt); } 

2、清华角度滤波

// // 清华角度滤波方案 //* /* * 功能说明：清华角度滤波 * 参数说明： G_angle 加速度计角度0-90内 * Gyro 陀螺仪角速度转花后的数值 * GRAVITY_ADJUST_TIME_CONSTANT 时间校正系数 DT 定时器时间 单位s * 函数返回：无符号结果值 * 修改时间：2013-2-10 * 备注：参考清华源码 */ // //* void QingHua_AngleCalaulate(float G_angle,float Gyro) { 
    float fDeltaValue; g_fCarAngle = g_fGyroscopeAngleIntegral; //最终融合角度 fDeltaValue = (G_angle - g_fCarAngle) / GRAVITY_ADJUST_TIME_CONSTANT; //时间系数矫正 g_fGyroscopeAngleIntegral += (Gyro + fDeltaValue) * DT; //融合角度 } 

3、卡尔曼滤波(难理解)

*// // Kalman滤波 // float angle, angle_dot; //外部需要引用的变量 //angle_m为角速度角度和gyro_m为测到的角速度 void Kalman_Filter(float angle_m,float gyro_m) { 
    const float Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.005; //注意：dt的取值为kalman滤波器采样时间;  static float P[2][2] = { 
    { 
    1, 0 },{ 
    0, 1 } }; static float Pdot[4] ={ 
   0,0,0,0}; static const char 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;//角速度 }