简介STM32F1平台下基于双IIC接口读取MPU6050与MPU9250的完整工程面向嵌入式开发学习者及需要做姿态解算的工程师适用无人机姿态估计、机器人平衡控制、穿戴设备等场景。工程演示了如何通过IIC1、IIC2分别连接六轴与九轴传感器并将加速度计、陀螺仪、磁力计数据经卡尔曼滤波融合输出pitch、roll、yaw角度与指南针角度同时包含IIC总线时序配置、传感器寄存器初始化、数据读取流程。资源打包为7z压缩格式共179个文件以C源码、H头文件、Keil工程配置与编译输出uvprojx、uvoptx、hex、axf为主约1.91MB结构完整可直接在MDK中打开分析。已有1066人学习下载。除基础IIC读写与寄存器配置外还涵盖传感器数据校准、卡尔曼滤波参数调试思路以及姿态解算中的常见噪声处理适合对照代码理解M3平台下传感器融合项目的完整实现。 做姿态解算的人应该都有过这种经历手里一块STM32F1先调通MPU6050读出加速度和角速度算pitch/roll的时候挺开心结果一到yaw就头疼——纯靠陀螺仪积分两分钟不到角度就开始飘半个小时后甚至能转出三百多度。你以为是程序写错了其实不是这是航向角没有绝对参考的必然结果。后来我给系统加了一块9250磁力计传感器用IIC1挂MPU6050、IIC2挂9250两路IIC并行读数据再用卡尔曼滤波把六轴和磁力计数据融合起来终于同时稳定输出了pitch、roll、yaw和指南针角度。这个过程里踩的坑不少从IIC总线的地址冲突到磁力计的椭圆畸变校准每一步都值得单独拿出来说说。这篇文章写给手里已经有MPU6050和9250模块、想在STM32F1上把九轴姿态解算完整跑通的人。我会从两路IIC的设计思路、传感器初始化、数据校准、卡尔曼滤波实现、yaw融合这几个方面把整个项目的关键节点和调参经验都过一遍。1. 为什么把MPU6050和9250分开挂两路IIC1.1 地址冲突只是表象更关键的是总线容错很多人第一反应是两个传感器挂同一路IIC不就行了IIC协议本身支持一主多从理论上完全可以。但实际做项目时你会发现MPU6050的IIC地址是0x68或0x69由AD0引脚电平决定而如果你的9250模块是MPU9250这种九轴芯片它的加速度计和陀螺仪部分地址同样落在0x68/0x69上。就算你把一个设成0x68、另一个设成0x69从地址角度解决了冲突还有个隐患——IIC总线上某个器件一旦异常拉低SDA整条总线都会锁死姿态数据直接全断。我做这个项目之前就犯过这样的错误把MPU6050和磁力计模块挂在同一路IIC上结果某次电机启动时大电流干扰把其中一个芯片的状态搞乱了SDA被拉死程序死在读数据函数里连串口调试都没法看。后来改成两路IICMPU6050挂在IIC19250挂在IIC2一路出问题最多死一路另一路的数据还能帮你判断是传感器坏了还是通信链路断了排查效率完全不是一个级别。1.2 STM32F1上的IIC资源分配与上拉电阻选择STM32F103系列自带两个硬件IIC外设I2C1对应PB6SCL和PB7SDAI2C2对应PB10SCL和PB11SDA。如果引脚没被其他外设占用我建议直接用硬件IIC省CPU资源。但说句实话F1的硬件IIC确实有些历史问题事件标志处理不好容易卡死。如果你不想在这个上面耗时间用普通GPIO模拟IIC反而更稳引脚选择也自由。我用的分配方式是总线引脚挂载设备模式IIC1PB6 / PB7MPU6050硬件I2C1IIC2PB10 / PB119250磁力计软件模拟IIC这样做的理由有两个MPU6050是主传感器读取频率高用硬件IIC可以减少模拟时序的CPU开销9250的数据读取频率可以低一些软件模拟完全够用。上拉电阻方面IIC是开漏结构必须有上拉才能输出高电平。标准IIC模式100kHz用4.7kΩ没问题快速模式400kHz建议用2.2kΩ。但要注意如果你的传感器模块板上已经自带IIC上拉电阻STM32引脚这边又加一个等效上拉电阻会变小反而可能导致信号边沿变差。我建议先看模块的原理图大多数MPU6050模块和9250模块板上都已经有上拉电阻了直接连就可以。2. 初始化与原始数据读取流程2.1 先从WHO_AM_I寄存器确认设备在线先把硬件连接搞定然后写一个最简单的读寄存器函数读每个传感器的ID寄存器。这一步看起来基础其实是排查接线错误最有效的手段如果读回来的值一直是0xFF说明SDA和SCL根本没通查线路如果读回来是0x00多半是地址不对或者设备还在复位状态只有读到正确ID才谈得上后续配置。MPU6050的WHO_AM_I寄存器地址是0x75默认复位值是0x68。9250这边要看具体芯片型号如果是MPU9250内置的磁力计AK8963它的WIA寄存器地址是0x00读出来是0x48如果是独立的磁力计芯片如HMC5883L或QMC5883LID寄存器地址和值又不一样。我的建议是拿到模块先看丝印查对应数据手册把WHO_AM_I这一步做扎实后面能省很多事。2.2 传感器配置参数的工程取舍MPU6050上电后默认处于睡眠模式要先在PWR_MGMT_1寄存器0x6B把SLEEP位清零顺便把DEVICE_RESET位置1做一次复位延时100ms再操作。接下来需要配置几个关键参数加速度计量程我选的±2g因为姿态解算时重力加速度本身只有1g量程太大精度反而浪费±2g下的灵敏度是16384 LSB/g。陀螺仪量程选的±500dps灵敏度是65.5 LSB/(°/s)。±250dps虽然精度更高但手工旋转模块稍微快一点就容易超量程±2000dps量程够大但量化噪声也大。±500dps是平衡之选。DLPF数字低通滤波器带宽设置为20Hz左右可以有效滤掉高频振动噪声。如果做四轴这种本身振动大的场景这个值再压低一点也行。采样率设成100Hz也就是陀螺仪输出速率寄存器0x19设置成99分频系数加1。这个频率和后续卡尔曼滤波的dt匹配很重要。9250磁力计这边如果是MPU9250走内部I2C master访问AK8963需要先把MPU9250的I2C master模式使能然后通过I2C_SLV0寄存器组指向AK8963的寄存器地址。这个过程稍微绕但其实就是往MPU9250的0x24I2C_SLV0_ADDR写0x0CAK8963的I2C地址往0x25I2C_SLV0_REG写要读的AK8963内部寄存器地址往0x26I2C_SLV0_CTRL写使能位和数据长度。如果你的9250是独立的磁力计芯片直接按芯片手册配置连续测量模式就行简单很多。2.3 数据读取的核心代码模式IIC读多字节数据是所有后续工作的基础寄存器地址在MPU6050里是自动递增的所以可以一次连续读6个字节拿全加速度或陀螺仪数据。以MPU6050读加速度为例uint8_t buf[6]; uint8_t reg 0x3B; // ACCEL_XOUT_H MPU6050_ReadRegs(reg, buf, 6); int16_t acc_x (int16_t)((buf[0] 8) | buf[1]); int16_t acc_y (int16_t)((buf[2] 8) | buf[3]); int16_t acc_z (int16_t)((buf[4] 8) | buf[5]);这里有个容易踩的坑MPU6050的寄存器数据是大端格式高字节在前。很多人第一次读发现数据忽大忽小、符号不对多半是把高低字节拼反了。转成int16_t的时候要注意虽然buf[0]是高字节但直接做位或运算后结果默认是int类型如果最高位是1转成int16_t才能得到正确的负数补码值。3. 数据校准不校准就别谈滤波3.1 陀螺仪零偏标定陀螺仪静止时输出应该全是0但实际芯片不会那么理想静止时每个轴都有一个固定的偏移量。这个偏移量虽然不大但在积分后会线性累积直接摧毁yaw。标定方法很简单模块静止放在桌面上采集200到500个样本对每个轴求平均这个平均值就是零偏。之后每次读陀螺仪都把这个零偏减掉。有一点需要注意陀螺仪零偏会随温度漂移刚开机和运行半小时后的零偏可能差不少。条件允许的话在传感器达到工作温度后再做一次标定或者程序中加温度补偿。做自平衡车这类对零偏敏感的项目这个细节非常关键。3.2 加速度计六面校准加速度计校准的核心是消除零点偏移和尺度因子误差。贴片焊接的应力、模块制造公差都会导致静止时加速度计读到的数值不是标准的0、0、1g。最简单也最实用的校准方法是六面校准把模块分别以六个面朝下静止放置记录每个面的三轴读数。以Z轴为例正面朝上时Z轴读数应该是1g背面朝上时是-1g两者的中值就是Z轴的零偏两者差值的一半就是Z轴的尺度因子修正。X轴和Y轴同理。如果嫌六面校准麻烦至少做一步“水平校准”把模块放在水平桌面上读出的加速度Z轴不接近16384对应1gX/Y轴不接近0那就说明有偏移需要把偏移记录下来补偿。这一步不做后面用加速度计计算pitch/roll时角度会在静止状态下出现明显的偏置误差而且怎么调卡尔曼参数都消不掉。3.3 磁力计椭球校准指南针准不准的胜负手磁力计是最需要校准的传感器没有之一。芯片自身有制造误差周围的电路、螺丝、外壳等铁磁性物质会产生硬磁干扰和软磁干扰导致磁力计在空间中旋转时输出不再是一个圆心在原点的球面而是一个偏移了圆心、半径方向上有缩放的不规则椭球。最常用的校准方案是椭球拟合拿着模块在空中画“8”字或者绕各种方向旋转让X/Y/Z三轴磁场数据充分覆盖空间中各个方向采集几百组数据后用最小二乘法拟合椭球求出中心偏移量和三轴缩放因子。实际操作中如果只关心指南针的水平航向角一个偷懒但有效的做法是只做中心校准采集过程中记录三轴的最大值和最小值中心偏移就是(max min) / 2。这个方法能解决大部分的硬磁干扰指南针角度从偏十几度修正到偏两三度以内。想要更进一步就需要完整的椭球拟合网上有现成的上位机工具可以做。磁力计校准后指南针角度才真正可信。我见过太多人问“为什么我的指南针角度偏了20度”结果都是跳过校准直接读原始数据。这一关不迈过去后面的融合算法做得再漂亮也是白搭。4. 卡尔曼滤波器解算pitch和roll4.1 为什么选卡尔曼滤波而不是互补滤波现在姿态解算主流方案有互补滤波、Mahony滤波和卡尔曼滤波三种。互补滤波的核心是用一个可调系数在加速度计角度和陀螺仪积分角度之间做加权代码简单、运行快但alpha参数调来调去总有个死结抗噪强就响应慢响应快就噪声大。卡尔曼滤波的厉害之处在于它能根据当前运动状态动态调整“更相信模型预测”还是“更相信测量数据”效果更好代价是需要理解和调试相对多一些。但有一点要说明白这里说的卡尔曼滤波是工程上非常经典的一维卡尔曼姿态估计状态量只有2个协方差矩阵是2乘2计算量极小。它和四元数卡尔曼滤波不是一回事。对于pitch和roll重力方向是天然的绝对参考所以用加速度计算出的角度作为观测量用陀螺仪角速度做状态预测两者融合就能得到平滑且不漂移的倾角。4.2 卡尔曼滤波的状态方程和核心代码状态量定义为typedef struct { float angle; // 当前角度 float bias; // 陀螺仪零偏估计 } KalmanState; typedef struct { float Q_angle; // 角度过程噪声协方差 float Q_bias; // 零偏过程噪声协方差 float R_measure; // 测量噪声协方差 float P[2][2]; // 协方差矩阵 KalmanState state; float dt; } KalmanFilter;状态预测方程是angle angle (gyro_rate - bias) * dt bias bias对应代码void Kalman_Update(KalmanFilter *kf, float gyro_rate, float acc_angle) { float S, K_0, K_1, y; // 预测 kf-state.angle (gyro_rate - kf-state.bias) * kf-dt; kf-P[0][0] kf-dt * (kf-dt * kf-P[1][1] - kf-P[0][1] - kf-P[1][0] kf-Q_angle); kf-P[0][1] - kf-dt * kf-P[1][1]; kf-P[1][0] - kf-dt * kf-P[1][1]; kf-P[1][1] kf-Q_bias * kf-dt; // 计算卡尔曼增益 S kf-P[0][0] kf-R_measure; K_0 kf-P[0][0] / S; K_1 kf-P[1][0] / S; // 使用加速度计角度更新 y acc_angle - kf-state.angle; kf-state.angle K_0 * y; kf-state.bias K_1 * y; kf-P[0][0] - K_0 * kf-P[0][0]; kf-P[0][1] - K_0 * kf-P[0][1]; kf-P[1][0] - K_1 * kf-P[0][0]; kf-P[1][1] - K_1 * kf-P[0][1]; }4.3 加速度计角度怎么算加速度计计算pitch和roll的标准公式是float acc_pitch atan2f(-acc_x, sqrtf(acc_y * acc_y acc_z * acc_z)) * 57.29578f; float acc_roll atan2f(acc_y, acc_z) * 57.29578f;注意坐标系约定我这里默认模块水平放置时X轴朝前、Y轴朝右、Z轴朝下加速度单位已经换算成g。如果你的模块安装方向不一样符号和公式可能需要翻转建议用一个已知倾角去验证。另外加速度计在剧烈运动时会受到运动加速度干扰此时算出的角度会带着很大的毛刺这不是卡尔曼滤波的bug而是加速度计本身的物理特性。加大Q_angle可以加快恢复速度但也会让静止时的噪声变大这个平衡要自己试。4.4 调参经验三个参数的物理意义卡尔曼滤波看起来参数不少实际要调的只有三个Q_angle、Q_bias、R_measure。我的初始值一般取Q_angle0.001、Q_bias0.003、R_measure0.03从这三个值出发按下面思路调整Q_angle表示你对陀螺仪积分角度的信任程度。值越大滤波器越相信加速度计观测响应越快但噪声越大。Q_bias表示陀螺仪零偏漂移的速度。值越大滤波器越积极地估计零偏但过大时角度会出现过冲。R_measure表示加速度计角度的噪声水平。值越大滤波器越不相信测量值曲线越平滑但滞后越大。调参方法建议先静止观察角度噪声再用手快速晃动看跟踪速度。如果静止时角度曲线毛毛躁躁减小R_measure或增大Q_angle如果转动时感觉“拖泥带水”增大Q_angle或减小R_measure。温度变化大导致角度缓慢漂移时检查Q_bias是否需要调大。5. yaw角和指南针角度的融合策略5.1 为什么纯积分yaw必然漂移pitch和roll有重力这个绝对参考可以通过加速度计不断修正。但yaw绕着重力方向旋转加速度计提供的重力信息在这个轴上是零投影一点忙都帮不上。陀螺仪yaw的积分完全靠初始值起步而角速度零偏哪怕只有1度每秒一分钟也会积累60度的误差。所以只要你还把yaw交给纯积分无论怎么调卡尔曼参数都没救必须有磁力计提供绝对航向参考。5.2 磁力计航向角的计算与水平投影修正磁力计读到的原始值经过椭球校准后还只是在传感器坐标系下的磁场分量。如果模块是完全水平的指南针角度可以直接用atan2f(my, mx)计算单位转换后得到0到360度或-180到180度的航向角。但实际应用中模块很难保证完全水平一旦有倾斜磁力计X和Y轴的读数就会混入Z轴分量直接算出来的角度会明显失真。正确的做法是把磁场向量投影到水平面上。利用已经解算出的pitch和roll做倾角补偿float mx_h mx * cosf(pitch) my * sinf(pitch) * sinf(roll) mz * sinf(pitch) * cosf(roll); float my_h my * cosf(roll) - mz * sinf(roll); float mag_yaw atan2f(my_h, mx_h) * 57.29578f;这里的pitch和roll用卡尔曼滤波输出的角度精度要求不高也能用。注意补偿前后的坐标轴方向要和你的安装方式对得上否则出来的yaw方向可能是反的或是错的。5.3 磁力计航向角与陀螺仪yaw的融合现在有了两个表示yaw的信息一个是从陀螺仪积分来的yaw_gyro短期准、长期漂一个是磁力计算出的mag_yaw绝对准、但噪声大且响应慢。工程上最简单的融合方法是互补修正环在陀螺仪积分基础上叠加一个比例修正项float angle_diff mag_yaw - yaw_est; // 角度差转到 -180 ~ 180 while (angle_diff 180.0f) angle_diff - 360.0f; while (angle_diff -180.0f) angle_diff 360.0f; yaw_est (gyro_z - bias_z) * dt k_yaw * angle_diff * dt;k_yaw是一个比例系数控制磁力计修正的力度可以取2到5之间。这个方法的原理就是比例控制器角度差为零时不改变积分输出角度偏差越大修正越快。角度差包裹到-180到180之间的处理特别重要否则在正负180度跳变时修正方向会突然反掉导致yaw疯狂旋转。如果想把yaw融合做得更“正规”也可以把mag_yaw作为观测量写进卡尔曼滤波器但需要额外处理角度环绕问题代码量会明显增加。我的经验是对于大多数需要稳定航向角输出的项目互补修正环在工程效果上已经够用做四轴、云台、小车都没问题。5.4 磁偏角补偿atan2f算出来的是相对于磁北的方向角和地理北之间还差一个磁偏角。不同地域磁偏角不一样一般在-10度到20度之间。如果你需要输出地理坐标系下的真实航向角可以在最后加一个磁偏角常量比如中国中部地区大约偏西4到6度。写成代码就是float yaw_true yaw_est MAG_DECLINATION;如果不确定当地磁偏角可以用在线工具查一下也可以直接在指南针角度上加个可配置的偏置实地旋转对比补偿。6. 移植和调试中遇到的几个工程坑6.1 IIC总线被拉死与恢复办法这个问题出现得最频繁。表现是程序偶尔卡在读传感器数据的函数里或者串口输出固定在一组值不动了。原因多种多样上电时序不对、外部干扰、空闲状态下SDA被某个器件错误拉低。处理办法分两步一是程序上在每次通信前检查超时超时后把SCL翻转9个时钟周期强制释放总线再重新初始化SDA和SCL引脚电平二是硬件上检查上拉电阻和电源稳定性磁力计尤其要远离电机大电流走线。6.2 dt抖动对卡尔曼滤波的影响卡尔曼滤波的预测方程里有个dt这个值必须是真实的采样间隔。很多人图省事直接写一个固定值但实际程序里如果循环中有串口打印之类的耗时操作dt就会抖动角度就会表现出诡异的振荡。我的做法是用定时器或者DWT计数器在每次更新前记录时间戳算出真实dt再传给滤波器。相比之下传感器采样率设置成多少是次要的dt的不确定性才对稳定性影响最大。6.3 角度跳变与上位机显示问题pitch范围一般限制在正负90度roll限制在正负180度yaw和指南针角度都是环绕量在正负180度交界处会突然跳变。这种跳变在数学上完全正常但上位机如果用直线画曲线就会出现一条从179度拉到-179度的斜线视觉上非常吓人。处理方法有两个一是上位机解码时做unwrap操作把相邻两次采样角度差超过180度的情况转换成连续的累加角度二是在单片机端输出连续累积的yaw不取模让曲线不跳变。具体用哪种看你的显示需求但一定要提前想好别等看到跳线了再抓瞎。串口输出方面浮点数用printf格式化在F1上占资源也比较多如果数据量大建议把浮点转成整数后发送或者用联合体把float拆成4字节上位机再拼回去效率高得多。6.4 安装方向和坐标轴方向一致性最后提醒一个经常让人怀疑人生的点传感器在你的板子上的安装方向决定了读回来的数据和数学公式里的坐标轴是否匹配。同样的代码模块水平放置时数据正常竖起来装就全乱了几乎可以肯定是坐标方向不匹配。先把每个轴的正方向标出来再按实际安装调整公式里的符号。我在做这个项目的第一天pitch和roll老是反的后来发现是模块X轴方向和我预期相反把atan2f里的负号改一下就好了。这种错误不解决后面所有滤波和融合都会在错误的基础上越走越远。这个项目整体做下来我最深的体会是姿态解算的算法本身并不神秘真正的难点在于把传感器的数据弄干净、把坐标方向弄对、把总线通信弄稳定。只要你把校准和IIC这两关迈过去卡尔曼滤波的融合效果其实是水到渠成的事。本文还有配套的精品资源点击获取