以下两篇文章可作为参考：

姿态传感器MPU6050原理及应用超详细讲解 - 采芯网

基于STM32的四旋翼无人机项目（二）：MPU6050姿态解算（含上位机3D姿态显示教学）-CSDN博客

基本介绍

随着科技的发展，我们经常看到有人用平衡车代步、使用无人机进行航拍。但这些是怎么实现的呢？平衡车和无人机在使用的时候是怎样保持平衡与稳定的呢？

其实，平衡车和无人机在内部都用到陀螺仪传感器和加速度计传感器，用来检测车体的姿态以及运动时发生的变化，再通过各方面的共同协调配合，从而保持平衡车和无人机的平衡与稳定。本文主要介绍MPU6050的原理及应用。

MPU6050是InvenSense公司推出的全球首款整合性6轴运动处理组件，内带3轴陀螺仪和3轴加速度传感器（3+3=6，因此我们常说是6轴传感器，如果再加上个3轴磁力计，就一共是9轴传感器），并且含有一个第二IIC接口（XCL和XDA），可用于连接外部磁力传感器，利用自带数字运动处理器（DMP: Digital Motion Processor）硬件加速引擎，通过主IIC接口，可以向应用端输出完整的9轴姿态融合演算数据。有了DMP，我们可以使用InvenSense公司提供的运动处理资料库，非常方便的实现姿态解算，降低了运动处理运算对操作系统的负荷，同时大大降低了开发难度 。

MPU6050芯片一共有24个引脚

完整引脚定义如下：

从表中能看到，有些引脚是复用的。

MPU6050本身是可以支持IIC或者SPI接口来读取数据的，但是不同的模块可能引出来的接口不一样，比如我们常用的引出8脚带IIC接口的模块如下：

其引出的8个引脚定义表如下：

其中，SCL 和 SDA 是连接 MCU 的 IIC 接口，MCU 通过这个 IIC 接口来控制 MPU6050，另外还有一个 IIC 接口：AUX_CL 和 AUX_DA，这个接口可用来连接外部从设备，比如磁传感器，这样就可以组成一个九轴传感器。VLOGIC 是 IO 口电压，该引脚最低可以到 1.8V，我们一般直接接 VDD 即可。AD0 是从 IIC 接口（接 MCU）的地址控制引脚，该引脚控制IIC 地址的最低位。如果接 GND，则 MPU6050 的 IIC 地址是：0X68，如果接 VDD，则是0X69，注意：这里的地址是不包含数据传输的最低位的（最低位用来表示读写）。例 0x68：1101000 + 最低位表示读写，构成8位的IIC地址。

模块自带了 3.3V 超低压差稳压芯片，给 MPU6050 供电，因此外部供电可以选择：3.3V / 5V 都可以。模块通过 P1 排针与外部连接，引出了 VCC、GND、IIC_SDA、IIC_SCL、MPU_INT 和 MPU_AD0 等信号，其中，IIC_SDA 和 IIC_SCL 带了 4.7K上拉电阻，外部可以不用再加上拉电阻了，另外 MPU_AD0 自带了 10K 下拉电阻，当 AD0悬空时，默认 IIC 地址为（0X68）。

MPU6050的固连参考坐标如下图所示：

MPU6050具有如下特性：

① 以数字形式输出 6 轴或 9 轴（需外接磁传感器）的旋转矩阵、四元数(quaternion)、欧

拉角格式(Euler Angle forma)的融合演算数据（需 DMP 支持）

② 具有 131 LSBs/°/sec 敏感度与全格感测范围为±250、±500、±1000 与±2000°/sec

的 3 轴角速度感测器(陀螺仪)

③ 集成可程序控制，范围为±2g、±4g、±8g 和±16g 的 3 轴加速度传感器

④ 移除加速器与陀螺仪轴间敏感度，降低设定给予的影响与感测器的飘移

