STM32读取MPU6050四元数姿态解算：从原始数据到稳定欧拉角

简介：一份基于STM32F103与MPU6050的四元数姿态解算工程，面向嵌入式开发、传感器融合及无人机/机器人姿态检测方向的学习者。程序通过IO模拟IIC总线读取六轴加速度与角速度数据，在uCosII实时操作系统下完成数据采集、低通滤波、四元数初始化与更新运算，有效避开欧拉角的万向锁问题，并输出稳定的设备姿态信息。压缩包共239个文件，大小仅4.92MB，其中h与c源码构成程序主体，uvproj/uvopt等为Keil工程配置，o/axf/map为编译产物，txt/htm等包含说明与日志，目录结构清晰。已有4663人学习下载。资源最大价值在于完整呈现从MPU6050驱动、模拟IIC时序到四元数解算、任务调度与系统集成的全链路实现，同时保留工程备份与中间文件，便于对照编译过程理解代码组织。适合需要参考实际姿态解算框架、学习IO模拟IIC或uCosII任务设计的嵌入式开发者。

1. 项目概述与核心思路

很多人第一次拿到MPU6050这颗六轴传感器时，第一反应是直接读加速度和角速度的原始数据，然后自己拼一个角度出来。但真到做平衡车、四轴或者机械臂的时候，才发现用原始数据硬算出来的角度根本没法用——稍微动一下就满屏乱跳，静态放着也会慢慢漂。这个项目的核心价值，就是用STM32读取MPU6050的原始数据，通过四元数姿态解算，输出稳定的欧拉角（横滚角Roll、俯仰角Pitch、偏航角Yaw），直接提供给上层控制逻辑使用。

四元数相比欧拉角最大的优势是没有万向锁问题，而且计算过程主要是乘法运算，非常适合单片机这种算力有限的平台。项目中采用的方案是经典的四元数互补滤波（Mahony算法），计算量小，在72MHz主频的STM32F103上跑得毫无压力，还可以顺便做一下串口打印或OLED显示。

这个项目适合两类人：一是做毕业设计或电子竞赛的学生，需要快速得到稳定的姿态数据；二是准备做平衡车、无人机飞控或机器人关节角度反馈的开发者，需要理解姿态解算的完整链路。读完这篇文章，你可以获得一套可以直接“抄作业”的完整代码框架，同时理解每一步背后的原理，知道遇到问题应该从哪个环节排查。

2. 硬件连接与基础配置

2.1 接线方案与I2C地址

MPU6050支持I2C通信，标准接线只需要四根线，连接方式非常固定。我这里给出最常用的接法，注意STM32的I2C引脚在不同开发板上位置不同，最好对照原理图确认。

MPU6050的I2C地址由AD0引脚决定，ADR接低电平时是0x68，接高电平时是0x69。大多数模块上AD0已经默认接地，所以地址就是0x68。如果你发现读不到数据，先查一下是不是模块上这个引脚被拉高了。

注意：MPU6050是3.3V器件，绝对不要直接接5V电源，不然模块会烧掉。如果开发板的是5V引脚供电，必须用稳压或者买带稳压电路的模块。

2.2 电源与滤波电容的小讲究

我踩过一次坑，就是给MPU6050供电时偷懒直接用杜邦线从头板引3.3V，结果数字跳动特别大。后来加上一个10uF钽电容加一个0.1uF陶瓷电容并联在VCC和GND之间，数据明显稳定多了。MPU6050内部有MEMS结构，对电源纹波敏感，特别是数字通信时电流变化会产生毛刺。这个小细节很多人忽略，做高精度姿态采集时需要留意。

如果你打算做四轴或平衡车这种电机干扰比较大的场景，我建议更进一步，在供电线上串联一个磁珠，并且让传感器尽量远离电机和电调，I2C线用双绞线方式接线，可以有效降低EMI干扰。对于普通的桌面实验和毕业设计，并两个电容就够了。

2.3 I2C初始化关键参数

使用STM32标准外设库配置I2C1时，有几个参数值得留意。I2C时钟频率配置为400kHz（快速模式）即可，MPU6050完全支持。GPIO要配置为开漏输出，并外接上拉电阻——很多开发板I2C引脚已经自带上拉，但如果你是自己画的板子，记得接上两个4.7kΩ的上拉电阻到3.3V，否则通信会时好时坏。

初始化代码大致长这样：

