第4章传感器建模与仿真

第4章：传感器建模与仿真——IMU、GPS、磁力计与噪声建模

各位同学，今天我们来聊聊飞控系统里最基础也最头疼的部分——传感器建模。说实话，我在做飞控算法那几年，踩过最大的坑就是传感器模型建得不够细。你以为仿真里跑得好好的，一上真机就炸，十有八九是传感器模型太理想了。

这一章，我会带着大家把IMU（加速度计、陀螺仪）、GPS、磁力计这几个核心传感器的数学模型掰开揉碎，再把噪声和漂移加进去，最后在Simulink里搭一套能用的传感器仿真模块。嗯，咱们直接开干。

4.1 为什么传感器建模这么重要？

你想想看，飞控算法本质上是在做什么？说白了，就是根据传感器读数去估计飞行器的状态，然后输出控制量。如果传感器模型不准，你仿真里验证的卡尔曼滤波、互补滤波全是白搭。

我个人习惯是：先建一个带噪声的传感器模型，再在这个模型上调试算法。这样算法到了真机上，至少能扛住80%的传感器问题。剩下的20%，那是硬件本身的坑，咱们后面再聊。

核心观点：传感器建模不是越复杂越好，而是要抓住影响算法性能的关键因素——噪声、偏置、尺度因子、温度漂移。

4.2 IMU数学模型

IMU包含加速度计和陀螺仪，咱们一个一个来。

4.2.1 加速度计模型

理想情况下，加速度计测量的是比力（specific force），也就是物体受到的合力减去重力。但在实际中，输出是这样的：

a_meas = S_a * (a_true + g) + b_a + n_a + 温度漂移项

其中：

S_a：尺度因子矩阵（通常接近单位阵）
b_a：零偏（bias），单位m/s²
n_a：高斯白噪声
温度漂移项：我一般用一阶线性近似

我在项目中遇到过一个问题：某款MEMS加速度计，温度从25°C升到60°C，零偏漂了将近0.5 m/s²。如果不建模这个漂移，高度估计直接偏出去好几米。

4.2.2 陀螺仪模型

陀螺仪测量角速度，模型类似：

ω_meas = S_g * ω_true + b_g + n_g + 尺度因子非线性项

这里有个坑：陀螺仪的零偏稳定性通常比加速度计差。我见过一些低成本的MEMS陀螺，上电后前几分钟零偏能漂移0.5°/s。嗯，这时候你要是直接用原始数据做姿态解算，结果可想而知。

小技巧：在Simulink里建模时，我习惯把零偏分成两部分：固定零偏（上电校准可消除）和随机游走零偏（用积分白噪声模拟）。这样更贴近真实物理过程。

4.3 GPS模型

GPS模型相对简单，但要注意几个关键点：

位置测量：通常用经纬高，噪声标准差约1-5米（取决于卫星颗数和多径效应）
速度测量：多普勒测速，精度比位置高，约0.1-0.5 m/s
更新频率：典型值5-10Hz，比IMU慢得多
时延：这个容易被忽略。GPS从接收到解算，通常有100-200ms的延迟

我曾经在仿真里没加时延，结果组合导航算法在真机上表现很差。后来加上一个固定延迟模块，效果立马就对了。

% GPS测量模型（简化版）
gps_pos = true_pos + randn(3,1)*pos_noise_std;
gps_vel = true_vel + randn(3,1)*vel_noise_std;
% 注意：输出要延迟N个采样周期

4.4 磁力计模型

磁力计这玩意儿，说实话，挺让人头疼的。它测量的是地球磁场，但实际输出受很多因素干扰：

硬铁干扰：飞行器上的永磁体、电流产生的固定偏置
软铁干扰：铁磁性材料对磁场的扭曲，表现为尺度因子和交叉耦合
安装误差：传感器坐标系与机体坐标系不重合

模型可以写成：

m_meas = S_m * (C * m_true + b_hard) + n_m

其中C是软铁矩阵，b_hard是硬铁偏置。我在做四旋翼时，电机大电流一开，磁力计读数能偏10°以上。后来加了在线校准算法，才勉强能用。

注意：磁力计校准不是一劳永逸的。飞行器结构变化、挂载不同载荷，都可能改变干扰特性。建议在飞控里留一个在线校准接口。

4.5 传感器噪声与漂移建模

这部分是传感器建模的精髓。我见过很多同学在仿真里只加高斯白噪声，结果算法对有色噪声、随机游走完全没有鲁棒性。

常用的噪声模型包括：

白噪声：用Band-Limited White Noise模块
随机游走：对白噪声积分，模拟零偏漂移
1/f噪声：用传递函数近似，模拟低频漂移
量化噪声：用Quantizer模块模拟ADC量化效应

我个人习惯用Allan方差来标定噪声参数。简单说，就是采集一段静态数据，画出Allan方差曲线，然后反推出白噪声、随机游走等参数。这个方法在《IEEE标准》里有详细说明，感兴趣的同学可以查一下。

4.6 Simulink传感器仿真模块搭建

好了，理论讲完，咱们动手搭模块。我一般用Simulink的MATLAB Function模块来封装传感器模型，这样调试起来方便。

下面是一个IMU模块的框架：

function [acc_meas, gyro_meas] = imu_model(acc_true, gyro_true, dt)
    % 参数定义
    persistent acc_bias gyro_bias acc_noise gyro_noise
    if isempty(acc_bias)
        acc_bias = [0.01; 0.01; 0.02];  % m/s^2
        gyro_bias = [0.001; 0.001; 0.002];  % rad/s
        acc_noise = 0.01;  % m/s^2/sqrt(Hz)
        gyro_noise = 0.001;  % rad/s/sqrt(Hz)
    end
    
    % 随机游走更新
    acc_bias = acc_bias + sqrt(dt) * randn(3,1) * 0.001;
    gyro_bias = gyro_bias + sqrt(dt) * randn(3,1) * 0.0001;
    
    % 测量输出
    acc_meas = acc_true + acc_bias + acc_noise * randn(3,1) / sqrt(dt);
    gyro_meas = gyro_true + gyro_bias + gyro_noise * randn(3,1) / sqrt(dt);
end

这个模型虽然简单，但已经包含了白噪声和随机游走。你可以在此基础上加尺度因子、温度漂移、非线性项等。

下面这张图是我常用的传感器仿真架构，大家可以参考：

这个架构图里，真实状态分三路进入IMU、GPS、磁力计模型，每个模型都配有独立的噪声生成器、时延模块和校准参数。最后输出带噪声的测量值，供飞控算法使用。

搭建建议：先用子系统封装每个传感器模型，再在顶层用总线连接。这样后期换传感器型号，只需要改子系统的参数，不用动整体架构。

4.7 小结

这一章我们聊了IMU、GPS、磁力计的数学模型，以及噪声和漂移怎么建模。最后在Simulink里搭了一套可扩展的传感器仿真模块。说实话，传感器建模这件事，没有标准答案，关键是要理解物理本质，然后根据你的算法需求去取舍。

我个人经验是：先粗后细。刚开始用简单的白噪声模型，等算法跑通了，再逐步加入随机游走、温度漂移、时延等细节。这样既不会一开始就被复杂模型吓到，又能保证最终仿真结果的可靠性。

好了，这一章就到这里。大家动手搭一下，有问题随时交流。

📚 课程目录