⑤ 自带数字运动处理(DMP: Digital Motion Processing)引擎可减少 MCU 复杂的融合演算

数据、感测器同步化、姿势感应等的负荷

⑥ 内建运作时间偏差与磁力感测器校正演算技术，免除了客户须另外进行校正的需求

⑦ 自带一个数字温度传感器

⑧ 带数字输入同步引脚(Sync pin)支持视频电子影相稳定技术与 GPS

⑨ 可程序控制的中断(interrupt)，支持姿势识别、摇摄、画面放大缩小、滚动、快速下降

中断、high-G 中断、零动作感应、触击感应、摇动感应功能

⑩ VDD 供电电压为 2.5V±5%、3.0V±5%、3.3V±5%；VLOGIC 可低至 1.8V± 5%

⑪ 陀螺仪工作电流：5mA，陀螺仪待机电流：5uA；加速器工作电流：500uA，加速器省

电模式电流：40uA@10Hz

⑫ 自带 1024 字节 FIFO，有助于降低系统功耗

⑬ 高达 400Khz 的 IIC 通信接口

⑭ 超小封装尺寸：4x4x0.9mm（QFN）

内部结构

 MPU6050三大部件总结：

（1）陀螺仪传感器（Gyroscope sensor）

传感器在它的内部有一个陀螺，因为陀螺效应始终与初始方向平行，这样就能通过与初始方向的偏差计算出旋转方向与角度。

（2）加速度计传感器（Accelerometer Sensor）

加速度传感器是一种能够测量加速度的传感器。它采用的是压电效应的原理。传感器在加速过程中，通过对质量块所受惯性力的测量，利用牛顿第二定律获取加速度值。

（3）数字运动处理器（Digital Motion Processor）

DMP是MPU6050芯片中的数据处理模块，（内置卡尔曼滤波算法）获取陀螺仪和加速度传感器数据，并处理输出四元数，可以减轻外围微处理器的工作负担且避免了繁琐的滤波和数据融合。

重要寄存器介绍

首先，我们先来介绍几个比较重要的寄存器。

① 电源管理寄存器1（寄存器地址为 0X6B）

其中，DEVICE_RESET 位用来控制复位，设置为 1，复位 MPU6050，复位结束后，MPU硬件自动清零该位。SLEEEP 位用于控制 MPU6050 的工作模式，复位后，该位为 1，即进入了睡眠模式（低功耗），所以我们要清零该位，以进入正常工作模式。TEMP_DIS 用于设置是否使能温度传感器，设置为 0，则使能。最后 CLKSEL[2:0]用于选择系统时钟源，选择关系如下表所示：

默认是使用内部 8M RC 晶振的，精度不高，所以我们一般选择 X/Y/Z 轴陀螺作为参考的 PLL 作为时钟源，一般设置 CLKSEL=001 即可。

② 陀螺仪配置寄存器（寄存器地址为0X1B）

该寄存器我们只关心 FS_SEL[1:0]这两个位，用于设置陀螺仪的满量程范围：0，±250°/S；1，±500°/S；2，±1000°/S；3，±2000°/S；我们一般设置为 3，即±2000°/S，因为陀螺仪的 ADC 为 16 位分辨率，所以得到灵敏度为：65536/4000=16.4LSB/(°/S)。

这里的单位啥意思？

先了解弧度和角度

弧度就是我们常说的π的形式，角度就是对应的180°

rad被称为弧度角，在数学计算物理计算中非常常用。弧度和角度的换算方式：1rad=180/pi。1rad约等于57.3度。因为角度制是60进位制，所以在计算两个角的加减时经常会遇到单位转换上的麻烦，因此在数学和科学研究中经常使用另一种方式度量角：把等于半径长的圆弧所对的圆心角叫做1弧度的角，记作1(rad)。弧度制的基本思想是使圆半径与圆周长有同一度量单位，然后用对应的弧长与圆半径之比来度量角度，一周的弧度数为2πr/r=2π，360°角=2π弧度，因此，1弧度约为57.3°

