MPU6050芯片内自带了一个数据处理子模块DMP，已经内置了滤波算法，在许多应用中使用DMP输出的数据已经能够很好的满足要求。关于如何获取DMP的输出数据，我将在以后的文章中介绍。本文将直接面对原始测量数据，从连线、芯片通信开始一步一步教你如何利用Arduino获取MPU6050的数据并进行卡尔曼滤波，最终获得稳定的系统运动状态。

3 工作原理

加速度计采用压电效应的工作原理，就像上面的图片一样，在一个立方体的盒子里面有一个小球，盒子的四壁是用压电晶体材料，当盒子倾斜时，由于重力的作用，球就会向倾斜的方向移动，当小球碰到墙壁就会产生压电电流。盒子中有上下、左右、前后三对相对的墙壁，每一对墙对应于三维空间中的一个轴：X轴、Y轴、Z轴。根据压电壁产生的电流，我们就可以确定倾角的方向和大小。

4 单片机与MPU6050通信

这里以arduino单片机为例

为避免纠缠于电路细节，我们直接使用集成的MPU6050模块。MPU6050的数据接口用的是I2C总线协议，因此我们需要Wire程序库的帮助来实现Arduino与MPU6050之间的通信。请先确认你的Arduino编程环境中已安装Wire库。

Wire库的官方文档中指出：在UNO板子上，SDA接口对应的是A4引脚，SCL对应的是A5引脚。MPU6050需要5V的电源，可由UNO板直接供电。按照下图连线。

4.1 mpu6050 数据格式

我们感兴趣的数据位于0x3B到0x48这14个字节的寄存器中。这些数据会被动态更新，更新频率最高可达1000HZ。下面列出相关寄存器的地址，数据的名称。注意，每个数据都是2个字节。

0x3B，加速度计的X轴分量ACC_X

0x3D，加速度计的Y轴分量ACC_Y

0x3F，加速度计的Z轴分量ACC_Z

0x41，当前温度TEMP

0x43，绕X轴旋转的角速度GYR_X

0x45，绕Y轴旋转的角速度GYR_Y

0x47，绕Z轴旋转的角速度GYR_Z

4.2 倾角计算方法

Roll-pitch-yaw模型与姿态计算

表示飞行器当前飞行姿态的一个通用模型就是建立下图所示坐标系，并用Roll表示绕X轴的旋转，Pitch表示绕Y轴的旋转，Yaw表示绕Z轴的旋转。

Yaw角的问题

因为没有参考量，所以无法求出当前的Yaw角的绝对角度，只能得到Yaw的变化量，也就是角速度GYR_Z。当然，我们可以通过对GYR_Z积分的方法来推算当前Yaw角（以初始值为准），但由于测量精度的问题，推算值会发生漂移，一段时间后就完全失去意义了。然而在大多数应用中，比如无人机，只需要获得GRY_Z就可以了。

如果必须要获得绝对的Yaw角，那么应当选用MPU9250这款九轴运动跟踪芯片，它可以提供额外的三轴罗盘数据，这样我们就可以根据地球磁场方向来计算Yaw角了，具体方法此处不再赘述。

5 实现代码

// 本代码版权归Devymex所有，以GNU GENERAL PUBLIC LICENSE V3.0发布
// http://www.gnu.org/licenses/gpl-3.0.en.html
// 相关文档参见作者于知乎专栏发表的原创文章：
// http://zhuanlan.zhihu.com/devymex/20082486

//连线方法
//MPU-UNO
//VCC-VCC
//GND-GND
//SCL-A5
//SDA-A4
//INT-2 (Optional)

#include <Kalman.h>
#include <Wire.h>
#include <Math.h>

float fRad2Deg = 57.295779513f; //将弧度转为角度的乘数
const int MPU = 0x68; //MPU-6050的I2C地址
const int nValCnt = 7; //一次读取寄存器的数量

const int nCalibTimes = 1000; //校准时读数的次数
int calibData[nValCnt]; //校准数据

unsigned long nLastTime = 0; //上一次读数的时间
float fLastRoll = 0.0f; //上一次滤波得到的Roll角
float fLastPitch = 0.0f; //上一次滤波得到的Pitch角
Kalman kalmanRoll; //Roll角滤波器
Kalman kalmanPitch; //Pitch角滤波器

void setup() {
  Serial.begin(9600); //初始化串口，指定波特率
  Wire.begin(); //初始化Wire库
  WriteMPUReg(0x6B, 0); //启动MPU6050设备

  Calibration(); //执行校准
  nLastTime = micros(); //记录当前时间
}

