简介面向STM32F1嵌入式开发者的姿态解算资源包专门演示通过IIC1与IIC2分别挂载MPU6050和MPU9250传感器读取加速度、角速度与地磁场原始数据再经卡尔曼滤波融合输出pitch、roll、yaw三个姿态角及指南针角度。资源内容覆盖传感器寄存器初始化、IIC时序读写、数据预处理与姿态解算流程代码结构清晰适合正在学习惯性导航、四轴飞行器姿态估计或STM32外设应用的工程师借鉴。压缩包共179个文件约1.91MB主要包含C源码、头文件、启动汇编、Keil工程配置文件以及编译生成的目标文件、hex和axf等可用于直接打开工程编译或烧录验证。从底层驱动到上层算法均提供完整源码方便对照芯片手册逐模块学习。目前该资源已有1066人下载学习项目具备较好完整性传感器驱动与算法模块耦合度低便于迁移到其他STM32F1系列板卡对理解多传感器数据融合和卡尔曼滤波的工程落地具有直接的参考价值。 前阵子把一块吃灰很久的STM32F103开发板翻出来决定做个姿态估计小系统MPU6050负责六轴数据9250模组上的磁力计负责地磁航向两条独立IIC总线分别读取最后在MCU里用卡尔曼滤波把pitch、roll、yaw三个欧拉角算出来顺便把指南针方向也解算出来。这套组合在飞控、平衡小车、云台稳定器、机器人姿态检测里太常见了网上资料虽然多但把硬件连接、双IIC读取、滤波融合一条龙讲透的并不多尤其是磁力计那边的倾斜补偿和校准很多新手会卡住。我直接把这大半天调试过程中踩过的坑、验证过的参数、能直接抄的代码全部整理出来。无论你是刚入门MPU6050还是已经在用但被yaw漂移折磨这篇都能给你省下不少时间。整个方案用软件模拟IIC两条总线分别挂在两组GPIO上可靠性比很多人吐槽的STM32F1硬件I2C要高不少。1. 系统整体方案与硬件选型思路1.1 为什么是MPU6050加9250这个组合MPU6050集成三轴加速度计和三轴陀螺仪加速度计能感知重力在三个轴上的分量陀螺仪能感知角速度。但单独的MPU6050只能解算出roll横滚和pitch俯仰yaw偏航靠陀螺仪积分会随时间越飘越离谱。原因很简单陀螺仪有零偏即使静止不动积分也会让角度慢慢跑掉。这时候就需要磁力计来拉一把。磁力计测量地磁场方向相当于给系统提供了一个绝对的“北方参考”就像指南针一样。标题里的“9250”我按最常见的理解来处理它是MPU9250里那颗AK8963磁力计或者兼容的独立磁力计芯片总之是负责输出三轴磁场原始数据用来修正yaw。这样六轴IMU提供姿态角速度磁力计提供地磁航向互补关系非常清晰。1.2 双IIC总线的设计原因很多教程喜欢把所有传感器挂到同一条IIC总线上我这里刻意用了两条独立的IICIIC1接MPU6050IIC2接磁力计。原因有三个。第一地址冲突问题。虽然MPU6050和AK8963的默认地址不同但如果你用了一些兼容模块、或者后续还要挂OLED、气压计之类的设备共用一个总线迟早会遇到地址打架的问题。第二条总线把传感器完全隔离互不干扰。第二信号完整性。IIC总线是开漏结构挂的设备越多总线上等效电容越大上拉电阻的匹配就越麻烦。两条总线可以各自调整上拉电阻保证信号质量。我实测下来单总线上挂三个设备时400kbps下波形就已经有明显振铃了。第三逻辑上解耦。MPU6050的读取频率一般要高一些100Hz以上而磁力计的读取频率有上限AK8963连续模式最高100Hz。分开总线后每条总线的读写时序可以独立控制不会因为某个设备响应慢而拖累另一个。1.3 硬件连接与上拉电阻的取值STM32F103的硬件I2C1引脚是PB6/PB7I2C2是PB10/PB11。但我强烈建议直接改用GPIO模拟IIC因为STM32F1的硬件I2C在高速连续读写时容易卡在BUSY标志上这是老生常谈的坑。软件模拟的好处是引脚自由、时序可控、出了问题好排查。我实际用的引脚分配功能SCL引脚SDA引脚IIC1MPU6050PB6PB7IIC2磁力计PB10PB11注意这两组引脚恰好是STM32F103的硬件I2C引脚用GPIO模拟时可以直接复用这些位置PCB布线也方便。所有SDA和SCL引脚都要配置为开漏输出同时外接4.7kΩ上拉电阻到3.3V。关于上拉电阻的取值这个很多人容易忽略。标准IIC模式下4.7kΩ是最常见的值。如果你的总线速率提到400kbps或者导线比较长超过20cm建议换成2.2kΩ否则上升沿会变缓导致数据出错。我调试时用的就是4.7kΩ杜邦线控制在10cm以内波形干净稳定。2. 双IIC读取从寄存器配置到数据上线2.1 MPU6050关键寄存器与初始化流程MPU6050上电默认处于睡眠模式第一步必须先唤醒。初始化顺序很重要我的固定流程是通过0x75寄存器读取WHO_AM_I值正常应该读到0x68这一步能确认IIC通信通路是否正常。写0x6BPWR_MGMT_1为0x00唤醒传感器使用内部时钟。写0x19SMPLRT_DIV为0x07配合默认采样率得到约1kHz的内部采样频率。写0x1ACONFIG为0x06开启内部数字低通滤波DLPF截止频率5Hz左右可以有效滤掉高频机械振动。写0x1BGYRO_CONFIG为0x18陀螺仪量程设为±2000dps。写0x1CACCEL_CONFIG为0x10加速度计量程设为±8g。量程的选择直接关系到数据分辨率。MPU6050内置16位ADC量程越小分辨率越高。实测下来±8g配±2000dps是最常用的组合尤其在平衡车一类需要大角速度的场景±250dps虽然分辨率高但很容易超量程数据直接就截断了。读取数据时从0x3B开始连续读取14个字节分别是加速度计X/Y/Z、温度、陀螺仪X/Y/Z每轴占2个字节高字节在前。用软件IIC一次性连续读14字节比单次单轴读取效率高很多。2.2 磁力计的数据读取流程AK8963磁力计挂在IIC2上器件地址0x0C。这个芯片初始化比MPU6050简单关键就是让它进入连续测量模式写0x0ACNTL为0x16设置16位分辨率、连续测量模式2100Hz。每次读取前先读0x02ST1寄存器判断DRDY位是否为1为1表示数据已就绪。从0x03开始连续读取7个字节X低字节、X高字节、Y低字节、Y高字节、Z低字节、Z高字节以及ST2寄存器。读最后这个ST2寄存器很关键它相当于一次“数据锁存释放”操作。如果不读它下一次数据就绪标志位可能不正常导致重复读到同一帧数据。2.3 原始数据到物理量的转换原始寄存器值是int16类型需要经过换算才是实际物理量。加速度计和陀螺仪的换算公式是固定的加速度值 原始值 / 量程对应的灵敏度。以±8g为例灵敏度是4096 LSB/g陀螺仪±2000dps的灵敏度是16.4 LSB/(°/s)。磁力计的转换比较简单AK8963在16位分辨率下每个LSB对应0.15μT。但磁力计芯片出厂时每颗的灵敏度有细微差异每个轴都有独立的灵敏度调整系数存在芯片的ASA寄存器里。要获得更精准的结果应该读取0x10开始的3个字节按公式换算成调整系数再乘到原始数据上。3. 卡尔曼滤波从公式到能跑的代码3.1 为什么选卡尔曼而不是互补滤波互补滤波实现简单就是高速滤波器和低通滤波器的结合陀螺仪积分结果用高通加速度计测量值用低通两者相加。但在动态性能要求高、或者传感器噪声特性不稳定的场景下互补滤波的固定权重就显得粗糙了。卡尔曼滤波的本质是在每一步迭代中根据当前系统的噪声统计特性动态计算最优权重。它不需要把过去所有数据都记下来只需要维护一个状态向量和误差协方差矩阵非常适合MCU这种资源有限的环境。在STM32F103上跑三个一维卡尔曼滤波器CPU占用几乎可以忽略。3.2 一维卡尔曼的基本原理对于姿态角我用的是“一维卡尔曼陀螺零偏”的经典模型。每个角度比如pitch独立建一个滤波器状态变量有两个角度值和陀螺仪零偏。陀螺仪角速度作为控制输入加速度计解算出的角度作为观测值。预测阶段根据陀螺仪角速度推进角度angle angle (gyro_rate - bias) * dt。这里减去bias很重要陀螺仪零偏如果不估计积分出来的角度会匀速漂移。观测阶段把加速度计算出的角度当成带噪声的测量值用卡尔曼增益去修正预测值。五个公式每次更新都会执行一遍。Q_angle是角度过程噪声方差Q_bias是零偏过程噪声方差R_measure是测量噪声方差。这三个参数需要根据实际传感器和运动特性去调没有通用的万能值。我提供一组稳定好用的初始值Q_angle 0.001Q_bias 0.003R_measure 0.5调参经验是静止时角度噪声大就加大R_measure运动过程中角度滞后严重就减小Q_angle。这组参数在大多数小车和机械臂场景下表现都不错。3.3 C语言完整实现typedef struct { float Q_angle; // 角度过程噪声方差 float Q_bias; // 角速度零偏过程噪声方差 float R_measure; // 角度测量噪声方差 float angle; // 最优角度估计 float bias; // 陀螺仪零偏估计 float P[2][2]; // 误差协方差矩阵 } Kalman_t; float Kalman_update(Kalman_t *kal, float new_angle, float gyro_rate, float dt) { // 预测阶段 float rate gyro_rate - kal-bias; kal-angle dt * rate; kal-P[0][0] dt * (dt * kal-P[1][1] - kal-P[0][1] - kal-P[1][0] kal-Q_angle); kal-P[0][1] - dt * kal-P[1][1]; kal-P[1][0] - dt * kal-P[1][1]; kal-P[1][1] kal-Q_bias * dt; // 更新阶段 float S kal-P[0][0] kal-R_measure; float K[2] { kal-P[0][0] / S, kal-P[1][0] / S }; float y new_angle - kal-angle; kal-angle K[0] * y; kal-bias K[1] * y; float P00 kal-P[0][0]; float P01 kal-P[0][1]; kal-P[0][0] - K[0] * P00; kal-P[0][1] - K[0] * P01; kal-P[1][0] - K[1] * P00; kal-P[1][1] - K[1] * P01; return kal-angle; }这段代码直接把结构体和更新函数封装好pitch、roll、yaw各开一个Kalman_t实例就行。dt是两次调用的时间间隔单位秒。我一般用一个定时器中断固定1ms调用一次这样dt恒定滤波效果更稳定。注意dt不固定会导致卡尔曼增益波动角度会有细微的抖动。4. 姿态解算与指南针角度把数据变成可用角度4.1 加速度计计算roll和pitch初值加速度计在静止或匀速运动时测量的是重力加速度在机体坐标系下的分量。反正切公式是标准做法roll atan2f(accel_y, accel_z) * 180.0f / PI; pitch atan2f(-accel_x, sqrtf(accel_y * accel_y accel_z * accel_z)) * 180.0f / PI;用atan2而不是atan好处是能自动处理象限问题输出范围覆盖全角度。加速度计动态性能差运动剧烈时输出的角度会带噪声和尖刺所以它只能做卡尔曼滤波的观测值。静止时可以把这两个值打印出来看看传感器安装是否水平。4.2 陀螺仪积分与卡尔曼融合陀螺仪短时间精度极高角速度响应几乎无延迟但积分会产生累计误差。卡尔曼滤波干的事情就是在运行中不断用加速度计的角度“纠正”陀螺仪的积分结果。我只需要在每次采样时把加速度计算出的roll_meas和陀螺仪原始角速度gyro_rate喂给Kalman_update获得的返回值就是融合后的roll角。pitch同理。需要注意陀螺仪数据要减去静态零偏。上电时让传感器静止连续采样100次求平均值把这个平均值作为零偏存下来。这一步不做卡尔曼里的bias估计会花很长时间才能收敛期间角度会有明显的漂移。4.3 磁力计计算yaw与倾斜补偿磁力计直接输出的三个轴磁场向量是在机体坐标系下的。如果传感器是水平的yaw atan2(mag_y, mag_x)就能得到磁航向角。但实际安装总有倾角不平的时候直接算yaw会有很大的误差。所以必须先做倾斜补偿把磁场向量从机体坐标转换到水平坐标float bx mag_x * cosf(pitch) mag_y * sinf(roll) * sinf(pitch) mag_z * cosf(roll) * sinf(pitch); float by mag_y * cosf(roll) - mag_z * sinf(roll); float yaw atan2f(by, bx) * 180.0f / PI; if (yaw 0) { yaw 360.0f; }这里用的pitch和roll就是上一步卡尔曼滤波后的输出。倾斜补偿后得到的bx、by是水平面上的磁向量投影。atan2的结果加个取正处理让航向角范围在0到360度之间符合指南针的习惯。这个yaw方向是磁北方向不是真北。如果需要真北还要根据当地磁偏角进行修正。我所在的地区大概4度左右不修正也够用但如果你做精确导航建议查一下当地磁偏角补上。4.4 磁力计校准流程磁力计最容易出问题的地方是不校准直接用。周围环境中的铁磁材料、电机、大电流导线都会叠加一个固定磁场到传感器上这就是硬铁干扰。我用最简单有效的“画圆法”校准把传感器模块水平放置绕Z轴缓慢旋转至少一圈过程中保持模块尽量水平。记录X轴和Y轴磁场数据的最大值和最小值。每个轴的偏移量offset (max min) / 2将原始数据减去offset即可。做完这个校准航向角基本就能用了。如果发现某个方向角度偏差特别大多半是周围有大块金属或者磁源需要调整安装位置。另外模块离电机和供电线路越远越好我一开始把传感器装在电机旁边陀螺仪数据正常但磁力计航向角乱跳挪开5厘米后立刻稳定。5. 现场调试实录与常见问题排查5.1 IIC读不到数据的排查软件IIC写好后第一步先读设备地址。读不到数据时按顺序检查以下几项示波器看SCL和SDA引脚波形确认有没有时钟信号。没有波形八成是GPIO模式配置错了SDA和SCL都必须配置为开漏输出。配置成推挽输出虽然也能通信但电平冲突风险大长期用不可靠。检查上拉电阻。如果完全没接上拉总线电平会被拉低通信直接失败。确认地址是否正确。7位地址0x68需要左移一位变成0xD0作为写地址很多新手在这里搞错。我那次碰到MPU6050返回的WHO_AM_I一直是0xFF排查了半天发现是SDA线虚接杜邦线松了一根。建议所有线用镊子压紧或者直接焊接。5.2 角度跳变和噪声问题加速度计对振动特别敏感电机转动产生的机械振动会直接叠加到加速度数据上。表现为静止时roll和pitch有小幅高频抖动剧烈振动时角度会有尖刺。解决办法有三个在MPU6050的CONFIG寄存器里开启DLPF设置到较低的截止频率我用的5Hz档实测能滤掉大部分振动噪声。适当增大卡尔曼滤波的R_measure让滤波器更信任陀螺仪的预测减少对加速度计观测值的敏感度。但R不能太大否则动态响应会变得迟钝。在机械结构上做减震比如用软胶垫隔离传感器和电机。yaw的跳变则基本都来自磁力计。磁力计最容易受周围磁场突变影响比如旁边有电机启动、手机扬声器靠近之类的情况。遇到yaw突然跳几十度先检查现场有没有新增的磁场源。我在调试时笔记本电脑的扬声器离传感器不到10厘米就足以让航向角跳变5度以上。5.3 常见问题速查表现象可能原因解决方案IIC通信失败读不到设备接线错误、上拉电阻缺失、地址配置错检查设备地址平移确认GPIO开漏模式补4.7kΩ上拉角度静止时漂移陀螺仪零偏未去除上电静止采样100次求平均零偏角度高频抖动振动干扰加速度计开启DLPF适当增大R_measureyaw角突然跳变附近磁场源干扰远离电机/电流线做硬铁校准动态响应严重滞后Q_angle太小或R_measure太大减小Q_angle增大R_measure倾斜补偿后yaw仍不准校准不完整或未做倾斜补偿重新转圈校准检查补偿公式符号调试这类传感器系统我强烈建议先把原始数据和最终角度通过串口dump出来在电脑上画成曲线再观察。肉眼直接看数字变化很难看出问题画出来一眼就能分辨是噪声、跳变还是滞后。我第一次调通整套流程时为了验证yaw的准确性把传感器固定在一个转台上每次精确转90度看角度输出是否准确对应。实测下来卡尔曼滤波后的roll和pitch在静止时误差能控制在0.5度以内运动过程中yaw短时间也能保持在2度以内磁力计校准后长时间稳定没有明显漂移。最后分享一个我一开始忽略的小技巧MPU6050的内部温度传感器可以顺便读出来虽然精度不高但能用来做温度补偿的参考。陀螺仪零偏是随温度缓慢变化的如果设备工作温度范围大建议在不同温度下采样零偏做一个线性补偿表。另外如果你发现卡尔曼滤波后的角度偶尔出现“瞬移”很可能是采样时间抖动导致dt不准这时候一定要用定时器中断保证固定采样周期而不是在main循环里用while延时。这两种做法在视觉上差距不大但看数据曲线时能明显感觉到稳定性完全不同。本文还有配套的精品资源点击获取 
长河君 · 2026/9/9 15:44:58 · 阅读 1.2k STM32F103双IIC读取MPU6050与磁力计，卡尔曼滤波姿态解算实战
简介面向STM32F1嵌入式开发者的姿态解算资源包专门演示通过IIC1与IIC2分别挂载MPU6050和MPU9250传感器读取加速度、角速度与地磁场原始数据再经卡尔曼滤波融合输出pitch、roll、yaw三个姿态角及指南针角度。资源内容覆盖传感器寄存器初始化、IIC时序读写、数据预处理与姿态解算流程代码结构清晰适合正在学习惯性导航、四轴飞行器姿态估计或STM32外设应用的工程师借鉴。压缩包共179个文件约1.91MB主要包含C源码、头文件、启动汇编、Keil工程配置文件以及编译生成的目标文件、hex和axf等可用于直接打开工程编译或烧录验证。从底层驱动到上层算法均提供完整源码方便对照芯片手册逐模块学习。目前该资源已有1066人下载学习项目具备较好完整性传感器驱动与算法模块耦合度低便于迁移到其他STM32F1系列板卡对理解多传感器数据融合和卡尔曼滤波的工程落地具有直接的参考价值。 前阵子把一块吃灰很久的STM32F103开发板翻出来决定做个姿态估计小系统MPU6050负责六轴数据9250模组上的磁力计负责地磁航向两条独立IIC总线分别读取最后在MCU里用卡尔曼滤波把pitch、roll、yaw三个欧拉角算出来顺便把指南针方向也解算出来。这套组合在飞控、平衡小车、云台稳定器、机器人姿态检测里太常见了网上资料虽然多但把硬件连接、双IIC读取、滤波融合一条龙讲透的并不多尤其是磁力计那边的倾斜补偿和校准很多新手会卡住。我直接把这大半天调试过程中踩过的坑、验证过的参数、能直接抄的代码全部整理出来。无论你是刚入门MPU6050还是已经在用但被yaw漂移折磨这篇都能给你省下不少时间。整个方案用软件模拟IIC两条总线分别挂在两组GPIO上可靠性比很多人吐槽的STM32F1硬件I2C要高不少。1. 系统整体方案与硬件选型思路1.1 为什么是MPU6050加9250这个组合MPU6050集成三轴加速度计和三轴陀螺仪加速度计能感知重力在三个轴上的分量陀螺仪能感知角速度。但单独的MPU6050只能解算出roll横滚和pitch俯仰yaw偏航靠陀螺仪积分会随时间越飘越离谱。原因很简单陀螺仪有零偏即使静止不动积分也会让角度慢慢跑掉。这时候就需要磁力计来拉一把。磁力计测量地磁场方向相当于给系统提供了一个绝对的“北方参考”就像指南针一样。标题里的“9250”我按最常见的理解来处理它是MPU9250里那颗AK8963磁力计或者兼容的独立磁力计芯片总之是负责输出三轴磁场原始数据用来修正yaw。这样六轴IMU提供姿态角速度磁力计提供地磁航向互补关系非常清晰。1.2 双IIC总线的设计原因很多教程喜欢把所有传感器挂到同一条IIC总线上我这里刻意用了两条独立的IICIIC1接MPU6050IIC2接磁力计。原因有三个。第一地址冲突问题。虽然MPU6050和AK8963的默认地址不同但如果你用了一些兼容模块、或者后续还要挂OLED、气压计之类的设备共用一个总线迟早会遇到地址打架的问题。第二条总线把传感器完全隔离互不干扰。第二信号完整性。IIC总线是开漏结构挂的设备越多总线上等效电容越大上拉电阻的匹配就越麻烦。两条总线可以各自调整上拉电阻保证信号质量。我实测下来单总线上挂三个设备时400kbps下波形就已经有明显振铃了。第三逻辑上解耦。MPU6050的读取频率一般要高一些100Hz以上而磁力计的读取频率有上限AK8963连续模式最高100Hz。分开总线后每条总线的读写时序可以独立控制不会因为某个设备响应慢而拖累另一个。1.3 硬件连接与上拉电阻的取值STM32F103的硬件I2C1引脚是PB6/PB7I2C2是PB10/PB11。但我强烈建议直接改用GPIO模拟IIC因为STM32F1的硬件I2C在高速连续读写时容易卡在BUSY标志上这是老生常谈的坑。软件模拟的好处是引脚自由、时序可控、出了问题好排查。我实际用的引脚分配功能SCL引脚SDA引脚IIC1MPU6050PB6PB7IIC2磁力计PB10PB11注意这两组引脚恰好是STM32F103的硬件I2C引脚用GPIO模拟时可以直接复用这些位置PCB布线也方便。所有SDA和SCL引脚都要配置为开漏输出同时外接4.7kΩ上拉电阻到3.3V。关于上拉电阻的取值这个很多人容易忽略。标准IIC模式下4.7kΩ是最常见的值。如果你的总线速率提到400kbps或者导线比较长超过20cm建议换成2.2kΩ否则上升沿会变缓导致数据出错。我调试时用的就是4.7kΩ杜邦线控制在10cm以内波形干净稳定。2. 双IIC读取从寄存器配置到数据上线2.1 MPU6050关键寄存器与初始化流程MPU6050上电默认处于睡眠模式第一步必须先唤醒。初始化顺序很重要我的固定流程是通过0x75寄存器读取WHO_AM_I值正常应该读到0x68这一步能确认IIC通信通路是否正常。写0x6BPWR_MGMT_1为0x00唤醒传感器使用内部时钟。写0x19SMPLRT_DIV为0x07配合默认采样率得到约1kHz的内部采样频率。写0x1ACONFIG为0x06开启内部数字低通滤波DLPF截止频率5Hz左右可以有效滤掉高频机械振动。写0x1BGYRO_CONFIG为0x18陀螺仪量程设为±2000dps。写0x1CACCEL_CONFIG为0x10加速度计量程设为±8g。量程的选择直接关系到数据分辨率。MPU6050内置16位ADC量程越小分辨率越高。实测下来±8g配±2000dps是最常用的组合尤其在平衡车一类需要大角速度的场景±250dps虽然分辨率高但很容易超量程数据直接就截断了。读取数据时从0x3B开始连续读取14个字节分别是加速度计X/Y/Z、温度、陀螺仪X/Y/Z每轴占2个字节高字节在前。用软件IIC一次性连续读14字节比单次单轴读取效率高很多。2.2 磁力计的数据读取流程AK8963磁力计挂在IIC2上器件地址0x0C。这个芯片初始化比MPU6050简单关键就是让它进入连续测量模式写0x0ACNTL为0x16设置16位分辨率、连续测量模式2100Hz。每次读取前先读0x02ST1寄存器判断DRDY位是否为1为1表示数据已就绪。从0x03开始连续读取7个字节X低字节、X高字节、Y低字节、Y高字节、Z低字节、Z高字节以及ST2寄存器。读最后这个ST2寄存器很关键它相当于一次“数据锁存释放”操作。如果不读它下一次数据就绪标志位可能不正常导致重复读到同一帧数据。2.3 原始数据到物理量的转换原始寄存器值是int16类型需要经过换算才是实际物理量。加速度计和陀螺仪的换算公式是固定的加速度值 原始值 / 量程对应的灵敏度。以±8g为例灵敏度是4096 LSB/g陀螺仪±2000dps的灵敏度是16.4 LSB/(°/s)。磁力计的转换比较简单AK8963在16位分辨率下每个LSB对应0.15μT。但磁力计芯片出厂时每颗的灵敏度有细微差异每个轴都有独立的灵敏度调整系数存在芯片的ASA寄存器里。要获得更精准的结果应该读取0x10开始的3个字节按公式换算成调整系数再乘到原始数据上。3. 卡尔曼滤波从公式到能跑的代码3.1 为什么选卡尔曼而不是互补滤波互补滤波实现简单就是高速滤波器和低通滤波器的结合陀螺仪积分结果用高通加速度计测量值用低通两者相加。但在动态性能要求高、或者传感器噪声特性不稳定的场景下互补滤波的固定权重就显得粗糙了。卡尔曼滤波的本质是在每一步迭代中根据当前系统的噪声统计特性动态计算最优权重。它不需要把过去所有数据都记下来只需要维护一个状态向量和误差协方差矩阵非常适合MCU这种资源有限的环境。在STM32F103上跑三个一维卡尔曼滤波器CPU占用几乎可以忽略。3.2 一维卡尔曼的基本原理对于姿态角我用的是“一维卡尔曼陀螺零偏”的经典模型。每个角度比如pitch独立建一个滤波器状态变量有两个角度值和陀螺仪零偏。陀螺仪角速度作为控制输入加速度计解算出的角度作为观测值。预测阶段根据陀螺仪角速度推进角度angle angle (gyro_rate - bias) * dt。这里减去bias很重要陀螺仪零偏如果不估计积分出来的角度会匀速漂移。观测阶段把加速度计算出的角度当成带噪声的测量值用卡尔曼增益去修正预测值。五个公式每次更新都会执行一遍。Q_angle是角度过程噪声方差Q_bias是零偏过程噪声方差R_measure是测量噪声方差。这三个参数需要根据实际传感器和运动特性去调没有通用的万能值。我提供一组稳定好用的初始值Q_angle 0.001Q_bias 0.003R_measure 0.5调参经验是静止时角度噪声大就加大R_measure运动过程中角度滞后严重就减小Q_angle。这组参数在大多数小车和机械臂场景下表现都不错。3.3 C语言完整实现typedef struct { float Q_angle; // 角度过程噪声方差 float Q_bias; // 角速度零偏过程噪声方差 float R_measure; // 角度测量噪声方差 float angle; // 最优角度估计 float bias; // 陀螺仪零偏估计 float P[2][2]; // 误差协方差矩阵 } Kalman_t; float Kalman_update(Kalman_t *kal, float new_angle, float gyro_rate, float dt) { // 预测阶段 float rate gyro_rate - kal-bias; kal-angle dt * rate; kal-P[0][0] dt * (dt * kal-P[1][1] - kal-P[0][1] - kal-P[1][0] kal-Q_angle); kal-P[0][1] - dt * kal-P[1][1]; kal-P[1][0] - dt * kal-P[1][1]; kal-P[1][1] kal-Q_bias * dt; // 更新阶段 float S kal-P[0][0] kal-R_measure; float K[2] { kal-P[0][0] / S, kal-P[1][0] / S }; float y new_angle - kal-angle; kal-angle K[0] * y; kal-bias K[1] * y; float P00 kal-P[0][0]; float P01 kal-P[0][1]; kal-P[0][0] - K[0] * P00; kal-P[0][1] - K[0] * P01; kal-P[1][0] - K[1] * P00; kal-P[1][1] - K[1] * P01; return kal-angle; }这段代码直接把结构体和更新函数封装好pitch、roll、yaw各开一个Kalman_t实例就行。dt是两次调用的时间间隔单位秒。我一般用一个定时器中断固定1ms调用一次这样dt恒定滤波效果更稳定。注意dt不固定会导致卡尔曼增益波动角度会有细微的抖动。4. 姿态解算与指南针角度把数据变成可用角度4.1 加速度计计算roll和pitch初值加速度计在静止或匀速运动时测量的是重力加速度在机体坐标系下的分量。反正切公式是标准做法roll atan2f(accel_y, accel_z) * 180.0f / PI; pitch atan2f(-accel_x, sqrtf(accel_y * accel_y accel_z * accel_z)) * 180.0f / PI;用atan2而不是atan好处是能自动处理象限问题输出范围覆盖全角度。加速度计动态性能差运动剧烈时输出的角度会带噪声和尖刺所以它只能做卡尔曼滤波的观测值。静止时可以把这两个值打印出来看看传感器安装是否水平。4.2 陀螺仪积分与卡尔曼融合陀螺仪短时间精度极高角速度响应几乎无延迟但积分会产生累计误差。卡尔曼滤波干的事情就是在运行中不断用加速度计的角度“纠正”陀螺仪的积分结果。我只需要在每次采样时把加速度计算出的roll_meas和陀螺仪原始角速度gyro_rate喂给Kalman_update获得的返回值就是融合后的roll角。pitch同理。需要注意陀螺仪数据要减去静态零偏。上电时让传感器静止连续采样100次求平均值把这个平均值作为零偏存下来。这一步不做卡尔曼里的bias估计会花很长时间才能收敛期间角度会有明显的漂移。4.3 磁力计计算yaw与倾斜补偿磁力计直接输出的三个轴磁场向量是在机体坐标系下的。如果传感器是水平的yaw atan2(mag_y, mag_x)就能得到磁航向角。但实际安装总有倾角不平的时候直接算yaw会有很大的误差。所以必须先做倾斜补偿把磁场向量从机体坐标转换到水平坐标float bx mag_x * cosf(pitch) mag_y * sinf(roll) * sinf(pitch) mag_z * cosf(roll) * sinf(pitch); float by mag_y * cosf(roll) - mag_z * sinf(roll); float yaw atan2f(by, bx) * 180.0f / PI; if (yaw 0) { yaw 360.0f; }这里用的pitch和roll就是上一步卡尔曼滤波后的输出。倾斜补偿后得到的bx、by是水平面上的磁向量投影。atan2的结果加个取正处理让航向角范围在0到360度之间符合指南针的习惯。这个yaw方向是磁北方向不是真北。如果需要真北还要根据当地磁偏角进行修正。我所在的地区大概4度左右不修正也够用但如果你做精确导航建议查一下当地磁偏角补上。4.4 磁力计校准流程磁力计最容易出问题的地方是不校准直接用。周围环境中的铁磁材料、电机、大电流导线都会叠加一个固定磁场到传感器上这就是硬铁干扰。我用最简单有效的“画圆法”校准把传感器模块水平放置绕Z轴缓慢旋转至少一圈过程中保持模块尽量水平。记录X轴和Y轴磁场数据的最大值和最小值。每个轴的偏移量offset (max min) / 2将原始数据减去offset即可。做完这个校准航向角基本就能用了。如果发现某个方向角度偏差特别大多半是周围有大块金属或者磁源需要调整安装位置。另外模块离电机和供电线路越远越好我一开始把传感器装在电机旁边陀螺仪数据正常但磁力计航向角乱跳挪开5厘米后立刻稳定。5. 现场调试实录与常见问题排查5.1 IIC读不到数据的排查软件IIC写好后第一步先读设备地址。读不到数据时按顺序检查以下几项示波器看SCL和SDA引脚波形确认有没有时钟信号。没有波形八成是GPIO模式配置错了SDA和SCL都必须配置为开漏输出。配置成推挽输出虽然也能通信但电平冲突风险大长期用不可靠。检查上拉电阻。如果完全没接上拉总线电平会被拉低通信直接失败。确认地址是否正确。7位地址0x68需要左移一位变成0xD0作为写地址很多新手在这里搞错。我那次碰到MPU6050返回的WHO_AM_I一直是0xFF排查了半天发现是SDA线虚接杜邦线松了一根。建议所有线用镊子压紧或者直接焊接。5.2 角度跳变和噪声问题加速度计对振动特别敏感电机转动产生的机械振动会直接叠加到加速度数据上。表现为静止时roll和pitch有小幅高频抖动剧烈振动时角度会有尖刺。解决办法有三个在MPU6050的CONFIG寄存器里开启DLPF设置到较低的截止频率我用的5Hz档实测能滤掉大部分振动噪声。适当增大卡尔曼滤波的R_measure让滤波器更信任陀螺仪的预测减少对加速度计观测值的敏感度。但R不能太大否则动态响应会变得迟钝。在机械结构上做减震比如用软胶垫隔离传感器和电机。yaw的跳变则基本都来自磁力计。磁力计最容易受周围磁场突变影响比如旁边有电机启动、手机扬声器靠近之类的情况。遇到yaw突然跳几十度先检查现场有没有新增的磁场源。我在调试时笔记本电脑的扬声器离传感器不到10厘米就足以让航向角跳变5度以上。5.3 常见问题速查表现象可能原因解决方案IIC通信失败读不到设备接线错误、上拉电阻缺失、地址配置错检查设备地址平移确认GPIO开漏模式补4.7kΩ上拉角度静止时漂移陀螺仪零偏未去除上电静止采样100次求平均零偏角度高频抖动振动干扰加速度计开启DLPF适当增大R_measureyaw角突然跳变附近磁场源干扰远离电机/电流线做硬铁校准动态响应严重滞后Q_angle太小或R_measure太大减小Q_angle增大R_measure倾斜补偿后yaw仍不准校准不完整或未做倾斜补偿重新转圈校准检查补偿公式符号调试这类传感器系统我强烈建议先把原始数据和最终角度通过串口dump出来在电脑上画成曲线再观察。肉眼直接看数字变化很难看出问题画出来一眼就能分辨是噪声、跳变还是滞后。我第一次调通整套流程时为了验证yaw的准确性把传感器固定在一个转台上每次精确转90度看角度输出是否准确对应。实测下来卡尔曼滤波后的roll和pitch在静止时误差能控制在0.5度以内运动过程中yaw短时间也能保持在2度以内磁力计校准后长时间稳定没有明显漂移。最后分享一个我一开始忽略的小技巧MPU6050的内部温度传感器可以顺便读出来虽然精度不高但能用来做温度补偿的参考。陀螺仪零偏是随温度缓慢变化的如果设备工作温度范围大建议在不同温度下采样零偏做一个线性补偿表。另外如果你发现卡尔曼滤波后的角度偶尔出现“瞬移”很可能是采样时间抖动导致dt不准这时候一定要用定时器中断保证固定采样周期而不是在main循环里用while延时。这两种做法在视觉上差距不大但看数据曲线时能明显感觉到稳定性完全不同。本文还有配套的精品资源点击获取