3、高度解算算法：从原始数据到稳定高度
各位同学，今天我们来聊聊高度解算这个核心话题。说实话，很多做飞控的工程师，代码写得飞起，但一遇到高度不准就抓瞎。我当年也踩过这个坑——有一次调试四旋翼，气压计高度在悬停时上下乱跳，飞机像喝醉了酒一样。后来才发现，问题出在高度解算算法上。
说白了，气压计测高就是利用大气压随高度变化的规律。但实际应用中，有四个关键环节必须处理好：标准大气压公式推导、静压与动压补偿、海平面气压校准、高度数据平滑。咱们一个一个来拆解。
3.1 标准大气压公式推导
先问个问题：气压和高度到底是什么关系？
理想情况下，大气压随高度增加呈指数衰减。国际标准大气模型（ISA）给出了一个经典公式：
P = P0 * (1 - L * h / T0)^(g * M / (R * L))其中：
- P0：海平面标准大气压（101325 Pa）
- T0：海平面标准温度（288.15 K）
- L：温度递减率（0.0065 K/m）
- g：重力加速度（9.80665 m/s²）
- M：空气摩尔质量（0.0289644 kg/mol）
- R：通用气体常数（8.31432 J/(mol·K)）
这个公式看着复杂，但实际用起来，我们通常把它简化成更实用的形式。我个人习惯用这个近似公式：
h = (1 - (P / P0)^(1/5.257)) * 44330嗯，44330这个常数，其实就是标准大气模型下的标高。你想想看，当P/P0=0.5时，高度大约是5500米左右，这个公式算出来是5486米，误差很小。
核心要点：标准大气压公式假设大气是静止的、干燥的、成分均匀的。实际环境会有偏差，但作为基础模型已经足够。
3.2 静压与动压补偿
这里有个容易忽略的问题：气压计测量的是静压还是总压？
答案是静压。但飞行器运动时，气流会在气压计开口处产生动压，导致测量值偏高。我在项目中遇到过，一架固定翼飞机平飞时高度显示比实际低了20多米，就是因为动压干扰。
动压补偿公式很简单：
P_static = P_measured - 0.5 * ρ * v²其中ρ是空气密度，v是空速。但实际应用中，空速测量本身就有误差，所以补偿要谨慎。
我的经验：对于多旋翼这类低速飞行器，动压影响很小（通常小于1米），可以忽略。但固定翼或高速无人机，必须做补偿。我曾经在风洞测试中发现，速度超过20m/s时，动压误差会达到5米以上。
另外，气压计的安装位置也很关键。我建议：
- 远离螺旋桨气流
- 开口朝下或侧面，避免迎风
- 加装海绵或阻尼网，减少气流脉动
3.3 海平面气压校准
这个环节，说白了就是让气压计知道「当前高度对应的海平面气压是多少」。
为什么需要校准？因为实际海平面气压每天都在变，受天气影响很大。同一个地点，今天和明天的海平面气压可能差几百帕，对应高度误差几十米。
校准方法有两种：
| 方法 | 原理 | 适用场景 |
|---|---|---|
| 已知高度校准 | 在已知海拔高度的地方，反算海平面气压 | 起飞前已知场地海拔 |
| GPS辅助校准 | 利用GPS高度反推海平面气压 | 飞行中动态校准 |
| 气象站数据 | 从附近气象站获取QNH（修正海平面气压） | 有网络连接时 |
我个人最常用的是已知高度校准法。起飞前，把飞机放在地面上，输入场地海拔高度，程序自动反算P0。代码实现很简单：
// 已知地面高度h_ground，测量当前气压P_measured
// 反算海平面气压P0
P0 = P_measured / pow(1 - 0.0065 * h_ground / 288.15, 5.257);注意：GPS高度本身有误差（通常±5米），用GPS辅助校准时，建议取一段时间内的平均值。我曾经因为GPS高度跳变，导致P0校准值来回波动，高度也跟着乱跳。后来加了10秒滑动平均才稳定下来。
3.4 高度数据平滑
原始气压计数据噪声很大，直接用来控制飞机会抖成筛子。平滑是必须的。这里介绍两种常用方法：滑动平均和卡尔曼滤波。
3.4.1 滑动平均
最简单有效的方法。维护一个固定长度的缓冲区，每次取平均值：
#define WINDOW_SIZE 10
float buffer[WINDOW_SIZE];
int index = 0;
float sum = 0;
float smooth_height(float raw_height) {
sum -= buffer[index];
buffer[index] = raw_height;
sum += buffer[index];
index = (index + 1) % WINDOW_SIZE;
return sum / WINDOW_SIZE;
}窗口大小怎么选？我建议：
- 多旋翼悬停：5-10个样本（约0.1-0.2秒）
- 固定翼巡航：20-50个样本（约0.4-1秒）
- 快速机动：3-5个样本（减少延迟）
滑动平均的优点是计算量小，缺点是有延迟。窗口越大，延迟越高。
3.4.2 卡尔曼滤波
如果你需要更平滑、更智能的滤波，卡尔曼滤波是首选。它本质上是一个「预测+修正」的过程。
对于高度估计，状态量通常取高度和垂直速度：
状态向量: x = [h, v]^T
观测向量: z = [h_measured]预测方程：
h_pred = h + v * dt
v_pred = v观测方程：
z = h + noise卡尔曼滤波的核心代码（简化版）：
// 预测
x[0] += x[1] * dt; // 高度预测
P[0][0] += Q[0][0]; // 协方差更新
// 更新
float K = P[0][0] / (P[0][0] + R); // 卡尔曼增益
x[0] += K * (z - x[0]); // 状态更新
P[0][0] = (1 - K) * P[0][0]; // 协方差更新调参经验：Q（过程噪声协方差）控制对预测的信任程度，R（测量噪声协方差）控制对测量的信任程度。Q越大，滤波响应越快但噪声大；R越大，滤波越平滑但延迟大。我一般先设R=1，然后调整Q，让滤波后的高度噪声在±0.1米以内。
说实话，卡尔曼滤波的调参是个手艺活。我刚开始做的时候，调了一周都没调好。后来发现，把Q设成对角矩阵，只调对角线元素，简单很多。
知识体系总览
下面这张图，把整个高度解算的流程串起来了：
从这张图可以清楚看到，原始气压数据经过三个关键处理后，得到原始高度，再经过平滑滤波，最终输出稳定可靠的高度值。每一步都缺一不可。
避坑指南：我曾经在卡尔曼滤波的协方差矩阵初始化上吃过亏。如果初始协方差设得太大，滤波刚开始会剧烈震荡；设得太小，又跟不上变化。我的经验是：初始协方差设成测量噪声的10倍左右，让滤波器先「信任」测量值，再慢慢收敛。
好了，高度解算的核心内容就这些。记住，算法是死的，但应用是活的。不同的飞行器、不同的飞行场景，参数都要微调。多试、多测、多总结，你也能调出一套稳定的高度解算系统。
公众号:蓝海资料掘金营，微信deep3321