MPU6050的数据获取、分析与处理_优信 mpu-6050(1)_mpu6050数据处理

收集整理了一份《2024年最新物联网嵌入式全套学习资料》，初衷也很简单，就是希望能够帮助到想自学提升的朋友。

如果你需要这些资料，可以戳这里获取

需要这些体系化资料的朋友，可以加我V获取：vip1024c （备注嵌入式）

一个人可以走的很快，但一群人才能走的更远！不论你是正从事IT行业的老鸟或是对IT行业感兴趣的新人

都欢迎加入我们的的圈子（技术交流、学习资源、职场吐槽、大厂内推、面试辅导），让我们一起学习成长！

为了保证数据的物理意义，MPU6050的加速度计是以假想球在三轴上座标值的相反数作为三个轴的加速度值。当假想球的位置偏向一个轴的正向时，该轴的加速度读数为负值，当假想球的位置偏向一个轴的负向时，该轴的加速度读数为正值。

根据以上分析，当我们把MPU6050芯片水平放于地方，芯片表面朝向天空，此时由于受到地球重力的作用， 假想球的位置偏向Z轴的负向，因此Z轴的加速度读数应为正，且在理想情况下应为g。注意，此加速度的物理意义并不是重力加速度，而是自身运动的加速度，可以这样理解：正因为其自身运动的加速度与重力加速度大小相等方向相反，芯片才能保持静止。

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

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

由于MPU6050可以获取三个轴向上的加速度，而地球重力则是长期存在且永远竖直向下，因此我们可以根据重力加速度相对于芯片的指向为参考算得当前姿态。

为方便起见，我们让芯片正面朝下固定在上图飞机上，且座标系与飞机的坐标系完全重合，以三个轴向上的加速度为分量，可构成加速度向量。假设当前芯片处于匀速直线运动状态，那么应垂直于地面上向，即指向Z轴负方向，模长为（与重力加速度大小相等，方向相反，见3.1节）。若芯片（座标系）发生旋转，由于加速度向量仍然竖直向上，所以Z轴负方向将不再与重合。见下图。

为了方便表示，上图坐标系的Z轴正方向（机腹以及芯片正面）向下，X轴正方向（飞机前进方向）向右。此时芯片的Roll角（黄色）为加速度向量与其在XZ平面上投影的夹角，Pitch角（绿色）与其在YZ平面上投影的夹角。求两个向量的夹角可用点乘公式： ，简单推导可得：

，以及

注意，因为arccos函数只能返回正值角度，因此还需要根据不同情况来取角度的正负值。当y值为正时，Roll角要取负值，当x轴为负时，Pitch角要取负值。

3.4 Yaw角的问题

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

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

四、数据处理与实现

MPU6050芯片提供的数据夹杂有较严重的噪音，在芯片处理静止状态时数据摆动都可能超过2%。除了噪音，各项数据还会有偏移的现象，也就是说数据并不是围绕静止工作点摆动，因此要先对数据偏移进行校准 ，再通过滤波算法消除噪音。

4.1 校准

校准是比较简单的工作，我们只需要找出摆动的数据围绕的中心点即可。我们以GRY_X为例，在芯片处理静止状态时，这个读数理论上讲应当为0，但它往往会存在偏移量，比如我们以10ms的间隔读取了10个值如下：

-158.4, -172.9, -134.2, -155.1, -131.2, -146.8, -173.1, -188.6, -142.7, -179.5

1
2

这10个值的均值，也就是这个读数的偏移量为-158.25。在获取偏移量后，每次的读数都减去偏移量就可以得到校准后的读数了。当然这个偏移量只是估计值，比较准确的偏移量要对大量的数据进行统计才能获知，数据量越大越准，但统计的时间也就越慢。一般校准可以在每次启动系统时进行，那么你应当在准确度和启动时间之间做一个权衡。

三个角速度读数GYR_X、GYR_Y和GYR_Z均可通过统计求平均的方法来获得，但三个加速度分量就不能这样简单的完成了，因为芯片静止时的加速度并不为0。

