简介围绕MPU6050展开的陀螺仪数据分析文档面向制作自平衡小车、进行姿态解算或学习惯性传感器入门的开发者与学生重点解决加速度计与陀螺仪原始数据的理解、倾角换算及角度误差处理等常见问题。资源包共1个docx文件压缩包约689KB内容以原理说明、公式推导和Arduino实践思路为主便于按章节阅读、对照实验与复盘。正文从加速度计三轴分量与重力分解讲起推导基于正弦定理的倾斜角计算方法并讨论小角度线性近似、陀螺仪角速度积分累积误差以及卡尔曼滤波融合加速度计与陀螺仪角度的思路同时给出MPU6050与Arduino连接的学习参考为后续姿态融合与调试打下基础。已有893人学习适合需要把传感器读数转化为可理解姿态信息、补齐数据处理与调试方法基础的中初级读者。1. 从一个自平衡小车的倾角读数说起桌上摆着一台刚焊好的两轮小车通电瞬间轮子朝一边猛冲然后啪地倒下。很多人第一反应是 PID 没调好其实问题常常出在上游——陀螺仪数据分析没过关。MPU6050 在 I2C 上老老实实吐着六个 int16可这六个数既不是角度也不是角速度只是原始 ADC 计数把它们直接喂给电机驱动车不倒才怪。这份资料讲的就是从这六个数到可用倾角的完整链路加速度计怎么靠重力分量反推静态角度陀螺仪怎么靠角速度积分算动态角度两者又怎么用卡尔曼滤波揉成一个不飘不滞后的结果。它适合正在拿 MPU6050 做自平衡车、云台、姿态显示手上有 Arduino 和一块模块的人纯看理论不动手的收益有限。2. 硬件链路与工程骨架I2C 接线、库依赖和初始化自检动手前先把物理链路和工程骨架定下来。这一章看着像搬箱子但后面所有系数、所有角度符号都取决于这里的三件事模块怎么固定、量程设成多少、用的是哪套库。一开始图省事随便接后面调滤波时就会分不清是算法错了还是接线错了。2.1 六个引脚怎么接I2C 总线与供电边界MPU6050 模块对外一般引出 6 到 8 个脚实际用得上的就四个VCC、GND、SCL、SDA。SCL/SDA 是标准 I2C接到 Arduino UNO 或 Nano 上就是 A5 和 A4不是 D5/D4这一点新手很容易接错——A4/A5 在模拟口位置但功能是 I2C 的 SDA/SCL。模块引脚Arduino UNO/Nano说明VCC5V 或 3.3V模块板载 LDO两种都能喂5V 更稳GNDGND必须共地否则 I2C 通信时好时坏SCLA5I2C 时钟线SDAA4I2C 数据线AD0GND 或悬空接地时地址 0x68接高时 0x69INT不接或 D2用中断读数据才需要第一版可以不接I2C 是开漏总线必须有上拉电阻。市面上常见的 MPU6050 模块板上已经焊了 4.7k 或 10k 的上拉直接接就行如果你把模块和别的 I2C 器件挂在同一条总线上几个上拉并联后阻值会变小总线电容变大400kHz 下波形容易塌。血泪经验是总线一挂就是两三个器件时先拿示波器或者逻辑分析仪看一眼 SCL 上升沿别等数据错乱了再回头怀疑代码。还有安装方向。自平衡车上一般让模块平贴底盘让模块的 y 轴和车轴同向这样车身前后摆动就是绕 y 轴转x 轴和 z 轴在竖直平面里变化后面才能把三维问题压成二维。装歪了不是不能算只是所有公式都得重推一遍没必要给自己找事。2.2 库依赖与工程目录三个文件撑起整个项目这套代码依赖三个东西Arduino 自带的Wire.hI2C 底层驱动以及两个需要手动装进 libraries 的第三方库——I2Cdev和MPU6050。MPU6050库封装了寄存器读写、量程设置、getMotion6()这类接口I2Cdev是它的底层依赖两个必须同时存在只装一个会编译报I2Cdev.h: No such file or directory。目录结构大致长这样libraries是全局库目录sketch 目录里只放主程序Arduino/ ├── libraries/ │ ├── I2Cdev/ │ │ ├── I2Cdev.cpp │ │ ├── I2Cdev.h │ │ └── (若干辅助源文件) │ └── MPU6050/ │ ├── MPU6050.cpp │ ├── MPU6050.h │ └── helper_3dmath.h └── mpu6050_angle/ └── mpu6050_angle.ino装库有两个常见做法一是把解压后的文件夹整体丢进Arduino/libraries/重启 IDE二是在 IDE 的库管理器里搜名字装。前者可控性强能改源码——后面要改量程默认值就必须改源码所以建议走第一种。装完之后打开示例Examples - MPU6050 - MPU6050_raw能编译通过、串口能出数说明链路通了再动自己的代码。2.3 初始化与自检量程决定了后面所有系数初始化这一段是整个项目的“地基”。它要干三件事拉起 I2C、配好量程、确认模块真的在线。量程不是随便选的它直接决定了后面每一个换算系数的数值选完就写死在代码里中途改量程必须同步改系数否则角度会成倍地错。#include Wire.h #include I2Cdev.h #include MPU6050.h // 默认 I2C 地址 0x68AD0 接高时改成 MPU6050 accelgyro(0x69); MPU6050 accelgyro; int16_t ax, ay, az; // 加速度计三轴原始计数 int16_t gx, gy, gz; // 陀螺仪三轴原始计数 void setup() { Wire.begin(); // 加入 I2C 总线I2Cdev 不会自动调用 Wire.setClock(400000L); // 快速模式 400kHz缩短单次读取耗时 Serial.begin(38400); // 与上位机绘图工具对齐8MHz/16MHz 板子都稳 Serial.println(Initializing I2C devices...); accelgyro.initialize(); // 内部会配时钟源、量程、唤醒芯片 // 自检读一次 WHO_AM_I 并确认总线应答 Serial.println(accelgyro.testConnection() ? MPU6050 connection successful : MPU6050 connection failed); } void loop() { }Wire.begin()必须自己调I2Cdev库不会替你调Wire.setClock(400000L)是我一般会加的标准模式 100kHz 下一次getMotion6()大约 1ms 出头快速模式能压到 0.3ms 左右采样周期更均匀卡尔曼的 dt 也更接近真值。Serial.begin(38400)这个波特率是沿用原作的选择8MHz 的板子和 16MHz 的板子都能稳定跑上位机绘图工具里记得填一样的值不然收到的就是乱码。真正决定系数的是量程。accelgyro.initialize()内部会调用两个设置函数源码在MPU6050.cpp里void MPU6050::initialize() { setClockSource(MPU6050_CLOCK_PLL_XGYRO); // 用陀螺 X 轴 PLL 做时钟源抖动更小 setFullScaleGyroRange(MPU6050_GYRO_FS_250); // 陀螺量程 ±250 °/s灵敏度 131 LSB/(°/s) setFullScaleAccelRange(MPU6050_ACCEL_FS_2); // 加速度量程 ±2g灵敏度 16384 LSB/g setSleepEnabled(false); // 唤醒芯片否则读出来全是 0 }设置项取值灵敏度适用场景加速度量程±2g16384 LSB/g静态倾角分辨率最高加速度量程±4g8192 LSB/g有冲击、跌落陀螺量程±250 °/s131 LSB/(°/s)慢速摆动的小车陀螺量程±500 °/s65.5 LSB/(°/s)转速较快的云台自平衡车的摆动速度通常不超过 100°/s±250 量程够用而且分辨率最高。testConnection()返回失败九成是接线或地址问题先查 AD0 和 SDA/SCL 有没有接反再查模块供电——有的模块 3.3V 供电时 LDO 压差不够输出会不稳。3. 加速度计求倾角从重力分解到 sinx≈kx 的工程近似加速度计算角度这件事物理上讲只有一句话重力加速度在三轴上的分量比例反映了姿态。但工程上要把它变成一个能在 8 位单片机上每 10ms 跑一次的式子中间有两步关键简化——把三维压成二维把反正弦换成线性。这两步不做代码要么跑不动要么精度不够看。3.1 坐标系约定为什么只需要看 x-z 平面把模块平放在桌面上x、y、z 三个轴就定下来了。装到小车上时让车轴和模块的 y 轴平行那么车身前后摆动时y 轴和桌面的夹角恒为 0——因为车轴始终平行于桌面绕车轴的转动才是车身倾斜。变化的是 x 轴和 z 轴与水平面的夹角而且这两个夹角的变化量是一样的一个变大多少度另一个就变小多少度。于是从 y 轴方向俯视下去整个问题退化成一个二维平面原点 O 是 y 轴剩下 x 轴和 z 轴构成一个直角坐标系重力加速度 g 竖直向下在 x 轴上的投影是 gx在 z 轴上的投影是 gz。小车倾斜角 a 就是 x 轴与水平面的夹角。这个简化不是偷懒是安装方式带来的真实约束装对了就成立装错了整套推导作废。需要注意这个模型只在小车静止或匀速时准确。电机一用力车体上叠加了前进或后退方向的加速度加速度计读到的是重力加速度和运动加速度的合成量角度会跟着偏。原作的处理方式是先忽略它把静态角度算准动态误差交给后面的滤波去压。这个取舍在自平衡车上是对的车身倾角的主要信息在重力分量里运动加速度是短时扰动滤波能吸收掉大部分。3.2 从正弦定理到 sinx k·x·π/180设 g 与 x 轴的夹角为 dgx 是 g 在 x 轴上的分量。因为重力竖直向下x 轴与水平面夹角为 a几何上有a b 90°c b 90°所以 a c再由平行四边形g、gx、gz 构成矩形得到 c d最终 a d。这一步纯几何画个图就能看明白。接着用正弦定理sin d gx / g。g 是常数gx 读出来角度就能反算——理论上是d asin(gx/g)。问题在于asin在 AVR 上要拉浮点库一次调用几十微秒到上百微秒放在 100Hz 的循环里成本偏高而且靠近 ±90° 时数值敏感。于是换近似小角度下正弦值和弧度值近似相等sin x ≈ x·π/180。角度sin 真值线性近似k1相对误差加 0.92 系数后10°0.17360.17450.5%0.160520°0.34200.34912.1%0.321229°0.48480.50614.4%0.465645°0.70710.785411.1%0.7226k1 时±29° 以内误差可以接受超出后迅速变大。自平衡车的摆动范围通常就在 ±30° 内超过这个角度车已经救不回来了。如果想把可用范围推到 ±45°把公式乘一个 0.92 的系数sin x ≈ 0.92·x·π/180。代价是 45° 处仍有约 2% 的欠补偿换来的是量程翻倍这笔账在一辆小车上很划算。把两步合起来gx/g k·d·π/180整理得到d 180·gx / (k·g·π)。到这里角度就只跟一个读数和三个常数有关了。3.3 代码落地262 这个魔数是怎么算出来的把常数代进去k 0.92π 取 3.14g 是加速度计在 ±2g 量程下的满量程计数 16384。分母0.92 × 3.14 × 16384 ≈ 47338180 / 47338 ≈ 0.003803取倒数约等于 262。所以代码里那句ax_angle ax / 262不是拍脑袋来的它就是d 180·gx/(k·g·π)的数值化简。// 水平静置时读 2000 次求平均得到的零偏每块板子都不一样 #define AX_ZERO (-1476) // 加速度计 x 轴零偏单位 LSB #define GX_ZERO (-30.5) // 陀螺仪 x 轴零偏单位 LSB double ax_angle 0.0; // 在 loop 里读完之后 ax - AX_ZERO; // 先扣零偏再谈换算 ax_angle ax / 262.0; // ±2g 量程下的角度换算单位度AX_ZERO必须每块板子单独标定抄别人的值等于给自己埋雷。ax / 262.0里的.0不能省写成ax / 262会走整数除法小数部分直接截断角度会出现阶梯状跳变。更稳妥的写法是不写死魔数把量程和系数绑在一起换量程时只改一个常量const float LSB_PER_G 16384.0; // ±2g改 ±4g 时换成 8192.0 const float K_LINEAR 0.92; // 线性近似系数 const float PI_APPROX 3.1416; // 每 LSB 对应的角度量程一变这里自动跟着变 const float DEG_PER_LSB 180.0 / (K_LINEAR * PI_APPROX * LSB_PER_G); ax_angle ax * DEG_PER_LSB;这样改量程只动一行不会出现“改了量程忘了改系数”的翻车现场。另外ax_angle的正负号取决于模块的安装朝向模块转 180°同一方向的倾斜读出来符号相反。第一次装车时手动把车往前倾 30° 左右看串口输出的符号和数值对不对对不上就调换模块方向别急着在代码里加负号否则后面和陀螺仪融合时符号会互相打架。4. 陀螺仪积分与卡尔曼滤波把角速度累加成稳定角度加速度计算出来的角度静态准、动态抖陀螺仪算出来的角度动态稳、静态漂。这两句话基本概括了后面所有工作的动机。真正让自平衡车能站住的那个角度既不是前者也不是后者而是两者融合之后的值。4.1 角速度积分一次乘法加一次累加误差却在累积陀螺仪输出的是角速度单位 °/s换算方式是除以灵敏度系数。±250 量程下灵敏度是 131 LSB/(°/s)所以gyro gy / 131.0。角速度乘以时间就是转过的角度把每次采样的角度累加起来就是当前相对初始位置的总角度。数学上这就是一次离散积分代码上就是一行乘法和一行加法。static unsigned long pretime 0; unsigned long mictime millis(); unsigned long time mictime - pretime; // 本次采样间隔毫秒 gy - GX_ZERO; // 扣掉陀螺零偏 float gyro gy / 131.0; // °/s±250 量程 float gx_angle gyro * time / 1000.0; // 本次采样转过的角度单位度 total_angle - gx_angle; // 符号取决于安装方向别照抄 pretime mictime;问题出在“累积”两个字上。每次采样都有量化误差和零偏残差单次看只有零点几度但一秒 100 次、一分钟 6000 次误差会线性叠加。更麻烦的是time不是恒定的loop()里有 I2C 读取、浮点运算、串口打印每次循环耗时都在变用固定 dt 去乘积分结果和真实转动角度就对不上。这是陀螺仪积分最典型的漂移来源也是后面要上滤波的直接原因。代码里的total_angle - gx_angle用的是减号因为模块装在小车上时车往前倾对应的 gx 读数为正而角度定义往前倾为负。这个符号一旦搞反PID 会正反馈车会越调越猛直接冲出去。换模块朝向就要重新确认符号这是必须手动验证的一步。4.2 卡尔曼滤波五个方程对应到五行代码卡尔曼滤波在这里做的事很朴素用陀螺仪积分的结果做预测用加速度计算出的角度做观测按各自的噪声水平加权输出一个既不太抖也不太漂的角度。状态量只有两个——角度和陀螺零偏所以整套运算就是 2×2 矩阵的加减乘除在 AVR 上跑一次几十微秒完全扛得住。float Angle 0.0; // 融合后的最终角度 float Q_bias 0.0; // 估计出的陀螺零偏 float Q_angle 0.9; // 角度过程噪声 float Q_gyro 0.001; // 零偏过程噪声 float R_angle 0.5; // 加速度计测量噪声 float dt 0.01; // 采样周期必须与真实循环周期一致 float PP[2][2] {{1, 0}, {0, 1}}; // 状态协方差矩阵 float Pdot[4] {0, 0, 0, 0}; void Kalman_Filter(float Accel, float Gyro) { // 1. 状态预测上一时刻角度 (角速度 - 零偏) × dt Angle (Gyro - Q_bias) * dt; // 2. 协方差预测 Pdot[0] Q_angle - PP[0][1] - PP[1][0]; Pdot[1] -PP[1][1]; Pdot[2] -PP[1][1]; Pdot[3] Q_gyro; PP[0][0] Pdot[0] * dt; PP[0][1] Pdot[1] * dt; PP[1][0] Pdot[2] * dt; PP[1][1] Pdot[3] * dt; // 3. 计算卡尔曼增益 float Angle_err Accel - Angle; // 观测残差 float PCt_0 PP[0][0]; float PCt_1 PP[1][0]; float E R_angle PCt_0; if (E ! 0) { // 防除零虽然理论上不会到 0 float K_0 PCt_0 / E; float K_1 PCt_1 / E; // 4. 协方差更新 float t_0 PCt_0; float t_1 PP[0][1]; PP[0][0] - K_0 * t_0; PP[0][1] - K_0 * t_1; PP[1][0] - K_1 * t_0; PP[1][1] - K_1 * t_1; // 5. 状态更新角度和零偏同时被修正 Angle K_0 * Angle_err; Q_bias K_1 * Angle_err; } }五个步骤和五行核心语句是一一对应的预测角度、预测协方差、算增益、更新协方差、更新状态。Accel传进来的是加速度计算出的角度Gyro传进来的是扣掉零偏前的角速度函数内部用Q_bias再扣一次这样零偏本身也变成了被估计的量不需要一次标定到完美。三个噪声参数决定了融合的性格调它们比调 PID 更需要耐心参数原作取值物理含义调大之后Q_angle0.9角度过程噪声代表对陀螺积分的信任度更信加速度计响应快、毛刺多Q_gyro0.001零偏过程噪声代表零偏变化的快慢零偏跟随快但容易把真实转动误判成零偏R_angle0.5加速度计测量噪声更信陀螺仪平滑但滞后明显原作自己也提到了调完之后黄色融合曲线有滞后现象。原因就在Q_angle 0.9偏大——它让滤波器过度相信加速度计而加速度计在车体抖动时噪声很大滤波器为了压噪声只好把响应放慢。常见的工程取值是Q_angle 0.001、Q_gyro 0.003、R_angle 0.5响应会快不少。这三个数没有标准答案跟车重、电机振动、采样率都有关只能实测。4.3 串口三路输出与曲线观测只靠串口打印数字很难判断滤波到底有没有效果。把三路数据同时打出来丢给上位机绘成曲线一眼就能看出谁抖、谁漂、谁滞后。void loop() { // ... 前面读数据、算 ax_angle、算 total_angle ... dt time / 1000.0; // 用真实间隔更新采样周期 Kalman_Filter(ax_angle, gyro); // 三路输出加速度计角度、陀螺积分角度、融合角度 Serial.print(ax_angle); Serial.print(,); Serial.print(total_angle); Serial.print(,); Serial.println(Angle); }三列用逗号分隔上位机按 CSV 解析正好三条曲线。绿色是加速度计角度抖动明显但长期不漂红色是陀螺积分角度平滑但缓慢偏离黄色是融合结果理想状态下应该贴合红色曲线的平滑度、贴合绿色曲线的长期趋势。这里有个容易忽略的细节Serial.print在 38400 波特率下打印三行浮点大约要 3 到 5ms这段时间也被算进了time里dt 会偏大且不稳定。如果发现融合曲线在串口打开和关闭时表现不一样基本就是这个原因。处理办法是把输出降频比如每 10 次循环打印一次或者把打印放到 dt 计算之后、下一轮采样之前别让它污染采样间隔。5. 避坑与排查零偏、量程、时间戳这几个坑这套代码看着不长但每一行都藏着能让车趴下的细节。下面这几条是我在实际搭车过程中反复撞上的按“现象 → 原因 → 解决”记下来出问题时对着查比盲改参数快得多。5.1 上电就飘零偏与量程的三个坑坑一零偏照抄别人的值水平放置角度却不是 0。现象模块平放在桌面上串口输出的ax_angle稳定在 18° 或 -12°怎么调都不回零。原因AX_ZERO和GX_ZERO是每块模块自己的出厂偏差受焊接应力、芯片个体差异、供电电压影响不同板子能差几百个 LSB。抄别人的 -1476等于把别人的偏差装到自己车上。解决写一个独立的校准 sketch水平静置采 2000 次求平均把结果填回宏定义。// 校准用模块必须水平静置周围不要有振动 void calibrate() { long sax 0, sgx 0; const int N 2000; for (int i 0; i N; i) { accelgyro.getMotion6(ax, ay, az, gx, gy, gz); sax ax; sgx gx; delay(2); // 2ms 间隔避开芯片内部采样节拍 } Serial.print(AX_ZERO ); Serial.println((float)sax / N, 1); Serial.print(GX_ZERO ); Serial.println((float)sgx / N, 1); }采样期间别碰桌子空调出风口对着吹也会让读数偏。校完之后隔一段时间最好再校一次芯片发热后零偏会缓慢变化。坑二改了量程没改系数角度差一倍。现象把加速度量程从 ±2g 改成 ±4g角度读数直接变成实际值的一半左右。原因±2g 灵敏度 16384 LSB/g±4g 变成 8192 LSB/g同样的重力分量读出来的计数减半而换算系数还是按 16384 算的。解决把灵敏度做成常量换算系数由它推导量程改了只改一处。前面 3.3 节给过写法DEG_PER_LSB会跟着LSB_PER_G自动变。陀螺仪同理±250 换 ±500 时 131 要改成 65.5。坑三AD0 接错I2C 地址对不上。现象串口一直打印MPU6050 connection failed示例程序也读不到数。原因模块的 AD0 引脚接高电平时地址从 0x68 变成 0x69而代码里用的是默认构造函数还在按 0x68 通信。有些模块板上 AD0 默认被拉高自己没注意。解决把 AD0 接地改用 0x68或者在构造时显式指定地址MPU6050 accelgyro(0x69);。判断方法很简单接上逻辑分析仪看 SCL/SDA 上有没有 0x68 的应答没有就说明地址不对或者根本没接上。5.2 越跑越偏时间戳与符号的两个坑坑四dt 写死 0.01实际循环周期 30ms。现象陀螺仪积分角度明显跟不上真实转动快速摆动时融合角度滞后一大截慢慢转反而正常。原因dt 0.01是假设循环 100Hz但实际循环里有 I2C 读取、浮点运算、三行串口打印加起来 20 到 30ms 很正常。卡尔曼预测用的是 dtdt 偏小意味着预测的角度增量偏小滤波器一直在“追不上”。解决用millis()的差值算真实 dt并把它传进滤波函数而不是用全局常量。同时把串口打印降频减少对采样间隔的扰动。millis()是unsigned long溢出回绕时用无符号减法mictime - pretime能自动处理不用特殊判断但第一次进loop()时pretime是 0差值是个巨大的数必须单独返回一次只记录时间。坑五角度符号反了PID 越调越猛。现象手动把车往前倾串口角度往负方向走接上电机后车猛地朝前冲然后倒下。原因total_angle - gx_angle里的减号是按某一种模块安装朝向写的。模块转 180°、或者换了个安装面gx 的正负就反了积分角度也跟着反。解决不接电机手动倾斜车身观察角度符号是否和“前倾为负、后倾为正”的约定一致。不一致就把减号改成加号或者把模块物理转 180°。这一步必须在接电机之前做完否则第一次上电就可能烧驱动。5.3 一套从串口数字查起的排查顺序出问题时别一上来就怀疑卡尔曼参数按下面的顺序走绝大多数问题在前三步就能定位先看testConnection()。失败就是接线、地址、供电的问题跟算法无关。水平静置看ax_angle是否接近 0。不接近就是零偏没校准或者量程系数不对。手动倾斜看符号和幅度。幅度差一倍查量程符号反了查安装方向。看total_angle在静止时是否缓慢单向漂移。漂得快说明零偏没扣干净漂得慢是正常现象靠滤波压。最后才动 Q/R 参数。参数是在前三步都正常的前提下才有意义链路有问题时调参数只会越调越乱。6. 二次开发参数整定、移植和输出改造这套代码跑通之后真正花时间的是把它调准、移植到自己的板子上、再把输出改成适合控制环路的形式。下面三件事是我每次做新板子都会走一遍的。第一件是参数整定。别凭感觉改 Q 和 R用阶跃响应看。把车快速倾斜 30° 再回正看融合曲线追上加速度计曲线的速度和超调量。滞后明显就把Q_angle往小调0.01 量级起步抖动明显就把R_angle往大调。Q_gyro一般保持在 0.001 到 0.01 之间太大零偏会跟着真实转动跑太小零偏修正跟不上温漂。每次只改一个参数改完记录一组曲线别一次改三个然后不知道是谁起的作用。第二件是移植。Kalman_Filter本身不依赖 Arduino把它抽成一个纯 C 函数dt和三个 Q/R 作为参数传进去就能直接搬到 STM32 或者别的单片机上。采样周期建议改成定时器中断触发比如 5ms 一次比loop()里靠millis()自计时稳定得多。移植时唯一不能丢的是零偏校准——换了芯片AX_ZERO和GX_ZERO必须重新测。方案计算量滞后可调参数适用卡尔曼滤波中2×2 矩阵运算可调取决于 Q/RQ_angle、Q_gyro、R_angle对精度有要求互补滤波极低两次乘法固定取决于权重一个权重系数资源紧张的 8 位机一阶低通极低较大截止频率只求平滑第三件是输出改造。串口文本适合调试不适合控制环路。真要接 PID把三路数据打包成二进制帧发出去帧头加校验波特率提到 115200 以上采样率能翻好几倍。如果只是想快速验证滤波效果互补滤波是个不错的对照物// 互补滤波高频信陀螺低频信加速度计 float comp_angle 0.0; void complementary(float acc_angle, float gyro_rate, float dt_s) { // 0.98 给积分项0.02 给观测项权重越小越平滑但越滞后 comp_angle 0.98 * (comp_angle gyro_rate * dt_s) 0.02 * acc_angle; }把它和卡尔曼的输出画在同一张图上如果两条曲线走势一致说明卡尔曼没跑偏如果差得远先回头查 dt 和零偏而不是怀疑滤波算法本身。我最早做这块板子的时候调了两天参数都不满意最后发现是AX_ZERO用了示例代码里的默认值模块平放读出来就是 15°。从那以后我每次新焊一块板子第一件事就是跑校准 sketch把零偏打印出来贴在代码注释里再谈别的。希望帮到你。本文还有配套的精品资源点击获取
