STM32F103C8T6驱动MPU6050：卡尔曼滤波姿态解算实战

简介这份资源面向嵌入式开发初学者与需要做姿态检测的工程师提供STM32F103C8T6驱动MPU6050并完成卡尔曼滤波的完整工程。内容围绕I2C通信、MPU6050寄存器读写、USART1串口输出以及卡尔曼滤波的预测与更新流程展开可用于平衡车、运动追踪等场景的姿态解算。压缩包共120个文件约2.44MB包含c与h源码、uvproj与uvopt工程配置、axf与hex可执行文件、map与lst编译映射、crf与o中间产物及bak备份等工程结构完整可直接编译下载。目前已有6107人学习下载。读者可从中获得传感器初始化、原始数据读取、滤波参数调试与串口实时传输的参考实现并借助中间文件与映射信息排查编译链接问题适合作为嵌入式滤波入门与课程设计的实践素材。1. 从一串跳动的原始数据说起MPU6050 在 STM32F103C8T6 上到底该怎么读MPU6050 这颗六轴传感器便宜、好买、资料多几乎是每个嵌入式新手接触姿态解算的第一站。但很多人把它接上 STM32F103C8T6、用 I2C 读出加速度和角速度之后会发现一个很尴尬的事串口助手里那三行数字一直在跳静止放着也跳稍微碰一下桌子跳得更厉害直接拿这个数据去做平衡车或者云台电机能抖成筛子。问题不在传感器坏了而在于你拿到的是「生肉」——原始寄存器值里混着零偏、温漂和振动噪声。卡尔曼滤波要解决的就是把这堆跳动的数字变成一条能用的、平滑的姿态角曲线。这篇笔记面向的是手里已经有 C8T6 最小系统和 MPU6050 模块、能点灯能串口打印、但卡在「数据读出来了不知道怎么用」这一步的开发者。我会把 I2C 读取、量程配置、零偏校准、卡尔曼滤波的落地代码和参数整定一次讲透新手能照着复现熟手能对照着看自己的参数边界在哪。2. 硬件连接与 I2C 初始化先把「能读到数」这件事做扎实2.1 为什么 C8T6 读 MPU6050 优先用硬件 I2C 而不是软件模拟STM32F103C8T6 有两路硬件 I2CI2C1 默认挂在 PB6SCL和 PB7SDA上。很多教程让你用 GPIO 模拟 I2C理由是「硬件 I2C 有坑、容易卡死」。这话在十年前有一定道理早期标准库的 I2C 状态机确实容易在总线异常时死等。但现在用 HAL 库配合超时机制硬件 I2C 的稳定性完全够用而且它不占用 CPU 去翻转电平读 14 个字节的传感器数据时 CPU 占用几乎为零。我一般会优先用硬件 I2C1只有在引脚被占用或者需要挂多路同地址设备时才退回软件模拟。接线本身很简单VCC 接 3.3VGND 共地SCL 接 PB6SDA 接 PB7。MPU6050 模块上通常自带 4.7k 上拉电阻如果你的模块没有需要在 SCL 和 SDA 上各加一个 4.7k 到 3.3V。AD0 引脚决定器件地址接地时地址是 0x68接 VCC 时是 0x69。INT 引脚可以先不接轮询读取对大多数应用足够了。2.2 用 CubeMX 配置 I2C1 与串口生成可编译的骨架在 CubeMX 里选好 STM32F103C8T6把 I2C1 设为 I2C 模式速度标准模式 100kHz 或快速模式 400kHz 都行MPU6050 两者都支持。串口 USART1 设为 115200-8-N-1用来打印数据。时钟树按 72MHz 主频配好I2C 的时钟源来自 APB1注意 APB1 最高 36MHz。生成代码后先写一个最简的读取函数确认能拿到 WHO_AM_I 寄存器的值。MPU6050 的 WHO_AM_I 寄存器地址是 0x75正常应该返回 0x68。这一步是分水岭读不到就查接线和上拉读到了再往下走。// mpu6050_basic.c #include main.h extern I2C_HandleTypeDef hi2c1; #define MPU6050_ADDR (0x68 1) // HAL 库需要左移一位 #define MPU6050_WHO_AM_I 0x75 #define MPU6050_PWR_MGMT_1 0x6B // 读取单个寄存器 uint8_t MPU6050_ReadReg(uint8_t reg) { uint8_t val 0; HAL_I2C_Mem_Read(hi2c1, MPU6050_ADDR, reg, I2C_MEMADD_SIZE_8BIT, val, 1, 100); return val; } // 写单个寄存器 void MPU6050_WriteReg(uint8_t reg, uint8_t val) { HAL_I2C_Mem_Write(hi2c1, MPU6050_ADDR, reg, I2C_MEMADD_SIZE_8BIT, val, 1, 100); } // 初始化唤醒并检查身份 uint8_t MPU6050_Init(void) { if (MPU6050_ReadReg(MPU6050_WHO_AM_I) ! 0x68) return 1; // 身份不对返回错误 MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x00); // 解除睡眠 HAL_Delay(50); return 0; }这段代码里MPU6050_ADDR左移一位是 HAL 库的约定7 位地址要放到高 7 位。HAL_I2C_Mem_Read的最后一个参数是超时时间单位毫秒给 100ms 足够。如果这里返回 HAL_ERROR 或 HAL_TIMEOUT先别怀疑代码用逻辑分析仪或者示波器看 SCL/SDA 上有没有波形多数问题是上拉电阻没接或者地址搞错了。2.3 配置量程与采样率别让默认值坑了你MPU6050 上电默认加速度量程是 ±2g角速度量程是 ±250°/s采样率 1kHz。对于大多数姿态应用这个默认值其实偏保守±2g 意味着稍微剧烈一点的运动就会削顶±250°/s 对于快速转动也不够。我一般会把加速度设成 ±4g角速度设成 ±500°/s兼顾精度和动态范围。配置寄存器如下寄存器地址作用常用值SMPLRT_DIV0x19采样率分频0x09100HzCONFIG0x1A数字低通滤波0x0344HzGYRO_CONFIG0x1B角速度量程0x08±500°/sACCEL_CONFIG0x1C加速度量程0x08±4g采样率分频的计算公式是采样率 陀螺仪输出率 / (1 SMPLRT_DIV)。陀螺仪输出率默认 1kHz所以写 0x09 得到 100Hz。数字低通滤波设 44Hz 可以滤掉一部分高频振动但注意它和卡尔曼滤波不冲突前者是硬件级的抗混叠后者是算法级的姿态融合。void MPU6050_Config(void) { MPU6050_WriteReg(0x19, 0x09); // 100Hz 采样 MPU6050_WriteReg(0x1A, 0x03); // 44Hz 低通 MPU6050_WriteReg(0x1B, 0x08); // ±500°/s MPU6050_WriteReg(0x1C, 0x08); // ±4g }量程改了之后灵敏度也跟着变。±500°/s 对应 65.5 LSB/(°/s)±4g 对应 8192 LSB/g。这两个系数在后面的数据换算里必须用对用错了角度会差好几倍这是新手最容易翻车的地方之一。3. 原始数据读取与零偏校准卡尔曼滤波的输入质量决定上限3.1 一次性读出 14 个字节加速度、温度和角速度MPU6050 的数据寄存器从 0x3B 开始连续 14 个字节依次是加速度 X/Y/Z 各 2 字节、温度 2 字节、角速度 X/Y/Z 各 2 字节。用一次HAL_I2C_Mem_Read连续读出来比逐个寄存器读效率高得多也避免了两帧数据之间传感器更新导致的时间不一致。typedef struct { int16_t acc_x, acc_y, acc_z; int16_t temp; int16_t gyro_x, gyro_y, gyro_z; } MPU6050_RawData; void MPU6050_ReadRaw(MPU6050_RawData *raw) { uint8_t buf[14]; HAL_I2C_Mem_Read(hi2c1, MPU6050_ADDR, 0x3B, I2C_MEMADD_SIZE_8BIT, buf, 14, 100); raw-acc_x (int16_t)((buf[0] 8) | buf[1]); raw-acc_y (int16_t)((buf[2] 8) | buf[3]); raw-acc_z (int16_t)((buf[4] 8) | buf[5]); raw-temp (int16_t)((buf[6] 8) | buf[7]); raw-gyro_x (int16_t)((buf[8] 8) | buf[9]); raw-gyro_y (int16_t)((buf[10] 8) | buf[11]); raw-gyro_z (int16_t)((buf[12] 8) | buf[13]); }注意这里用int16_t强转因为原始数据是有符号的直接拼成uint16_t再当正数用会得到完全错误的结果。温度值的换算公式是temp / 340.0 36.53单位摄氏度可以用来做简单的温漂观察但一般不需要参与滤波。3.2 零偏校准静止采样 500 次取平均别省这一步陀螺仪的零偏是卡尔曼滤波里最要命的误差源。所谓零偏就是传感器静止时输出的不是零而是一个非零的常数。这个常数还会随温度变化。如果你不校准滤波出来的角度会以一个稳定的速度漂移积分时间越长偏得越离谱。校准方法很直接把模块水平静止放好连续读 500 到 1000 次角速度求平均值把这个平均值作为零偏存起来之后每次读取都减去它。typedef struct { float gyro_x_offset; float gyro_y_offset; float gyro_z_offset; } GyroOffset; GyroOffset gyro_calibrate(void) { GyroOffset off {0}; MPU6050_RawData raw; int32_t sum_x 0, sum_y 0, sum_z 0; const int N 500; for (int i 0; i N; i) { MPU6050_ReadRaw(raw); sum_x raw.gyro_x; sum_y raw.gyro_y; sum_z raw.gyro_z; HAL_Delay(2); // 约 500Hz 采样避开传感器更新节奏 } off.gyro_x_offset (float)sum_x / N; off.gyro_y_offset (float)sum_y / N; off.gyro_z_offset (float)sum_z / N; return off; }校准的时候模块必须真的静止放在桌面上别碰风扇吹着或者桌子在晃都会让零偏不准。500 次采样在 2ms 间隔下大约 1 秒这个时间够用。如果你发现校准后角度还是漂先检查是不是校准时没放稳而不是急着调卡尔曼参数。3.3 把原始值换算成物理量灵敏度系数别用错读出来的int16_t要换算成有物理意义的单位卡尔曼滤波里用的是角速度°/s和加速度g。换算关系取决于你前面设的量程角速度gyro_dps raw_gyro / 65.5±500°/s 量程加速度acc_g raw_acc / 8192.0±4g 量程void MPU6050_ToPhysical(MPU6050_RawData *raw, GyroOffset *off, float *gx, float *gy, float *gz, float *ax, float *ay, float *az) { *gx (raw-gyro_x - off-gyro_x_offset) / 65.5f; *gy (raw-gyro_y - off-gyro_y_offset) / 65.5f; *gz (raw-gyro_z - off-gyro_z_offset) / 65.5f; *ax raw-acc_x / 8192.0f; *ay raw-acc_y / 8192.0f; *az raw-acc_z / 8192.0f; }这里减零偏只对角速度做加速度的零偏通常很小而且加速度计可以通过后续的姿态解算自动修正不必单独校准。如果你用的是 ±2g 和 ±250°/s 的默认量程系数要换成 16384 和 131用错了数据会差 2 到 4 倍这种错误在串口上看起来「有数」但滤波结果完全不对属于典型的玄学问题源头。4. 卡尔曼滤波落地从状态方程到 C 代码的完整映射4.1 为什么姿态解算用卡尔曼而不是互补滤波互补滤波和卡尔曼滤波都能做姿态融合区别在于互补滤波是固定权重的加权平均实现简单但权重不会根据实际情况自适应卡尔曼滤波则根据「预测的不确定度」和「观测的不确定度」动态调整权重在传感器噪声变化时表现更稳。对于 MPU6050 这种噪声特性比较明确的传感器卡尔曼滤波能给出更平滑的角度曲线尤其是在有振动或者快速运动时。不过要说明一点标准卡尔曼滤波假设系统是线性的而姿态解算本质是非线性的。工程上常用的做法是只对俯仰角和横滚角分别做一维卡尔曼滤波把非线性问题局部线性化。对于大多数平衡车、云台、倾角监测场景这个简化完全够用。如果你要做四元数级别的全姿态融合那需要扩展卡尔曼或者 Mahony 滤波复杂度会高一个量级。4.2 一维卡尔曼滤波的五个方程与代码对应一维卡尔曼滤波的核心就五个方程我把它和代码一一对应起来状态预测angle angle (gyro - bias) * dt误差协方差预测P P Q * dt卡尔曼增益K P / (P R)状态更新angle angle K * (accel_angle - angle)误差协方差更新P (1 - K) * P其中Q是过程噪声协方差R是观测噪声协方差bias是陀螺仪零偏的在线估计。下面是一个可以直接用的实现// kalman.c typedef struct { float angle; // 滤波后的角度 float bias; // 陀螺仪零偏估计 float P[2][2]; // 误差协方差矩阵 float Q_angle; // 角度过程噪声 float Q_bias; // 零偏过程噪声 float R_measure; // 观测噪声 } KalmanFilter; void Kalman_Init(KalmanFilter *kf) { kf-angle 0.0f; kf-bias 0.0f; kf-P[0][0] 0.0f; kf-P[0][1] 0.0f; kf-P[1][0] 0.0f; kf-P[1][1] 0.0f; kf-Q_angle 0.001f; kf-Q_bias 0.003f; kf-R_measure 0.03f; } float Kalman_Update(KalmanFilter *kf, float new_angle, float new_rate, float dt) { // 预测 float rate new_rate - kf-bias; kf-angle dt * rate; kf-P[0][0] dt * (dt * kf-P[1][1] - kf-P[0][1] - kf-P[1][0] kf-Q_angle); kf-P[0][1] - dt * kf-P[1][1]; kf-P[1][0] - dt * kf-P[1][1]; kf-P[1][1] kf-Q_bias * dt; // 更新 float S kf-P[0][0] kf-R_measure; float K[2]; K[0] kf-P[0][0] / S; K[1] kf-P[1][0] / S; float y new_angle - kf-angle; kf-angle K[0] * y; kf-bias K[1] * y; float P00_temp kf-P[0][0]; float P01_temp kf-P[0][1]; kf-P[0][0] - K[0] * P00_temp; kf-P[0][1] - K[0] * P01_temp; kf-P[1][0] - K[1] * P00_temp; kf-P[1][1] - K[1] * P01_temp; return kf-angle; }new_angle是从加速度计算出来的角度new_rate是陀螺仪的角速度dt是两次调用的时间间隔。注意dt必须用实际的时间差不能用固定的 10ms 去糊弄因为你的循环里可能还有串口打印、其他任务实际间隔会波动。我一般用 SysTick 或者定时器记录微秒级时间戳每次算差值。4.3 加速度计角度计算与 Q、R 参数的整定逻辑加速度计算角度用反三角函数俯仰角pitch atan2(-ax, sqrt(ay*ay az*az)) * 57.29578横滚角roll atan2(ay, az) * 57.29578。注意atan2的参数顺序和符号搞反了角度方向会反这是另一个常见翻车点。float accel_pitch atan2f(-ax, sqrtf(ay * ay az * az)) * 57.29578f; float accel_roll atan2f(ay, az) * 57.29578f;Q 和 R 的整定没有公式靠试。Q_angle越大滤波器越信任陀螺仪的积分角度响应快但噪声大R_measure越大越信任加速度计角度平滑但滞后明显。我的经验起点是Q_angle 0.001、Q_bias 0.003、R_measure 0.03然后根据现象调现象调整方向角度跟随慢、滞后减小 R_measure 或增大 Q_angle角度抖动大增大 R_measure 或减小 Q_angle静止时缓慢漂移增大 Q_bias让零偏估计更快收敛快速运动时角度冲过头减小 Q_angle调参的时候用串口把原始角度和滤波角度一起打印出来在电脑上画曲线看比盯着数字调直观得多。5. 避坑与排查那些让数据「看起来有数但根本不能用」的问题5.1 现象串口有数据但全是 0 或者全是 65535原因通常是 I2C 通信根本没成功HAL_I2C_Mem_Read返回了错误但代码没检查buf 里保持初始值。解决方法是每个 I2C 调用后检查返回值失败时打印错误码。另外确认MPU6050_ADDR左移了一位HAL 库不接受 7 位地址直接传入。5.2 现象角度静止时缓慢单向漂移这是陀螺仪零偏没校准或者校准不准。先确认校准时代码真的在静止状态采样再确认零偏减在了正确的位置。如果校准后仍然漂可能是温度变化导致零偏漂移可以在卡尔曼滤波里让Q_bias大一点让零偏在线估计跟上变化。注意Q_bias太大会让角度在运动时被「拉偏」需要折中。5.3 现象角度方向反了或者轴对不上MPU6050 的芯片坐标系和你的安装方向不一定一致。如果俯仰角正方向反了把atan2里的符号改一下如果横滚和俯仰对调了检查你用的是哪两个轴算的。最稳妥的办法是拿模块实际倾斜观察串口输出哪个角在变、往哪个方向变然后对应调整公式别死记教程里的轴映射。5.4 现象快速转动时角度明显滞后或冲过头这是 Q_angle 和 R_measure 的比例不合适。快速运动时加速度计会受到离心力干扰观测值本身不可信此时应该更信任陀螺仪也就是增大 Q_angle 或增大 R_measure。但增大 R_measure 会让静止时的噪声抑制变弱所以要在「静止稳」和「动态准」之间找平衡。如果应用场景以静态倾角为主优先保静止稳定如果是平衡车优先保动态响应。5.5 现象dt不稳定导致滤波结果忽好忽坏如果你的主循环里有HAL_Delay或者串口打印实际dt会随打印内容长度变化。解决办法是用定时器中断以固定频率触发采样和滤波主循环只负责取结果和打印。我一般用 TIM2 配 5ms 中断在中断里读传感器、跑卡尔曼主循环只读全局变量。这样dt恒定为 0.005s滤波表现会稳定很多。6. 进阶技巧把卡尔曼滤波从「能跑」推到「好用」6.1 用定时器中断固定采样周期消除 dt 抖动前面提过dt抖动是滤波效果不稳定的隐形杀手。具体做法是配置 TIM2 为 200Hz 更新中断在中断服务函数里置一个标志位主循环检测到标志位后执行读取和滤波。或者更直接一点把读取和滤波都放在中断里但注意中断里不要做串口打印这种耗时操作。// tim.c 中断回调 void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { if (htim-Instance TIM2) { MPU6050_ReadRaw(raw); MPU6050_ToPhysical(raw, offset, gx, gy, gz, ax, ay, az); float accel_angle atan2f(-ax, sqrtf(ay*ay az*az)) * 57.29578f; pitch Kalman_Update(kf_pitch, accel_angle, gy, 0.005f); } }这样dt恒为 0.005s卡尔曼滤波的预测步长稳定角度曲线会明显更干净。6.2 双轴独立滤波与串口波形观察俯仰和横滚各用一个KalmanFilter实例互不干扰。调参时把accel_angle和pitch一起打印成两列用串口波形软件看。你会看到原始角度毛刺很多滤波角度平滑但有轻微滞后这个滞后就是卡尔曼增益在起作用。如果滞后大到影响使用就按前面表格减小 R_measure。6.3 温度补偿的取舍什么时候值得做MPU6050 自带温度传感器零偏随温度变化大约是每摄氏度零点几度每秒的量级。对于室温下短时间运行的项目零偏校准一次就够。但如果你的设备要在户外或者温度变化大的环境长时间运行零偏会慢慢漂这时候可以定期用静止时的角速度重新估计零偏或者建一个简单的温度-零偏查找表。不过对大多数桌面级项目把校准做扎实比加温度补偿更划算。6.4 一个我踩过的坑别在中断里调用 HAL_Delay早期我把卡尔曼滤波放在定时器中断里然后在中断里调用了HAL_Delay等 I2C 完成结果系统直接卡死。原因是HAL_Delay依赖 SysTick 中断而定时器中断优先级如果高于 SysTick就会死等。后来改成 I2C 用带超时的阻塞式读取超时给 10ms不再用HAL_Delay问题消失。这个坑不限于 MPU6050任何在中断里做阻塞操作的项目都会遇到。6.5 验证滤波效果的土办法拿已知角度对照没有转台的情况下可以用手机的水平仪 App 做粗略对照。把模块和手机固定在同一块板子上倾斜到几个已知角度看串口输出的滤波角度和手机读数差多少。如果误差在 1 到 2 度以内说明滤波和校准都到位了。如果差得远先查灵敏度系数和零偏再查卡尔曼参数别一上来就怀疑算法。我自己的习惯是每次改完参数先静止看 30 秒漂不漂再慢速倾斜看跟随最后快速晃动看有没有发散。这三步走完基本就能判断这组参数能不能用。希望帮到你。本文还有配套的精品资源点击获取