4. 传感器建模：IMU模型、GPS模型、气压计模型、磁力计模型

做飞控的人都知道一句话：“传感器是飞控的眼睛”。但说实话，真实世界里的眼睛是会近视、会散光的。传感器也一样——有噪声、有偏差、有温漂。如果你在Simulink里直接拿理想传感器信号去调PID，那代码生成后一上真机，大概率会摔得很惨。

我个人习惯是，在快速原型阶段就把传感器模型搭好。这样控制器从一开始就在“对抗噪声”的环境里训练，后期移植到硬件上时，调试周期能缩短一半以上。今天我们就来聊聊四种核心传感器的建模方法：IMU、GPS、气压计、磁力计。

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

IMU是飞控里最核心的传感器，没有之一。它提供加速度和角速度，是姿态估计和控制的基石。但IMU的噪声模型如果建不好，你的卡尔曼滤波器在仿真里跑得再漂亮，上了真机也是白搭。

我在项目中遇到过最典型的问题：仿真里姿态估计误差只有0.5度，一上真机直接飘到5度。查了半天，原来是仿真里用的IMU噪声参数是理想值，而实际传感器的噪声方差大了两个数量级。

IMU模型的核心要素：

• 加速度计模型：包含比力输入、常值偏置、高斯白噪声、尺度因子误差
• 陀螺仪模型：包含角速度输入、常值偏置、高斯白噪声、角度随机游走
• 温度效应：偏置随温度变化，可以用多项式拟合

下面是一个典型的IMU加速度计模型代码片段，用MATLAB Function实现：

function acc_meas = accel_model(acc_true, bias, noise_std)
% 加速度计模型
% acc_true: 真实比力 [ax, ay, az] (m/s^2)
% bias: 常值偏置 [bx, by, bz] (m/s^2)
% noise_std: 噪声标准差 [std_x, std_y, std_z] (m/s^2/sqrt(Hz))

persistent noise_seed
if isempty(noise_seed)
    noise_seed = randi(10000);
end

% 生成高斯白噪声
noise = noise_std .* randn(3,1);

% 输出测量值
acc_meas = acc_true + bias + noise;
end

陀螺仪的模型类似，只是噪声特性不同。陀螺仪的噪声通常用“角度随机游走”（ARW）来描述，单位是 °/√h。你想想看，这个参数在仿真里如果不设对，积分出来的角度误差会随时间累积，飞控的航向估计就会慢慢漂走。

4.2 GPS模型：位置与速度

GPS提供绝对位置和速度信息，用来修正IMU的积分漂移。但GPS的噪声模型和IMU完全不同——它是低频的、有延迟的、而且受卫星几何分布影响。

说白了，GPS的误差可以分为两部分：

• 高斯噪声：水平位置精度一般在2-5米（1σ），垂直精度更差，约3-8米
• 时间相关误差：多路径效应、电离层延迟等，表现为低频漂移

我记得有一次做物流无人机项目，仿真里GPS定位精度设成了1米，结果真机试飞时定位误差到了5米，飞机差点飞到禁飞区。从那以后，我建GPS模型时一定会加上时间相关误差，用一阶马尔可夫过程来模拟。

GPS模型的关键参数：

参数	典型值	说明
水平位置噪声 (1σ)	2.5 m	单点定位模式
垂直位置噪声 (1σ)	5.0 m	比水平差，这是物理限制
速度噪声 (1σ)	0.1 m/s	多普勒测速，相对较准
更新频率	5-20 Hz	比IMU低很多
时间延迟	50-200 ms	信号处理+传输延迟

4.3 气压计模型：高度测量

气压计用来测量高度，原理很简单——气压随高度增加而降低。但实际用起来，坑比想象的多。

为什么？因为气压受天气影响太大了。你想想看，同一地点，晴天和暴雨天的气压差可能对应几十米的高度误差。所以气压计模型里，除了传感器本身的噪声，还要考虑环境气压变化。

气压计模型的核心：

