4. 传感器仿真建模：IMU、GPS、气压计与噪声注入

传感器仿真，说白了就是给飞控造一个「虚拟的感知世界」。你想想看，在HIL测试里，飞控芯片跑的是真实代码，但它以为自己在天上飞。它读到的加速度、角速度、位置、高度，全是我们通过模型算出来喂给它的。

我个人习惯把传感器建模分成两块：理想模型和误差模型。理想模型告诉你「应该是什么值」，误差模型告诉你「实际传感器有多烂」。两者加在一起，才是真实的仿真。

4.1 IMU模型：加速度计与陀螺仪

IMU是飞控最核心的传感器。加速度计测比力，陀螺仪测角速度。注意，加速度计测的不是重力加速度，而是「载体受到的惯性力与重力的合力」。我在项目中遇到过有人直接把加速度计输出当重力方向用，结果悬停时没问题，一加速就炸了。

4.1.1 理想IMU模型

理想情况下，IMU输出就是载体坐标系下的真实物理量。假设机体坐标系为b系，导航坐标系为n系，那么：

// 加速度计理想输出（比力）
f_b = C_n^b * (a_n - g_n)

// 陀螺仪理想输出（角速度）
ω_b = ω_nb^b

其中C_n^b是旋转矩阵，a_n是导航系下的加速度，g_n是重力向量。嗯，这里要注意：加速度计永远测不到重力本身，它测的是「你推它的力」。自由落体时加速度计输出是0，这个坑我见过不少人踩。

4.1.2 误差模型与注入

真实IMU有三大类误差：

下面是我常用的IMU误差注入代码框架：

// IMU误差注入函数
void imu_error_inject(ImuData *ideal, ImuData *noisy, ImuErrorParam *param) {
    // 1. 加常值偏置
    noisy->accel.x = ideal->accel.x + param->accel_bias_x;
    noisy->gyro.z = ideal->gyro.z + param->gyro_bias_z;

    // 2. 加随机游走（累积）
    static float accel_rw_x = 0;
    accel_rw_x += param->accel_rw_std * randn() * sqrt(dt);
    noisy->accel.x += accel_rw_x;

    // 3. 尺度因子与交轴
    noisy->accel = scale_matrix * noisy->accel;
}

4.2 GPS模型

GPS仿真比IMU简单，但坑也不少。GPS输出的是位置和速度，更新频率低（通常5-20Hz），而且有延迟。

4.2.1 理想GPS模型

理想GPS直接输出导航系下的位置和速度。假设真实位置为p_n，速度为v_n：

// GPS理想输出
pos_gps = p_n + [0, 0, -altitude]  // 经纬高格式
vel_gps = v_n

注意GPS高度是椭球高，不是气压高。我见过有人直接把GPS高度当海拔用，结果在山区飞控高度控制一直震荡。

4.2.2 GPS误差特性

GPS误差有几个关键点：

• 水平精度：典型2-5米（单点定位），差分GPS可以到厘米级
• 垂直精度：比水平差，典型3-8米
• 速度精度：0.1-0.5 m/s（多普勒测速）
• 更新延迟：50-200ms，取决于接收机处理时间
• 丢星：城市峡谷、树荫下会短暂丢失

GPS误差注入我一般这样处理：

// GPS误差注入
void gps_error_inject(GpsData *ideal, GpsData *noisy, GpsErrorParam *param) {
    // 位置误差：高斯白噪声 + 相关噪声
    static float pos_err_x = 0;
    pos_err_x = 0.9 * pos_err_x + 0.1 * param->pos_std * randn();
    noisy->lat = ideal->lat + pos_err_x / DEG_TO_METER;

    // 速度误差
    noisy->vel_n = ideal->vel_n + param->vel_std * randn();

    // 延迟模拟：用环形缓冲区
    static GpsData buffer[10];
    static int idx = 0;
    buffer[idx] = *noisy;
    idx = (idx + 1) % 10;
    *noisy = buffer[(idx - DELAY_STEPS + 10) % 10];
}

4.3 气压计模型

气压计用来测高度，原理很简单：气压随高度指数衰减。但实际用起来问题很多。

4.3.1 标准大气模型

国际标准大气（ISA）公式：

// 气压高度换算（对流层，0-11000m）
P = P0 * (1 - 0.0065 * h / T0)^5.2561

// 反算高度
h = (T0 / 0.0065) * (1 - (P / P0)^(1/5.2561))

其中P0=101325 Pa，T0=288.15 K。注意这个公式假设标准大气，实际天气变化会导致气压偏移。

4.3.2 气压计误差源

4.4 噪声与误差注入策略

讲完了各个传感器的误差模型，我们来聊聊怎么把这些误差组合起来。我个人习惯用「误差注入层」的概念：

// 误差注入架构
真实物理量 → 理想传感器模型 → 误差注入层 → 带噪传感器输出
                                    ↓
                            偏置 + 噪声 + 延迟 + 丢包

下面是我在HIL平台里用的误差注入配置结构体：

typedef struct {
    // IMU误差
    float accel_bias[3];      // m/s²
    float accel_rw_std;       // m/s²/√Hz
    float gyro_bias[3];       // rad/s
    float gyro_rw_std;        // rad/s/√Hz
    float scale_factor[3];    // 无量纲

    // GPS误差
    float pos_std_horiz;      // m
    float pos_std_vert;       // m
    float vel_std;            // m/s
    float delay_ms;           // ms
    float dropout_prob;       // 丢星概率

    // 气压计误差
    float press_bias;         // Pa
    float press_noise_std;    // Pa
    float temp_drift_coeff;   // Pa/°C
} SensorErrorConfig;

4.5 传感器仿真整体架构

最后，我把整个传感器仿真建模的流程画成了一张图。你看完应该能明白各个模块怎么配合：

这张图把整个流程串起来了。你从左边开始看：真实物理量经过三个理想传感器模型，然后进入误差注入层，最后输出带噪数据给飞控。每个环节都可以独立开关，方便调试。