void MPU6050_I2C_Init(void) { GPIO_InitTypeDef GPIO_InitStructure; I2C_InitTypeDef I2C_InitStructure; RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE); GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_Init(GPIOB, &GPIO_InitStructure); I2C_InitStructure.I2C_Mode = I2C_Mode_I2C; I2C_InitStructure.I2C_ClockSpeed = 400000; I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2; I2C_InitStructure.I2C_Ack = I2C_Ack_Enable; I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; I2C_InitStructure.I2C_OwnAddress2 = 0x00; I2C_Init(I2C1, &I2C_InitStructure); I2C_Cmd(I2C1, ENABLE); }

如果你不想纠结硬件I2C的各种状态标志位，用软件模拟I2C反而更省心，稳定性也很好。GPIO配置为推挽输出，代码里用延时翻转电平实现时序。实际上我在多个项目里对比过，400kHz的硬件I2C和软件I2C在这个应用场景下没有本质区别，软件I2C的容错性甚至更好——因为它不受I2C总线阻塞的影响，调试也直观。

3. MPU6050初始化与数据读取

3.1 关键寄存器配置与计算过程

MPU6050的初始化本质上就是往几个关键寄存器写入配置值。这里逐个说明我的配置值和理由，可以帮你省去查数据手册的时间。

首先是电源管理寄存器1（地址0x6B），写入0x00表示使用内部时钟源并唤醒芯片。有人会问，为什么不选PLL时钟？后续如果要低噪声高精度，可以把位2~0设为001来选PLL，但对大多数项目来说内部时钟够了。

加速度计配置寄存器（0x1C）我设为量程正负4g，对应代码：

MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, 0x08);

这个0x08对应量程正负4g，灵敏度为8192 LSB/g。量程选择有一个平衡：量程越小分辨率越高，但是稍微倾斜大一点就超出范围了；量程太大分辨率下降。正负4g对于平衡车、手势识别、机器人姿态检测来说是比较折中的选择。如果你做的是剧烈运动的动作捕捉，可以用正负8g甚至16g，对应的寄存器值和灵敏度查表就能拿到。

陀螺仪配置寄存器（0x1B）我设为正负500度每秒，写入0x08，对应灵敏度65.5 LSB/(度/秒)。同理，这个量程对于绝大多数姿态检测场景都够用，而且分辨率又不会太低。

采样率分频寄存器（0x19）的值决定输出频率。当DLPF开启时，采样率 = 1kHz / (1 + SMPLRT_DIV)。我写入0x07，得到125Hz的采样率。对于姿态解算来说，100Hz以上基本够用，而且125Hz是个整数，便于定时器控制解算周期。

数字低通滤波器（0x1A）我设置为0x06，对应截止频率5Hz。这个参数非常重要，它直接决定了数据的平滑程度。如果设置为最高带宽，数据动态响应快但噪声大；设得太低则响应迟钝。5Hz截止频率对大多数姿态场景来说是通用选择，系统运行起来观察数据再微调也不迟。

注意：初始化完成后千万不要立刻读取数据。芯片上电后需要一段时间让内部MEMS结构稳定下来，我习惯在初始化后延时500ms到1s再开始读取，否则前几十帧数据会有一个明显的漂移过程。

3.2 读取原始数据与单位换算

MPU6050的加速度和角速度数据各占用6个寄存器，每个轴高8位和低8位，读取时要连续读取6个字节保证数据一致性。以下是读取函数的核心代码：

void MPU6050_ReadRawData(void) { uint8_t buf[14]; MPU6050_ReadRegs(MPU6050_ACCEL_XOUT_H, buf, 14); Accel_X = (int16_t)((buf[0] << 8) | buf[1]); Accel_Y = (int16_t)((buf[2] << 8) | buf[3]); Accel_Z = (int16_t)((buf[4] << 8) | buf[5]); Gyro_X = (int16_t)((buf[8] << 8) | buf[9]); Gyro_Y = (int16_t)((buf[10] << 8) | buf[11]); Gyro_Z = (int16_t)((buf[12] << 8) | buf[13]); }

注意这里是一次性读取14个字节，包括温度寄存器，这样省去多次I2C通信的耗时。读取后的原始值要除以灵敏度才能得到物理单位：

float accel_x_g = Accel_X / 8192.0f; float accel_y_g = Accel_Y / 8192.0f; float accel_z_g = Accel_Z / 8192.0f; float gyro_x_dps = Gyro_X / 65.5f; float gyro_y_dps = Gyro_Y / 65.5f; float gyro_z_dps = Gyro_Z / 65.5f;

