4. 传感器仿真与注入：GPS、IMU、气压计、视觉传感器

传感器仿真，说白了就是给飞控“喂假数据”。但假数据要假得真，假得有逻辑。我见过不少团队，仿真跑得飞起，一上真机就炸——原因往往出在传感器模型太理想化。

这一章，我们重点聊四个核心传感器：GPS、IMU、气压计、视觉传感器。怎么模拟？怎么注入故障？我把自己踩过的坑和经验都摊开来讲。

4.1 GPS 仿真：位置、速度、星数、精度因子

GPS 仿真其实不复杂。你只需要输出一组符合协议的数据。常用的协议是 NMEA 0183 或 UBX。仿真时，我习惯直接构造 NMEA 语句。

核心思路： 根据仿真时间戳和预设轨迹，反算出经纬度、海拔、地速、航向角。然后打包成 $GPGGA、$GPRMC 等语句。

举个例子，模拟一个悬停状态下的 GPS 输出：

// 伪代码：构造 GGA 语句
char gga[128];
sprintf(gga, "$GPGGA,%02d%02d%06.2f,%02d%%.4f,%c,%03d%.4f,%c,1,%02d,%.1f,%.1f,M,0.0,M,,*",
        hour, min, sec,
        lat_deg, lat_min, lat_ns,
        lon_deg, lon_min, lon_ew,
        sat_num, hdop, alt);
// 别忘了计算校验和

故障注入怎么做？

丢星： 将卫星数 sat_num 设为 0，或直接停止输出 NMEA 语句。
位置跳变： 在经纬度上叠加一个随机大偏移（比如 0.01 度）。
精度恶化： 把 HDOP 值从 0.8 改成 5.0 以上。

我的经验： 曾经有一次，仿真时 GPS 一直正常，但实飞时发现飞机在楼宇间定位漂移。后来我在仿真里加入了“城市峡谷”场景——随机遮挡部分卫星，才把问题复现出来。所以，别只给完美信号。

4.2 IMU 仿真：加速度计与陀螺仪

IMU 是飞控最底层的传感器。仿真 IMU，本质上就是根据运动学模型计算比力和角速度。

我常用的方法：

从仿真引擎拿到当前加速度（含重力）和角速度。
加上传感器噪声（高斯白噪声 + 零偏）。
加上安装误差（小角度旋转矩阵）。
输出到飞控的 IMU 驱动接口。

噪声参数怎么设？我一般参考真实 IMU 的 datasheet：

参数	典型值（消费级）	典型值（工业级）
加速度计噪声密度	100 μg/√Hz	20 μg/√Hz
陀螺仪噪声密度	0.005 °/s/√Hz	0.001 °/s/√Hz
零偏稳定性	0.1 mg	0.01 mg

注意： 千万别忘了加重力分量！我见过有人直接把仿真加速度传给飞控，结果飞控以为飞机在自由落体，直接触发降落保护。嗯，那个画面挺尴尬的。

故障注入：

饱和： 当角速度超过 ±2000°/s 时，输出钳位到最大值。
死零： 强制输出全零。
温漂： 随时间缓慢增加零偏值。

4.3 气压计仿真：高度与温漂

气压计用来测高度。仿真时，我通常用标准大气模型：

// 标准大气压模型（简化版）
float pressure_at_altitude(float alt_m) {
    float p0 = 101325.0f;  // 海平面气压 (Pa)
    float T0 = 288.15f;    // 海平面温度 (K)
    float L = 0.0065f;     // 温度递减率 (K/m)
    float g = 9.80665f;
    float M = 0.0289644f;  // 摩尔质量 (kg/mol)
    float R = 8.31432f;    // 通用气体常数

    float exponent = g * M / (R * L);
    return p0 * powf(1.0f - L * alt_m / T0, exponent);
}

然后加上噪声和量化误差。气压计的分辨率一般是 10cm 左右，别输出太精细的值。

