4. 传感器仿真：IMU、GPS、气压计、磁力计与故障注入
传感器是飞控系统的眼睛和耳朵。没有准确的传感器数据，再好的控制算法也是白搭。在SITL仿真里，我们得把这些传感器的行为模拟得足够真实，才能让后续的稳定性验证有意义。
我个人习惯把传感器仿真分成两层：物理模型层和噪声/故障层。物理模型层负责把真实世界的运动状态转换成传感器读数；噪声/故障层则负责让这些读数变得“不完美”——毕竟真实传感器从来不会给你完美的数据。
4.1 IMU仿真：加速度计与陀螺仪
IMU是飞控里最核心的传感器。它提供线加速度和角速度，是姿态估计和导航的基础。
4.1.1 加速度计仿真
加速度计测量的是比力（specific force），也就是物体受到的合力减去重力。在仿真里，我们通常这样建模：
// 伪代码：加速度计仿真模型
Vector3f accel_measurement = body_acceleration - gravity_vector + noise + bias;
这里有个坑——很多人直接把世界坐标系下的加速度拿来用，忘了做坐标系变换。加速度计测量的是机体坐标系下的比力，所以你得先把加速度从世界系转到体系，再减去重力在体系下的投影。
4.1.2 陀螺仪仿真
陀螺仪测量角速度。仿真模型相对简单：
// 伪代码：陀螺仪仿真模型
Vector3f gyro_measurement = body_angular_velocity + noise + bias + scale_factor_error;
陀螺仪的噪声通常比加速度计大，而且有温度漂移。我在项目中遇到过一个问题：仿真里陀螺仪偏置设得太小，导致实际飞行时姿态发散。后来我养成了一个习惯——仿真时的偏置量至少要比传感器数据手册标称值大两倍，给自己留点余量。
| 参数 | 典型值（仿真） | 说明 |
|---|---|---|
| 加速度计噪声密度 | 100 μg/√Hz | 高斯白噪声标准差 |
| 加速度计偏置稳定性 | 0.1 mg | Allan方差拐点值 |
| 陀螺仪噪声密度 | 0.01 °/s/√Hz | 比加速度计大一个量级 |
| 陀螺仪偏置稳定性 | 10 °/h | MEMS级别典型值 |
4.2 GPS仿真
GPS提供位置和速度信息，但更新频率低（通常5-10Hz），而且有延迟。仿真时要注意两个关键点：
- 更新率匹配：GPS仿真器应该以真实GPS的更新率输出数据，而不是每个仿真步长都输出
- 延迟模拟：从测量到输出通常有100-200ms的延迟，这个延迟会影响位置控制环的稳定性
我曾经在仿真里忽略了GPS延迟，结果位置控制调得特别好，一上真机就振荡。后来才意识到是仿真太“理想”了。
4.3 气压计仿真
气压计用于测量高度。它的特点是：精度不高（米级），但连续性好，不会像GPS那样跳变。
气压计仿真模型：
// 伪代码：气压计仿真
float pressure = sea_level_pressure * pow(1 - altitude * 0.0065 / 288.15, 5.256);
float measurement = pressure + noise + wind_effect;
这里有个容易被忽略的点——风的影响。真实飞行中，气流流过气压计开口会产生动压误差。我在做一款垂直起降固定翼时，发现悬停时高度波动很大，排查了很久才发现是旋翼下洗气流影响了气压计读数。仿真里如果不加这个效应，你永远发现不了这个问题。
4.4 磁力计仿真
磁力计提供航向参考，但它特别容易受干扰。仿真时主要模拟两类干扰：
- 硬铁干扰：机载电子设备产生的固定磁场偏置
- 软铁干扰：机身铁磁材料对地磁场的扭曲
磁力计仿真代码示例：
// 伪代码：磁力计仿真（含硬铁和软铁）
Vector3f mag_earth = getEarthMagneticField(lat, lon);
Vector3f mag_body = rotation_matrix * mag_earth;
Vector3f hard_iron = Vector3f(0.1, 0.05, -0.03); // 硬铁偏置
Matrix3f soft_iron = Matrix3f(1.02, 0, 0, 0, 0.98, 0, 0, 0, 1.01); // 软铁缩放
Vector3f measurement = soft_iron * mag_body + hard_iron + noise;
关键认知：磁力计校准不是一次性的。在仿真里，你可以故意改变硬铁偏置来模拟“机载设备上电后磁场变化”的场景。我见过不少团队在仿真里磁力计完美无缺，真机一飞航向就飘——就是因为没模拟这个动态变化。
4.5 传感器噪声与故障注入
这部分是仿真最有价值的地方。没有故障注入的仿真，只能叫“理想情况演示”。
4.5.1 噪声模型
我通常用三种噪声模型：
- 高斯白噪声：最基本的，模拟电子热噪声
- 随机游走：模拟偏置漂移，IMU里最常见
- 量化噪声：模拟ADC量化误差，低端传感器比较明显
4.5.2 故障注入场景
以下是我认为必须做的故障注入测试：
| 故障类型 | 注入方式 | 预期系统响应 |
|---|---|---|
| IMU数据冻结 | 输出值保持不变 | 姿态估计应触发故障检测，切换备用传感器 |
| GPS跳变 | 位置突然偏移10米 | 位置控制应限幅，避免剧烈响应 |
| 气压计堵塞 | 高度读数缓慢漂移 | 高度融合应能通过加速度计抑制漂移 |
| 磁力计饱和 | 输出限幅在最大值 | 航向估计应降低磁力计权重 |
嗯，这里要注意——故障注入不是随便乱加的。你得先定义清楚“系统应该怎么响应”，再设计故障场景。我曾经在项目里一股脑加了十几种故障，结果仿真日志根本分析不过来。后来学乖了，每次只测1-2种故障，把响应行为看清楚再测下一个。
4.6 传感器仿真知识体系
下面这张图总结了传感器仿真的核心逻辑：
说白了，传感器仿真的核心就一句话：让仿真里的传感器尽可能像真机里的传感器一样“不完美”。你想想看，如果仿真里所有传感器数据都完美无缺，那你验证出来的稳定性结论，拿到真机上大概率要翻车。