3.3 静态校准——为什么不能跳过

加速度计和陀螺仪都会有零偏，就是静止时读数不为0。陀螺仪的零偏尤其麻烦，它会随时间积分变成巨大的角度误差。所以在正式解算之前，必须做一次静态校准，采集一段静止数据取平均，得到修正量。

我通常在校准时让传感器静止放置，采集100到200组数据，计算平均值作为偏移量。在工程代码中，我会定义一个全局结构体来存偏移值：

typedef struct { int16_t accel_offset_x; int16_t accel_offset_y; int16_t accel_offset_z; int16_t gyro_offset_x; int16_t gyro_offset_y; int16_t gyro_offset_z; } MPU6050_Offset_t; MPU6050_Offset_t offset;

然后每次读取数据后减去对应偏移量，再参与解算。关于校准有个细节：加速度计的零偏不完全等同于静态时的读数偏差，因为静止时加速度计理论上只受重力影响，三轴合成模长应为1g。如果你发现合成的模长明显偏离1g，说明传感器本身误差较大，可以考虑用六面校准法分别标定三轴比例因子。但对于入门和大多数工程应用，只做零偏校准就够了。

4. 四元数姿态解算核心实现

4.1 为什么要用四元数而不是欧拉角

先讲清楚欧拉角的痛点。用欧拉角表示姿态，可以看作绕三个轴依次旋转，但有著名的万向锁问题，就是当某个轴旋转到90度时，另外两个轴的旋转会耦合，导致自由度丢失。而且欧拉角的微分方程里存在三角函数计算，实时计算开销大，也容易发散。

四元数则是一个四维超复数，用一个标量加三个虚部表示三维旋转。它没有奇异点，运算只涉及加减乘除，特别适合嵌入式实时计算。论文和开源飞控里几乎统一使用四元数做中间表示，只在最后需要显示或控制时转换为欧拉角。

4.2 互补滤波的原理与参数选择

解算姿态最核心的问题是如何融合加速度计和陀螺仪的数据。陀螺仪的短时精度高，动态响应快，但因为积分作用会产生零漂累积，长时间会慢慢偏掉。加速度计没有累积漂移问题——静止时通过重力方向可以直接算出横滚和俯仰角，但它的噪声大，对振动非常敏感，动态时数据不可靠。

互补滤波的思路就是取长补短：在频域上让低频部分以加速度计为准，高频部分以陀螺仪为准。简单来说，先用陀螺仪积分得到姿态，然后利用加速度计测得的重力方向与当前姿态推算出的重力方向之间的误差去修正陀螺仪角速度。这个修正量通过比例系数KP和积分系数KI来调节，本质上就是Mahony姿态解算算法。

KP和KI的选取直接影响效果。我把KP设为10，KI设为0.5，这是一个相当保守的起点，系统响应偏慢但很稳定。如果想提高动态响应，可以把KP调到30甚至50，但注意太大时候会因为加速度计噪声被过度引入而导致角度抖动。KI一般取得很小，主要用于消除陀螺仪常值漂移。很多人调参时只关注了比例部分而忽略了积分项，导致静态时有小幅漂移，这就是KI的作用所在，不能省。

4.3 Mahony算法完整代码

下面是移植到STM32标准外设库的Mahony姿态解算核心代码，经过多个项目验证，直接可用：

#define KP_VALUE 10.0f #define KI_VALUE 0.5f #define HALF_SAMPLE_TIME 0.004f float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az) { float norm; float vx, vy, vz; float ex, ey, ez; float halfTx = HALF_SAMPLE_TIME, halfTy = HALF_SAMPLE_TIME, halfTz = HALF_SAMPLE_TIME; // 加速度计归一化 norm = sqrtf(ax*ax + ay*ay + az*az); if (norm == 0.0f) return; ax /= norm; ay /= norm; az /= norm; // 使用当前四元数估算重力方向 vx = 2.0f * (q1*q3 - q0*q2); vy = 2.0f * (q0*q1 + q2*q3); vz = q0*q0 - q1*q1 - q2*q2 + q3*q3; // 叉积求误差 ex = ay*vz - az*vy; ey = az*vx - ax*vz; ez = ax*vy - ay*vx; // 误差积分 integralFBx += KI_VALUE * ex * HALF_SAMPLE_TIME; integralFBy += KI_VALUE * ey * HALF_SAMPLE_TIME; integralFBz += KI_VALUE * ez * HALF_SAMPLE_TIME; // 修正陀螺仪角速度 gx += KP_VALUE * ex + integralFBx; gy += KP_VALUE * ey + integralFBy; gz += KP_VALUE * ez + integralFBz; // 四元数一阶龙格库塔更新 q0 += halfTx * (-q1*gx - q2*gy - q3*gz); q1 += halfTy * ( q0*gx + q2*gz - q3*gy); q2 += halfTz * ( q0*gy - q1*gz + q3*gx); q3 += halfTz * ( q0*gz + q1*gy - q2*gx); // 归一化四元数，防止累积误差导致模长漂移 norm = sqrtf(q0*q0 + q1*q1 + q2*q2 + q3*q3); q0 /= norm; q1 /= norm; q2 /= norm; q3 /= norm; }