加速度值的偏移来自两个方面，一是由于芯片的测量精度，导至它测得的加速度向量并不垂直于大地；二是芯片在整个系统（如无人机）上安装的精度是有限的，系统与芯片的座标系很难达到完美重合。前者我们称为读数偏移，后者我们称为角度偏移。因为读数和角度之间是非线性关系，所以要想以高精度进行校准必须先单独校准读数偏移，再把芯片固定在系统中后校准角度偏移。然而，由于校准角度偏移需要专业设备，且对于一般应用来说，两步校准带来的精度提升并不大，因此通常只进行读数校准即可。下面介绍读数校准的方法。我们还3.2节的飞机为例，分以下几个步骤：

1. 首先要确定飞机的坐标系，对于多轴飞行器来说这非常重要。如果坐标系原点的位置或坐标轴的方向存在较大偏差，将会给后面的飞控造成不良影响。
2. 在确定了飞机的坐标系后，为了尽量避免读数偏移带来的影响，首先将MPU6050牢牢地固定在飞机上，并使二者座标系尽可能的重合。当然把Z轴反过来装也是可以的，就是需要重新推算一套角度换算公式。
3. 将飞机置于水平、坚固的平面上，并充分预热。对于多轴无人机而言，空中悬停时的XY平面应当平行于校准时的XY平面。此时，我们认为芯片的加速度方向应当与Z轴负方向重合，且加速度向量的模长为g，因此ACC_X和ACC_Y的理论值应为0，ACC_Z的理论值应为-16384（假设我们设定2g的倍率，1g的加速度的读数应为最大值-32768的一半）。
4. 由于ACC_X和ACC_Y的理论值应为0，与角速度量的校准类似，这两个读数偏移量可用统计均值的方式校准。ACC_Z则需要多一步处理，即在统计偏移量的过程中，每次读数都要加上16384，再进行统计均值校准。 4.2 卡尔曼滤波

对于夹杂了大量噪音的数据，卡尔曼滤波器的效果无疑是最好的。如果不想考虑算法细节，可以直接使用Arduino的Klaman Filter库完成。在我们的模型中，一个卡尔曼滤波器接受一个轴上的角度值、角速度值以及时间增量，估计出一个消除噪音的角度值。跟据当前的角度值和上一轮估计的角度值，以及这两轮估计的间隔时间，我们还可以反推出消除噪音的角速度。

实现代码见4.3节。下面介绍卡尔曼滤波算法细节，不感兴趣的可跳过。

（想看的人多了再写）

4.3 实现代码

以下代码在Arduino软件1.65版本中编译、烧写以及测试通过。

// 本代码版权归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;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154

参考：https://zhuanlan.zhihu.com/p/20082486?columnSlug=devymex

  



**收集整理了一份《2024年最新物联网嵌入式全套学习资料》，初衷也很简单，就是希望能够帮助到想自学提升的朋友。**
![img](https://img-blog.csdnimg.cn/img_convert/1573ed269736d877ca62c46e3459b979.png)
![img](https://img-blog.csdnimg.cn/img_convert/c7d1208b97d369b06c59df2e929eca16.png)

**[如果你需要这些资料，可以戳这里获取](https://bbs.csdn.net/topics/618679757)**

**需要这些体系化资料的朋友，可以加我V获取：vip1024c （备注嵌入式）**

**一个人可以走的很快，但一群人才能走的更远！不论你是正从事IT行业的老鸟或是对IT行业感兴趣的新人**

**都欢迎加入我们的的圈子（技术交流、学习资源、职场吐槽、大厂内推、面试辅导），让我们一起学习成长！**

15889668311)]
[外链图片转存中...(img-TicPxQP2-1715889668311)]

**[如果你需要这些资料，可以戳这里获取](https://bbs.csdn.net/topics/618679757)**

**需要这些体系化资料的朋友，可以加我V获取：vip1024c （备注嵌入式）**

**一个人可以走的很快，但一群人才能走的更远！不论你是正从事IT行业的老鸟或是对IT行业感兴趣的新人**

**都欢迎加入我们的的圈子（技术交流、学习资源、职场吐槽、大厂内推、面试辅导），让我们一起学习成长！**

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29