故障注入：

堵塞： 输出值卡在某个固定高度。
温漂： 模拟阳光暴晒导致的气压计温度升高，输出高度缓慢下降。
突变： 模拟开窗或气流扰动，瞬间跳变几米。

避坑指南： 我曾经在仿真里把气压计噪声设得太小，结果飞控的高度估计滤波器收敛得特别好。一上真机，高度波动大了一倍，滤波器直接发散。所以，噪声别省，往大了加一点，反而更安全。

4.4 视觉传感器仿真：光流与视觉里程计

视觉传感器仿真比较麻烦。简单场景下，我直接用理想模型：根据相机运动输出光流向量或位姿变化。

复杂场景下，我会用 Gazebo + 插件，或者 Unreal Engine 渲染。但说实话，大部分联调阶段，理想模型就够了。

光流仿真：

// 理想光流模型
// 输入：机体角速度 w, 地速 v, 高度 h
// 输出：x 方向光流 fx, y 方向光流 fy
fx = vx / h + wy;
fy = vy / h - wx;

视觉里程计仿真： 直接输出相对位姿变换（Δx, Δy, Δz, Δroll, Δpitch, Δyaw），加上漂移和丢帧。

故障注入：

丢帧： 随机跳过 10%~50% 的输出。
纹理缺失： 输出光流为零，模拟白墙或水面。
尺度漂移： 单目视觉的典型问题，输出位移逐渐偏大或偏小。

4.5 传感器仿真整体架构

下面这张图，是我个人习惯的传感器仿真注入架构。你看一眼就明白了：

4.6 故障注入的统一管理

你想想看，如果每个传感器的故障注入逻辑都散落在代码里，后期维护会多痛苦？我建议统一管理。

我的做法是定义一个故障注入接口：

// 故障注入配置结构体
typedef struct {
    bool enable_gps_loss;
    bool enable_gps_jump;
    float gps_jump_range_deg;

    bool enable_imu_saturation;
    float imu_saturation_threshold;

    bool enable_baro_blockage;
    float baro_blockage_alt_m;

    bool enable_vision_drop;
    float vision_drop_rate;
} FaultConfig;

// 统一注入函数
void inject_sensor_fault(SensorData *data, FaultConfig *cfg, uint64_t timestamp);

这样，测试人员只需要改一个 JSON 或 YAML 文件，就能组合出各种故障场景。

核心原则： 故障注入要可重复、可配置、可观测。别搞随机不可控的故障，否则出了问题你都不知道是仿真 bug 还是飞控 bug。

4.7 时间同步：容易被忽略的细节

最后说一个我吃过亏的地方——时间同步。仿真时，四个传感器可能来自不同的线程或进程，时间戳如果不统一，飞控的 EKF 会直接崩掉。

我的建议：

所有传感器模型使用同一个仿真时钟源。
输出数据时，时间戳统一用仿真时间（uint64_t 微秒）。
如果传感器有延迟（比如视觉处理延迟 50ms），在时间戳上主动减去延迟量。

警告： 别相信“反正仿真里时间同步没问题，实飞再调”。我见过一个团队，仿真里所有传感器时间戳都是 0，飞控居然也能跑。一上真机，EKF 直接发散。嗯，排查了三天才发现是时间戳没对齐。

好了，传感器仿真与注入的核心内容就这些。记住：仿真不是目的，验证才是。把故障注入做扎实，实飞时才能心里有底。

📚 课程目录

4. 传感器仿真与注入：GPS、IMU、气压计、视觉传感器

4.1 GPS 仿真：位置、速度、星数、精度因子

4.2 IMU 仿真：加速度计与陀螺仪

4.3 气压计仿真：高度与温漂

4.4 视觉传感器仿真：光流与视觉里程计

4.5 传感器仿真整体架构

4.6 故障注入的统一管理

4.7 时间同步：容易被忽略的细节