有几个细节需要说明。HALF_SAMPLE_TIME设置为0.004对应125Hz的采样率，也就是dt等于8毫秒，取半周期为4毫秒。如果你更改了采样率，这个值必须同步改，否则四元数更新的步长就错了，姿态会剧烈震荡甚至发散。另一个细节是代码里的halfTy和halfTz我都写成了HALF_SAMPLE_TIME，但如果你追求更严格的时间步长，可以分别用三个轴的半周期值。我在实现里统一用同一个值，因为125Hz和125Hz三个轴没有差别，只是代码结构上保留了这种可能。

四元数更新的本质是一阶龙格库塔积分。这个近似的精度与陀螺仪角速度的平滑度和采样率有关，125Hz的采样率下，角速度变化平缓的场景完全够用。如果需要更高精度，可以换成二阶龙格库塔或者增加采样率，但要注意CPU开销和传感器本身的噪声是否支持。

4.4 欧拉角转换与边界处理

四元数在线解算出来，如果要用于显示串口输出或者PID控制，通常需要转换为欧拉角。转换公式如下：

float roll = atan2f(2.0f*(q0*q1 + q2*q3), 1.0f - 2.0f*(q1*q1 + q2*q2)) * 57.29578f; float pitch = asinf(2.0f*(q0*q2 - q1*q3)) * 57.29578f; float yaw = atan2f(2.0f*(q0*q3 + q1*q2), 1.0f - 2.0f*(q2*q2 + q3*q3)) * 57.29578f;

注意这里的yaw是相对角度，因为MPU6050的陀螺仪只能感知旋转，没有绝对的方向参考（比如磁力计）。如果做四轴或需要绝对航向，需要额外挂载磁力计HMC5883L或IST8310来纠正偏航漂移。对于平衡车和机械臂来说，一般只需要roll和pitch，yaw漂移不影响主要功能。

转换角度后的范围也有讲究。roll和pitch应该在正负90度的边界附近做一下限制或判断，防止显示跳变。atan2f本身能处理正负180度，但如果你在控制算法里直接用这个角度值做PID，要注意角度跨越正负180度时可能产生跳变——比如从179度转到-179度，实际上只转了2度，但PID看到的误差是358度，会产生反向满油门。我一般会先把误差归一化到正负180度范围内再输入PID。

5. 常见问题与调试技巧实录

5.1 数据读取失败与I2C通信问题

这是新手最常遇到的问题。如果I2C读取一直返回超时，首先确认接线正确，SDA和SCL有没有接反，VCC和GND有没有电压。其次检查AD0引脚的电平，读地址到底是0x68还是0x69。最后用示波器或者逻辑分析仪看I2C波形，确认STOP条件有没有正常产生。

如果是硬件I2C卡死，有一个很常见的坑：在I2C通信过程中发生干扰时，外设的状态寄存器会进入BUSY状态，之后任何读写操作都会超时。手头有示波器的话可以看到SCL或SDA线一直被拉低。最简单的解决方案是重启I2C外设或者将GPIO复位后重新初始化。我的做法是封装一个I2C总线的复位函数，在读取超时时自动执行，代码层面加了一个计数器，连续超时3次就重新初始化I2C。

5.2 串口输出的角度值乱跳或明显漂移

先检查一下滤波是否生效，把DLPF寄存器关掉对比看看。如果滤波已经开启还是乱跳，大概率是以下几个原因：

