3. 传感器原理与数据融合：IMU、磁力计、GPS与姿态解算

各位同学，欢迎来到第三讲。今天我们要聊的，是飞控系统里最核心、也最让人头疼的部分——传感器与数据融合。

说白了，无人机在空中飞，它怎么知道自己“歪了”？怎么知道自己在往哪飞？靠的就是这一堆传感器。但每个传感器都有脾气，都有毛病。我的任务，就是教你怎么“哄”好它们，让它们齐心协力，给你一个靠谱的姿态。

3.1 IMU：加速度计与陀螺仪

IMU，惯性测量单元，是飞控的“前庭系统”。它通常包含两个关键部件：加速度计和陀螺仪。

3.4.1 加速度计：感知“重力方向”

加速度计测量的是比力，也就是物体受到的惯性力与引力的合力。在静止状态下，它测到的就是重力加速度g。

原理： 内部有一个微小的质量块，当有加速度时，质量块会移动，改变电容或压电效应，从而输出电压信号。

关键点：

  • 只能测“姿态角”中的俯仰和横滚： 因为它无法区分重力与水平加速度。你想想看，如果无人机在水平加速，加速度计会误以为机身倾斜了。
  • 高频噪声大： 振动、电机干扰都会叠加在信号上。
  • 长期稳定，短期不准： 静态时非常准，动态时一塌糊涂。
避坑指南： 我曾经在调试一款四旋翼时，发现悬停时姿态一直在缓慢漂移。查了半天，发现是机臂的振动频率刚好和加速度计的采样频率产生了共振。后来加了低通滤波器，问题才解决。记住，加速度计的原始数据，一定要先滤波再用。

3.4.2 陀螺仪：感知“角速度”

陀螺仪测量的是绕各轴的旋转角速度。通过积分，就能得到角度变化。

原理： 基于科里奥利力。一个振动的质量块，当有旋转时，会受到一个垂直于振动方向的力，从而产生位移。

关键点：

  • 动态响应快： 瞬间的旋转，陀螺仪立刻就能感知到。
  • 有零偏和温漂： 静止时输出不为0，且随温度变化。
  • 积分会累积误差： 这是最要命的。微小的零偏，积分几秒钟后就会变成几度的角度误差。
我的习惯： 每次上电后，我都会让无人机静置2-3秒，采集陀螺仪的静态数据，计算零偏，然后在后续的积分中减去这个零偏。这能显著减少漂移。

3.2 磁力计与GPS：提供“绝对参考”

IMU只能提供相对变化，时间长了会漂。我们需要绝对参考来“拉”它一把。这就是磁力计和GPS的用武之地。

3.2.1 磁力计：告诉你“北”在哪

磁力计测量地球磁场，从而确定航向角（偏航角）。

原理： 基于霍尔效应或磁阻效应。不同方向的磁场强度不同，通过三轴测量，就能解算出方向。

问题：

  • 极易受干扰： 电机、大电流导线、铁磁物质都会扭曲磁场。
  • 需要校准： 硬铁干扰和软铁干扰必须通过旋转校准来消除。
实战要点： 磁力计一定要远离电机和电调。我见过一个项目，把磁力计装在机臂上，结果航向角乱跳。后来移到机架中心，远离大电流回路，才稳定下来。校准方法很简单：让无人机水平旋转360度，再俯仰旋转360度，采集数据拟合椭球。

3.2.2 GPS：告诉你“在哪”和“速度”

GPS提供位置（经纬度）和速度（地速）。

原理： 通过接收多颗卫星的信号，计算时间差来定位。

关键点：

  • 更新频率低： 通常5-10Hz，远低于IMU的100-1000Hz。
  • 有延迟： 信号传输和处理需要时间。
  • 受环境影响大： 高楼、树荫下信号差。

GPS的速度信息比位置信息更可靠，因为它是通过多普勒频移直接测量的，没有积分漂移。我个人习惯用GPS速度来辅助修正加速度计的积分漂移。

3.3 卡尔曼滤波基础

好了，现在我们有了多个传感器，每个都有优点和缺点。怎么把它们融合起来，得到最优的姿态估计？卡尔曼滤波就是答案。

核心思想： 卡尔曼滤波是一种最优估计算法。它根据系统的“预测”和“观测”，不断修正状态估计。

