简介：面向物联网与嵌入式开发者的六轴加速度传感器姿态解算资料包，围绕ST LSM6DS3TR_C芯片，覆盖驱动移植、数据采集、姿态角计算以及互补滤波/卡尔曼滤波算法实现，适用于无人机、机器人、智能手机及运动健康监测等场景。压缩包共686个文件，约10.52MB，包含350个C源文件、128个H头文件、62个汇编文件，以及工程配置文件（icf/uvprojx/ioc）、链接脚本、库文件（lib/a）、编译输出（o/axf/hex/map）和调试辅助文件，可在Keil/STM32CubeMX等环境中直接打开或对照参考。目前已有11916人浏览学习。资源中提供了基于STM32L0的HAL驱动、DSP数学库调用和四轴上位机调试协议，并附有完整工程结构与编译配置，方便开发者直接查看初始化、I2C读写和数据融合的具体实现，快速验证互补滤波与卡尔曼滤波对姿态角估计的效果，并迁移到自己的硬件平台，是一份兼具理论学习和工程落地价值的参考资料。 做姿态解算这行有几年了，见过不少新手拿到MPU6050这类6轴传感器后，第一件事就是把原始数据打印出来，然后对着串口助手里跳来跳去的数值发愁。明明芯片手册上说精度很高，为什么静止放在桌面上，角度还在 ±2 度之间来回晃？为什么快速转动一下，角度就飘出去回不来了？这篇东西就是来解决这个问题的：把6轴加速度传感器（加速度计+陀螺仪）的数据怎么变成稳定可靠的姿态角，以及互补滤波和卡尔曼滤波这两种主流算法各自的原理、代码实现和实际调参经验，一次讲清楚。
我默认你手上已经有一块能读出传感器原始数据的板子，不管是STM32、Arduino还是ESP32都行。文章不会去重复I2C时序怎么配置、寄存器怎么读取这类基础内容，重点放在“拿到原始数据之后怎么办”这个环节。内容适合正在做平衡车、四轴飞行器、两轮自平衡机器人、云台稳定器或者穿戴设备姿态检测的开发者，也适合刚接触惯导融合算法、想弄明白滤波为什么能把数据变干净的学生。
1. 先别急着写代码，搞明白加速度计和陀螺仪各自的“脾气”
做姿态解算之前，必须清楚一件事：6轴传感器里的两类器件，单拎出来哪个都不能直接用，但它们的缺点恰好是互补的。
加速度计测量的是比力，静止时它输出的就是重力加速度在三个轴上的分量。因为重力方向始终竖直向下，所以只要把加速度计的三个轴分量做个三角函数运算，就能算出当前平面相对水平面的倾斜角。问题在于，加速度计分不清“重力”和“运动产生的加速度”。你拿着板子水平一晃，X轴方向和Y轴方向就会出现额外的线性加速度，这时候算出来的角度就混入了运动噪声。换句话说，加速度计长期稳定、短期跳动大，低频可靠、高频噪声重。
陀螺仪测量的是角速度，单位通常是度/秒（deg/s）或者弧度/秒（rad/s）。它响应快，瞬间转动多少、转得多快都能量得很准，短时间内的变化非常跟手。但是要得到角度，必须对角速度做积分，而积分有个致命的毛病：误差会累积。只要有零点几个度每秒的零漂没被消除，积分几十秒之后姿态角就会漂移好几度，而且这个漂移是单向增加的，时间越长越离谱。
所以姿态解算的本质，就是把“长期稳但短期噪声大”的加速度计和“短期准但长期会漂”的陀螺仪融合起来——让陀螺仪负责短时间的姿态变化，让加速度计不停地把长时间尺度上的漂移拉回来。两种滤波算法做的事情都是这个，只是一个做得很“朴素”，一个做得相对“讲究”。
这里还要提醒一个新手特别容易忽略的点。读取原始数据后，加速度计的值要除以量程对应的灵敏度，转成以g为单位的物理量；陀螺仪要乘以灵敏度系数，转成度/秒。很多人拿着寄存器读出来的原始整数直接算角度，结果静止时角度都是错的，还以为是滤波算法有问题。
2. 姿态角的三个“表示法”，以及为什么大多选四元数
在写滤波代码之前，还要决定一件事：姿态角用什么形式来表示。这个选择会直接影响后面所有公式的写法。常用的有三种：欧拉角、方向余弦矩阵和四元数。
欧拉角最直观，就是横滚角（Roll）、俯仰角（Pitch）和偏航角（Yaw）三个分量，人眼能直接看懂。它的缺点是存在“万向节死锁”问题：当俯仰角接近 ±90 度时，横滚和偏航会变得不可区分，数学上会出现除零或者奇异。如果只是做平衡车这种小角度场景，欧拉角完全够用；但要是做四轴那样可以任意翻转的飞行器，欧拉角就危险了。
方向余弦矩阵没有奇异问题，三个轴之间用一个3x3的旋转矩阵表示姿态，概念上也比较好懂。但它是9个元素，每次更新要算9个微分方程，在MCU上计算量偏大，实时性要求高的场合一般不首选。
四元数是用四个元素 q0、q1、q2、q3 来表示三维旋转，没有万向节死锁，计算量比方向余弦矩阵小，适合在嵌入式平台上跑。它的缺点是不直观，四元数没法直接看出当前角度是多少，最后还得转成欧拉角输出给人看。工程上比较常见的做法是：内部滤波和融合全部用四元数，只在需要显示或者控制时转成欧拉角。
再说坐标系。不同开发板、不同传感器封装定义的坐标轴朝向不一样，有的Z轴向上，有的Z轴向下，这会导致同样的物理倾斜，算出来的角度正负是反的。建议拿到板子后先做一个简单测试：把板子平放，分别绕X轴和Y轴手动倾斜，观察原始加速度计三个轴的数据变化规律，确认坐标系后再定公式里的正负号。这个步骤省不得，我见过太多人卡在“角度怎么算都不对”，最后发现只是轴的方向搞反了。
3. 互补滤波：原理只有一句话，代码不到三十行
互补滤波是我个人最推荐新手第一个实现的融合算法，不是因为精度最高，而是因为它逻辑最清楚，代码量最小，而且对参数不敏感，调起来不容易炸。它的核心思想可以概括成一句话：用高通滤波处理陀螺仪积分，用低通滤波处理加速度计角度，然后加权相加。
陀螺仪积分得到的角度，变化快、跟随好，但存在低频漂移，所以要让它通过高通滤波，把低频漂移去掉；加速度计计算出来的角度，噪声大、动态响应差，但不会长期漂移，所以要让它通过低通滤波，把高频噪声滤掉。两个结果相加，正好得到“既不漂移、又不抖”的角度。互补滤波的“互补”，指的就是高通和低通恰好互相补全对方的频段。
写成数学表达式非常短：
angle = (1 - a) * (angle + gyro_rate * dt) + a * accel_angle
其中a是加速度计的权重系数，取值在0到1之间。这个式子有两种直观理解方式。第一种：陀螺仪积分得到的角度angle + gyro_rate * dt占大头，加速度计的角度accel_angle只占很小一部分，用来慢悠悠地把漂移拉回来。第二种：一阶低通滤波和一阶高通滤波的组合，a越小，滤波截止频率越低，对加速度计的信任越少，滤得越狠但动态延迟也越大。
完整的C语言实现大概是这样的：
#define DT 0.005f // 采样周期，单位秒，根据实际定时器设置 #define ALPHA 0.02f // 加速度计权重系数，典型值范围 0.005 ~ 0.05 static float angle_x = 0.0f; static float angle_y = 0.0f; static float gyro_x_bias = 0.0f; // 陀螺仪X轴零漂，静止标定得到 void complementary_filter(float acc_x, float acc_y, float acc_z, float gyro_x, float gyro_y, float gyro_z) { // 由加速度计计算倾斜角度（单位：度） // 注意：这里的正负号和坐标系强相关，实测时可能需要对调或取反 float accel_angle_x = atan2f(acc_y, acc_z) * 180.0f / M_PI; float accel_angle_y = atan2f(acc_x, acc_z) * 180.0f / M_PI; // 陀螺仪积分得到角度增量，再与加速度计角度做互补融合 angle_x = (1.0f - ALPHA) * (angle_x + (gyro_x - gyro_x_bias) * DT) + ALPHA * accel_angle_x; angle_y = (1.0f - ALPHA) * (angle_y + gyro_y * DT) + ALPHA * accel_angle_y; }这段代码里有两个细节需要特别说明。第一，atan2f比atanf更安全，它能根据两个参数的正负号自动判断角度所在的象限，避免出现 180 度跳变。第二，angle_x的初始值最好在系统上电后直接用加速度计的角度赋值一次，这样开机瞬间输出就是准的，而不是从 0 度慢慢爬过去。
关于ALPHA的取值，我实测下来的经验是：采样频率越高，ALPHA应该选得越小。采样周期 5ms（200Hz）时，0.02 是个不错的起点；如果采样周期降到 2ms，0.01 左右更合适。判断ALPHA是否调好的最直白方法：静止时角度稳定，拿在手里快速晃动时角度没有明显的“拖尾”感，也没有高频毛刺。如果晃动时角度跟不上，说明ALPHA太大或者陀螺仪权重不够；如果静止时角度还在小幅高频抖动，说明ALPHA太小、滤得不够。
互补滤波在平衡车这类动态不太剧烈的场景下完全够用。它的一个明显短板是：ALPHA一旦定死，它对“缓慢转动”和“快速晃动”的响应就是固定的，没法自适应调节。当运动加速度比较大的时候，加速度计的那部分输入会引入比较大的误差。如果项目对动态精度要求很高，就要考虑卡尔曼滤波了。
4. 卡尔曼滤波的工程落地：一维简化版，才是MCU上最实用的写法
很多初学者一听“卡尔曼滤波”就发怵，觉得那是自动控制原理里的高级数学，要懂一堆矩阵运算和随机过程。实际上在姿态解算这个具体场景里，我们根本不需要做什么太高维的事情，因为关心的主要状态——比如横滚角或者俯仰角——是一维的。所以工程上最实用的做法，是对每个轴单独跑一个一维卡尔曼滤波器，而不是搞一个完整的三维姿态卡尔曼。
卡尔曼滤波的核心假设是：系统的真实状态符合线性动态模型，传感器测量值带有高斯白噪声。它通过“预测-更新”两个步骤交替进行，在每一步根据测量值和先验估计的协方差来动态计算卡尔曼增益，从而在“信任模型预测”和“信任传感器测量”之间做一个最优权衡。这正好匹配我们的场景：陀螺仪提供预测（积分角度），加速度计提供测量（绝对角度）。卡尔曼滤波会在每个采样周期自动算出应该更信谁多一点。
姿态角场景下，我觉得最经典的简化状态模型是两维的：角度和陀螺仪零漂。状态向量定义为[angle, bias]，其中angle是当前姿态角，bias是陀螺仪的零漂估计值。为什么把零漂也放进状态量？因为零漂如果不实时估计，积分漂移就没法消除，而卡尔曼滤波可以把它作为一个未知状态估计出来，用完之后直接补偿陀螺仪输出，非常聪明。系统模型就是：
angle_new = angle_old + (gyro_rate - bias) * dt bias_new = bias_old
测量方程就是加速度计计算出的角度，它等于真实角度加上测量噪声。这个模型的两个方程都是线性的，完美符合卡尔曼滤波的适用条件。
下面的代码是一个适合跑在Cortex-M系列MCU上的实现，我实际在多个项目里用过，稳定可靠：
typedef struct { float Q_angle; // 角度过程噪声协方差 float Q_bias; // 零漂过程噪声协方差 float R_angle; // 角度测量噪声协方差 float angle; // 姿态角状态 float bias; // 陀螺仪零漂状态 float P[2][2]; // 状态协方差矩阵 float dt; // 采样时间，单位秒 } Kalman_t; void kalman_init(Kalman_t *kf, float init_angle, float dt) { kf->angle = init_angle; kf->bias = 0.0f; kf->P[0][0] = 0.01f; kf->P[0][1] = 0.0f; kf->P[1][0] = 0.0f; kf->P[1][1] = 0.01f; kf->dt = dt; kf->Q_angle = 0.001f; kf->Q_bias = 0.003f; kf->R_angle = 0.03f; } float kalman_update(Kalman_t *kf, float accel_angle, float gyro_rate) { // Step 1: 先验预测 float rate = gyro_rate - kf->bias; float angle_pred = kf->angle + rate * kf->dt; // Step 2: 先验协方差预测 // P_pred = F * P * F^T + Q kf->P[0][0] += kf->dt * (2.0f * kf->P[1][1] - kf->dt * kf->P[0][1] - kf->dt * 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; // Step 3: 计算卡尔曼增益 float S = kf->P[0][0] + kf->R_angle; float K0 = kf->P[0][0] / S; float K1 = kf->P[1][0] / S; // Step 4: 用加速度计测量值做校正 float innovation = accel_angle - angle_pred; kf->angle = angle_pred + K0 * innovation; kf->bias += K1 * innovation; // Step 5: 后验协方差更新 float P00_pred = kf->P[0][0]; float P01_pred = kf->P[0][1]; kf->P[0][0] -= K0 * P00_pred; kf->P[0][1] -= K0 * P01_pred; kf->P[1][0] -= K1 * P00_pred; kf->P[1][1] -= K1 * P01_pred; return kf->angle; }上面这五步对应的就是卡尔曼滤波标准的五个公式，只不过把矩阵运算按照 2x2 规模展开了，省掉了矩阵乘法库的开销。从代码量来看，完整的一维卡尔曼滤波大概是互补滤波的两到三倍，但从运行时长来看，在72MHz的Cortex-M3上跑一次也就是几个微秒的事，完全不是负担。
使用的时候就很简单了：每个采样周期调用一次kalman_update，传入加速度计算出的角度和陀螺仪的原始角速度，返回的就是融合后的姿态角。每个轴维护一个独立的Kalman_t结构体即可。
关于调参，这是卡尔曼滤波最“玄学”的地方，但其实也不难。核心就三个参数：Q_angle、Q_bias和R_angle。
Q_angle是角度模型的过程噪声，它反映了你对“陀螺仪积分预测”的不信任程度。这个值调大，卡尔曼增益会变大，滤波会更激进地跟随加速度计测量值，响应快但噪声大。Q_bias是零漂估计的过程噪声，它决定了滤波对陀螺仪零漂变化的跟踪速度。如果传感器温漂明显，可以适度调大。R_angle是加速度计测量噪声的方差，它反映你对加速度计角度的信任程度。调大这个值，滤波会更保守、更平滑，但动态响应会变慢。
我的调试经验是：先固定Q_angle和Q_bias，把R_angle从小往大调，找到一个“静止时角度平滑、快速转动时还能跟上”的中间值。然后在动态场景里微调Q_angle。如果输出曲线有高频小锯齿，说明R_angle太小或者Q_angle太大。如果角度有延迟，说明R_angle太大，要往回调。实际调试中，这几个参数在数量级上凑得差不多就不会出大问题，比互补滤波容错性好很多。
5. 跑板实测：互补滤波和卡尔曼滤波在不同工况下的表现差异
说完了原理和代码，我把同一个传感器装上转台和手持设备，分别跑了互补滤波和卡尔曼滤波，对比几种典型工况下的表现。这里只说我实测下来的感受，不是理论推导。
第一组是静止工况。板子放在桌面上不动，互补滤波在ALPHA=0.02时，角度输出大约在 ±0.3 度范围内小幅波动；卡尔曼滤波在R_angle=0.03时，波动范围大约在 ±0.2 度。卡尔曼略占优势，但差距不明显，两者都完全够用。
第二组是快速晃动。手持板子快速甩动，模拟平衡车急加速、无人机受外力扰动的情况。互补滤波由于ALPHA固定，快速甩动时加速度计引入的运动加速度会明显干扰角度输出，表现为角度轨迹有“毛刺”或者尖峰，响应越快、毛刺越明显。卡尔曼滤波因为会动态调整增益，实测在高动态工况下角度曲线明显更平滑，尖峰幅度只有互补滤波的约三分之一。
第三组是长时间运行漂移测试。把板子固定在一个倾斜10度的支架上，连续运行半小时。互补滤波的输出稳定在 10 度附近，没有明显累积漂移，因为加速度计一直在持续拉回陀螺仪漂移。卡尔曼滤波则更稳，几乎是一条直线，因为它连陀螺仪零漂都在实时估计并补偿。这个场景两者都能胜任，但卡尔曼的系统性误差略小。
第四组工况比较有意思：持续用恒定角速度缓慢转动。互补滤波的输出可能会跟随加速度计产生一个轻微的“阶梯感”，因为加速度计角度在低速转动时数值变化不平滑。卡尔曼则基本没有这个问题，输出很线性。
把结论整理成一张表，看起来更直观：
| 对比维度 | 互补滤波 | 卡尔曼滤波（一维简化版） |
|---|---|---|
| 代码量 | 约20行 | 约60行 |
| 计算开销 | 极低 | 低，2x2矩阵运算 |
| 参数数量 | 1个（ALPHA） | 3个（Q_angle/Q_bias/R_angle） |
| 调参难度 | 低，一个值决定平滑度 | 中等，需要理解参数含义 |
| 静止稳定性 | 良好 | 优秀 |
| 高动态响应 | 有毛刺风险 | 平滑，响应快 |
| 长期漂移 | 会被拉回 | 零漂也在线估计 |
| 适合场景 | 平衡车、低成本小产品 | 四轴、机器人、云台 |
所以我的选型建议是这样的：如果项目是平衡车、自稳云台这种运动加速度不算极端、MCU资源非常紧张的场合，互补滤波完全够用，没必要上卡尔曼。但如果是四轴飞行器、动态机器人这类姿态变化剧烈、对角度精度和响应速度都有要求的项目，老老实实用卡尔曼滤波，那点计算开销换来的稳定性很值。
另外提醒一句：无论用哪种滤波，陀螺仪零漂的静态标定都得做。上电后让板子静止一两秒，取这段期间陀螺仪输出的平均值作为gyro_bias。卡尔曼滤波虽然能在线估计零漂，但如果初始零漂太大，滤波初期的收敛会变慢，提前标定一下能省很多事。
6. 别忘了这些坑：采样时间波动、量程溢出和滤波初值
滤波算法写好了，如果工程细节没注意，一样会掉链子。分享几个我吃过亏的地方，每一个都是真实排查过的。
第一个坑是采样时间不固定导致的滤波发散。卡尔曼滤波的预测步骤和陀螺仪积分都依赖dt，如果你用delay()或者不定时中断来触发滤波更新，每次调用的时间间隔不一致，dt却固定写死，那积分就会忽快忽慢，卡尔曼滤波的状态协方差会越来越不准确，输出可能直接发散。解决办法是：要么用固定频率的定时器中断来驱动滤波更新，要么每次读系统时钟计算真实的delta_t传入滤波函数。我后来统一使用后者，代码看起来只有一行改动，但对稳定性影响极大。
第二个坑是加速度计量程问题。6轴传感器的加速度计量程一般在 ±2g 到 ±16g 之间可配置。如果你把量程配成 ±2g，最大只能测2g，那在剧烈运动时加速度很容易超出量程，加速度计的原始数据会饱和在最大值，算出来的角度自然就是错的，整个滤波输出也会跟着失真。如果你做的是高速动态项目，建议把量程调到 ±4g 或 ±8g。代价是灵敏度降低、测量分辨率变小，但动态可靠性大幅提升。
第三个坑是角度初始值。卡尔曼滤波和互补滤波在启动时如果angle初始化为0，而板子实际上不是水平的，那么滤波输出要从0度慢慢“爬”到真实角度，这个过程可能需要几百毫秒到一秒。解决办法是上电后先用加速度计数据计算一次初始角度，直接赋给滤波器的init_angle，然后才开始正常滤波。这个处理对起飞前的无人机姿态初始化尤其重要。
第四个坑是角度正负方向和轴间耦合。前面讲过坐标系的问题，这里再强调一次：不同的安装方式会改变同一个倾斜方向对应的角和轴，最终表现出来的可能是正负号相反、或者横滚和俯仰对调。调试时，给板子一个已知方向的倾斜，然后看滤波输出的角度方向是否一致，不一致就在公式前面加负号或者交换轴的对应关系。不要着急，这一步做对了，后面整个控制或者显示逻辑才可能正确。
还有一点，上位机观察波形时，建议把陀螺仪原始角速度、加速度计角度、滤波后角度一起打印出来。三者对比，你能清楚地看到滤波到底做了什么：陀螺仪角度在漂、加速度计角度在跳、滤波结果走在两者中间，平滑且稳定。这个过程比单纯看滤波结果能帮你更快理解算法的行为，也能更精准地定位是参数问题、数据问题还是代码问题。
7. 根据我的实际经验，给你几条更进一步的建议
如果你把前面的一维卡尔曼或者互补滤波调通了，下一步有几个非常自然的方向可以继续深入。
第一，把两个轴的一维滤波扩展成完整的三维姿态解算。你可以用四元数 + 一阶或二阶龙格库塔积分，做完整的捷联惯导姿态解算。这个方向在无人机领域非常成熟，代码也有很多开源参考。一维卡尔曼适合快速落地，但三维空间里，四元数才是更完备的描述方式，它能避免欧拉角奇异性，还能同时处理三个轴的耦合。
第二，如果项目里还有磁力计，可以考虑把磁力计的数据也融合进来，组合成9轴传感器。磁力计可以矫正偏航角的漂移——陀螺仪积分的偏航角在没有外部参考时会慢慢偏，磁力计提供的就是地球磁场这个绝对参考。这时候常见的做法是：先分别用加速度计和磁力计计算横滚角、俯仰角、偏航角的参考值，再用滤波算法融合，或者使用更完整的9轴融合算法。值得注意的是，磁力计对环境磁场敏感，电机、电流产生的磁场都会干扰它，所以安装位置要离磁性源远一点。
第三，对卡尔曼滤波理解深了之后，可以去接触扩展卡尔曼滤波或者无迹卡尔曼滤波。当系统模型不再线性时，比如用四元数做状态向量时，基本卡尔曼的线性假设就不成立了，这时需要EKF或者UKF来近似处理。很多开源飞控的地磁-惯导融合就用了EKF思路，理解了一维卡尔曼的“预测-更新”框架后再看那些代码，思路会顺很多。
还有一个小技巧：在真机调试前，可以用MATLAB或者Python先把传感器数据存成文件，在电脑上离线跑滤波算法，对比不同参数组合的输出效果。这样可以避免每次调参都要烧录固件、观察串口的低效循环。离线调好一组初始参数，再放到板子上精调，效率能翻倍。我自己习惯用Python的matplotlib直接画波形，观察静止段、快速晃动段的响应，比在串口助手里看数字直观太多了。
最后再给一个实用建议：给滤波代码加上时间戳日志，把每次滤波调用的dt、原始角度、融合角度都打点记录下来。当系统行为异常时，这些日志能帮你快速定位是算法问题还是时序问题，比对着示波器猜省事得多。姿态解算的坑大多不在算法公式本身，而在工程细节——时序、标定、量程、坐标系，这些细节处理好了，姿态角自然就稳了。
本文还有配套的精品资源，点击获取
