简介面向无人机、机器人及运动设备开发者的MPU6050姿态解算工程源码基于STM32平台使用卡尔曼滤波融合三轴陀螺仪与加速度计数据实现稳定精确的姿态输出。代码通过I2C接口与MPU6050通信涵盖硬件初始化、传感器读取、滤波处理、欧拉角/四元数解算与串口输出可直接移植或二次开发。资源共122个文件核心代码包括MPU6050.c、inv_mpu.c等C源程序及对应头文件同时包含Keil工程配置、编译生成的axf/hex文件及备份数据压缩包约2.52MB目录结构清晰便于定位与调试。已有1611人学习下载。通过学习可掌握I2C驱动、DMP数字运动处理器调用、卡尔曼滤波实现与姿态解算流程适合希望深入嵌入式传感器融合开发的爱好者。1. MPU6050为什么需要卡尔曼滤波六轴数据里的真实与噪声直接读MPU6050的寄存器你会拿到3轴角速度和3轴加速度。把角速度积分会得到一个角度对加速度做反正切也会得到一个角度。但这两个角度往往对不上陀螺仪刚上电时很好3秒后开始往一边飘加速度计静止时基本稳定可手一抖就剧烈跳动。这不是传感器坏了而是各自的误差模型不同。有人把两个角度加权平均效果差强人意真正稳的还是卡尔曼滤波它把陀螺仪的短期可信和加速度计的长期可信用状态估计的方式融合起来并且能实时估计陀螺仪的零偏。这个STM32工程里同时包含了MPU6050驱动、软件I2C和卡尔曼滤波实现另外还保留了InvenSense官方的inv_mpu.c等DMP库文件相当于给了你两条姿态解算路线。如果你在做平衡车、两轮自平衡或者机械臂姿态反馈这个项目值得拆开看。2. 软件I2C与MPU6050寄存器先把原始数据读出来2.1 MPU6050的I2C地址和关键寄存器MPU6050作为I2C从机地址由AD0引脚决定。AD0接GND时地址是0x68接3.3V时是0x69。在Minibalance这类板子上多数接GND所以源码里如果看到#define MPU6050_ADDR 0x68那就是默认接法。如果读出来的数据全是0xFF或0x00先量AD0的电平。驱动MPU6050前要搞清楚几个寄存器。下面这张表是初始化最常用的几个寄存器名地址作用典型值PWR_MGMT_10x6B电源管理和复位bit7置1复位整个芯片0x00 或 0x01SMPLRT_DIV0x19采样率分频器输出频率 内部采样率/(1分频值)0x07 左右CONFIG0x1A低通滤波配置设置DLPF截止频率常见 0x06 (约5Hz)GYRO_CONFIG0x1B陀螺仪量程和自检0x18 表示±2000°/sACCEL_CONFIG0x1C加速度计量程和自检0x00 表示±2gACCEL_XOUT_H0x3B加速度计X轴高字节从这里连续读取14字节可拿到所有数据-CONFIG寄存器里的DLPF不仅滤波原始数据还会影响陀螺仪延迟。比如你要做平衡车截止频率太高会把噪声带进去太低会让姿态滞后。源码里我习惯先配置为0x06也就是约5Hz后面根据波形再调。2.2 软件I2C的读写实现工程里IOI2C.c提供的是软件模拟I2C。为什么不直接用STM32的硬件I2C外设ST早期的硬件I2C有时序上的坑一旦遇到要查忙标志、重发START对刚起步的开发者很不友好。软件模拟把SCL和SDA拉高拉低时序一目了然出问题好排查。下面是一段典型的读写寄存器代码来自IOI2C.c#define SCL_H() GPIO_SetBits(GPIOB, GPIO_Pin_6) #define SCL_L() GPIO_ResetBits(GPIOB, GPIO_Pin_6) #define SDA_H() GPIO_SetBits(GPIOB, GPIO_Pin_7) #define SDA_L() GPIO_ResetBits(GPIOB, GPIO_Pin_7) // 向MPU6050写一个字节寄存器 void I2C_WriteReg(uint8_t devAddr, uint8_t regAddr, uint8_t data) { I2C_Start(); I2C_SendByte(devAddr 1); // 高7位是器件地址最低位写0 I2C_WaitAck(); I2C_SendByte(regAddr); // 寄存器地址 I2C_WaitAck(); I2C_SendByte(data); // 要写入的值 I2C_WaitAck(); I2C_Stop(); } // 从MPU6050连续读取多个字节 void I2C_ReadRegs(uint8_t devAddr, uint8_t regAddr, uint8_t *buf, uint16_t len) { I2C_Start(); I2C_SendByte(devAddr 1); I2C_WaitAck(); I2C_SendByte(regAddr); I2C_WaitAck(); I2C_Start(); // 主发送模式下重复起始信号 I2C_SendByte((devAddr 1) | 0x01); // 读方向 I2C_WaitAck(); while (len--) { if (len 0) { *buf I2C_RecvByte(ACK); } else { *buf I2C_RecvByte(NACK); // 最后一个字节回NACK } } I2C_Stop(); }这里有几个容易抄错的地方。第一I2C_SendByte(devAddr 1)MPU6050地址是7位的I2C总线上发送的是8位的地址1 | 读写位如果直接把0x68发出去就错了。第二I2C_WriteReg里发送完寄存器地址后MPU6050要回ACK软件模拟里I2C_WaitAck()必须限时退出否则芯片没连上时程序会卡死。第三连续读的最后一个字节要回NACK这样MPU6050才会释放SDA让主机发STOP。I2C_Start()和I2C_Stop()之间必须有足够的延时一般不能比1μs更短我通常用几个__NOP()。2.3 MPU6050初始化和数据读取驱动跑通后初始化顺序决定数据是否正常。下面这段是被很多平衡车项目采用的初始化流程void MPU6050_Init(void) { I2C_WriteReg(MPU6050_ADDR, PWR_MGMT_1, 0x80); // bit71: 复位整个芯片 DelayMs(100); // 等芯片复位完成 I2C_WriteReg(MPU6050_ADDR, PWR_MGMT_1, 0x01); // 使用PLL X轴陀螺仪作为时钟源 I2C_WriteReg(MPU6050_ADDR, SMPLRT_DIV, 0x07); // 采样率 内部1kHz/(17) 125Hz I2C_WriteReg(MPU6050_ADDR, CONFIG, 0x06); // DLPF加速度和陀螺仪的截止频率约5Hz I2C_WriteReg(MPU6050_ADDR, GYRO_CONFIG, 0x18); // 陀螺仪量程±2000°/s I2C_WriteReg(MPU6050_ADDR, ACCEL_CONFIG, 0x00); // 加速度计量程±2g DelayMs(10); }复位那一步很重要。如果不先写0x80芯片可能处于默认的休眠模式或者时钟源不对读出来的陀螺仪数据是乱跳的。时钟源配置0x01表示用X轴陀螺仪的PLL作为系统时钟比内部RC振荡器准确能明显降低角速度积分造成的漂移。量程这里GYRO_CONFIG是0x18对应±2000°/s此时角速度灵敏度是16.4 LSB/(°/s)算角速度时要用raw / 16.4而不是粗暴地除以某个自定义值。加速度计量程±2g时灵敏度是16384 LSB/g。数据读取一般放在主循环或定时中断里。推荐用连续读取的方式从ACCEL_XOUT_H寄存器一次性读14字节避免多次I2C传输导致的数据撕裂uint8_t buf[14]; int16_t acc_x, acc_y, acc_z; int16_t gyro_x, gyro_y, gyro_z; MPU6050_ReadData(buf); // 从0x3B开始读14字节 acc_x (buf[0] 8) | buf[1]; gyro_x (buf[8] 8) | buf[9]; float gyro_x_deg gyro_x / 16.4f; // 转换为 °/s float acc_x_g acc_x / 16384.0f; // 转换为 g因为是二进制补码(buf[0]8)|buf[1]在buf[0]为负数时符号位是保留的。如果你用uint8_t的组合赋值给int16_t编译器会保留符号位这是正确的。这里有个新手容易犯的错把高字节和低字节顺序搞反。寄存器地址0x3B是高字节在先所以buf[0]是高位。如果顺序反了角度会高频抖动而且数值完全不对。3. 卡尔曼滤波在STM32上的实现不只有公式3.1 为什么先讲一维卡尔曼姿态解算里的卡尔曼滤波很多人一上来就上一堆矩阵满屏的四阶矩阵乘法最后把MCU跑死也没算出结果。实际上在平衡车、两轮自平衡这种应用里我们最关心的是俯仰角Pitch和横滚角Roll这两个角可以分别用一个一维系统来描述。而偏航角Yaw没有绝对参考单纯靠陀螺仪积分必然漂移卡尔曼也只能估计零偏不能纠正绝对角度。一维卡尔曼的状态量取为x [angle, bias]^T其中bias是陀螺仪零偏。状态方程是angle_new angle (gyro - bias) * dt bias_new bias加速度计测量值作为观测量提供对angle的修正。这个模型简单到可以在中断里几百微秒算完STM32F103主频72MHz跑起来毫无压力。相比完整的四元数卡尔曼一维方案在Pitch接近±90°时会失效但对于平衡车应用工作范围一般不超过±30°所以一维卡尔曼够用。3.2 卡尔曼结构体和核心代码源码里的卡尔曼滤波一般封装成下面这种形式。以Kalman_t结构体保存所有状态每次读取新数据后调用Kalman_Update函数typedef struct { float Q_angle; // 角度状态噪声协方差 float Q_bias; // 陀螺仪零偏噪声协方差 float R_measure; // 加速度计测量噪声协方差 float angle; // 滤波后的角度 float bias; // 陀螺仪零偏 float P[2][2]; // 误差协方差矩阵 float dt; // 采样时间 } Kalman_t; void Kalman_Init(Kalman_t *k, float qa, float qb, float r) { k-Q_angle qa; k-Q_bias qb; k-R_measure r; k-P[0][0] 1.0f; k-P[0][1] 0.0f; k-P[1][0] 0.0f; k-P[1][1] 1.0f; k-angle 0.0f; k-bias 0.0f; } float Kalman_Update(Kalman_t *k, float newAngle, float newRate, float dt) { float y, S, K0, K1; // 预测阶段根据陀螺仪积分 k-angle dt * (newRate - k-bias); k-P[0][0] dt * (dt * k-P[1][1] - k-P[0][1] - k-P[1][0] k-Q_angle); k-P[0][1] - dt * k-P[1][1]; k-P[1][0] - dt * k-P[1][1]; k-P[1][1] k-Q_bias * dt; // 计算卡尔曼增益 S k-P[0][0] k-R_measure; K0 k-P[0][0] / S; K1 k-P[1][0] / S; // 更新阶段用加速度计角度修正 y newAngle - k-angle; k-angle K0 * y; k-bias K1 * y; k-P[0][0] - K0 * k-P[0][0]; k-P[0][1] - K0 * k-P[0][1]; k-P[1][0] - K1 * k-P[0][0]; k-P[1][1] - K1 * k-P[0][1]; return k-angle; }这段代码里newRate是陀螺仪测量的角速度单位°/snewAngle是加速度计计算得到的角度单位°。调用之前一定要把dt算准它是两次传感器读取的时间间隔。dt不对会直接导致滤波发散。常见做法是用定时器计数比如每次进入采样中断后用SysTick的差值计算而不是固定写死一个0.005。三个参数Q_angle、Q_bias、R_measure决定了滤波器是偏向陀螺仪还是加速度计。我的经验是固定R_measure然后从小到大调Q_angle。Q_angle越大滤波器越信任加速度计波形更贴实测但毛刺多Q_angle越小越信任陀螺仪波形平滑但响应慢。Q_bias影响零偏跟踪速度如果上电后角度缓慢漂移往往是Q_bias设得太小。3.3 滤波参数表与调参顺序下面是一组常见项目的典型起始参数参数取值范围起始建议现象与调整Q_angle0.001 ~ 0.010.001值太小→波形跟不上快速动作太大→毛刺明显Q_bias0.003 ~ 0.010.003太小→零偏收敛慢太大→角度被加速度计噪声带偏R_measure0.01 ~ 0.10.03值越小越信任加速度计毛刺越多越大越平滑滞后越明显dt由采样频率决定0.002~0.01必须和实际采样周期一致否则滤波发散调参的顺序我会先让MPU6050平放在桌面上依次把Q_angle从0.001调到0.01观察静止时波形的峰峰值。然后手拿板子做快速摆动的阶跃响应观察是否过冲和回稳时间。最后再调Q_bias让板子静置5分钟不出现可见角度漂移。记住一点没有一组参数全场景通用。装在平衡车上和拿在手上有完全不同的振动特性换机构后必须重新调。4. DMP四元数与卡尔曼滤波工程里两套方案的取舍4.1 inv_mpu.c为什么会在工程里工程文件清单里出现了inv_mpu.c、inv_mpu_dmp_motion_driver.c它们是InvenSense官方DMP驱动库。DMP是MPU6050内置的数字运动处理器可以在传感器内部完成四元数结算主控只需要读取DMP输出的四元数再转换成欧拉角。这样主控代码里就不需要写卡尔曼滤波了。但工程里同时又实现了卡尔曼滤波这两条路线在资源占用和姿态质量上的差别很大。DMP方式的优势是省代码官方库经过大量测试且基于四元数可以全姿态工作没有欧拉角死锁。缺点也不小DMP库对I2C时序要求高初始化流程长而且可定制性差如果想加入磁场修正或者自定义滤波就麻烦了。卡尔曼滤波则完全由你控制代码量不大但需要自己理解状态模型并且在动态环境下需要反复调参。4.2 DMP读取四元数和欧拉角转换如果要用DMP路线先让DMP加载固件再做基本的姿态设置之后在主循环中读取四元数。典型调用如下#include inv_mpu.h #include inv_mpu_dmp_motion_driver.h short quat[4]; float pitch, roll, yaw; dmp_init(); // 内部完成mpu_init、dmp_load_firmware等 while (1) { if (dmp_read_fifo(gyro, accel, quat, sensor_timestamp) 0) { // 四元数顺序为 w, x, y, z float qw quat[0] / 16384.0f; float qx quat[1] / 16384.0f; float qy quat[2] / 16384.0f; float qz quat[3] / 16384.0f; roll atan2f(2.0f * (qw * qx qy * qz), 1.0f - 2.0f * (qx * qx qy * qy)) * 57.2958f; pitch asinf(2.0f * (qw * qy - qz * qx)) * 57.2958f; yaw atan2f(2.0f * (qw * qz qx * qy), 1.0f - 2.0f * (qy * qy qz * qz)) * 57.2958f; } }DMP输出的四元数每个分量需要除以16384因为内部用14位整数和小数。转换欧拉角的三个公式不能随意改roll用atan2pitch用asin。pitch在±90°附近误差很大因为asin在±1附近斜率趋向无穷大所以工程上常常限制pitch范围。这里还有一点值得注意DMP输出的yaw角漂移非常快因为单块MPU6050没有磁力计yaw只能靠陀螺仪积分的相对角度。4.3 两套方案怎么选我整理了一张对比表格方便你根据项目类型做选择维度卡尔曼滤波本工程主路径DMP四元数inv_mpu库主控负担每轴约500次乘加可轻松跑1000Hz更小四元数运算在MPU6050内部完成代码量几十行封装成Kalman_t官方库上千行初始化流程复杂可定制性高Q/R参数可调可扩展零偏估计低除了开关DMP无法介入内部算法全姿态能力一维模型只在±90°内有效四元数无死锁适合全角度调试难度需要理解噪声特性、调参初始化若失败很难定位原因这个工程把两套都留在了源码里说明作者当时也在这两种方案间切换过。如果你做的是两轮平衡车使用范围的俯仰角不超过30度卡尔曼滤波完全够用代码还能完全掌控。如果你做的是四轴飞行器或者全姿态传感器直接上DMP四元数更省事但要处理随时可能会出现的DMP初始化失败问题。故障表现通常是dmp_read_fifo返回不为0解决手段是重新执行完整初始化序列而不是简单复位MPU6050。5. 零漂校准与波形验证让姿态数据真正可用5.1 启动时的陀螺仪零偏校准卡尔曼滤波只能实时估计零偏但如果启动时零偏太大收敛过程会留下一个几乎不可消除的角度偏差。所以工程上普遍的做法是上电后让设备保持静止取前N个陀螺仪样本的平均值作为bias初值。下面是一段典型的校准代码放在初始化之后、滤波开始之前#define CALIB_SAMPLES 100 float gyro_offset[3] {0}; void Gyro_Calibrate(void) { int32_t sum_x 0, sum_y 0, sum_z 0; int16_t gx, gy, gz; for (int i 0; i CALIB_SAMPLES; i) { MPU6050_ReadGyro(gx, gy, gz); sum_x gx; sum_y gy; sum_z gz; DelayMs(1); // 采样间隔 } gyro_offset[0] sum_x / CALIB_SAMPLES; // 传感器原始零偏单位LSB gyro_offset[1] sum_y / CALIB_SAMPLES; gyro_offset[2] sum_z / CALIB_SAMPLES; }注意这里保存的是原始LSB值不是除以灵敏度的物理值。卡尔曼滤波更新时newRate要从原始读数里减去这个零偏再换算成°/s。如果把校准结果存成浮点角速度会和滤波内部的bias单位混淆。这是我能看到的最常见的单位错误。校准完成后把Kalman_t.bias初始化为0让卡尔曼自己去修正剩余的一点点偏差。校准期间如果板子发生晃动采样值会受影响我一般会在校准函数里加一个简单的晃动检测如果相邻样本的差超过某个阈值就重新采样。5.2 串口输出姿态波形肉眼定位问题要在电脑上观察卡尔曼滤波效果最直接的方式是用串口把三个角度打出来然后在上位机画波形。可以用printf重定向也可以用DMA加缓冲区的方式避免阻塞主循环。简单场景下重定向足够int fputc(int ch, FILE *f) { USART_SendData(USART1, (uint8_t)ch); while (USART_GetFlagStatus(USART1, USART_FLAG_TXE) RESET); return ch; } // 在主循环里每秒输出50次姿态 printf(%.2f,%.2f,%.2f\r\n, roll_kalman, pitch_kalman, yaw_kalman);使用串口助手或vscode里的串口监视器选对应的波特率就能看到数据流。观察波形时重点看三类问题。一类是毛刺多静止时角度曲线有大量尖峰说明Q_angle设置太大或DLPF截止频率太高应把CONFIG寄存器调成0x06以下。另一类是相位延迟过大手快速摆动时角度跟不上或回正后要几百毫秒才归零这往往是R_measure太大滤波器过度信任陀螺仪。还有一类是缓慢漂移静止10分钟后角度偏了2度以上首先确认Gyro_Calibrate是否生效其次把Q_bias调大一倍再观察。5.3 参数整定的三步实验法最后给一个可复现的参数整定流程比凭感觉调参快得多。第一步零偏校准后把板子水平静止放置记录100秒波形。此时理想曲线是水平线如果波峰峰谷超过1度优先减小Q_angle或者把R_measure调大直到静止噪声小于0.5度。第二步做阶跃测试把板子快速从水平转动到45度角然后保持。观察过冲量过冲超过5度说明Q_angle太小或R_measure太小滤波器过于激进如果上升时间超过1秒说明滤波器滞后严重。第三步连续运动2分钟结束后立刻恢复水平看角度误差。误差小于2度说明零偏估计正常如果误差持续增长说明Q_bias需要调大。调参时每次只改一个参数并记录当时的波形特征。下面是我用过的一张表格格式实验序号Q_angleQ_biasR_measure静止噪声(°)阶跃过冲(°)恢复误差(°)结论10.0010.0030.030.34.50.8过冲偏大20.0020.0030.050.42.80.7可接受30.0020.0050.050.43.00.4推荐实际工程中卡尔曼滤波参数和机械结构强相关。轮子的震动、电机PWM频率、结构共振都会反馈到MPU6050的输出上。如果数据里存在固定频率的噪声尖峰可以在MPU6050的DLPF里直接滤掉这比一味增大R_measure更有效。做完以上三步把最终的参数写进工程姿态数据才算真正可用的状态。本文还有配套的精品资源点击获取