简介面向刚接触惯性传感器与自平衡车开发的初学者这份陀螺仪数据分析文档围绕MPU6050模块展开重点梳理加速度计与陀螺仪的工作原理、三轴加速度分解、倾角换算及角速度积分等基础问题并说明如何用近似线性关系绕开单片机中反正弦计算不便的难点。压缩包内仅有1个docx文件约689KB内容以学习笔记与心得汇总的形式组织便于按原理、测量、计算和模块连接等线索阅读。已有893人学习下载适合作为MPU6050入门实践、课程设计或自平衡小车调试前的参考资料。文中还涉及加速度计动态误差、陀螺仪积分误差以及卡尔曼滤波融合思路能帮助读者理解倾角获取流程与常见误差来源并为进一步编写Arduino采集与姿态解算程序提供概念基础。1. 从 0x68 开始陀螺仪数据分析真正难的不是读寄存器模块买回来接线十分钟库装好示例代码跑起来串口一开六个数在跳——很多人到这一步就以为搞定了。可把板子倾斜到 30 度串口打出来的角度却在 -5 到 12 之间乱飘静止放着不动数值又自己慢慢往上爬。陀螺仪数据分析这件事难的地方从来不是从 0x68 这个地址把寄存器读出来而是读出来之后那一串 int16 该怎么变成能喂给控制环的、可信的角度。这篇讲的就是这段转换加速度计怎么算倾角、陀螺仪怎么积分、两路都不准的数怎么用卡尔曼滤波凑成一路能用的以及我在调这几步时踩过的坑。新手可以照着公式和代码直接跑通熟手可以对着参数表和误差分析看看自己那套判断依据站不站得住。2. 加速度计算倾角把三轴重力拆成一条能塞进单片机的公式2.1 先锁死一个轴为什么只需算 x 与桌面的夹角很多人一上来就想解三元方程用三个轴的合成重力反推姿态结果在单片机上又是开方又是反三角算到最后自己都不知道哪一步错了。真正省事的做法是先看安装方式模块平放在小车底盘上两个车轮的车轴和模块的 y 轴在同一条直线上那么车子前后摆动、整体前进后退的过程中y 轴始终和桌面平行它跟桌面的夹角恒为 0 度。变化的只有 x 轴和 z 轴与桌面的夹角而且这两个角的变化量是相等的——绕 y 轴转x 和 z 是一起转的。这一步判断的价值在于三维问题被压成了二维。从 y 轴方向俯视下去坐标系原点 o 和 y 轴重合剩下的就是 x 轴、z 轴和重力加速度 g 构成的平面几何。你只需要算出 x 轴与桌面的夹角就能完整反映小车的倾斜情况。再看几何关系。设小车倾斜角为 a重力 g 在 x 轴和 z 轴上的分量分别是 gx、gz。重力加速度垂直于水平面所以 g 与桌面的法线平行。由 a b 90 度a 是 x 轴与桌面夹角b 是 g 与 x 轴的夹角又因为 x 轴与 y 轴垂直得到 c b 90 度于是 a c。而 g、gx、gz 三者构成一个矩形对角相等得到 c d。所以算 d 就等于算 a而 d 直接对应 gx 与 g 的比值sin d gx / g。到这一步公式已经成立了但麻烦在于要算反正弦。单片机做 asin 要么查表要么跑浮点库代价不小而且精度还不一定够。真正让这条公式变得「能塞进去」的是下一步的近似。2.2 正弦定理推到线性近似asin 是怎么被绕过去的关键事实是在小角度范围内角度的正弦值和它对应的弧度值几乎成正比。把 0 到 90 度的 sin x 曲线、y x·π/180 这条弧度直线、以及 y 0.92·x·π/180 这条带系数的直线画在同一张图上就能看出来0 到 29 度区间内sin x 和 x·π/180 基本重合超过这个范围两条线开始分开但乘上 0.92 之后再比重合区间能拉到 ±45 度左右。对一台两轮自平衡车来说摆动范围本来就在 ±29 度以内超过这个角度车已经救不回来了。所以 sin x ≈ x·π/180 这个近似在工程上是成立的。如果担心余量不够就乘系数 ksin x ≈ k·x·π/180。两个公式联立gx/g k·d·π/180解出 d 180·gx / (k·g·π)。g 是常数k 是你自己选的剩下唯一要读的就是 gx。反三角被彻底绕掉了整个倾角计算退化成一次除法。代价也很明确值得算清楚。下面这张表是按 k 0.92 算的近似误差实际角度sin 值k·弧度值k0.92相对误差10°0.173650.16057约 7.5%20°0.342020.32113约 6.1%29°0.484810.46564约 4.0%45°0.707110.72257约 2.2%60°0.866030.96342约 11.2%可以看到误差并不是单调的45 度附近反而最贴合60 度之后迅速崩掉。这解释了一个常见现象——车在中间位置附近读数还挺准一旦大幅倾斜加速度计算出来的角度就开始明显偏大。所以不要把 k 当成「精度旋钮」随便调它决定的是近似成立的区间宽度选 1.0 就是 ±29 度选 0.92 就是 ±45 度再往外就没有意义了。提示这层近似成立的前提是「重力是唯一加速度」。一旦电机出力加速度计读到的是重力加速度和运动加速度的合成矢量算出来的角度会跟着运动方向偏移。这一步先不处理交给后面的滤波。2.3 量程与 LSB代码里那个 262 到底怎么来的模块出来的是原始 ADC 值得先换算成物理量。加速度计的分辨率取决于设定的量程量程越小每 g 对应的数字越大分辨率越高加速度量程灵敏度LSB/g说明±2g16384静态倾角首选分辨率最高±4g8192有较大冲击时用±8g4096跌落、碰撞检测±16g2048高动态场景把 2.3 节那条公式里的 g 换成 LSB 数就得到了代码里那个常数。按 ±2g 量程g 16384k 0.92π 取 3.14。分母是 0.92 × 3.14 × 16384 / 180 ≈ 263.1。所以角度 ax / 263.1代码里写成 ax / 262差了 0.4%属于取整时的手误实测影响不到半度可以接受但心里要有数。陀螺仪那边同理它输出的是角速度量程和灵敏度对应关系如下陀螺仪量程灵敏度LSB/(°/s)适用场景±250131自平衡车、云台最常用±50065.5有一定快速转动±100032.8姿态剧烈变化±200016.4高动态分辨率牺牲大选了 ±250 就意味着每一度每秒的变化只对应 131 个数字静止时的零偏噪声在原始值上可能就有 ±30 到 ±50换算过来接近 0.3 °/s。这个量级听着小但后面会看到它积分起来是要命的。3. 陀螺仪积分角速度乘时间背后有一本漂移账3.1 三行代码的积分为什么必须带时间戳陀螺仪读的是角速度要得到角度就得积分角度 角速度 × 时间然后累加。听起来简单但实现上有两个必须做对的地方。第一时间必须是真实测出来的不能拍脑袋写一个常数。常见的错误写法是angle gyro * 0.01;默认循环刚好 10ms 一次。可只要串口打印、滤波计算、别的任务插进来循环周期立刻就变了实际可能是 12ms、15ms甚至因为某次 I2C 读失败变成 30ms。用固定 dt 积分等于在给角度注入一个和负载相关的系统误差跑久了必然飘。第二符号方向要和加速度计对齐。两路数据要融合前提是它们描述的是同一个物理量、同一个正方向。如果陀螺仪积分出来是「车往前倾为正」加速度计算出来是「车往后倾为正」融合出来的结果就是两路互相拉扯你会以为是滤波参数没调好实际上是符号反了。按时间戳积分的骨架就三行// pretime/mictime 用 millis() 取单位毫秒 unsigned long time mictime - pretime; // 本次循环实际间隔ms float gyro gy / 131.0; // ±250 量程131 LSB/(°/s) float gx_angle gyro * (time / 1000.0); // 角速度 × 秒 本次转过的角度 total_angle - gx_angle; // 累加符号按安装方向取gy / 131.0这里必须写.0写成gy / 131就是整数除法角度会被截断成整度分辨率直接掉一个量级。time / 1000.0同理。这类问题在编译时不会有任何警告跑起来看着也「有数」但就是不准。3.2 零偏校准静止采 2000 次求均值不是玄学陀螺仪有个绕不开的毛病明明一动不动读出来的角速度不是 0而是某个固定偏移。这个偏移来自器件本身的制造差异和焊接应力每个模块都不一样手册上给的典型值只能当参考。处理办法很土但很有效——上电后让模块静止放几秒连续读 2000 次求平均把这个均值当作零偏存下来之后每次读数都减掉它。采 2000 次是为了让随机噪声互相抵消剩下的就是稳定的直流偏置。采样次数太少均值本身还带着噪声太多则上电等待时间变长。// 静止标定串口打印出均值手动填回宏定义 long sum_ax 0, sum_gx 0; for (int i 0; i 2000; i) { accelgyro.getMotion6(ax, ay, az, gx, gy, gz); sum_ax ax; sum_gx gx; delay(2); // 给 I2C 一点喘息避免读太快拿到重复值 } Serial.print(AX_ZERO); Serial.println(sum_ax / 2000); Serial.print(GX_ZERO); Serial.println(sum_gx / 2000);代码里直接把标定结果写成宏#define AX_ZERO (-1476) // 加速度计 x 轴零偏实测填入 #define GX_ZERO (-30.5) // 陀螺仪零偏实测填入注意GX_ZERO写成小数是有意义的因为减完之后还要除以 131小数位会保留下来而AX_ZERO是整数因为加速度原始值本身就是整数小数部分没有信息量。提示零偏会随温度变。冷机启动标定一次跑十分钟板子热了零偏可能漂几个 LSB。要求高的场景得做温补或者运行时估计普通自平衡车用「上电静止标定 卡尔曼里的 Q_bias 在线修正」就够了。3.3 算一笔漂移账0.5 °/s 能在一分钟内偏出多少度很多人对漂移没概念觉得零点几度每秒不算什么。那就把账算清楚假设零偏没校准干净残余 0.5 °/s那么10 秒后偏 5 度30 秒后偏 15 度60 秒后偏 30 度一台自平衡车的安全倾角范围也就 ±30 度左右。也就是说一分钟之内陀螺仪单独积分出来的角度能自己「走」到临界值而此时车其实是正正当当立着的。这就是纯陀螺仪方案不可用的原因——它不是不准是误差会累积而且累积速度跟时间成正比没有任何自我纠正的机制。加速度计恰好相反它没有累积误差长时间看是稳的但短时间内的噪声大而且一遇到运动加速度就被污染。两者特性正好互补这才是要做融合的根本原因不是为了「用上高级算法」。4. 卡尔曼融合两路都不准的数据怎么凑出一路能用的4.1 五个参数各管一段Q_angle、Q_gyro、R_angle、dt、C_0卡尔曼滤波的代码在网上被抄了无数遍但真正决定输出好不好的不是那几行矩阵运算而是五个参数。它们各自控制一段逻辑改错一个方向现象完全不一样。参数含义调大的效果典型初值Q_angle角度过程噪声更信任加速度计响应快但抖0.001 ~ 0.01Q_gyro陀螺仪过程噪声更信任陀螺仪平滑但滞后0.0001 ~ 0.001R_angle加速度计测量噪声更不信任加速度计抑制抖动但滞后加重0.1 ~ 1.0dt采样周期秒必须等于真实循环间隔否则全盘失真按实测填C_0观测系数一般是 1改了等于改了观测模型1代码里那组Q_angle0.9, Q_gyro0.001, R_angle0.5其实偏激进Q_angle 给到 0.9意味着非常信任加速度计好处是跟得快坏处是车一动、加速度计被运动加速度污染输出立刻跟着抖。这就是为什么调这组参数不能照抄得看你的车重、电机响应和采样率。滤波函数本身的结构不算复杂核心是「先用陀螺仪预测再用加速度计修正」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) 计算卡尔曼增益E 做除零保护 Angle_err Accel - Angle; PCt_0 C_0 * PP[0][0]; PCt_1 C_0 * PP[1][0]; E R_angle C_0 * PCt_0; if (E ! 0) { K_0 PCt_0 / E; K_1 PCt_1 / E; } // 4) 更新协方差 t_0 PCt_0; t_1 C_0 * 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; }第 5 步里的Q_bias是这套实现比「比例融合」更好的地方它把陀螺仪零偏当成一个状态量一起估计跑一段时间之后零偏会被自动修正掉一部分等于给零偏校准上了一道保险。if (E ! 0)那句也别删R_angle被误设成 0 而协方差又恰好为 0 时会直接除零得到一个非法值之后整条曲线就废了。4.2 调参顺序和对应的现象先定 dt再压 R最后放 Q调参最忌讳五个数一起动改完看到曲线变了也不知道是谁的功劳。可行的顺序是第一步把 dt 定死。在loop()里实测循环间隔串口打印出来看波动范围如果抖动超过 ±2ms先把串口打印频率降下来或者把Serial.print挪到慢速分支里。dt 不准后面所有参数都是在给一个错误的模型调参。第二步压 R_angle。把车静置看融合后的角度波动幅度。波动大就加大 R_angle更不信任加速度计直到静止时曲线基本拉平。这一步只追求「静得下来」不管响应快慢。第三步放 Q_angle。推一把车看融合角度跟不跟得上。跟不上就加大 Q_angle跟得太跳就减小。Q_angle 和 R_angle 是一对跷跷板通常固定一个调另一个更快收敛。第四步微调 Q_gyro。这个值影响的是长期漂移的抑制强度一般放在最后改的幅度要小因为它的作用在几秒内看不出来得跑一分钟以上才能比较。4.3 黄色曲线的滞后三个原因两个能改用串口绘图工具把三路数据打出来红色是陀螺仪积分、绿色是加速度计算、黄色是融合结果最典型的现象就是——黄色曲线整体比红色慢半拍而且幅度还被削平了。原因是三层第一层是参数。R_angle 偏大或者 Q_gyro 偏大都会让滤波器更相信陀螺仪表现出来的就是「平滑但迟钝」。这在静态下是优点在动态下就是缺点属于必须接受的取舍。第二层是模型失配。加速度计算出来的角度在车身运动时本身就被污染了滤波器把它当观测值去修正等于用错的尺子量东西。这时候加大 R_angle 反而更合理——不是加速度计不准是它在动态下不可信。第三层是采样相位。串口打印、滤波计算、I2C 读取挤在同一个循环里三路数据其实不是同一时刻的。加速度计读一次大概 1ms 上下如果先读加速度再读陀螺仪两者之间已经隔了一段时间融合时就带着相位差。这一层靠调参解决不了只能靠把读取和计算拆开、或者用更高的采样率把间隔压小。第三层最容易被忽略也最容易让人误判成「滤波器不行」。我一般的判断方法是把车完全静止看黄色曲线是否还有滞后——静止时如果还滞后就是参数问题静止时贴合得很好一动就滞后那多半是模型失配或者相位问题。5. 避坑与排查五条我用血泪换来的记录5.1 硬件与通信接上了但不来数现象一串口打印 connection failed或者六个数全是 0。原因通常是 I2C 根本没通。常见的有三种SDA 和 SCL 接反了这两根线很多人是照着模块丝印接的丝印顺序各家不一样模块没有上拉电阻而主控也没开内部上拉线一长波形就塌AD0 引脚悬空导致器件地址在 0x68 和 0x69 之间随机跳。解决办法先跑一遍 I2C 扫描把总线上所有应答地址打出来能扫到 0x68 或 0x69 说明硬件通了扫不到就是接线问题。AD0 必须明确接 GND 或 VCC不要悬空。线长控制在 20cm 以内超过这个长度把 I2C 速率降到 100kHz。现象二数据偶尔卡住不变过一会儿又恢复。原因是 I2C 总线被拉死通常发生在主控复位而模块没断电的瞬间从机还在等时钟。解决加超时判断读失败超过若干次就重新initialize()一次更彻底的做法是把模块电源和主控电源做成一起上电一起断。5.2 算法与参数数来了但角度不对现象三静止放着角度缓慢但持续地往一个方向爬。这是零偏没校准干净。先确认标定时的静止是真的静止——放在桌上但桌子被旁边的人碰了一下2000 次采样里混进了几百次晃动均值就偏了。重做标定并且上电后留出 2 到 3 秒的静止时间再开始积分。如果标定后还是爬检查是不是把GX_ZERO减错了轴减成gy去了。现象四倾斜方向反了或者倾斜 30 度只显示 3 度。方向反是符号约定没统一加速度计和陀螺仪必须用同一套正方向。倾斜 30 度只显示 3 度八成是整数除法惹的祸。原始代码里ax_angle ax / 262;这一句ax是 int16_t262是 int整个表达式按整数运算结果被截断成整数度。表现就是角度只有 0、1、2 这种跳变分辨率 1 度。改成ax / 262.0就恢复正常。现象五静止时很准一推车就乱跳而且比加速度计单独输出还乱。这不是滤波做坏了恰恰是滤波「做对了」——在车运动的瞬间加速度计读到的合矢量里混进了运动加速度滤波器认为观测值可信就把这个错误信息吸收了进去。处理办法是在控制环里根据电机输出给 R_angle 动态加权电机出力大时认为加速度计不可信加大 R_angle静止或匀速时恢复正常值。这一步没有现成公式得对着自己的车试。6. 一次完整复现从静止标定到推一把验证把前面几步串成一条能照着走的路径一共四关每关都有明确的通过标准过不了就不要往下走。第一关通信。I2C 扫描能扫到 0x68或 0x69testConnection()返回成功。通过标准连续读 1000 次不出现失败。这一关不过后面全是白费。第二关标定。模块水平静置至少 5 秒采 2000 次求均值把结果填进AX_ZERO和GX_ZERO。通过标准重新上电后不倾斜的情况下total_angle在 60 秒内的漂移小于 3 度。到不了这个数说明零偏还没吃干净或者采样期间桌面不稳。第三关单路验证。先把三路数据都从串口打出来用上位机画成三条曲线。手里拿着模块缓慢倾斜到 20 度左右再回正观察绿色加速度计应该立刻响应回正后稳定在 0 附近红色陀螺仪积分响应更平滑但回正后不一定回到 0。通过标准绿色在静止时抖动小于 1 度红色在 30 秒内的漂移在可接受范围内。第四关融合验证。把三路同时画出来重点看两件事静止时黄色是否比绿色更平推一把车之后黄色是否比红色更快回到真实姿态。这两条都满足参数就算调通了。如果黄色既抖又慢回过头按上一章的调参顺序重来一遍不要五个参数一起动。回过头看我在第二关和第四关之间反复折腾的时间最长原因很简单——总想通过改滤波参数去弥补前两关的误差。零偏没标定干净滤波器再准也只是在拟合一个错的输入dt 用固定常数参数表上写得再漂亮也是纸上谈兵。从那以后我每次动这套代码都强制先打印三路曲线跑满一分钟确认静止漂移、确认 dt 波动再动参数。希望这个顺序也能帮你少走两圈弯路。本文还有配套的精品资源点击获取 
长河君 · 2026/11/3 13:45:27 · 阅读 1.2k 从0x68到卡尔曼滤波：陀螺仪姿态解算与零偏校准实战
简介面向刚接触惯性传感器与自平衡车开发的初学者这份陀螺仪数据分析文档围绕MPU6050模块展开重点梳理加速度计与陀螺仪的工作原理、三轴加速度分解、倾角换算及角速度积分等基础问题并说明如何用近似线性关系绕开单片机中反正弦计算不便的难点。压缩包内仅有1个docx文件约689KB内容以学习笔记与心得汇总的形式组织便于按原理、测量、计算和模块连接等线索阅读。已有893人学习下载适合作为MPU6050入门实践、课程设计或自平衡小车调试前的参考资料。文中还涉及加速度计动态误差、陀螺仪积分误差以及卡尔曼滤波融合思路能帮助读者理解倾角获取流程与常见误差来源并为进一步编写Arduino采集与姿态解算程序提供概念基础。1. 从 0x68 开始陀螺仪数据分析真正难的不是读寄存器模块买回来接线十分钟库装好示例代码跑起来串口一开六个数在跳——很多人到这一步就以为搞定了。可把板子倾斜到 30 度串口打出来的角度却在 -5 到 12 之间乱飘静止放着不动数值又自己慢慢往上爬。陀螺仪数据分析这件事难的地方从来不是从 0x68 这个地址把寄存器读出来而是读出来之后那一串 int16 该怎么变成能喂给控制环的、可信的角度。这篇讲的就是这段转换加速度计怎么算倾角、陀螺仪怎么积分、两路都不准的数怎么用卡尔曼滤波凑成一路能用的以及我在调这几步时踩过的坑。新手可以照着公式和代码直接跑通熟手可以对着参数表和误差分析看看自己那套判断依据站不站得住。2. 加速度计算倾角把三轴重力拆成一条能塞进单片机的公式2.1 先锁死一个轴为什么只需算 x 与桌面的夹角很多人一上来就想解三元方程用三个轴的合成重力反推姿态结果在单片机上又是开方又是反三角算到最后自己都不知道哪一步错了。真正省事的做法是先看安装方式模块平放在小车底盘上两个车轮的车轴和模块的 y 轴在同一条直线上那么车子前后摆动、整体前进后退的过程中y 轴始终和桌面平行它跟桌面的夹角恒为 0 度。变化的只有 x 轴和 z 轴与桌面的夹角而且这两个角的变化量是相等的——绕 y 轴转x 和 z 是一起转的。这一步判断的价值在于三维问题被压成了二维。从 y 轴方向俯视下去坐标系原点 o 和 y 轴重合剩下的就是 x 轴、z 轴和重力加速度 g 构成的平面几何。你只需要算出 x 轴与桌面的夹角就能完整反映小车的倾斜情况。再看几何关系。设小车倾斜角为 a重力 g 在 x 轴和 z 轴上的分量分别是 gx、gz。重力加速度垂直于水平面所以 g 与桌面的法线平行。由 a b 90 度a 是 x 轴与桌面夹角b 是 g 与 x 轴的夹角又因为 x 轴与 y 轴垂直得到 c b 90 度于是 a c。而 g、gx、gz 三者构成一个矩形对角相等得到 c d。所以算 d 就等于算 a而 d 直接对应 gx 与 g 的比值sin d gx / g。到这一步公式已经成立了但麻烦在于要算反正弦。单片机做 asin 要么查表要么跑浮点库代价不小而且精度还不一定够。真正让这条公式变得「能塞进去」的是下一步的近似。2.2 正弦定理推到线性近似asin 是怎么被绕过去的关键事实是在小角度范围内角度的正弦值和它对应的弧度值几乎成正比。把 0 到 90 度的 sin x 曲线、y x·π/180 这条弧度直线、以及 y 0.92·x·π/180 这条带系数的直线画在同一张图上就能看出来0 到 29 度区间内sin x 和 x·π/180 基本重合超过这个范围两条线开始分开但乘上 0.92 之后再比重合区间能拉到 ±45 度左右。对一台两轮自平衡车来说摆动范围本来就在 ±29 度以内超过这个角度车已经救不回来了。所以 sin x ≈ x·π/180 这个近似在工程上是成立的。如果担心余量不够就乘系数 ksin x ≈ k·x·π/180。两个公式联立gx/g k·d·π/180解出 d 180·gx / (k·g·π)。g 是常数k 是你自己选的剩下唯一要读的就是 gx。反三角被彻底绕掉了整个倾角计算退化成一次除法。代价也很明确值得算清楚。下面这张表是按 k 0.92 算的近似误差实际角度sin 值k·弧度值k0.92相对误差10°0.173650.16057约 7.5%20°0.342020.32113约 6.1%29°0.484810.46564约 4.0%45°0.707110.72257约 2.2%60°0.866030.96342约 11.2%可以看到误差并不是单调的45 度附近反而最贴合60 度之后迅速崩掉。这解释了一个常见现象——车在中间位置附近读数还挺准一旦大幅倾斜加速度计算出来的角度就开始明显偏大。所以不要把 k 当成「精度旋钮」随便调它决定的是近似成立的区间宽度选 1.0 就是 ±29 度选 0.92 就是 ±45 度再往外就没有意义了。提示这层近似成立的前提是「重力是唯一加速度」。一旦电机出力加速度计读到的是重力加速度和运动加速度的合成矢量算出来的角度会跟着运动方向偏移。这一步先不处理交给后面的滤波。2.3 量程与 LSB代码里那个 262 到底怎么来的模块出来的是原始 ADC 值得先换算成物理量。加速度计的分辨率取决于设定的量程量程越小每 g 对应的数字越大分辨率越高加速度量程灵敏度LSB/g说明±2g16384静态倾角首选分辨率最高±4g8192有较大冲击时用±8g4096跌落、碰撞检测±16g2048高动态场景把 2.3 节那条公式里的 g 换成 LSB 数就得到了代码里那个常数。按 ±2g 量程g 16384k 0.92π 取 3.14。分母是 0.92 × 3.14 × 16384 / 180 ≈ 263.1。所以角度 ax / 263.1代码里写成 ax / 262差了 0.4%属于取整时的手误实测影响不到半度可以接受但心里要有数。陀螺仪那边同理它输出的是角速度量程和灵敏度对应关系如下陀螺仪量程灵敏度LSB/(°/s)适用场景±250131自平衡车、云台最常用±50065.5有一定快速转动±100032.8姿态剧烈变化±200016.4高动态分辨率牺牲大选了 ±250 就意味着每一度每秒的变化只对应 131 个数字静止时的零偏噪声在原始值上可能就有 ±30 到 ±50换算过来接近 0.3 °/s。这个量级听着小但后面会看到它积分起来是要命的。3. 陀螺仪积分角速度乘时间背后有一本漂移账3.1 三行代码的积分为什么必须带时间戳陀螺仪读的是角速度要得到角度就得积分角度 角速度 × 时间然后累加。听起来简单但实现上有两个必须做对的地方。第一时间必须是真实测出来的不能拍脑袋写一个常数。常见的错误写法是angle gyro * 0.01;默认循环刚好 10ms 一次。可只要串口打印、滤波计算、别的任务插进来循环周期立刻就变了实际可能是 12ms、15ms甚至因为某次 I2C 读失败变成 30ms。用固定 dt 积分等于在给角度注入一个和负载相关的系统误差跑久了必然飘。第二符号方向要和加速度计对齐。两路数据要融合前提是它们描述的是同一个物理量、同一个正方向。如果陀螺仪积分出来是「车往前倾为正」加速度计算出来是「车往后倾为正」融合出来的结果就是两路互相拉扯你会以为是滤波参数没调好实际上是符号反了。按时间戳积分的骨架就三行// pretime/mictime 用 millis() 取单位毫秒 unsigned long time mictime - pretime; // 本次循环实际间隔ms float gyro gy / 131.0; // ±250 量程131 LSB/(°/s) float gx_angle gyro * (time / 1000.0); // 角速度 × 秒 本次转过的角度 total_angle - gx_angle; // 累加符号按安装方向取gy / 131.0这里必须写.0写成gy / 131就是整数除法角度会被截断成整度分辨率直接掉一个量级。time / 1000.0同理。这类问题在编译时不会有任何警告跑起来看着也「有数」但就是不准。3.2 零偏校准静止采 2000 次求均值不是玄学陀螺仪有个绕不开的毛病明明一动不动读出来的角速度不是 0而是某个固定偏移。这个偏移来自器件本身的制造差异和焊接应力每个模块都不一样手册上给的典型值只能当参考。处理办法很土但很有效——上电后让模块静止放几秒连续读 2000 次求平均把这个均值当作零偏存下来之后每次读数都减掉它。采 2000 次是为了让随机噪声互相抵消剩下的就是稳定的直流偏置。采样次数太少均值本身还带着噪声太多则上电等待时间变长。// 静止标定串口打印出均值手动填回宏定义 long sum_ax 0, sum_gx 0; for (int i 0; i 2000; i) { accelgyro.getMotion6(ax, ay, az, gx, gy, gz); sum_ax ax; sum_gx gx; delay(2); // 给 I2C 一点喘息避免读太快拿到重复值 } Serial.print(AX_ZERO); Serial.println(sum_ax / 2000); Serial.print(GX_ZERO); Serial.println(sum_gx / 2000);代码里直接把标定结果写成宏#define AX_ZERO (-1476) // 加速度计 x 轴零偏实测填入 #define GX_ZERO (-30.5) // 陀螺仪零偏实测填入注意GX_ZERO写成小数是有意义的因为减完之后还要除以 131小数位会保留下来而AX_ZERO是整数因为加速度原始值本身就是整数小数部分没有信息量。提示零偏会随温度变。冷机启动标定一次跑十分钟板子热了零偏可能漂几个 LSB。要求高的场景得做温补或者运行时估计普通自平衡车用「上电静止标定 卡尔曼里的 Q_bias 在线修正」就够了。3.3 算一笔漂移账0.5 °/s 能在一分钟内偏出多少度很多人对漂移没概念觉得零点几度每秒不算什么。那就把账算清楚假设零偏没校准干净残余 0.5 °/s那么10 秒后偏 5 度30 秒后偏 15 度60 秒后偏 30 度一台自平衡车的安全倾角范围也就 ±30 度左右。也就是说一分钟之内陀螺仪单独积分出来的角度能自己「走」到临界值而此时车其实是正正当当立着的。这就是纯陀螺仪方案不可用的原因——它不是不准是误差会累积而且累积速度跟时间成正比没有任何自我纠正的机制。加速度计恰好相反它没有累积误差长时间看是稳的但短时间内的噪声大而且一遇到运动加速度就被污染。两者特性正好互补这才是要做融合的根本原因不是为了「用上高级算法」。4. 卡尔曼融合两路都不准的数据怎么凑出一路能用的4.1 五个参数各管一段Q_angle、Q_gyro、R_angle、dt、C_0卡尔曼滤波的代码在网上被抄了无数遍但真正决定输出好不好的不是那几行矩阵运算而是五个参数。它们各自控制一段逻辑改错一个方向现象完全不一样。参数含义调大的效果典型初值Q_angle角度过程噪声更信任加速度计响应快但抖0.001 ~ 0.01Q_gyro陀螺仪过程噪声更信任陀螺仪平滑但滞后0.0001 ~ 0.001R_angle加速度计测量噪声更不信任加速度计抑制抖动但滞后加重0.1 ~ 1.0dt采样周期秒必须等于真实循环间隔否则全盘失真按实测填C_0观测系数一般是 1改了等于改了观测模型1代码里那组Q_angle0.9, Q_gyro0.001, R_angle0.5其实偏激进Q_angle 给到 0.9意味着非常信任加速度计好处是跟得快坏处是车一动、加速度计被运动加速度污染输出立刻跟着抖。这就是为什么调这组参数不能照抄得看你的车重、电机响应和采样率。滤波函数本身的结构不算复杂核心是「先用陀螺仪预测再用加速度计修正」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) 计算卡尔曼增益E 做除零保护 Angle_err Accel - Angle; PCt_0 C_0 * PP[0][0]; PCt_1 C_0 * PP[1][0]; E R_angle C_0 * PCt_0; if (E ! 0) { K_0 PCt_0 / E; K_1 PCt_1 / E; } // 4) 更新协方差 t_0 PCt_0; t_1 C_0 * 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; }第 5 步里的Q_bias是这套实现比「比例融合」更好的地方它把陀螺仪零偏当成一个状态量一起估计跑一段时间之后零偏会被自动修正掉一部分等于给零偏校准上了一道保险。if (E ! 0)那句也别删R_angle被误设成 0 而协方差又恰好为 0 时会直接除零得到一个非法值之后整条曲线就废了。4.2 调参顺序和对应的现象先定 dt再压 R最后放 Q调参最忌讳五个数一起动改完看到曲线变了也不知道是谁的功劳。可行的顺序是第一步把 dt 定死。在loop()里实测循环间隔串口打印出来看波动范围如果抖动超过 ±2ms先把串口打印频率降下来或者把Serial.print挪到慢速分支里。dt 不准后面所有参数都是在给一个错误的模型调参。第二步压 R_angle。把车静置看融合后的角度波动幅度。波动大就加大 R_angle更不信任加速度计直到静止时曲线基本拉平。这一步只追求「静得下来」不管响应快慢。第三步放 Q_angle。推一把车看融合角度跟不跟得上。跟不上就加大 Q_angle跟得太跳就减小。Q_angle 和 R_angle 是一对跷跷板通常固定一个调另一个更快收敛。第四步微调 Q_gyro。这个值影响的是长期漂移的抑制强度一般放在最后改的幅度要小因为它的作用在几秒内看不出来得跑一分钟以上才能比较。4.3 黄色曲线的滞后三个原因两个能改用串口绘图工具把三路数据打出来红色是陀螺仪积分、绿色是加速度计算、黄色是融合结果最典型的现象就是——黄色曲线整体比红色慢半拍而且幅度还被削平了。原因是三层第一层是参数。R_angle 偏大或者 Q_gyro 偏大都会让滤波器更相信陀螺仪表现出来的就是「平滑但迟钝」。这在静态下是优点在动态下就是缺点属于必须接受的取舍。第二层是模型失配。加速度计算出来的角度在车身运动时本身就被污染了滤波器把它当观测值去修正等于用错的尺子量东西。这时候加大 R_angle 反而更合理——不是加速度计不准是它在动态下不可信。第三层是采样相位。串口打印、滤波计算、I2C 读取挤在同一个循环里三路数据其实不是同一时刻的。加速度计读一次大概 1ms 上下如果先读加速度再读陀螺仪两者之间已经隔了一段时间融合时就带着相位差。这一层靠调参解决不了只能靠把读取和计算拆开、或者用更高的采样率把间隔压小。第三层最容易被忽略也最容易让人误判成「滤波器不行」。我一般的判断方法是把车完全静止看黄色曲线是否还有滞后——静止时如果还滞后就是参数问题静止时贴合得很好一动就滞后那多半是模型失配或者相位问题。5. 避坑与排查五条我用血泪换来的记录5.1 硬件与通信接上了但不来数现象一串口打印 connection failed或者六个数全是 0。原因通常是 I2C 根本没通。常见的有三种SDA 和 SCL 接反了这两根线很多人是照着模块丝印接的丝印顺序各家不一样模块没有上拉电阻而主控也没开内部上拉线一长波形就塌AD0 引脚悬空导致器件地址在 0x68 和 0x69 之间随机跳。解决办法先跑一遍 I2C 扫描把总线上所有应答地址打出来能扫到 0x68 或 0x69 说明硬件通了扫不到就是接线问题。AD0 必须明确接 GND 或 VCC不要悬空。线长控制在 20cm 以内超过这个长度把 I2C 速率降到 100kHz。现象二数据偶尔卡住不变过一会儿又恢复。原因是 I2C 总线被拉死通常发生在主控复位而模块没断电的瞬间从机还在等时钟。解决加超时判断读失败超过若干次就重新initialize()一次更彻底的做法是把模块电源和主控电源做成一起上电一起断。5.2 算法与参数数来了但角度不对现象三静止放着角度缓慢但持续地往一个方向爬。这是零偏没校准干净。先确认标定时的静止是真的静止——放在桌上但桌子被旁边的人碰了一下2000 次采样里混进了几百次晃动均值就偏了。重做标定并且上电后留出 2 到 3 秒的静止时间再开始积分。如果标定后还是爬检查是不是把GX_ZERO减错了轴减成gy去了。现象四倾斜方向反了或者倾斜 30 度只显示 3 度。方向反是符号约定没统一加速度计和陀螺仪必须用同一套正方向。倾斜 30 度只显示 3 度八成是整数除法惹的祸。原始代码里ax_angle ax / 262;这一句ax是 int16_t262是 int整个表达式按整数运算结果被截断成整数度。表现就是角度只有 0、1、2 这种跳变分辨率 1 度。改成ax / 262.0就恢复正常。现象五静止时很准一推车就乱跳而且比加速度计单独输出还乱。这不是滤波做坏了恰恰是滤波「做对了」——在车运动的瞬间加速度计读到的合矢量里混进了运动加速度滤波器认为观测值可信就把这个错误信息吸收了进去。处理办法是在控制环里根据电机输出给 R_angle 动态加权电机出力大时认为加速度计不可信加大 R_angle静止或匀速时恢复正常值。这一步没有现成公式得对着自己的车试。6. 一次完整复现从静止标定到推一把验证把前面几步串成一条能照着走的路径一共四关每关都有明确的通过标准过不了就不要往下走。第一关通信。I2C 扫描能扫到 0x68或 0x69testConnection()返回成功。通过标准连续读 1000 次不出现失败。这一关不过后面全是白费。第二关标定。模块水平静置至少 5 秒采 2000 次求均值把结果填进AX_ZERO和GX_ZERO。通过标准重新上电后不倾斜的情况下total_angle在 60 秒内的漂移小于 3 度。到不了这个数说明零偏还没吃干净或者采样期间桌面不稳。第三关单路验证。先把三路数据都从串口打出来用上位机画成三条曲线。手里拿着模块缓慢倾斜到 20 度左右再回正观察绿色加速度计应该立刻响应回正后稳定在 0 附近红色陀螺仪积分响应更平滑但回正后不一定回到 0。通过标准绿色在静止时抖动小于 1 度红色在 30 秒内的漂移在可接受范围内。第四关融合验证。把三路同时画出来重点看两件事静止时黄色是否比绿色更平推一把车之后黄色是否比红色更快回到真实姿态。这两条都满足参数就算调通了。如果黄色既抖又慢回过头按上一章的调参顺序重来一遍不要五个参数一起动。回过头看我在第二关和第四关之间反复折腾的时间最长原因很简单——总想通过改滤波参数去弥补前两关的误差。零偏没标定干净滤波器再准也只是在拟合一个错的输入dt 用固定常数参数表上写得再漂亮也是纸上谈兵。从那以后我每次动这套代码都强制先打印三路曲线跑满一分钟确认静止漂移、确认 dt 波动再动参数。希望这个顺序也能帮你少走两圈弯路。本文还有配套的精品资源点击获取