第三章 传感器融合基础：IMU原理、加速度计/陀螺仪模型、卡尔曼滤波入门

各位同学，欢迎来到第三章。

说实话，做飞行器控制这么多年，我最大的感触就是：没有好的传感器融合，再牛的控制算法都是空中楼阁。你想想看，如果连飞机当前是正着飞还是倒着飞都不知道，PID调得再好又有什么用？

这一章，我们就来啃下传感器融合这块硬骨头。我会从IMU的原理讲起，带你一步步理解加速度计和陀螺仪到底在测什么，最后手把手推导卡尔曼滤波——这个在工程界被用到烂的经典算法。

本章核心目标：理解IMU的物理原理，掌握加速度计/陀螺仪的数学模型，能够独立推导并实现一维卡尔曼滤波器。

3.1 IMU原理：它到底在测什么？

IMU，全称Inertial Measurement Unit，惯性测量单元。说白了，就是一块能感知自身运动和姿态的芯片。

一个典型的六轴IMU包含：

  • 三轴加速度计：测量三个方向的加速度（包括重力）
  • 三轴陀螺仪：测量三个轴的角速度

我刚开始接触IMU时，犯过一个低级错误——以为加速度计测的是「运动加速度」。其实不是的。加速度计测的是「比力」，也就是物体受到的合力减去重力。换句话说，静止放在桌面上时，加速度计读数是1g（9.8 m/s²）向上，而不是0。

一个小技巧：判断IMU是否正常工作，最简单的办法就是把它平放在桌上。如果Z轴读数不是接近9.8，那多半有问题。我在项目中遇到过好几次，新焊好的板子Z轴读数只有5.0，查了半天发现是供电电压不稳。

3.2 加速度计模型：别被「g」骗了

加速度计的数学模型其实很简单：

a_measured = a_true + b_a + n_a

其中：

  • a_measured：传感器实际输出的加速度值
  • a_true：真实的比力（包含重力）
  • b_a：零偏（bias），一个缓慢变化的误差项
  • n_a：高斯白噪声

嗯，这里要注意：零偏是加速度计最头疼的问题。我做过一个实验，把IMU放在恒温箱里，连续采集24小时数据。结果发现零偏会随着温度漂移，最大能差出0.2 m/s²。对于姿态估计来说，这个误差足以让角度漂移好几度。

怎么解决？两个办法：

  1. 出厂校准：在已知姿态下采集数据，拟合出零偏和尺度因子
  2. 在线估计：用卡尔曼滤波把零偏当作状态量一起估计

避坑指南：我曾经在无人机上直接用未校准的加速度计数据做姿态解算，结果悬停时飞机一直往北偏。查了两天才发现是加速度计的X轴零偏大了0.15 m/s²。从那以后，我每块新板子都会先跑一遍六面校准程序。

3.3 陀螺仪模型：角速度的「积分陷阱」

陀螺仪的数学模型和加速度计类似：

ω_measured = ω_true + b_g + n_g

但陀螺仪有一个致命问题——积分误差会累积。你想想看，如果陀螺仪的零偏是0.01 rad/s，积分10秒后，角度误差就是0.1 rad，约5.7度。飞控要是靠这个数据控制，飞机早就翻了个儿。

所以，工程上从来不会只用陀螺仪积分来算姿态。一般都是：

  • 短时间：用陀螺仪，因为动态响应快
  • 长时间：用加速度计+磁力计来修正，因为零偏漂移

这就是传感器融合的核心思想——取长补短。

3.4 卡尔曼滤波入门：从「猜」到「准」

卡尔曼滤波，说白了就是一种「猜」的艺术。它根据系统的运动模型和传感器的观测值，不断修正对系统状态的估计。

我刚开始学卡尔曼滤波时，被那一堆矩阵公式搞得头晕。后来我发现，只要理解了它的两个核心步骤，一切就清晰了：

  1. 预测（Predict）：根据上一时刻的状态，猜当前时刻的状态
  2. 更新（Update）：用传感器的测量值，修正这个猜测

下面这张图，是我自己画的一维卡尔曼滤波流程图，你可以对照着看：

一维卡尔曼滤波流程图 初始状态 x₀, P₀ 预测步骤 (Predict) x̂ₖ⁻ = A·x̂ₖ₋₁ + B·uₖ Pₖ⁻ = A·Pₖ₋₁·Aᵀ + Q 更新步骤 (Update) Kₖ = Pₖ⁻·Hᵀ·(H·Pₖ⁻·Hᵀ + R)⁻¹ x̂ₖ = x̂ₖ⁻ + Kₖ·(zₖ - H·x̂ₖ⁻) 输出 x̂ₖ, Pₖ 下一时刻迭代

你看，整个流程就是一个循环。每次预测完，马上用测量值更新，然后进入下一轮。这就是卡尔曼滤波的精髓——预测-更新-再预测-再更新。