• 供电不稳：这是我遇到最多的情况，传感器与电机共用电源或者用了劣质USB线供电，电压跌落导致的。
• 采样率与解算周期不匹配：定时器触发解算的频率和实际读取数据的频率不一致，导致四元数更新的dt与实际不符。比如定时器设置的是125Hz，但是主循环中还有其他阻塞代码，实际只有80Hz，姿态就会慢慢失调。
• 传感器固定不牢：MPU6050模块没有固定好，或者通过长的杜邦线连接，机械振动会污染加速度计数据。

关于漂移，要分清楚是陀螺仪零偏没校准到还是积分系数KI太小。我发现一个很实用的判断方法：静止状态下记录yaw角的输出，如果在几十秒内缓慢漂移且roll和pitch基本稳定，那么主要问题在yaw的零偏。可以在初始化之前延长校准时间，在算法的积分项上稍微加大KI。

5.3 姿态解算发散或角度跳变

如果运行几秒后角度直接变成几万度或者NaN，基本是四元数更新步长有问题。检查HALF_SAMPLE_TIME与实际采样周期是否一致，检查是否忘了归一化四元数导致模长爆炸。还有很常见的一个坑：Mahony函数里用了sqrtf和atan2f这类浮点数学库函数，如果没有开启FPU（对F103来说没有硬件FPU，属于正常）或编译器的优化级别太低，程序运行时间会变长，导致实际解算频率低于预期，最终姿态发散。解决办法是让定时器中断触发的优先级尽量高，解算代码不要在中断里做串口打印，打印放到主循环。

如果是角度跳变，比如从正89度突然跳到负89度，这在roll和pitch轴意味着物体已经过了垂直状态，这是正常的物理现象，不是bug。只要你做的是平衡车的控制，要注意控制周期和处理跳变，为PID做角度误差的环绕处理。

5.4 调参经验与波形观察

调KP/ KI参数时，一定要用可视化的方式观察曲线。我常用的方法是通过串口把roll、pitch和原始加速计推算角度一起发送到PC端，用匿名上位机或者VOFA+绘波形。注意观察两点：一是静止时的噪声幅度，二是快速晃动传感器后姿态跟踪是否快速稳定下来。

我建议的调参顺序是从小到大：先设KP为5，观察响应慢但稳定；然后逐步增大到20、30，每次加5，观察噪声和震荡。如果出现高频抖动，就把KP回调一点。集成项系数先设0.2，静态漂移大就增大，动态受干扰就减小。实际操作中，有一个经验公式可以参考：KP和采样率相关，采样率越高可以设更大的KP。125Hz采样率下，KP在10到50之间，KI在0.2到2之间，基本覆盖了大多数应用场景。

提示：不要在房间里快速挥舞传感器时调参数，这时候加速度计数据包含大量运动加速度，属于高频噪声远大于正常信号的场景，会让你误以为参数不合适。要模拟真实使用场景来调参，比如平衡车就放在地上让轮子转起来，观察输出。

6. 扩展方向与实际项目落地

姿态解算做出来后，可以往很多方向扩展。最常见的是做两轮自平衡车，把roll或pitch角度和角速度作为PD控制的输入，控制电机转速保持平衡。平衡车的核心就是把解算出的角度送进PD控制环，此时KP参数不仅影响姿态解算的响应，也和PD的参数耦合，需要整体调试。

另一个方向是用姿态数据做手势识别。把解算出的角度轨迹输入到模板匹配或者简单分类器，比如通过判断手腕在三维空间中的翻转角度变化来判断动作。我的一个智能台灯项目里，就是通过检测手腕翻转90度来切换灯光的色温和亮度，整体实时性完全够用。

如果想把姿态解算的精度再推一个台阶，可以在MPU6050上扩展磁力计，或者直接换用MPU9250（同为InvenSense的九轴芯片）。融合磁力计后，yaw角也有绝对的参考方向，配合EKF或更完善的融合算法，就基本达到了飞控级的精度。但需要注意，九轴融合的代码复杂度和参数调节难度会明显增大，是不是值得，取决于具体的项目需求。

用STM32做MPU6050的四元数姿态解算，技术方案相对成熟，几乎每一步都有成熟的开源代码和现成的设计思路可以参考。真正需要用心的地方在工程细节：采样率的准确性、供电的稳定、参数的调校、数据的滤波。把这些细节处理好了，姿态数据才会稳定、可靠，项目才能顺利落地。

本文还有配套的精品资源，点击获取