简介：这份资源面向物联网、嵌入式与运动姿态感知方向的开发者，围绕6轴加速度传感器LSM6DS3TR_C，系统整理了姿态角解算中互补滤波与卡尔曼滤波两种融合算法的实现代码与工程文件，可用于无人机、机器人、可穿戴设备等需要实时姿态估计的项目参考。压缩包共686个文件，约10.52MB，以350个C源文件与128个头文件为核心，配合62个汇编文件、36个ICF链接脚本及STM32L0系列HAL库、CMSIS-DSP数学库等，构成一套可直接编译调试的完整工程，另含hex、axf、map等构建产物与uvprojx工程配置。已有11925人学习下载。读者可从中获取传感器驱动初始化、数据采集、互补滤波与卡尔曼滤波融合的完整代码框架，以及四轴上位机通信协议与调试思路，便于对照移植到自有硬件平台并深入理解姿态解算的工程落地细节。
1. 六轴传感器读出来的姿态角为什么总在跳：从原始数据到可用角度的距离
六轴加速度传感器加姿态角，再叠上互补滤波和卡尔曼滤波，这套组合在嵌入式姿态解算里几乎是标配。但很多人第一次把 MPU6050 或 ICM 系列传感器接上单片机，把原始数据换算成角度后会发现一个让人抓狂的现象：静止时角度还算稳，一旦晃动，俯仰角和横滚角就开始剧烈跳动，偏航角更是漂得没边。问题不在于传感器坏了，而在于你拿到的加速度计数据和陀螺仪数据各自都有致命缺陷——加速度计对振动和线性加速度极度敏感，陀螺仪积分又会随时间累积漂移。互补滤波和卡尔曼滤波要解决的，正是怎么把这两路都不完美的数据融合成一个可用的姿态角。
这套方案适合做平衡车、云台稳定、四轴飞控、人体动作捕捉、车载姿态监测的开发者。你不需要高深的数学功底，但需要理解几个核心概念：姿态角怎么定义、传感器数据怎么读、滤波器参数怎么调。接下来的内容会从传感器原始数据采集讲起，一步步推到互补滤波和卡尔曼滤波的落地实现，中间会给出可直接编译的代码和参数调节方法，也会把我在实际项目中踩过的坑摊开来说。
2. 六轴传感器原始数据怎么变成姿态角：坐标系、量程与单位换算
2.1 加速度计和陀螺仪各自输出什么
六轴传感器包含三轴加速度计和三轴陀螺仪。加速度计测量的是比力，静止时输出的是重力加速度在三个轴上的分量，单位通常是 g 或 m/s²。陀螺仪测量的是角速度，单位通常是 °/s 或 rad/s。以常见的 MPU6050 为例，加速度计量程可配为 ±2g、±4g、±8g、±16g，陀螺仪量程可配为 ±250°/s、±500°/s、±1000°/s、±2000°/s。量程越小，分辨率越高，但容易饱和；量程越大，动态范围越宽，但噪声也越大。
我一般会这样选：做平衡车或云台，加速度计用 ±4g，陀螺仪用 ±500°/s；做飞控或高动态场景，加速度计用 ±8g 或 ±16g，陀螺仪用 ±1000°/s 或 ±2000°/s。量程选好之后，原始寄存器值要除以灵敏度系数才能得到物理量。比如 MPU6050 在 ±2g 量程下，灵敏度是 16384 LSB/g；在 ±500°/s 量程下，灵敏度是 65.5 LSB/(°/s)。这些系数在数据手册里都能查到，不同型号的传感器系数不同，不能混用。
2.2 从加速度计算俯仰角和横滚角
静止状态下，加速度计的三个轴输出可以反推出俯仰角和横滚角。假设加速度计三轴输出为 ax、ay、az，单位是 g，那么：
- 俯仰角 pitch = atan2(-ax, sqrt(ay² + az²)) × 180 / π
- 横滚角 roll = atan2(ay, az) × 180 / π
这两个公式在静止或低速运动时比较准，但一旦有线性加速度或振动，计算结果就会跳。偏航角 yaw 无法仅靠加速度计得到，因为重力方向在水平面内没有分量，必须借助磁力计或陀螺仪积分。六轴传感器没有磁力计，所以偏航角只能靠陀螺仪积分，漂移不可避免。
下面是一段用 Python 模拟从原始数据计算姿态角的代码，方便你在电脑上先验证逻辑：
import math def accel_to_angle(ax, ay, az): """ 从加速度计数据计算俯仰角和横滚角 ax, ay, az: 加速度计三轴输出，单位 g 返回: (pitch, roll) 单位度 """ # 俯仰角：绕 Y 轴旋转，使用 atan2 避免除零 pitch = math.atan2(-ax, math.sqrt(ay * ay + az * az)) * 180.0 / math.pi # 横滚角：绕 X 轴旋转 roll = math.atan2(ay, az) * 180.0 / math.pi return pitch, roll # 模拟静止时重力在 Z 轴，ax=0, ay=0, az=1 print(accel_to_angle(0, 0, 1)) # 输出 (0.0, 0.0) # 模拟倾斜 30 度 ax = -math.sin(math.radians(30)) az = math.cos(math.radians(30)) print(accel_to_angle(ax, 0, az)) # 输出约 (30.0, 0.0)这段代码的逻辑很直接：用 atan2 代替 atan，避免分母为零的情况，同时利用 sqrt(ay² + az²) 作为参考分量，保证俯仰角在 ±90° 范围内连续。参数说明：ax、ay、az 必须是经过量程换算后的物理量，单位统一为 g；如果传感器安装方向不同，需要先做轴映射，否则算出来的角度符号会反。
2.3 陀螺仪积分与漂移的直观理解
陀螺仪输出的是角速度，对角速度做时间积分就能得到角度变化。假设陀螺仪输出为 gx、gy、gz，单位 °/s，采样周期为 dt 秒，那么：
- pitch_gyro += gx × dt
- roll_gyro += gy × dt
- yaw_gyro += gz × dt
积分看起来简单，但问题在于零偏。陀螺仪静止时输出并不严格为零，总有一个小偏置，比如 0.5°/s。这个偏置积分 60 秒就会产生 30° 的误差，而且误差会一直累积。温度变化、电源波动都会让零偏漂移。所以单纯靠陀螺仪积分，角度只能短时间可信，长时间必然漂走。
实际项目中，我会先做零偏校准：上电后让传感器静止 1 到 2 秒，采集几百个样本求平均，把这个平均值作为零偏，后续积分时减掉。但零偏会随温度变化，所以高精度场景还需要温度补偿，或者用滤波器在线估计零偏。
3. 互补滤波算法落地：一阶融合的代码实现与参数整定
3.1 互补滤波的核心思想
互补滤波的思路很朴素：加速度计在低频段可信，因为重力方向稳定，但高频振动噪声大；陀螺仪在高频段可信，因为响应快，但低频积分漂移。两者正好互补。用一个低通滤波器滤加速度计数据，用一个高通滤波器滤陀螺仪积分数据，再把两者相加，就能得到全频段相对稳定的角度。
一阶互补滤波的公式可以写成：
angle = α × (angle + gyro × dt) + (1 - α) × accel_angle
其中 α 是滤波系数，通常取 0.95 到 0.99 之间。α 越大，越信任陀螺仪，动态响应好但漂移抑制弱；α 越小，越信任加速度计，静态稳但动态滞后。这个系数没有绝对最优值，取决于你的应用场景和传感器噪声水平。
3.2 一阶互补滤波的 C 代码实现
下面是一段可以直接移植到单片机上的互补滤波代码，以 MPU6050 为例：
#include <math.h> #define ALPHA 0.98f // 互补滤波系数 #define DT 0.005f // 采样周期 5ms，对应 200Hz float pitch = 0.0f; // 俯仰角 float roll = 0.0f; // 横滚角 float gyro_bias_x = 0.0f; // 陀螺仪零偏 float gyro_bias_y = 0.0f; // 从加速度计计算角度 void accel_to_angle(float ax, float ay, float az, float *pitch_acc, float *roll_acc) { *pitch_acc = atan2f(-ax, sqrtf(ay * ay + az * az)) * 57.29578f; *roll_acc = atan2f(ay, az) * 57.29578f; } // 互补滤波更新 void complementary_filter_update(float ax, float ay, float az, float gx, float gy) { float pitch_acc, roll_acc; accel_to_angle(ax, ay, az, &pitch_acc, &roll_acc); // 陀螺仪积分，减去零偏 float pitch_gyro = pitch + (gx - gyro_bias_x) * DT; float roll_gyro = roll + (gy - gyro_bias_y) * DT; // 互补融合 pitch = ALPHA * pitch_gyro + (1.0f - ALPHA) * pitch_acc; roll = ALPHA * roll_gyro + (1.0f - ALPHA) * roll_acc; }这段代码的逻辑是：每次采样先算加速度计角度，再用陀螺仪积分预测新角度，最后按 α 加权融合。参数说明：ALPHA 决定信任分配，DT 必须和实际采样周期一致，否则积分比例会错。gyro_bias_x 和 gyro_bias_y 需要在上电校准阶段获取，校准方法就是静止时采集 N 个样本求平均。
3.3 参数整定与动态响应测试
α 的整定没有公式，只能试。我的习惯是：先把 α 设为 0.95，用手快速转动传感器，观察角度跟随是否滞后；如果滞后明显，增大 α 到 0.98 或 0.99；如果静止时角度缓慢漂移，减小 α 到 0.9 或 0.85。测试时最好把角度数据通过串口传到电脑上画曲线，肉眼判断比看数字直观得多。
还有一个容易被忽略的点：加速度计角度在动态时不可信，但互补滤波并没有判断动态条件，所以剧烈运动时加速度计噪声会拉低融合精度。改进方法是加一个动态阈值：当加速度计合矢量偏离 1g 超过一定范围时，临时降低加速度计权重。这个技巧在平衡车和飞控里很常用。
提示：互补滤波的 α 系数和采样周期强相关。如果你把采样率从 200Hz 改成 100Hz，DT 变了，α 也要重新调，否则动态响应会变差。
4. 卡尔曼滤波算法落地：从状态方程到可调参数
4.1 卡尔曼滤波在姿态解算中的状态定义
卡尔曼滤波比互补滤波更严谨的地方在于，它把角度和陀螺仪零偏都作为状态变量，通过协方差矩阵自动调节信任权重。对于一维角度解算，状态向量可以取：
x = [angle, gyro_bias]ᵀ
状态转移方程：
angle(k) = angle(k-1) + (gyro(k-1) - bias(k-1)) × dt bias(k) = bias(k-1)
观测方程：
angle_acc(k) = angle(k) + 观测噪声
这样卡尔曼滤波不仅能融合加速度计和陀螺仪，还能在线估计陀螺仪零偏，比互补滤波多了一个自适应能力。
4.2 一维卡尔曼滤波的 C 代码实现
下面是一段经典的一维卡尔曼滤波代码，适合在单片机上运行：
typedef struct { float angle; // 角度估计值 float bias; // 陀螺仪零偏估计 float P[2][2]; // 协方差矩阵 float Q_angle; // 角度过程噪声 float Q_bias; // 零偏过程噪声 float R_measure; // 测量噪声 } KalmanFilter; void kalman_init(KalmanFilter *kf) { kf->angle = 0.0f; kf->bias = 0.0f; kf->P[0][0] = 0.0f; kf->P[0][1] = 0.0f; kf->P[1][0] = 0.0f; kf->P[1][1] = 0.0f; kf->Q_angle = 0.001f; kf->Q_bias = 0.003f; kf->R_measure = 0.03f; } float kalman_update(KalmanFilter *kf, float new_angle, float new_rate, float dt) { // 预测步骤 float rate = new_rate - kf->bias; kf->angle += dt * rate; // 更新协方差矩阵 kf->P[0][0] += dt * (dt * kf->P[1][1] - kf->P[0][1] - kf->P[1][0] + kf->Q_angle); kf->P[0][1] -= dt * kf->P[1][1]; kf->P[1][0] -= dt * kf->P[1][1]; kf->P[1][1] += kf->Q_bias * dt; // 计算卡尔曼增益 float S = kf->P[0][0] + kf->R_measure; float K[2]; K[0] = kf->P[0][0] / S; K[1] = kf->P[1][0] / S; // 更新步骤 float y = new_angle - kf->angle; kf->angle += K[0] * y; kf->bias += K[1] * y; // 更新协方差矩阵 float P00_temp = kf->P[0][0]; float P01_temp = kf->P[0][1]; kf->P[0][0] -= K[0] * P00_temp; kf->P[0][1] -= K[0] * P01_temp; kf->P[1][0] -= K[1] * P00_temp; kf->P[1][1] -= K[1] * P01_temp; return kf->angle; }这段代码的逻辑是标准卡尔曼滤波的预测-更新循环。参数说明：Q_angle 是角度过程噪声，反映你对陀螺仪积分的信任程度，越大越信任加速度计；Q_bias 是零偏过程噪声，反映零偏变化速度，一般取比 Q_angle 小一个数量级；R_measure 是加速度计测量噪声，越大越不信任加速度计。这三个参数需要根据实际传感器噪声水平调整。
4.3 卡尔曼滤波参数调节的实操方法
调卡尔曼参数比调互补滤波的 α 更麻烦，因为有三个参数互相影响。我的经验是分步来：先把 Q_angle 和 Q_bias 固定，调 R_measure。R_measure 越大，角度越平滑但跟随越慢；R_measure 越小，跟随快但噪声大。找到合适的 R_measure 后，再调 Q_angle。如果静止时角度漂移，减小 Q_angle；如果动态时跟随滞后，增大 Q_angle。最后调 Q_bias，一般取 Q_angle 的十分之一到三分之一。
实际测试时，我会用串口把原始加速度计角度、陀螺仪积分角度和卡尔曼输出角度同时传出来，画三条曲线对比。如果卡尔曼输出在静止时比加速度计还抖，说明 R_measure 太小；如果动态时明显滞后于陀螺仪积分，说明 Q_angle 太小。这种可视化调试比盲调快得多。
注意：卡尔曼滤波的 dt 必须和实际采样周期严格一致。如果采样周期抖动大，协方差矩阵的更新会失准，滤波效果反而变差。建议用定时器触发采样，保证 dt 稳定。
5. 互补滤波与卡尔曼滤波的避坑指南：五个血泪教训
5.1 坑一：传感器安装方向与坐标系不匹配
现象：算出来的俯仰角和横滚角符号相反，或者角度值完全不对。原因：传感器贴板方向不同，芯片的 X、Y、Z 轴和你的机体坐标系不一致。解决：先确认传感器数据手册里的轴方向图，然后在代码里做轴映射。比如芯片 X 轴对应机体 Y 轴，就要交换变量并调整符号。我一般会在代码里加一个宏定义，方便不同安装方向切换。
5.2 坑二：陀螺仪零偏未校准或校准不充分
现象：静止时角度缓慢漂移，几分钟就偏十几度。原因：陀螺仪零偏没有减掉，或者校准时间太短，样本均值不准。解决：上电后静止 2 秒，采集至少 500 个样本求平均作为零偏。如果条件允许，做温度补偿：在不同温度下测零偏，拟合曲线。实际项目中，我会在开机校准阶段加一个 LED 指示，校准完成前不允许电机启动。
5.3 坑三：采样周期不稳定导致积分误差
现象：动态时角度跟随忽快忽慢，卡尔曼滤波输出抖动。原因：主循环里还有其他任务，采样周期不固定，dt 时大时小。解决：用硬件定时器触发采样，或者在代码里用实际时间差代替固定 dt。如果用的是 RTOS，把姿态解算任务优先级设高，保证周期稳定。我见过有人把姿态解算放在 while 循环里，结果串口打印一开，dt 直接翻倍，角度全乱。
5.4 坑四：加速度计动态噪声未处理
现象：电机一转，角度就跳，静止时正常。原因：电机振动通过机架传到传感器，加速度计输出叠加了高频噪声。解决：硬件上加减震棉或减震板，软件上加低通滤波或动态权重。我一般会在加速度计数据进入滤波器之前，先做一个截止频率 20Hz 左右的低通滤波，能滤掉大部分振动噪声。如果振动特别强，还要考虑用陀螺仪短时主导，动态时降低加速度计权重。
5.5 坑五：卡尔曼滤波参数在不同场景下不通用
现象：在平衡车上调好的参数，换到飞控上就不好用。原因：不同场景的动态特性和噪声水平不同，Q_angle、Q_bias、R_measure 需要重新整定。解决：不要指望一套参数走天下。我的做法是给每个项目单独建一个参数配置文件，记录当时的传感器型号、量程、采样率和应用场景。换硬件或换场景时，先跑静态测试，再跑动态测试，重新调参。
6. 进阶技巧：用串口波形和阶跃响应验证滤波效果
6.1 把角度数据传到电脑上画曲线
调滤波器的前提是能看见数据。我习惯用串口把三路数据传出来：加速度计角度、陀螺仪积分角度、滤波输出角度。格式用简单的 CSV，每行四个数：时间戳、加速度计角度、陀螺仪角度、滤波角度。电脑端用 Python 的 matplotlib 实时画图，或者用串口示波器工具。这样一眼就能看出滤波器是在跟随、滞后还是抖动。
import serial import matplotlib.pyplot as plt import matplotlib.animation as animation ser = serial.Serial('COM3', 115200) fig, ax = plt.subplots() xs, y1, y2, y3 = [], [], [], [] def update(frame): line = ser.readline().decode().strip() if not line: return parts = line.split(',') if len(parts) != 4: return t, acc_angle, gyro_angle, fused_angle = map(float, parts) xs.append(t) y1.append(acc_angle) y2.append(gyro_angle) y3.append(fused_angle) ax.clear() ax.plot(xs, y1, label='Accel') ax.plot(xs, y2, label='Gyro') ax.plot(xs, y3, label='Fused') ax.legend() ani = animation.FuncAnimation(fig, update, interval=20) plt.show()这段代码的逻辑是：从串口逐行读取数据，解析后追加到列表，然后重绘三条曲线。参数说明：串口波特率要和单片机一致，interval 控制刷新间隔，太小会卡，太大反应慢。实际使用时，把传感器拿在手里做慢速倾斜和快速晃动，观察三条曲线的差异，就能判断滤波器参数是否合适。
6.2 用阶跃响应判断动态性能
阶跃响应是检验滤波器动态性能的好方法。把传感器从一个静止角度快速转到另一个角度，比如从 0° 快速转到 30°，记录滤波输出到达 30° 的时间。互补滤波的 α 越大，到达时间越短但超调越大；卡尔曼滤波的 Q_angle 越大，响应越快但噪声越大。我的经验是：平衡车场景允许 100ms 左右的响应时间，飞控场景要求 50ms 以内，云台场景可以放宽到 200ms。
6.3 长期稳定性测试与零偏在线估计
短期调好还不够，还要做长期测试。把传感器静止放置 30 分钟，观察滤波输出的漂移量。互补滤波的漂移主要来自陀螺仪零偏，卡尔曼滤波因为在线估计零偏，漂移会小很多。如果卡尔曼滤波长期漂移仍然明显，检查 Q_bias 是否太小，导致零偏估计跟不上实际变化。我一般会把 Q_bias 设为 Q_angle 的 1/10 到 1/3，具体值通过长期测试确定。
最后说一个我自己的习惯：每次调完参数，都会把配置和测试数据存档，标注日期、传感器型号、量程、采样率和应用场景。下次遇到类似项目，直接翻档案，能省很多时间。姿态解算这件事，玄学成分有，但更多是耐心和记录。希望帮到你。
本文还有配套的精品资源，点击获取