简单来说，它做两件事：

  1. 预测： 根据上一时刻的状态和陀螺仪数据，预测当前时刻的状态（姿态角）。
  2. 更新： 用加速度计和磁力计的观测值，来修正这个预测。

卡尔曼滤波假设噪声是高斯白噪声。它通过计算“卡尔曼增益”，来决定更相信预测还是更相信观测。

公式简化版： 
// 预测步骤
x_pred = A * x_prev + B * u
P_pred = A * P_prev * A^T + Q

// 更新步骤
K = P_pred * H^T * (H * P_pred * H^T + R)^-1
x_est = x_pred + K * (z - H * x_pred)
P_est = (I - K * H) * P_pred

其中，Q是过程噪声协方差（陀螺仪的噪声），R是观测噪声协方差（加速度计/磁力计的噪声）。调参就是调Q和R。

避坑指南： 卡尔曼滤波不是万能的。如果系统模型不准确，或者噪声特性不符合假设，它反而会发散。我曾经在强振动环境下，卡尔曼滤波直接炸了，姿态角乱飞。后来改用互补滤波，反而更稳定。记住，工程上，简单有效才是王道。

3.4 姿态解算：互补滤波与Mahony滤波

卡尔曼滤波计算量大，在低算力的MCU上跑起来吃力。互补滤波和Mahony滤波是更轻量、更实用的选择。

3.4.1 互补滤波

原理： 陀螺仪动态响应快但低频漂移，加速度计静态准但高频噪声大。互补滤波就是让它们“互补”：

  • 陀螺仪积分得到的角度，通过高通滤波器（保留快速变化）。
  • 加速度计解算的角度，通过低通滤波器（保留静态准确值）。
  • 两者相加，得到最终角度。

公式：

angle = (1 - alpha) * (angle + gyro * dt) + alpha * acc_angle

其中，alpha是互补系数，通常在0.01-0.1之间。alpha越大，越相信加速度计；越小，越相信陀螺仪。

我的经验： 对于大多数四旋翼，alpha取0.02左右效果不错。如果飞行器振动大，可以适当增大alpha，让加速度计多拉一拉。但注意，alpha太大，动态响应会变慢，飞起来感觉“肉”。

3.4.2 Mahony滤波

Mahony滤波是互补滤波的进阶版。它不直接融合角度，而是融合四元数。

核心思想： 用加速度计和磁力计的观测，来修正陀螺仪的积分误差。它通过一个PI控制器，来估计陀螺仪的零偏。

步骤：

  1. 用陀螺仪数据更新四元数。
  2. 计算加速度计和磁力计的“预测”方向。
  3. 计算预测方向与实际观测方向的误差。
  4. 用PI控制器将这个误差反馈到陀螺仪数据上，修正积分。
代码核心片段： 
// 计算误差
error = cross(acc_measured, acc_predicted) + cross(mag_measured, mag_predicted);

// PI控制器修正陀螺仪
gyro_corrected = gyro_raw + Kp * error + Ki * integral_error;

// 用修正后的陀螺仪更新四元数
q = q + 0.5 * omega_matrix * q * dt;

Kp和Ki是PI参数。Kp决定了修正的强度，Ki决定了消除稳态误差的能力。

避坑指南： Mahony滤波的Ki不能太大，否则会导致陀螺仪零偏估计震荡。我一般先调Kp，让姿态响应快速且不超调，再慢慢加Ki。另外，磁力计数据一定要先校准，否则误差项会引入错误修正，导致航向角漂移。

3.5 总结与对比

方法 优点 缺点 适用场景
互补滤波 简单、计算量小、易调参 无法估计陀螺仪零偏 低算力MCU、振动小
Mahony滤波 能估计零偏、抗干扰强 参数调校稍复杂 大多数四旋翼、固定翼
卡尔曼滤波 理论最优、精度高 计算量大、模型依赖强 高算力平台、高精度需求

嗯，这一章内容不少。传感器原理是基础，数据融合是灵魂。我个人建议，初学者先从互补滤波入手，理解“互补”的思想，再过渡到Mahony滤波。等你把Mahony滤波调明白了，再考虑卡尔曼滤波。

下一章，我们会把这些算法真正跑在STM32上，看看实际效果。到时候，你会发现，理论是一回事，实战又是另一回事。做好准备吧。

← 上一章 📖 返回目录 下一章 →