补充：为什么角度是60进制？

∵角度中使用的单位是度，分，秒，

且1°=60′，1′=60″，

∴角度是六十进制.

陀螺仪的单位是角速度，一般使用弧度/秒（rad/s）或者角度/秒（°/s）表示。如果使用弧度/秒表示，那么一个陀螺仪输出x表示该陀螺仪在单位时间内转动了x弧度。而如果使用角度/秒表示，则输出x表示该陀螺仪在单位时间内转动了x度。

角速度越大，说明陀螺的角度可以变化得越快。

角速度对时间的积分就能得到旋转过的角度。

这里的不同量程，表示的是什么意思呢？

陀螺仪常用的量程有2000°/s、500°/s、250°/s、100°/s等。量程是指陀螺仪能够测量的最大旋转速度，如果超过了量程就会造成数据溢出，导致测量错误。选择量程时需要根据实际应用场景进行选择。

1.陀螺仪量程越大越好吗？

答：不一定，根据实际情况选择合适的量程才是最好的选择。

2.陀螺仪输出的数据是连续的吗？

答：陀螺仪输出的是离散的数据，需要通过微处理器或者数据采集卡进行采样。

3.陀螺仪有没有误差？

答：任何仪器都有误差，陀螺仪也不例外。误差是由于各种因素引起的，如温度、振动等。

这里的满量程和ADC的满量程电压类似，但是灵敏度的计算好像跟ADC的分辨率不是一个意思。

那这里的灵敏度是什么意思？

灵敏度是指最小能分辨的角速度，概念倒是和ADC的分辨率挺像的，但是ADC里的算法是3.3/65536，表示的是分辨率，但是这里的算法为啥是65536/4000，而且，结果单位里的LSB是个啥？

不论是加速还是陀螺仪，数据寄存器都是16位的，并且最高位是符号位，故而数据寄存器的输出范围是-7FFF~7FFF (-32767~32767)

假如，我们选择陀螺仪的测量范围是±2000，在数据手册中 16.4 LSB/(°/s)是 32767/2000=16.4 计算出来的

那么寄存器的数字和测量范围的对应关系

假如现在通过寄存器读取陀螺仪的值是1000，那么我们求角速度 X

其实从这里可以看出来，和ADC算电压的核心思路是一样的，只是这里将得到的数16.4定义为了陀螺仪的灵敏度。

LSB/°/s 这个参数的中文含义表示，数据读取的最低位数/角度/秒，一般用来表示陀螺仪的灵敏度。

怎么理解呢，以16.4LSB/(°/s)为例

我们这样来看

也就是说，最小读出16.4的值，就对应了1°/s的角速度。

表达的是1°/s的角速度，也就是单位角速度所对应的ADC的值，即定义为陀螺仪的灵敏度。

那么每一份ADC值对应的角速度就是1/16.4，其实就是4000/65536，约0.06°/s，也就是说，能测到的最小角速度是0.06°/s

分辨率是从ADC的角度来看的，每1个单位对应的陀螺仪的角速度；

灵敏度是从陀螺仪的角度来看的，每1个单位的角速度对应的ADC的读取值；

③ 加速度传感器配置寄存器（寄存器地址为0X1C）

该寄存器我们只关心 AFS_SEL[1:0]这两个位，用于设置加速度传感器的满量程范围：0，±2g；1，±4g；2，±8g；3，±16g；我们一般设置为 0，即±2g，因为加速度传感器的ADC 也是 16 位，所以得到灵敏度为：65536/4=16384LSB/g。

加速度（Acceleration）是速度变化量与发生这一变化所用时间的比值Δv/Δt，是描述物体速度变化快慢的物理量，通常用a表示，单位是米每二次方秒。加速度是矢量，它的方向是物体速度变化（量）的方向，与合外力的方向相同。