• 标准大气模型：用国际标准大气公式将气压换算为高度
• 传感器噪声：分辨率通常在10-30 cm，但短期噪声约0.1-0.5 m
• 环境扰动：风、温度变化、气流扰动都会影响气压读数

我在做植保无人机时遇到过一个问题：飞机在田间低空飞行时，旋翼下洗气流打到地面再反弹回来，导致气压计读数剧烈波动，高度控制直接发散。后来我在模型里加了一个“气流扰动项”，用正弦波模拟旋翼下洗的影响，才在仿真里复现了这个问题。

function h_meas = baro_model(h_true, P0, T0, noise_std, wind_disturb)
% 气压计高度模型
% h_true: 真实高度 (m)
% P0: 海平面气压 (Pa)
% T0: 海平面温度 (K)
% noise_std: 测量噪声标准差 (m)
% wind_disturb: 气流扰动幅度 (m)

% 标准大气公式
g = 9.80665;
R = 287.058;
L = 0.0065;  % 温度递减率

% 真实气压
P_true = P0 * (1 - L*h_true/T0)^(g/(R*L));

% 加噪声和扰动
P_meas = P_true + noise_std*randn + wind_disturb*sin(2*pi*0.5*t);

% 反算高度
h_meas = (T0/L) * (1 - (P_meas/P0)^(R*L/g));
end

4.4 磁力计模型：航向参考

磁力计测量地球磁场，用来提供航向信息。但说实话，磁力计是所有传感器里最“娇气”的——它太容易被干扰了。

电机电流、电源线、甚至附近的铁磁性物质都会让磁力计读数偏掉。这就是所谓的“硬铁干扰”和“软铁干扰”。

磁力计模型的三个层次：

1. 理想模型：直接输出当地地磁场矢量（查IGRF模型）
2. 噪声模型：加高斯白噪声，噪声水平约0.1-0.5 μT
3. 干扰模型：硬铁偏置 + 软铁缩放 + 随机干扰

我个人习惯在Simulink里把磁力计模型做成一个子系统，里面包含一个“干扰注入”开关。仿真初期关掉干扰，先调通基本控制逻辑；后期打开干扰，测试磁力计校准算法的鲁棒性。

磁力计模型还有一个容易被忽略的点——倾斜补偿。当飞行器倾斜时，磁力计测到的水平分量会混入垂直分量，如果不做补偿，航向误差会随着姿态角增大而急剧增加。所以在模型里，一定要把姿态角（来自IMU）作为输入，做倾斜补偿后再输出航向。

function yaw_mag = mag_heading_model(mag_body, roll, pitch, declination)
% 磁力计航向模型（含倾斜补偿）
% mag_body: 机体坐标系下的磁场测量值 [mx, my, mz]
% roll, pitch: 横滚角、俯仰角 (rad)
% declination: 磁偏角 (rad)

% 倾斜补偿：将磁场从机体坐标系转换到水平坐标系
m_horiz_x = mag_body(1)*cos(pitch) + mag_body(2)*sin(roll)*sin(pitch) + mag_body(3)*cos(roll)*sin(pitch);
m_horiz_y = mag_body(2)*cos(roll) - mag_body(3)*sin(roll);

% 计算磁航向
yaw_mag = atan2(-m_horiz_y, m_horiz_x);

% 加磁偏角得到真航向
yaw_true = yaw_mag + declination;
end

4.5 传感器融合的仿真策略

四种传感器模型都搭好了，接下来就是怎么把它们组合起来。我个人推荐一种“渐进式融合”的仿真策略：

• 第一阶段：只用IMU模型，测试姿态估计和控制
• 第二阶段：加入GPS模型，测试位置控制和外环导航
• 第三阶段：加入气压计，测试高度控制
• 第四阶段：加入磁力计，测试航向保持和自主导航

这样做的好处是，每个阶段只引入一个新的不确定性源，出了问题能快速定位。我曾经见过有人一上来就把四个传感器模型全打开，结果仿真跑飞了，花了两天时间才找到是磁力计干扰参数设错了。

---

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