void loop() {
  int readouts[nValCnt];
  ReadAccGyr(readouts); //读出测量值
  
  float realVals[7];
  Rectify(readouts, realVals); //根据校准的偏移量进行纠正

  //计算加速度向量的模长，均以g为单位
  float fNorm = sqrt(realVals[0] * realVals[0] + realVals[1] * realVals[1] + realVals[2] * realVals[2]);
  float fRoll = GetRoll(realVals, fNorm); //计算Roll角
  if (realVals[1] > 0) {
    fRoll = -fRoll;
  }
  float fPitch = GetPitch(realVals, fNorm); //计算Pitch角
  if (realVals[0] < 0) {
    fPitch = -fPitch;
  }

  //计算两次测量的时间间隔dt，以秒为单位
  unsigned long nCurTime = micros();
  float dt = (double)(nCurTime - nLastTime) / 1000000.0;
  //对Roll角和Pitch角进行卡尔曼滤波
  float fNewRoll = kalmanRoll.getAngle(fRoll, realVals[4], dt);
  float fNewPitch = kalmanPitch.getAngle(fPitch, realVals[5], dt);
  //跟据滤波值计算角度速
  float fRollRate = (fNewRoll - fLastRoll) / dt;
  float fPitchRate = (fNewPitch - fLastPitch) / dt;
 
 //更新Roll角和Pitch角
  fLastRoll = fNewRoll;
  fLastPitch = fNewPitch;
  //更新本次测的时间
  nLastTime = nCurTime;

  //向串口打印输出Roll角和Pitch角，运行时在Arduino的串口监视器中查看
  Serial.print("Roll:");
  Serial.print(fNewRoll); Serial.print('(');
  Serial.print(fRollRate); Serial.print("),\tPitch:");
  Serial.print(fNewPitch); Serial.print('(');
  Serial.print(fPitchRate); Serial.print(")\n");
  delay(10);
}

//向MPU6050写入一个字节的数据
//指定寄存器地址与一个字节的值
void WriteMPUReg(int nReg, unsigned char nVal) {
  Wire.beginTransmission(MPU);
  Wire.write(nReg);
  Wire.write(nVal);
  Wire.endTransmission(true);
}

//从MPU6050读出一个字节的数据
//指定寄存器地址，返回读出的值
unsigned char ReadMPUReg(int nReg) {
  Wire.beginTransmission(MPU);
  Wire.write(nReg);
  Wire.requestFrom(MPU, 1, true);
  Wire.endTransmission(true);
  return Wire.read();
}

//从MPU6050读出加速度计三个分量、温度和三个角速度计
//保存在指定的数组中
void ReadAccGyr(int *pVals) {
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);
  Wire.requestFrom(MPU, nValCnt * 2, true);
  Wire.endTransmission(true);
  for (long i = 0; i < nValCnt; ++i) {
    pVals[i] = Wire.read() << 8 | Wire.read();
  }
}

//对大量读数进行统计，校准平均偏移量
void Calibration()
{
  float valSums[7] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0};
  //先求和
  for (int i = 0; i < nCalibTimes; ++i) {
    int mpuVals[nValCnt];
    ReadAccGyr(mpuVals);
    for (int j = 0; j < nValCnt; ++j) {
      valSums[j] += mpuVals[j];
    }
  }
  //再求平均
  for (int i = 0; i < nValCnt; ++i) {
    calibData[i] = int(valSums[i] / nCalibTimes);
  }
  calibData[2] += 16384; //设芯片Z轴竖直向下，设定静态工作点。
}

//算得Roll角。算法见文档。
float GetRoll(float *pRealVals, float fNorm) {
  float fNormXZ = sqrt(pRealVals[0] * pRealVals[0] + pRealVals[2] * pRealVals[2]);
  float fCos = fNormXZ / fNorm;
  return acos(fCos) * fRad2Deg;
}

//算得Pitch角。算法见文档。
float GetPitch(float *pRealVals, float fNorm) {
  float fNormYZ = sqrt(pRealVals[1] * pRealVals[1] + pRealVals[2] * pRealVals[2]);
  float fCos = fNormYZ / fNorm;
  return acos(fCos) * fRad2Deg;
}

//对读数进行纠正，消除偏移，并转换为物理量。公式见文档。
void Rectify(int *pReadout, float *pRealVals) {
  for (int i = 0; i < 3; ++i) {
    pRealVals[i] = (float)(pReadout[i] - calibData[i]) / 16384.0f;
  }
  pRealVals[3] = pReadout[3] / 340.0f + 36.53;
  for (int i = 4; i < 7; ++i) {
    pRealVals[i] = (float)(pReadout[i] - calibData[i]) / 131.0f;
  }
}