简介面向STM32F1嵌入式开发者以MPU6050IIC1与MPU9250IIC2双传感器读取为基础结合卡尔曼滤波输出pitch、roll、yaw姿态角及指南针角度适合学习惯性数据融合与姿态解算的入门及进阶实践。工程共179个文件约1.91MB以C/H/C源码为主辅以IAR与Keil工程文件uvprojx/uvoptx、编译生成文件o/hex/axf及说明文档覆盖双路IIC通信配置、传感器寄存器读写、滤波参数调整等关键代码可帮助读者快速定位逻辑并理解工程结构。已有1066人学习下载。通过学习此工程读者不仅能了解双路IIC同时挂载不同传感器的设计思路还能掌握卡尔曼滤波在加速度计、陀螺仪、磁力计数据融合中的实际用法为平衡车、四轴飞行器、电子罗盘等方向提供可直接参考的代码基础尤其适合在Keil环境下对照学习实时姿态解算流程。 做姿态解算绕不开三个核心传感器陀螺仪拿角速度、加速度计拿重力方向、磁力计拿地磁北方向。这次我在 STM32F103 上同时挂了两颗传感器——MPU6050 走 IIC19250MPU9250走 IIC2——把 6 轴数据和磁力计数据合起来做卡尔曼滤波输出俯仰pitch、横滚roll、偏航yaw三个姿态角外带一路经过倾斜补偿的指南针角度。项目本身不复杂但真正跑稳却有几步关键动作尤其是两个传感器默认 I2C 地址相同、磁力计需要单独校准、卡尔曼滤波参数怎么配这三个点几乎决定了调试是否顺利。这篇就把整个链路从硬件接线到滤波解算再到问题排查完整复现一遍适合准备写飞控、平衡车或者想把手头 IMU 模块用起来的开发者参考。1. 项目整体设计与思路拆解1.1 为什么用 STM32F1 双 IIC 方案STM32F103 算是入门级 MCU 里最务实的选择价格低、资料多、两个硬件 I2C 外设刚好够用。同一个工程里I2C1 接 MPU6050I2C2 接 9250互不干扰。有人会纠结硬件 I2C 不如软件模拟稳定实测下来STM32 硬件 I2C 配合阻塞式读写在 400kHz 下读取 6 轴和磁力计数据完全够用真正影响稳定性的往往是接线和上拉电阻而不是芯片外设本身。标题里的“9250”我按最常见方案理解为 MPU9250也就是内部含 AK8963 磁力计的 9 轴传感器。在这个项目里它主要用来读磁场配合 MCU 算出指南针角度。如果你手头拿的是单独磁力计模块比如 HMC5883L 或 QMC5883L寄存器结构不同但读取思路完全一致。1.2 为什么不把两个传感器挂同一条 IIC 总线这个点值得单独说。MPU6050 和 MPU9250 的默认 I2C 地址都是 0x68如果挂同一条总线就必须通过 AD0 引脚改地址一个设 0x68另一个设 0x69。不过地址改完还有隐患总线上的电容负载变大接线一旦拉长通信时序容易劣化而且 MPU9250 内部的磁力计 AK8963 还挂在 0x0C 地址继续往同一条总线上增加设备后面出问题很难定位是哪个设备把总线拉死的。换用两条独立 IIC 总线之后I2C1 只管 6050I2C2 只管 9250 和 AK8963每个总线上的设备少、地址简单逻辑也干净。调试时还能单独拉一条总线的波形快速确认哪个设备没响应不用在混挂环境里猜。对于这种只有两路 I2C 外设的 MCU两个传感器各占一条是性价比最高的接线方式。1.3 卡尔曼滤波对比互补滤波很多人喜欢用互补滤波代码短、跑得快几行就能得到角度。但互补滤波本质是把加速度计得到的角度和陀螺仪积分的角速度做加权平均它对陀螺仪零漂的估计能力弱长时间静止会看到角度慢慢跑偏。卡尔曼滤波则把陀螺仪的零偏当成状态量和角度一起估计每次用加速度计或磁力计观测值去修正表现要稳不少。当然卡尔曼滤波不是万能的它需要先建模再调 Q 和 R 两个噪声协方差参数模型写错或参数差太远滤波效果还不如互补滤波。这个项目坚持用卡尔曼是因为偏航角yaw要融合磁力计数据磁力计信号容易被环境磁干扰污染有量测噪声模型来处理会比单纯的加权平均稳定得多。2. 硬件连接与 IIC 初始化2.1 引脚分配与接线对照STM32F103 的 I2C1 固定为 PB6SCL、PB7SDAI2C2 固定为 PB10SCL、PB11SDA接线如下表信号STM32F103MPU6050IIC1MPU9250IIC2电源3.3VVCCVCC地GNDGNDGNDSCL1PB6SCL-SDA1PB7SDA-SCL2PB10-SCLSDA2PB11-SDAAD0GNDAD0-供电用 3.3V不要图方便直接上 5V。虽然不少模块板载了稳压但上拉电阻很容易把 3.3V 的 MCU IO 灌坏。两根 IIC 信号线要各放一个 4.7kΩ 上拉电阻到 3.3V有些现成模块板上已经焊好上拉不需要额外加但也有模块为了兼容 5V 系统没焊上电后最好先用逻辑分析仪或示波器确认 SCL/SDA 空闲电平是 3.3V 左右。这个接线方案我在 20cm 杜邦线范围内验证过400kHz 速度稳定。如果飞线超过 30cm建议把速度降到 100kHz或者换带屏蔽的短线否则波形振铃会带来偶发的 ACK 丢包。2.2 IIC 总线参数设置使用 STM32CubeMX 生成工程时I2C1 和 I2C2 的参数可以按同一套配置初始化代码大致如下void MX_I2C1_Init(void) { hi2c1.Instance I2C1; hi2c1.Init.ClockSpeed 400000; hi2c1.Init.DutyCycle I2C_DUTYCYCLE_2; hi2c1.Init.OwnAddress1 0; hi2c1.Init.AddressingMode I2C_ADDRESSINGMODE_7BIT; hi2c1.Init.DualAddressMode I2C_DUALADDRESS_DISABLE; hi2c1.Init.GeneralCallMode I2C_GENERALCALL_DISABLE; hi2c1.Init.NoStretchMode I2C_NOSTRETCH_DISABLE; HAL_I2C_Init(hi2c1); }I2C2 的代码只是把hi2c1换成hi2c2、I2C1换成I2C2其余相同。MPU6050 数据手册规定 I2C 时钟最大 400kHzAK8963 也支持 400kHz所以统一设置为 400kHz。如果模块上的上拉电阻低于 1kΩ高速传输时信号会有过冲出现怪异的读写异常此时把 ClockSpeed 降到 200kHz 到 300kHz 就能解决。GPIO 配置时要注意I2C 引脚必须设置为开漏输出并开启内部上拉或外部上拉。有人习惯把引脚设成推挽输出这会直接把总线电平拉死导致 ACK 永远回不来属于入门阶段最隐蔽的坑之一。2.3 上电自检流程初始化顺序建议这样先初始化两个 I2C 外设再逐个读取 who_am_i 寄存器确认设备在线然后清除休眠位。MPU6050 的 who_am_i 返回 0x68MPU9250 的主芯片也返回 0x68AK8963 返回 0x48。上电后如果读回来 0xFF 或 0x00八成是接线、供电或上拉问题。可以写一个简单的探测函数在启动阶段扫描三个地址并把结果通过串口打出来uint8_t who; HAL_I2C_Mem_Read(hi2c1, 0x681, 0x75, I2C_MEMADD_SIZE_8BIT, who, 1, 100); printf(IIC1 MPU6050: 0x%02X\n, who); HAL_I2C_Mem_Read(hi2c2, 0x681, 0x75, I2C_MEMADD_SIZE_8BIT, who, 1, 100); printf(IIC2 MPU9250: 0x%02X\n, who); HAL_I2C_Mem_Read(hi2c2, 0x0C1, 0x00, I2C_MEMADD_SIZE_8BIT, who, 1, 100); printf(IIC2 AK8963: 0x%02X\n, who);看到 68、68、48 三个地址依次回 ACK才能进入姿态解算主循环。这一步能帮你把问题隔离在读写环节而不是错误地在滤波环节里找 bug。3. MPU6050 加速度计/陀螺仪读取3.1 核心寄存器配置MPU6050 的配置集中在几个寄存器用一张表可以讲清寄存器地址写入值含义PWR_MGMT_10x6B0x00退出休眠使用内部 8MHz 时钟SMPLRT_DIV0x190x07采样率 8kHz / 8 1kHzCONFIG0x1A0x01DLPF 184Hz滤掉高频噪声GYRO_CONFIG0x1B0x08±500°/s灵敏度 65.5 LSB/°/sACCEL_CONFIG0x1C0x00±2g灵敏度 16384 LSB/g选 ±2g 和 ±500°/s是为了平衡分辨率和量程。四轴、平衡车这类场景横滚和俯仰角速率一般不会超过 400°/s500°/s 够用如果做高速旋转平台就要扩到 ±2000°/s但灵敏度会从 65.5 掉到 16.4噪声会明显变大。DLPF 选 184Hz 是比较均衡的选择。如果发现数据在剧烈运动下有明显延迟可以改到 0x0298Hz或 0x0342Hz但滤波效果会变弱需要靠卡尔曼滤波去补。3.2 读取原始数据与单位转换MPU6050 的寄存器 0x3B 开始按 X/Y/Z 顺序存放 16 位加速度计数据跳过温度寄存器后0x43 开始存放 16 位陀螺仪数据。用一次连续读取16 个字节就能拿到全部数据uint8_t buf[14]; int16_t acc[3], gyro[3]; HAL_I2C_Mem_Read(hi2c1, 0x681, 0x3B, I2C_MEMADD_SIZE_8BIT, buf, 14, 100); acc[0] (int16_t)(buf[0]8 | buf[1]); acc[1] (int16_t)(buf[2]8 | buf[3]); acc[2] (int16_t)(buf[4]8 | buf[5]); gyro[0] (int16_t)(buf[8]8 | buf[9]); gyro[1] (int16_t)(buf[10]8 | buf[11]); gyro[2] (int16_t)(buf[12]8 | buf[13]);这里要注意寄存器读回的是大端字节序高字节在前、低字节在后直接buf[0]8 | buf[1]就能拼成 int16_t。如果发现符号不对多半是寄存器地址错了或者模块方向装反。单位转换很简单float acc_g[3], gyro_dps[3]; acc_g[0] acc[0] / 16384.0f; acc_g[1] acc[1] / 16384.0f; acc_g[2] acc[2] / 16384.0f; gyro_dps[0] gyro[0] / 65.5f; gyro_dps[1] gyro[1] / 65.5f; gyro_dps[2] gyro[2] / 65.5f;除法后面一定要加.0f否则 C 语言整数除法会把数值直接截断成 0这个低级错误我在调试群里见过不止一次。3.3 加速度计解算 roll/pitch 角度加速度计在静止状态下测的是比力也就是重力方向所以可以从它还原出横滚和俯仰角。以常见的 X 轴朝前、Y 轴朝左、Z 轴朝上的机体坐标系为例float roll_acc atan2(-acc_g[0], sqrt(acc_g[1]*acc_g[1] acc_g[2]*acc_g[2])) * 180.0f / M_PI; float pitch_acc atan2(acc_g[1], acc_g[2]) * 180.0f / M_PI;符号取决于你的安装方向和坐标系定义跑起来之后把板子绕对应轴转一下方向反了就改对应轴的正负号。需要提醒的是加速度计解算的 roll/pitch 在快速运动时会被线性加速度污染所以这里算出的角度只适合作为卡尔曼滤波的观测值而不是直接输出。yaw 角无法从加速度计求出因为它感受不到绕重力轴旋转的变化。这个缺口由陀螺仪积分和磁力计补上。4. 9250 磁力计读取与罗盘角计算4.1 AK8963 寄存器读取流程在 MPU9250 模块中AK8963 是独立挂在 I2C 总线上的磁力计芯片地址为 0x0C。读取流程比 MPU6050 多一步先配置工作模式再判断数据就绪位。AK8963 的 CNTL1 寄存器0x0A写入 0x16表示 16 位输出、100Hz 连续测量模式 2。这个模式适合一直在转动的机器人平台如果设备大部分时间静止用单次测量模式更省电。读数据时先读 ST10x02确认 bit0 的 DRDY 置 1然后连续读 0x03 到 0x08 六个字节最后必须读 ST20x09清除中断标志。跳过 ST2 会导致下次 DRDY 状态是脏的拿到的可能是上一组残留数据这是磁力计读数跳变的一个隐蔽原因。uint8_t status, raw[6], st2; HAL_I2C_Mem_Read(hi2c2, 0x0C1, 0x02, I2C_MEMADD_SIZE_8BIT, status, 1, 100); if (status 0x01) { HAL_I2C_Mem_Read(hi2c2, 0x0C1, 0x03, I2C_MEMADD_SIZE_8BIT, raw, 6, 100); HAL_I2C_Mem_Read(hi2c2, 0x0C1, 0x09, I2C_MEMADD_SIZE_8BIT, st2, 1, 100); mag[0] (int16_t)(raw[0]8 | raw[1]); mag[1] (int16_t)(raw[2]8 | raw[3]); mag[2] (int16_t)(raw[4]8 | raw[5]); }注意 AK8963 同样是大端输出拼字节时要保持一致。4.2 罗盘角度计算与倾斜补偿水平放置时罗盘角可以直接用 X、Y 轴的磁场分量计算float heading atan2((float)mag[1], (float)mag[0]) * 180.0f / M_PI; if (heading 0) heading 360.0f;但实际设备很少保持水平板子一旦有 pitch 或 roll直接算出来的航向角会跟着姿态一起倾斜误差很大。需要先把磁场矢量投影回水平面float mx mag[0], my mag[1], mz mag[2]; float xh mx * cos(pitch) my * sin(roll) * sin(pitch) mz * cos(roll) * sin(pitch); float yh my * cos(roll) - mz * sin(roll); float heading atan2(yh, xh) * 180.0f / M_PI; if (heading 0) heading 360.0f;这里的 pitch 和 roll 就是卡尔曼滤波输出的角度用上一帧的滤波值即可不需要再额外处理。做倾斜补偿时角度单位必须统一成弧度C 语言里的sin和cos只接受弧度入参这个错位也会导致航向角在特定姿态下突然翻转。4.3 磁力计校准直接读出来的磁力计数据通常是不准的因为环境磁场、PCB 上走线、附近金属结构都会叠加出偏移量。最常用的校准方法叫“最小值最大值校准”把模块朝各个方向缓慢旋转一个八字形每根轴都经历磁场最大值和最小值然后计算偏移和缩放mag_offset[i] (max[i] min[i]) / 2.0f; mag_scale[i] (max[i] - min[i]) / 2.0f; calib_mag[i] (raw_mag[i] - mag_offset[i]) / mag_scale[i];如果三个轴的 scale 差异不大说明软磁干扰较轻只做偏移修正就够了如果差异明显就要用完整的 3x3 软磁矩阵校准。实际操作中室内环境几乎都要做硬磁校准不然指南针角度会有一个固定的偏置角看起来“挺正常”一转弯就对不上。校准完成后把 offset 和 scale 存到 Flash 或 EEPROM冷启动直接加载。5. 卡尔曼滤波姿态解算实现5.1 状态方程与观测方程卡尔曼滤波在这个项目里每个角度roll、pitch、yaw各用一个独立的一维滤波器核心思想是维护一个状态向量 x [angle, bias]^T。angle 是融合后的角度bias 是陀螺仪零漂估计值。预测方程是angle angle (gyro_rate - bias) * dt bias bias这里的 gyro_rate 是陀螺仪输出的角速度bias 会随着观测不断修正。观测值是加速度计解算的 roll_acc / pitch_acc以及磁力计解算的 heading分别作为对应角度的测量输入。协方差预测和卡尔曼增益不细展开后面代码里直接体现。整个滤波器的输入有三个当前角速度、观测角度、时间差 dt。输出是融合后的角度同时内部维护 bias。5.2 核心 C 代码实现卡尔曼滤波代码可以直接复用经典实现结构体定义如下typedef struct { float Q_angle; float Q_bias; float R_measure; float angle; float bias; float P[2][2]; } Kalman_t;核心函数float Kalman_GetAngle(Kalman_t* k, float newAngle, float newRate, float dt) { float S, K0, K1, P00, P01, P10, P11, y; // 预测 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; // 协方差更新 P00 k-P[0][0]; P01 k-P[0][1]; P10 k-P[1][0]; P11 k-P[1][1]; k-P[0][0] - K0 * P00; k-P[0][1] - K0 * P01; k-P[1][0] - K1 * P10; k-P[1][1] - K1 * P11; return k-angle; }在主循环里roll 和 pitch 直接用加速度计算出的角度作观测yaw 用磁力计 heading 作观测Kalman_t kal_roll, kal_pitch, kal_yaw; // 初始化 kal_roll.angle roll_acc; kal_roll.bias 0; kal_pitch.angle pitch_acc; kal_pitch.bias 0; kal_yaw.angle heading; kal_yaw.bias 0; kal_roll.Q_angle 0.001f; kal_roll.Q_bias 0.003f; kal_roll.R_measure 0.03f; // 另外两个角度用相同参数 // 主循环 float dt 0.01f; // 根据实际调用间隔计算 float roll Kalman_GetAngle(kal_roll, roll_acc, gyro_dps[0], dt); float pitch Kalman_GetAngle(kal_pitch, pitch_acc, gyro_dps[1], dt); float yaw Kalman_GetAngle(kal_yaw, heading, gyro_dps[2], dt);yaw 的角度会跨越 -180°/180° 边界做观测更新前要先处理角度差值float delta heading - kal_yaw.angle; if (delta 180.0f) delta - 360.0f; if (delta -180.0f) delta 360.0f; kal_yaw.angle delta;这里直接修正状态角度再用Kalman_GetAngle的更新逻辑效果等同于处理过边界条件的卡尔曼观测。dt 一定不能用固定的假值要在每次调用时用定时器计时。我习惯用一个 1ms 系统节拍记录上次调用的时间戳相减得到真实 dt。如果循环偶尔被串口打印卡住dt 偏大固定值会导致角度积分过量最后整个数据流都会漂。这个问题在调试阶段很常见。5.3 参数调整与效果实测卡尔曼的调参比较玄学但有几个基本规律可以分享参数调大效果调小效果Q_angle响应更快噪声更大更平滑但跟随变慢Q_bias对零漂修正更激进动态好但抖动大零漂修正慢R_measure更信任角速度过滤噪声更强滞后明显更信任角速度观测响应快但噪声大我的初始建议是 Q_angle0.001、Q_bias0.003、R_measure0.03然后一边转动板子一边看稳定性和延迟。如果角度跟不上动作就把 Q_angle 调大 10 倍再试如果静止时角度还在小幅抖动就把 R_measure 调大 10 倍再试。这个调节过程不复杂但一次只调一个参数否则出了问题很难定位。实测下来滤波后的 roll/pitch 在静止时波动在 ±0.1° 以内快速晃动后能在 0.3 秒内回到稳定值yaw 在室内有磁干扰的环境下静止时波动约 ±0.5°经过磁力计校准后直线行走并转弯的累计误差明显小于直接用陀螺仪积分的方案。6. 常见问题与排查技巧实录6.1 IIC 通信失败的排查顺序遇到 I2C 读不到数据先不要怀疑代码按下面顺序排查现象原因排查方法who_am_i 读回 0xFF上拉缺失或接线错误测 SCL/SDA 空闲电平补充 4.7kΩ 上拉地址探测无 ACK传感器地址不对或 AD0 电平错误确认 0x68/0x69、0x0C 的地址转换偶尔通信超时飞线过长或干扰降速到 100kHz缩短杜邦线读回数据全为 0寄存器地址或字节序错误先读 who_am_i再逐寄存器验证用逻辑分析仪抓 IIC 时序是最直接的定位方式。正常通信时SCL 空闲为高起始位后能看到设备回 ACK如果 SDA 一直被拉低说明某个设备把总线劫持了把总线上的设备逐个拔掉就能找到罪魁祸首。6.2 数据跳变和漂移问题加速度计在震动环境下噪声会明显增大这时候不能只靠卡尔曼滤波硬撑。我在调试平衡车时遇到过角度在 ±2° 之间无规律跳变后来在卡尔曼之前加了一个简单的滑动窗口平均把加速度计原始值先平滑一遍问题立刻消失。注意窗口不能太大否则响应滞后我一般取 5 个采样点。陀螺仪零漂是所有角度漂移的直接原因。每次冷启动后让传感器保持静止 1 秒采集 100 次陀螺仪数据求平均得到零偏再把这个零偏写入卡尔曼的 bias 初值。这样能大幅缩短卡尔曼收敛时间刚上电的几分钟内角度不会乱跑。磁力计数据最容易受环境干扰。调试时让模块远离电机、电源线和铁磁性物体至少 10cm。如果指南针角度在某个特定方向明显偏差多半是该方向有金属物体反射或增强了磁场换个位置再校准。6.3 调参与检查顺序心得整个项目做完之后我个人的调试顺序已经固定下来每一步确认无误再走下一步先确认三个 I2C 地址都能 ACK并且 who_am_i 返回值正确。读原始加速度计和陀螺仪静止时观察输出是否稳定转动板子看数值是否跟着变化。算 roll_acc 和 pitch_acc水平放置时应接近 0°转动 90° 应接近 90°。加卡尔曼滤波先只做 roll 和 pitch确认输出平滑、响应灵敏。再读磁力计并做校准把倾斜补偿公式加进去。最后把 yaw 和磁力计融合整个系统才算完成。第一次调 yaw 时我吃了倾斜补偿的亏没做投影直接算角度结果板子一翘航向角就跟着翻排查了很久才发现公式少了一项。做这种多传感器融合调试节奏千万不能急把链路拆成一个个能验证的小块每块跑通再去拼下一个整个过程会顺利很多。卡尔曼滤波不是玄学就是把每个传感器的优点留下来、缺点交给队友去补调明白之后回头看整个系统其实非常清晰。本文还有配套的精品资源点击获取
简介面向STM32F1嵌入式开发者以MPU6050IIC1与MPU9250IIC2双传感器读取为基础结合卡尔曼滤波输出pitch、roll、yaw姿态角及指南针角度适合学习惯性数据融合与姿态解算的入门及进阶实践。工程共179个文件约1.91MB以C/H/C源码为主辅以IAR与Keil工程文件uvprojx/uvoptx、编译生成文件o/hex/axf及说明文档覆盖双路IIC通信配置、传感器寄存器读写、滤波参数调整等关键代码可帮助读者快速定位逻辑并理解工程结构。已有1066人学习下载。通过学习此工程读者不仅能了解双路IIC同时挂载不同传感器的设计思路还能掌握卡尔曼滤波在加速度计、陀螺仪、磁力计数据融合中的实际用法为平衡车、四轴飞行器、电子罗盘等方向提供可直接参考的代码基础尤其适合在Keil环境下对照学习实时姿态解算流程。 做姿态解算绕不开三个核心传感器陀螺仪拿角速度、加速度计拿重力方向、磁力计拿地磁北方向。这次我在 STM32F103 上同时挂了两颗传感器——MPU6050 走 IIC19250MPU9250走 IIC2——把 6 轴数据和磁力计数据合起来做卡尔曼滤波输出俯仰pitch、横滚roll、偏航yaw三个姿态角外带一路经过倾斜补偿的指南针角度。项目本身不复杂但真正跑稳却有几步关键动作尤其是两个传感器默认 I2C 地址相同、磁力计需要单独校准、卡尔曼滤波参数怎么配这三个点几乎决定了调试是否顺利。这篇就把整个链路从硬件接线到滤波解算再到问题排查完整复现一遍适合准备写飞控、平衡车或者想把手头 IMU 模块用起来的开发者参考。1. 项目整体设计与思路拆解1.1 为什么用 STM32F1 双 IIC 方案STM32F103 算是入门级 MCU 里最务实的选择价格低、资料多、两个硬件 I2C 外设刚好够用。同一个工程里I2C1 接 MPU6050I2C2 接 9250互不干扰。有人会纠结硬件 I2C 不如软件模拟稳定实测下来STM32 硬件 I2C 配合阻塞式读写在 400kHz 下读取 6 轴和磁力计数据完全够用真正影响稳定性的往往是接线和上拉电阻而不是芯片外设本身。标题里的“9250”我按最常见方案理解为 MPU9250也就是内部含 AK8963 磁力计的 9 轴传感器。在这个项目里它主要用来读磁场配合 MCU 算出指南针角度。如果你手头拿的是单独磁力计模块比如 HMC5883L 或 QMC5883L寄存器结构不同但读取思路完全一致。1.2 为什么不把两个传感器挂同一条 IIC 总线这个点值得单独说。MPU6050 和 MPU9250 的默认 I2C 地址都是 0x68如果挂同一条总线就必须通过 AD0 引脚改地址一个设 0x68另一个设 0x69。不过地址改完还有隐患总线上的电容负载变大接线一旦拉长通信时序容易劣化而且 MPU9250 内部的磁力计 AK8963 还挂在 0x0C 地址继续往同一条总线上增加设备后面出问题很难定位是哪个设备把总线拉死的。换用两条独立 IIC 总线之后I2C1 只管 6050I2C2 只管 9250 和 AK8963每个总线上的设备少、地址简单逻辑也干净。调试时还能单独拉一条总线的波形快速确认哪个设备没响应不用在混挂环境里猜。对于这种只有两路 I2C 外设的 MCU两个传感器各占一条是性价比最高的接线方式。1.3 卡尔曼滤波对比互补滤波很多人喜欢用互补滤波代码短、跑得快几行就能得到角度。但互补滤波本质是把加速度计得到的角度和陀螺仪积分的角速度做加权平均它对陀螺仪零漂的估计能力弱长时间静止会看到角度慢慢跑偏。卡尔曼滤波则把陀螺仪的零偏当成状态量和角度一起估计每次用加速度计或磁力计观测值去修正表现要稳不少。当然卡尔曼滤波不是万能的它需要先建模再调 Q 和 R 两个噪声协方差参数模型写错或参数差太远滤波效果还不如互补滤波。这个项目坚持用卡尔曼是因为偏航角yaw要融合磁力计数据磁力计信号容易被环境磁干扰污染有量测噪声模型来处理会比单纯的加权平均稳定得多。2. 硬件连接与 IIC 初始化2.1 引脚分配与接线对照STM32F103 的 I2C1 固定为 PB6SCL、PB7SDAI2C2 固定为 PB10SCL、PB11SDA接线如下表信号STM32F103MPU6050IIC1MPU9250IIC2电源3.3VVCCVCC地GNDGNDGNDSCL1PB6SCL-SDA1PB7SDA-SCL2PB10-SCLSDA2PB11-SDAAD0GNDAD0-供电用 3.3V不要图方便直接上 5V。虽然不少模块板载了稳压但上拉电阻很容易把 3.3V 的 MCU IO 灌坏。两根 IIC 信号线要各放一个 4.7kΩ 上拉电阻到 3.3V有些现成模块板上已经焊好上拉不需要额外加但也有模块为了兼容 5V 系统没焊上电后最好先用逻辑分析仪或示波器确认 SCL/SDA 空闲电平是 3.3V 左右。这个接线方案我在 20cm 杜邦线范围内验证过400kHz 速度稳定。如果飞线超过 30cm建议把速度降到 100kHz或者换带屏蔽的短线否则波形振铃会带来偶发的 ACK 丢包。2.2 IIC 总线参数设置使用 STM32CubeMX 生成工程时I2C1 和 I2C2 的参数可以按同一套配置初始化代码大致如下void MX_I2C1_Init(void) { hi2c1.Instance I2C1; hi2c1.Init.ClockSpeed 400000; hi2c1.Init.DutyCycle I2C_DUTYCYCLE_2; hi2c1.Init.OwnAddress1 0; hi2c1.Init.AddressingMode I2C_ADDRESSINGMODE_7BIT; hi2c1.Init.DualAddressMode I2C_DUALADDRESS_DISABLE; hi2c1.Init.GeneralCallMode I2C_GENERALCALL_DISABLE; hi2c1.Init.NoStretchMode I2C_NOSTRETCH_DISABLE; HAL_I2C_Init(hi2c1); }I2C2 的代码只是把hi2c1换成hi2c2、I2C1换成I2C2其余相同。MPU6050 数据手册规定 I2C 时钟最大 400kHzAK8963 也支持 400kHz所以统一设置为 400kHz。如果模块上的上拉电阻低于 1kΩ高速传输时信号会有过冲出现怪异的读写异常此时把 ClockSpeed 降到 200kHz 到 300kHz 就能解决。GPIO 配置时要注意I2C 引脚必须设置为开漏输出并开启内部上拉或外部上拉。有人习惯把引脚设成推挽输出这会直接把总线电平拉死导致 ACK 永远回不来属于入门阶段最隐蔽的坑之一。2.3 上电自检流程初始化顺序建议这样先初始化两个 I2C 外设再逐个读取 who_am_i 寄存器确认设备在线然后清除休眠位。MPU6050 的 who_am_i 返回 0x68MPU9250 的主芯片也返回 0x68AK8963 返回 0x48。上电后如果读回来 0xFF 或 0x00八成是接线、供电或上拉问题。可以写一个简单的探测函数在启动阶段扫描三个地址并把结果通过串口打出来uint8_t who; HAL_I2C_Mem_Read(hi2c1, 0x681, 0x75, I2C_MEMADD_SIZE_8BIT, who, 1, 100); printf(IIC1 MPU6050: 0x%02X\n, who); HAL_I2C_Mem_Read(hi2c2, 0x681, 0x75, I2C_MEMADD_SIZE_8BIT, who, 1, 100); printf(IIC2 MPU9250: 0x%02X\n, who); HAL_I2C_Mem_Read(hi2c2, 0x0C1, 0x00, I2C_MEMADD_SIZE_8BIT, who, 1, 100); printf(IIC2 AK8963: 0x%02X\n, who);看到 68、68、48 三个地址依次回 ACK才能进入姿态解算主循环。这一步能帮你把问题隔离在读写环节而不是错误地在滤波环节里找 bug。3. MPU6050 加速度计/陀螺仪读取3.1 核心寄存器配置MPU6050 的配置集中在几个寄存器用一张表可以讲清寄存器地址写入值含义PWR_MGMT_10x6B0x00退出休眠使用内部 8MHz 时钟SMPLRT_DIV0x190x07采样率 8kHz / 8 1kHzCONFIG0x1A0x01DLPF 184Hz滤掉高频噪声GYRO_CONFIG0x1B0x08±500°/s灵敏度 65.5 LSB/°/sACCEL_CONFIG0x1C0x00±2g灵敏度 16384 LSB/g选 ±2g 和 ±500°/s是为了平衡分辨率和量程。四轴、平衡车这类场景横滚和俯仰角速率一般不会超过 400°/s500°/s 够用如果做高速旋转平台就要扩到 ±2000°/s但灵敏度会从 65.5 掉到 16.4噪声会明显变大。DLPF 选 184Hz 是比较均衡的选择。如果发现数据在剧烈运动下有明显延迟可以改到 0x0298Hz或 0x0342Hz但滤波效果会变弱需要靠卡尔曼滤波去补。3.2 读取原始数据与单位转换MPU6050 的寄存器 0x3B 开始按 X/Y/Z 顺序存放 16 位加速度计数据跳过温度寄存器后0x43 开始存放 16 位陀螺仪数据。用一次连续读取16 个字节就能拿到全部数据uint8_t buf[14]; int16_t acc[3], gyro[3]; HAL_I2C_Mem_Read(hi2c1, 0x681, 0x3B, I2C_MEMADD_SIZE_8BIT, buf, 14, 100); acc[0] (int16_t)(buf[0]8 | buf[1]); acc[1] (int16_t)(buf[2]8 | buf[3]); acc[2] (int16_t)(buf[4]8 | buf[5]); gyro[0] (int16_t)(buf[8]8 | buf[9]); gyro[1] (int16_t)(buf[10]8 | buf[11]); gyro[2] (int16_t)(buf[12]8 | buf[13]);这里要注意寄存器读回的是大端字节序高字节在前、低字节在后直接buf[0]8 | buf[1]就能拼成 int16_t。如果发现符号不对多半是寄存器地址错了或者模块方向装反。单位转换很简单float acc_g[3], gyro_dps[3]; acc_g[0] acc[0] / 16384.0f; acc_g[1] acc[1] / 16384.0f; acc_g[2] acc[2] / 16384.0f; gyro_dps[0] gyro[0] / 65.5f; gyro_dps[1] gyro[1] / 65.5f; gyro_dps[2] gyro[2] / 65.5f;除法后面一定要加.0f否则 C 语言整数除法会把数值直接截断成 0这个低级错误我在调试群里见过不止一次。3.3 加速度计解算 roll/pitch 角度加速度计在静止状态下测的是比力也就是重力方向所以可以从它还原出横滚和俯仰角。以常见的 X 轴朝前、Y 轴朝左、Z 轴朝上的机体坐标系为例float roll_acc atan2(-acc_g[0], sqrt(acc_g[1]*acc_g[1] acc_g[2]*acc_g[2])) * 180.0f / M_PI; float pitch_acc atan2(acc_g[1], acc_g[2]) * 180.0f / M_PI;符号取决于你的安装方向和坐标系定义跑起来之后把板子绕对应轴转一下方向反了就改对应轴的正负号。需要提醒的是加速度计解算的 roll/pitch 在快速运动时会被线性加速度污染所以这里算出的角度只适合作为卡尔曼滤波的观测值而不是直接输出。yaw 角无法从加速度计求出因为它感受不到绕重力轴旋转的变化。这个缺口由陀螺仪积分和磁力计补上。4. 9250 磁力计读取与罗盘角计算4.1 AK8963 寄存器读取流程在 MPU9250 模块中AK8963 是独立挂在 I2C 总线上的磁力计芯片地址为 0x0C。读取流程比 MPU6050 多一步先配置工作模式再判断数据就绪位。AK8963 的 CNTL1 寄存器0x0A写入 0x16表示 16 位输出、100Hz 连续测量模式 2。这个模式适合一直在转动的机器人平台如果设备大部分时间静止用单次测量模式更省电。读数据时先读 ST10x02确认 bit0 的 DRDY 置 1然后连续读 0x03 到 0x08 六个字节最后必须读 ST20x09清除中断标志。跳过 ST2 会导致下次 DRDY 状态是脏的拿到的可能是上一组残留数据这是磁力计读数跳变的一个隐蔽原因。uint8_t status, raw[6], st2; HAL_I2C_Mem_Read(hi2c2, 0x0C1, 0x02, I2C_MEMADD_SIZE_8BIT, status, 1, 100); if (status 0x01) { HAL_I2C_Mem_Read(hi2c2, 0x0C1, 0x03, I2C_MEMADD_SIZE_8BIT, raw, 6, 100); HAL_I2C_Mem_Read(hi2c2, 0x0C1, 0x09, I2C_MEMADD_SIZE_8BIT, st2, 1, 100); mag[0] (int16_t)(raw[0]8 | raw[1]); mag[1] (int16_t)(raw[2]8 | raw[3]); mag[2] (int16_t)(raw[4]8 | raw[5]); }注意 AK8963 同样是大端输出拼字节时要保持一致。4.2 罗盘角度计算与倾斜补偿水平放置时罗盘角可以直接用 X、Y 轴的磁场分量计算float heading atan2((float)mag[1], (float)mag[0]) * 180.0f / M_PI; if (heading 0) heading 360.0f;但实际设备很少保持水平板子一旦有 pitch 或 roll直接算出来的航向角会跟着姿态一起倾斜误差很大。需要先把磁场矢量投影回水平面float mx mag[0], my mag[1], mz mag[2]; float xh mx * cos(pitch) my * sin(roll) * sin(pitch) mz * cos(roll) * sin(pitch); float yh my * cos(roll) - mz * sin(roll); float heading atan2(yh, xh) * 180.0f / M_PI; if (heading 0) heading 360.0f;这里的 pitch 和 roll 就是卡尔曼滤波输出的角度用上一帧的滤波值即可不需要再额外处理。做倾斜补偿时角度单位必须统一成弧度C 语言里的sin和cos只接受弧度入参这个错位也会导致航向角在特定姿态下突然翻转。4.3 磁力计校准直接读出来的磁力计数据通常是不准的因为环境磁场、PCB 上走线、附近金属结构都会叠加出偏移量。最常用的校准方法叫“最小值最大值校准”把模块朝各个方向缓慢旋转一个八字形每根轴都经历磁场最大值和最小值然后计算偏移和缩放mag_offset[i] (max[i] min[i]) / 2.0f; mag_scale[i] (max[i] - min[i]) / 2.0f; calib_mag[i] (raw_mag[i] - mag_offset[i]) / mag_scale[i];如果三个轴的 scale 差异不大说明软磁干扰较轻只做偏移修正就够了如果差异明显就要用完整的 3x3 软磁矩阵校准。实际操作中室内环境几乎都要做硬磁校准不然指南针角度会有一个固定的偏置角看起来“挺正常”一转弯就对不上。校准完成后把 offset 和 scale 存到 Flash 或 EEPROM冷启动直接加载。5. 卡尔曼滤波姿态解算实现5.1 状态方程与观测方程卡尔曼滤波在这个项目里每个角度roll、pitch、yaw各用一个独立的一维滤波器核心思想是维护一个状态向量 x [angle, bias]^T。angle 是融合后的角度bias 是陀螺仪零漂估计值。预测方程是angle angle (gyro_rate - bias) * dt bias bias这里的 gyro_rate 是陀螺仪输出的角速度bias 会随着观测不断修正。观测值是加速度计解算的 roll_acc / pitch_acc以及磁力计解算的 heading分别作为对应角度的测量输入。协方差预测和卡尔曼增益不细展开后面代码里直接体现。整个滤波器的输入有三个当前角速度、观测角度、时间差 dt。输出是融合后的角度同时内部维护 bias。5.2 核心 C 代码实现卡尔曼滤波代码可以直接复用经典实现结构体定义如下typedef struct { float Q_angle; float Q_bias; float R_measure; float angle; float bias; float P[2][2]; } Kalman_t;核心函数float Kalman_GetAngle(Kalman_t* k, float newAngle, float newRate, float dt) { float S, K0, K1, P00, P01, P10, P11, y; // 预测 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; // 协方差更新 P00 k-P[0][0]; P01 k-P[0][1]; P10 k-P[1][0]; P11 k-P[1][1]; k-P[0][0] - K0 * P00; k-P[0][1] - K0 * P01; k-P[1][0] - K1 * P10; k-P[1][1] - K1 * P11; return k-angle; }在主循环里roll 和 pitch 直接用加速度计算出的角度作观测yaw 用磁力计 heading 作观测Kalman_t kal_roll, kal_pitch, kal_yaw; // 初始化 kal_roll.angle roll_acc; kal_roll.bias 0; kal_pitch.angle pitch_acc; kal_pitch.bias 0; kal_yaw.angle heading; kal_yaw.bias 0; kal_roll.Q_angle 0.001f; kal_roll.Q_bias 0.003f; kal_roll.R_measure 0.03f; // 另外两个角度用相同参数 // 主循环 float dt 0.01f; // 根据实际调用间隔计算 float roll Kalman_GetAngle(kal_roll, roll_acc, gyro_dps[0], dt); float pitch Kalman_GetAngle(kal_pitch, pitch_acc, gyro_dps[1], dt); float yaw Kalman_GetAngle(kal_yaw, heading, gyro_dps[2], dt);yaw 的角度会跨越 -180°/180° 边界做观测更新前要先处理角度差值float delta heading - kal_yaw.angle; if (delta 180.0f) delta - 360.0f; if (delta -180.0f) delta 360.0f; kal_yaw.angle delta;这里直接修正状态角度再用Kalman_GetAngle的更新逻辑效果等同于处理过边界条件的卡尔曼观测。dt 一定不能用固定的假值要在每次调用时用定时器计时。我习惯用一个 1ms 系统节拍记录上次调用的时间戳相减得到真实 dt。如果循环偶尔被串口打印卡住dt 偏大固定值会导致角度积分过量最后整个数据流都会漂。这个问题在调试阶段很常见。5.3 参数调整与效果实测卡尔曼的调参比较玄学但有几个基本规律可以分享参数调大效果调小效果Q_angle响应更快噪声更大更平滑但跟随变慢Q_bias对零漂修正更激进动态好但抖动大零漂修正慢R_measure更信任角速度过滤噪声更强滞后明显更信任角速度观测响应快但噪声大我的初始建议是 Q_angle0.001、Q_bias0.003、R_measure0.03然后一边转动板子一边看稳定性和延迟。如果角度跟不上动作就把 Q_angle 调大 10 倍再试如果静止时角度还在小幅抖动就把 R_measure 调大 10 倍再试。这个调节过程不复杂但一次只调一个参数否则出了问题很难定位。实测下来滤波后的 roll/pitch 在静止时波动在 ±0.1° 以内快速晃动后能在 0.3 秒内回到稳定值yaw 在室内有磁干扰的环境下静止时波动约 ±0.5°经过磁力计校准后直线行走并转弯的累计误差明显小于直接用陀螺仪积分的方案。6. 常见问题与排查技巧实录6.1 IIC 通信失败的排查顺序遇到 I2C 读不到数据先不要怀疑代码按下面顺序排查现象原因排查方法who_am_i 读回 0xFF上拉缺失或接线错误测 SCL/SDA 空闲电平补充 4.7kΩ 上拉地址探测无 ACK传感器地址不对或 AD0 电平错误确认 0x68/0x69、0x0C 的地址转换偶尔通信超时飞线过长或干扰降速到 100kHz缩短杜邦线读回数据全为 0寄存器地址或字节序错误先读 who_am_i再逐寄存器验证用逻辑分析仪抓 IIC 时序是最直接的定位方式。正常通信时SCL 空闲为高起始位后能看到设备回 ACK如果 SDA 一直被拉低说明某个设备把总线劫持了把总线上的设备逐个拔掉就能找到罪魁祸首。6.2 数据跳变和漂移问题加速度计在震动环境下噪声会明显增大这时候不能只靠卡尔曼滤波硬撑。我在调试平衡车时遇到过角度在 ±2° 之间无规律跳变后来在卡尔曼之前加了一个简单的滑动窗口平均把加速度计原始值先平滑一遍问题立刻消失。注意窗口不能太大否则响应滞后我一般取 5 个采样点。陀螺仪零漂是所有角度漂移的直接原因。每次冷启动后让传感器保持静止 1 秒采集 100 次陀螺仪数据求平均得到零偏再把这个零偏写入卡尔曼的 bias 初值。这样能大幅缩短卡尔曼收敛时间刚上电的几分钟内角度不会乱跑。磁力计数据最容易受环境干扰。调试时让模块远离电机、电源线和铁磁性物体至少 10cm。如果指南针角度在某个特定方向明显偏差多半是该方向有金属物体反射或增强了磁场换个位置再校准。6.3 调参与检查顺序心得整个项目做完之后我个人的调试顺序已经固定下来每一步确认无误再走下一步先确认三个 I2C 地址都能 ACK并且 who_am_i 返回值正确。读原始加速度计和陀螺仪静止时观察输出是否稳定转动板子看数值是否跟着变化。算 roll_acc 和 pitch_acc水平放置时应接近 0°转动 90° 应接近 90°。加卡尔曼滤波先只做 roll 和 pitch确认输出平滑、响应灵敏。再读磁力计并做校准把倾斜补偿公式加进去。最后把 yaw 和磁力计融合整个系统才算完成。第一次调 yaw 时我吃了倾斜补偿的亏没做投影直接算角度结果板子一翘航向角就跟着翻排查了很久才发现公式少了一项。做这种多传感器融合调试节奏千万不能急把链路拆成一个个能验证的小块每块跑通再去拼下一个整个过程会顺利很多。卡尔曼滤波不是玄学就是把每个传感器的优点留下来、缺点交给队友去补调明白之后回头看整个系统其实非常清晰。本文还有配套的精品资源点击获取
知识建站
内容驱动
阅读友好版式
SEO 精细化
线索转化