速度的单位是m/s，再除以个时间s，就变成了m/s.s，即米每二次方秒。

重力单位（gravity unit）是一个常用于度量重力加速度的单位，符号g 。

物理上g表示重力加速度，G表示重力。重力对自由下落的物体产生的加速度，称为重力加速度也叫自由落体加速度。以m表示物体的质量，以g表示重力加速度，重力G可表示为G=mg。

g经常在航天工程和医学的研究中被用作重力单位，它是一个物体在某种环境下所受到的加速度。在地球上，由于地心引力的作用，一个静止的物体受到的是1g的重力加速度。

大小:通常在没有明确说明的时候g取9.80 m/s2。在进行粗略的计算或有说明时可以把g取作10m/s2。重力加速度的大小随其在地球上地点的不同而略有差异。

这里的满量程同样是指最大能测到的重力加速度。

加速度的灵敏度和陀螺仪的灵敏度是类似的意思，即每1个单位的重力加速度对应的ADC的读取值；

④ FIFO 使能寄存器（寄存器地址为0X23）

该寄存器用于控制 FIFO 使能，在简单读取传感器数据的时候，可以不用 FIFO，设置对应位为 0 即可禁止 FIFO，设置为 1，则使能 FIFO。注意加速度传感器的 3 个轴，全由 1个位（ACCEL_FIFO_EN）控制，只要该位置 1，则加速度传感器的三个通道都开启 FIFO了。

⑤ 陀螺仪采样率分频寄存器（寄存器地址为0X19）

该寄存器用于设置 MPU6050 的陀螺仪采样频率，计算公式为：

采样频率 = 陀螺仪输出频率 / (1+SMPLRT_DIV)

这里陀螺仪的输出频率，是 1Khz 或者 8Khz，与数字低通滤波器（DLPF）的设置有关，当 DLPF_CFG=0/7 的时候，频率为 8Khz，其他情况是 1Khz。而且 DLPF 滤波频率一般设置为采样率的一半。采样率，我们假定设置为 50Hz，那么 SMPLRT_DIV=1000/50-1=19。

⑥ 配置寄存器（寄存器地址为0X1A）

这里，我们主要关心数字低通滤波器（DLPF）的设置位，即：DLPF_CFG[2:0]，加速度计和陀螺仪，都是根据这三个位的配置进行过滤的。DLPF_CFG 不同配置对应的过滤情况如下表所示：

这里的加速度传感器，输出速率（Fs）固定是 1Khz，而角速度传感器的输出速率（Fs），则根据 DLPF_CFG 的配置有所不同。一般我们设置角速度传感器的带宽为其采样率的一半，如前面所说的，如果设置采样率为 50Hz，那么带宽就应该设置为 25Hz，取近似值 20Hz，就应该设置 DLPF_CFG=100。

⑦ 电源管理寄存器2（0X6C）

该寄存器的 LP_WAKE_CTRL 用于控制低功耗时的唤醒频率，本章用不到。剩下的 6位，分别控制加速度和陀螺仪的 x/y/z 轴是否进入待机模式，这里我们全部都不进入待机模式，所以全部设置为 0 即可。

⑧ 陀螺仪数据输出寄存器（共6个寄存器，地址为0X43~0X48）

陀螺仪数据输出寄存器总共由6个寄存器组成，输出X/Y/Z三个轴的陀螺仪传感器数据，高字节在前，低字节在后。

⑨ 加速度传感器数据输出寄存器（6个，地址为0X3B~0X40）

加速度传感器数据输出寄存器总共由6个寄存器组成，输出X/Y/Z三个轴的加速度传感器值，高字节在前，低字节在后。

⑩ 温度传感器数据输出寄存器（0X41~0X42）

通过读取0X41（高8位）和0X42（低8位）寄存器得到，温度换算公式为：

Temperature = 36.53 + regval/340

