简介针对物联网设备的高精度姿态感知需求这套以LSM6DS3TR_C六轴加速度传感器为核心的嵌入式工程完整实现了三轴加速度计与三轴陀螺仪驱动、互补滤波算法及卡尔曼滤波算法面向STM32L0平台开发者可应用于无人机、机器人、智能手机和运动健康监测等场景。压缩包约10.52MB共686个文件以350个C源码和128个H头文件为主另含62个汇编文件、36个ICF链接脚本、Keil工程文件、CubeMX配置及DSP数学库并附带HEX/AXF/Map等编译产物从源码到可运行固件一应俱全。已有11916人学习/下载。通过该工程开发者能快速掌握传感器驱动移植、加速度计与陀螺仪数据融合、互补滤波与卡尔曼滤波的代码实现结合四轴上位机工具协议与库文件可直接基于Keil工程二次开发显著缩短姿态检测模块的研发周期。 做平衡车、四轴、机械臂的时候姿态解算几乎是绕不开的坎。手里一颗六轴加速度传感器也就是三轴加速度计加三轴陀螺仪的组合配合姿态角估计就能让设备实时知道自己“斜了多少”“转了多少”。方法上最常见的两条路就是互补滤波算法和卡尔曼滤波算法。我最早入门用的是一颗MPU6050先把原始数据读出来再调姿态解算前后折腾了不少晚上也踩了不少坑。这篇文章就从传感器能做什么、姿态角怎么算、两种滤波算法的原理与代码、以及调参避坑这几个角度把我实际用过的方案完整写出来给准备做平衡车、云台、小型无人机或者AR设备的朋友做个参考。1. 六轴传感器到底能测什么1.1 加速度计和陀螺仪的分工六轴传感器并不是一个芯片测六种不同物理量而是把三轴加速度计和三轴陀螺仪封装在一起共用同一个I2C或SPI接口。加速度计测量的是“比力”也就是物体受到的加速度与重力加速度的矢量和。静止时加速度计的三轴读数合成之后严格等于1g方向指向重力反向所以我们可以从静止读数里反推出重力方向进而算出设备当前的倾斜角度。陀螺仪测量的则是绕三个轴的角速度单位一般是deg/s或rad/s。它在短时间内非常准能感知到非常细微的转动但有个致命问题它测的是“变化率”不是“角度”。想要得到角度必须对时间积分。积分就意味着误差累积陀螺仪的零偏、温度漂移都会被一点点积累起来导致角度慢慢飘走。这就是两类传感器天生的互补性加速度计低频稳定、高频噪声大陀螺仪高频响应快、低频会漂移。姿态解算的核心就是把这两者的优势结合起来让系统既有加速度计的长期稳定性又有陀螺仪的短期动态性。1.2 为什么加速度计不能单独用很多人刚接触时喜欢直接把加速度计算出来的角度当最终姿态这么做静止时问题不大一旦设备运动起来就露馅了。加速度计测的是“比力”不是“重力”。当你推着设备快速移动、旋转或者震动时运动加速度会叠加重力方向算出来的姿态角会剧烈跳动甚至出现几十度的尖峰。陀螺仪单独用同样不行。虽然短时间积分出来的角度很平滑但零偏误差会持续累积。我用一款常见模块做过测试静止状态下不校准陀螺仪零偏30秒内积分出的角度能漂移十几度这个速度对于任何控制类应用都是不可接受的。所以必须做数据融合。互补滤波和卡尔曼滤波本质上都是同一个目标短时间看陀螺仪长时间看加速度计最终给出一路连续、低延迟、不发散的角度估计。区别只是实现方式和调参思路。2. 姿态角表示与坐标约定2.1 欧拉角的定义与六轴传感器的局限姿态角一般用欧拉角表示绕X轴旋转叫横滚角roll绕Y轴旋转叫俯仰角pitch绕Z轴旋转叫偏航角yaw。在平衡车、云台这类场景里roll和pitch是核心控制量yaw往往只做辅助或者干脆不用。这里必须先说清楚一个很多人会忽略的坑六轴传感器没有磁力计所以yaw没有任何绝对参考。加速度计能测的是重力方向只能确定roll和pitchyaw绕着重力方向旋转时加速度计读数没有任何变化陀螺仪只能提供yaw的相对角速度长期积分必定漂移。所以严格来说六轴方案能稳定估计的姿态角只有roll和pitchyaw只能短时间参考。想要长时间准确的yaw需要再加磁力计或者视觉/里程计辅助这是选型时就要想清楚的事。坐标约定方面我建议统一采用“右手坐标系”X轴向前Y轴向右Z轴向下。不同传感器的轴向定义可能不一样代码里一定要先确认datasheet。如果发现某个角度反转了把对应的加速度符号或者三角函数的符号改掉即可不要盲目在滤波输出后加负号那样会掩盖问题。2.2 从加速度计原始值计算roll/pitch加速度计计算roll和pitch的标准公式如下#define DEG2RAD (0.01745329251f) #define RAD2DEG (57.295779513f) float roll atan2f(accel_y, accel_z) * RAD2DEG; float pitch atan2f(-accel_x, sqrtf(accel_y * accel_y accel_z * accel_z)) * RAD2DEG;这里accel_x、accel_y、accel_z可以直接使用原始ADC值也可以先除以灵敏度换成g。因为atan2内部的分子分母都包含同一个比例因子只要三个轴的单位一致最终角度结果是一样的所以我很少做单位换算省一次浮点运算。为什么pitch要用sqrtf(accel_y * accel_y accel_z * accel_z)作为分母因为Pitch是绕Y轴旋转Y轴和Z轴的合成向量共同决定重力在XZ平面的投影如果直接用atan2f(-accel_x, accel_z)当roll接近90度时会出问题。用atan2而不是atan则能避免角度落在坐标轴边界时产生跳变。这两个细节让我当初排查了整整一个晚上。3. 互补滤波又稳又快的方案3.1 互补滤波的核心思想互补滤波的思路非常直观把陀螺仪积分出的角度看成高频可信的信号把加速度计算出的角度看成低频可信的信号。陀螺仪角度通过高通滤波器滤掉低频漂移加速度计角度通过低通滤波器滤掉高频噪声两者相加就得到一路平滑且不发散的角度。用生活化的话说陀螺仪像短期记忆记得快但会慢慢忘事加速度计像全局地图稳定但更新得慢。互补滤波就是让系统平时跟着短期记忆走一旦发现长期趋势偏了就悄悄把地图上的正确位置拉回来。一阶互补滤波的核心公式是angle alpha * (angle gyro_rate * dt) (1 - alpha) * accel_angle其中dt是采样时间alpha是权重系数。alpha越接近1越相信陀螺仪角度越平滑但跟随加速度计越慢alpha越接近0越相信加速度计角度越灵敏但噪声越大。3.2 一阶互补滤波的C语言实现我实际用的实现如下将滤波参数集中在结构体里方便同时对roll和pitch建两个实例typedef struct { float angle; // 滤波后的角度 float bias; // 陀螺零偏可预先校准 } ComplementaryFilter; void ComplementaryFilter_Init(ComplementaryFilter *f, float init_angle) { f-angle init_angle; f-bias 0.0f; } void ComplementaryFilter_Update(ComplementaryFilter *f, float gyro_rate, // 陀螺仪角速度单位rad/s float accel_angle, // 加速度计计算的角度单位rad float dt, // 采样时间单位s float tau) { // 滤波时间常数单位s float alpha tau / (tau dt); f-angle alpha * (f-angle (gyro_rate - f-bias) * dt) (1.0f - alpha) * accel_angle; }需要注意这里gyro_rate和accel_angle都用弧度制。如果你的陀螺仪输出是deg/s要么在滤波前统一转成rad/s要么把代码里的角度都保持为deg但一致性必须保证。我见过不少人在同一个工程里一个用度一个用弧度结果滤波输出像发了疯一样跳排查很久才发现是单位问题。tau的含义是滤波时间常数。tau越大陀螺仪主导的时间越长滤波越平滑但响应越慢tau越小加速度计权重更大响应快但噪声大。常见取值范围在0.2到1.0秒之间具体看应用场景。3.3 alpha怎么调才合适很多人喜欢直接把系数写成0.98和0.02搭配某个固定dt使用。这没问题但一旦采样周期变了0.98的权重就不再匹配。我推荐用alpha tau / (tau dt)这种写法这样dt变化时算法能自动保持相同的时间常数移植到不同主频的板子时也更稳。调参时先静止放置设备观察角度是否稳定然后快速转动到90度观察角度是否跟得上、有没有明显超调最后做小幅摆动观察噪声水平。我个人的经验是平衡车类项目tau取0.4到0.8秒比较合适太小的tau会让电机震动直接串进角度手持云台类项目tau可以放到0.3到0.5秒因为云台更看重响应速度如果设备本身震动大就先在加速度计前面加一个低通滤波而不是一味增大tau否则会引入明显滞后。4. 卡尔曼滤波从原理到实现4.1 卡尔曼滤波该怎么理解卡尔曼滤波听起来高大上其实本质是“按照已知噪声统计对状态做最优线性估计”。在姿态解算这个场景里我们把状态设定为两个变量当前角度angle以及陀螺仪零偏bias。陀螺仪的角速度作为输入加速度计算出的角度作为观测。卡尔曼滤波每周期做两步先根据陀螺仪输入预测当前角度和协方差再用加速度计观测值修正预测。协方差P代表当前状态的不确定度Q是过程噪声模型不可信程度R是观测噪声加速度计数据不可信程度。Q越大卡尔曼增益越高越相信观测R越大增益越低越相信预测。很多教程把卡尔曼滤波讲成黑盒直接给代码我觉得这样不行。至少要理解bias这个状态量它会在运行中自动估计陀螺仪的零偏这也是卡尔曼滤波比互补滤波强的一个重要原因它能在线估计并补偿零偏。4.2 姿态卡尔曼滤波的方程采用经典的一维卡尔曼姿态滤波模型状态向量为[angle, bias]。预测方程为angle (gyro_rate - bias) * dt; // bias保持不变 P[0][0] dt * (dt * P[1][1] - P[0][1] - P[1][0] Q_angle); P[0][1] - dt * P[1][1]; P[1][0] - dt * P[1][1]; P[1][1] Q_bias * dt;更新方程为S P[0][0] R_measure; K0 P[0][0] / S; K1 P[1][0] / S; y accel_angle - angle; angle K0 * y; bias K1 * y; P[0][0] - K0 * P[0][0]; P[0][1] - K0 * P[0][1]; P[1][0] - K1 * P[0][0]; P[1][1] - K1 * P[0][1];P矩阵虽然实际是对称的但为了代码可读性我保留了四个元素方便对照公式排查。这里的Q_angle、Q_bias、R_measure就是三个需要调的噪声参数分别对应角度预测噪声、零偏漂移噪声和加速度计观测噪声。4.3 可直接用的卡尔曼滤波C代码下面是我在Cortex-M0主控上跑过的实现资源占用很小typedef struct { float Q_angle; float Q_bias; float R_measure; float angle; float bias; float P[2][2]; } KalmanFilter; void Kalman_Init(KalmanFilter *kf, float init_angle) { kf-Q_angle 0.001f; kf-Q_bias 0.003f; kf-R_measure 0.03f; kf-angle init_angle; 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; } float Kalman_Update(KalmanFilter *kf, float gyro_rate, float accel_angle, float dt) { kf-angle (gyro_rate - kf-bias) * dt; 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 K0 kf-P[0][0] / S; float K1 kf-P[1][0] / S; float y accel_angle - kf-angle; kf-angle K0 * y; kf-bias K1 * y; float P00 kf-P[0][0]; float P01 kf-P[0][1]; float P10 kf-P[1][0]; float P11 kf-P[1][1]; kf-P[0][0] - K0 * P00; kf-P[0][1] - K0 * P01; kf-P[1][0] - K1 * P00; kf-P[1][1] - K1 * P01; return kf-angle; }调用时对roll和pitch各建一个KalmanFilter实例即可。注意gyro_rate和accel_angle都要用弧度制dt用秒。如果你的系统采样周期不稳定建议用定时器精确计算dt而不是系统里随便一个延时变量。关于参数我的初始经验值就是代码里的三组Q_angle0.001Q_bias0.003R_measure0.03。实际使用时如果发现角度对快速转动跟踪很慢可以减小Q_angle或者增大R_measure如果发现角度抖动厉害可以增大Q_angle或者减小R_measure。Q_bias影响零偏估计的收敛速度一般保持比Q_angle大一点的量级调得太大反而会导致角度震荡。5. 两种算法的对比与选型5.1 实测效果对比我在同一个硬件平台上把两种算法各跑了一遍采样频率1kHz滤波输出200Hz。互补滤波用tau0.5卡尔曼滤波用上面那组参数。静态放置时两者的角度都能稳定在0度附近卡尔曼滤波的噪声略低零偏补偿更主动。快速往复摆动时卡尔曼滤波的相位滞后感觉更小尤其是运动停止后的回落过程更干脆。对比项互补滤波卡尔曼滤波CPU占用极低稍高但MCU都能承受内存占用几个浮点变量结构体加协方差矩阵调参数量1个核心参数3个噪声参数噪声抑制中依赖加速度计低通更好带状态估计动态响应调大tau会滞后调好参数后滞后更小零偏补偿需要外部校准在线估计bias代码复杂度简单易读中等适用场景平衡车、低成本MCU云台、高精度测量这只是我个人的实测感受不代表所有场景。需要说明的是互补滤波在合理调参后对绝大多数姿态控制项目的效果已经足够卡尔曼滤波的“优势”主要体现在噪声更低、能在线估计零偏但在主频只有几十MHz的小单片机上两者跑起来都没压力真正差异不在性能而在你愿不愿意花时间调那三个噪声参数。5.2 选型建议如果项目是8位机、简单平衡车、或者对成本敏感直接用互补滤波。它代码短、逻辑透明、出问题好排查。如果项目是云台、需要低延迟高平滑的测量输出或者你不想单独校准陀螺零偏可以用卡尔曼滤波。调参花的时间确实会多一些但换来的是更好的动态表现。还有一类场景需要注意如果你的系统后续要融合磁力计、气压计甚至GPS建议提前把姿态解算框架设计成四元数版本或者直接上Mahony互补滤波因为欧拉角在pitch接近90度时会出万向节锁卡尔曼滤波状态方程要重新推导。我在做一个小型云台时就因为没考虑这个后期扩展费了不少劲。6. 调试中常见问题和避坑经验6.1 常见问题速查表我把自己调试时踩过的问题整理成了表格方便你对照排查现象可能原因处理方法静止时角度缓慢漂移陀螺零偏未校准上电静止采集1000次求平均作为bias补偿角度一抖一抖加速度计噪声大增大tau或R_measure或先对加速度计做低通滤波快速转动后回不到原角度陀螺比例因子不准重新校准灵敏度或做温度补偿输出角度严重滞后滤波参数过于平滑减小tau、减小Q_angle或增大R_measure角度会跳变90度坐标轴符号或轴向定义错误核对传感器datasheet检查atan2参数符号运动过程中角度明显错误线性加速度干扰检测运动状态运动时提高陀螺仪权重yaw持续漂移六轴没有绝对参考加磁力计或接受yaw仅短时使用静止漂移和跳变是我见过最多的两个问题尤其跳变几乎所有新手都会遇到一次。排查方法是把原始accel_x、accel_y、accel_z用串口打出来平放时观察哪个轴接近1g、另外两轴接近0然后再对照公式检查符号。不要在上位机里看到角度不对劲就急着改滤波参数那是舍本逐末。6.2 调参个人心得调参这件事我的顺序是先校准陀螺零偏再确认坐标轴然后调试互补滤波最后才上卡尔曼滤波。很多人上来直接跑卡尔曼出了问题根本分不清是参数问题还是数据问题。先把最简单的互补滤波跑通至少能证明传感器数据链是干净的然后换卡尔曼才有意义。另一个容易被忽视的点是采样时间dt。我早期用HAL_Delay加上读数据的耗时来估算dt结果系统一忙起来延迟就有波动滤波输出时不时出现小台阶。后来改成定时器中断内读取传感器数据并用计数器精确计算dt问题立刻消失。这个坑极其隐蔽如果你发现滤波输出偶尔有规律性抖动先检查dt是不是固定的而不是怀疑算法。关于上位机调试我的建议是同时把accel_angle和滤波后的angle画出来。静止时看两条线是否贴合快速转动时看滤波后的线是否比加速度计原始角度更平滑。这个对比能让你一眼看出滤波是否工作正常。我在实际项目中就是用这种办法反复调整最后把互补滤波和卡尔曼滤波的输出贴合度调到几乎一致才敢把卡尔曼版本合入正式代码。最后再分享一个小技巧如果你只是想做姿态显示或者简单的体感控制我建议直接从互补滤波入手把tau调好就够了。等真正遇到噪声和响应速度的痛点再考虑卡尔曼滤波不迟。大多数项目卡住工程师的其实不是算法而是传感器数据质量本身。顺手把陀螺仪零偏校准写成上电自动执行把采样周期用定时器固定下来你会发现自己离稳定的姿态输出只差最后一步。后续如果想扩展准确yaw记得加磁力计做九轴融合如果想让俯仰角越过90度还不出问题就早点切换到四元数表示。先把当前这套跑稳剩下的路自然就清楚了。本文还有配套的精品资源点击获取