3.5 一维卡尔曼滤波代码实现

光说不练假把式。我们直接上代码，用Python实现一个一维卡尔曼滤波器。这个例子用来估计一个物体的位置，传感器是带噪声的测距仪。

import numpy as np
import matplotlib.pyplot as plt

class KalmanFilter1D:
    def __init__(self, x0, P0, Q, R):
        """
        初始化一维卡尔曼滤波器
        
        参数:
        x0: 初始状态估计
        P0: 初始误差协方差
        Q:  过程噪声协方差（模型的不确定性）
        R:  测量噪声协方差（传感器的噪声）
        """
        self.x = x0  # 状态估计
        self.P = P0  # 误差协方差
        self.Q = Q   # 过程噪声
        self.R = R   # 测量噪声
    
    def predict(self, dt=1.0):
        """
        预测步骤
        假设系统是匀速运动模型：x = x + v*dt
        这里简化处理，只估计位置
        """
        # 状态转移矩阵 A = 1（一维位置）
        # 控制输入 B = 0（无控制量）
        self.x = self.x  # 预测状态不变（假设静止模型）
        self.P = self.P + self.Q  # 协方差增加（不确定性变大）
        return self.x, self.P
    
    def update(self, z):
        """
        更新步骤
        z: 测量值
        """
        # 测量矩阵 H = 1（直接测量位置）
        H = 1.0
        
        # 计算卡尔曼增益
        K = self.P * H / (H * self.P * H + self.R)
        
        # 更新状态估计
        self.x = self.x + K * (z - H * self.x)
        
        # 更新误差协方差
        self.P = (1 - K * H) * self.P
        
        return self.x, self.P, K

# 模拟数据
np.random.seed(42)
true_position = 10.0  # 真实位置
measurements = true_position + np.random.randn(50) * 2.0  # 带噪声的测量值

# 初始化滤波器
kf = KalmanFilter1D(
    x0=0.0,    # 初始猜测：位置为0
    P0=10.0,   # 初始不确定性很大
    Q=0.1,     # 过程噪声较小（模型比较准）
    R=4.0      # 测量噪声方差（传感器噪声）
)

# 运行滤波
estimates = []
for z in measurements:
    kf.predict()
    x_est, P_est, K = kf.update(z)
    estimates.append(x_est)

# 打印结果
print(f"真实位置: {true_position:.2f}")
print(f"最终估计: {estimates[-1]:.2f}")
print(f"估计误差: {abs(true_position - estimates[-1]):.2f}")

个人经验：调卡尔曼滤波时，Q和R的比值决定了滤波器的「性格」。Q/R越大，滤波器越相信测量值，响应快但噪声大；Q/R越小，滤波器越相信模型，响应慢但平滑。我一般先让Q=R=1跑一遍，再根据实际效果微调。

3.6 卡尔曼滤波的五个核心公式

虽然上面只展示了一维情况，但卡尔曼滤波的通用形式就五个公式。我建议你背下来，面试和写代码都用得上：

步骤 公式 说明
预测 x̂ₖ⁻ = A·x̂ₖ₋₁ + B·uₖ 根据上一时刻状态预测当前状态
Pₖ⁻ = A·Pₖ₋₁·Aᵀ + Q 预测误差协方差（不确定性增加）
更新 Kₖ = Pₖ⁻·Hᵀ·(H·Pₖ⁻·Hᵀ + R)⁻¹ 计算卡尔曼增益（信任测量值的程度）
x̂ₖ = x̂ₖ⁻ + Kₖ·(zₖ - H·x̂ₖ⁻) 用测量值修正状态估计
Pₖ = (I - Kₖ·H)·Pₖ⁻ 更新误差协方差（不确定性减小）

避坑指南：我曾经在嵌入式平台上实现卡尔曼滤波时，直接用float32计算，结果矩阵求逆时出现了数值不稳定。后来改用double，并加了数值稳定性检查（比如检查P矩阵是否正定），问题才解决。记住：嵌入式上的浮点精度问题，能坑死人。

3.7 本章小结

这一章我们讲了三个核心内容：

  • IMU原理：加速度计测比力，陀螺仪测角速度，各有各的毛病
  • 传感器模型：零偏和噪声是主要误差源，必须校准或在线估计
  • 卡尔曼滤波：预测+更新，五个公式搞定状态估计

说实话，卡尔曼滤波入门不难，难的是在实际工程中调好参数。我建议你先把上面的代码跑一遍，改改Q和R的值，看看估计结果怎么变。亲手调过参数，才算真正入门。

下一章，我们会把卡尔曼滤波用到IMU数据上，实现真正的姿态估计。到时候，你会看到加速度计和陀螺仪是怎么「合作」的。

公众号:蓝海资料掘金营，微信deep3321

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