其中，Temperature为计算得到的温度值，单位为℃，regval为从0X41和0X42读到的温度传感器值。

MPU6050初始化

① 初始化 IIC 接口

MPU6050 采用 IIC 与 STM32F1 通信，所以我们需要先初始化与 MPU6050 连接的 SDA和 SCL 数据线。

② 复位 MPU6050

这一步让 MPU6050 内部所有寄存器恢复默认值，通过对电源管理寄存器 1（0X6B）的bit7 写 1 实现。 复位后，电源管理寄存器 1 恢复默认值(0X40)，然后必须设置该寄存器为0X00，以唤醒 MPU6050，进入正常工作状态。

③ 设置角速度传感器（陀螺仪）和加速度传感器的满量程范围

这一步，我们设置两个传感器的满量程范围(FSR)，分别通过陀螺仪配置寄存器（0X1B）和加速度传感器配置寄存器（0X1C）设置。我们一般设置陀螺仪的满量程范围为±2000dps，加速度传感器的满量程范围为±2g。

④ 设置其他参数

这里，我们还需要配置的参数有：关闭中断、关闭 AUX IIC 接口（没有磁力计）、禁止 FIFO、设置陀螺仪采样率和设置数字低通滤波器（DLPF）等。实验中我们不用中断方式读取数据，所以关闭中断，然后也没用到 AUX IIC 接口外接其他传感器，所以也关闭这个接口。分别通过中断使能寄存器（0X38）和用户控制寄存器（0X6A）控制。MPU6050 可以使用 FIFO 存储传感器数据，不过本章我们没有用到，所以关闭所有 FIFO 通道，这个通过 FIFO 使能寄存器（0X23）控制，默认都是 0（即禁止 FIFO），所以用默认值就可以了。陀螺仪采样率通过采样率分频寄存器（0X19）控制，这个采样率我们一般设置为 50 即可。数字低通滤波器(DLPF)则通过配置寄存器（0X1A）设置，一般设置 DLPF 为带宽的 1/2 即可。

⑤ 配置系统时钟源并使能角速度传感器和加速度传感器

系统时钟源同样是通过电源管理寄存器 1（0X1B）来设置，该寄存器的最低三位用于设置系统时钟源选择，默认值是 0（内部 8M RC 震荡），不过我们一般设置为 1，选择 x 轴陀螺 PLL 作为时钟源，以获得更高精度的时钟。同时，使能角速度传感器和加速度传感器，这两个操作通过电源管理寄存器 2（0X6C）来设置，设置对应位为 0 即可开启。

至此，MPU6050 的初始化就完成了，可以正常工作了（其他未设置的寄存器全部采用默认值即可），接下来，我们就可以读取相关寄存器，得到加速度传感器、角速度传感器和温度传感器的数据了。

DMP姿态解算

使用 MPU6050 的 DMP 输出的四元数是 q30 格式的，也就是浮点数放大了 2 的 30 次方倍。在换算成欧拉角之前，必须先将其转换为浮点数，也就是除以 2 的 30 次方，然后再进行计算，计算公式为：

q0=quat[0] / q30; //q30 格式转换为浮点数

q1=quat[1] / q30;

q2=quat[2] / q30;

q3=quat[3] / q30;

//计算得到俯仰角/横滚角/航向角

pitch=asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; //俯仰角

roll=atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; //横滚角

yaw=atan2(2*(q1q2 + q0q3),q0q0+q1q1-q2q2-q3q3) * 57.3; //航向角

其中 quat[0]~ quat[3]是 MPU6050 的 DMP 解算后的四元数，q30 格式，所以要除以一个2 的 30 次方，其中 q30 是一个常量：1073741824，即 2 的 30 次方，然后带入公式，计算出欧拉角。上述计算公式的 57.3 是弧度转换为角度，即 180/π，这样得到的结果就是以度（°）为单位的。

更多参考相关教学视频。

后续再根据实践进行单独记录。