4. 失速保护实现：基于ArduPilot/PX4的失速保护代码解析、状态机设计、参数调优

失速保护这东西，说白了就是给飞机装了个「安全气囊」。平时用不上，但真到了关键时刻，它能救飞机一命。

我在做飞控开发的头两年，总觉得失速保护是个「锦上添花」的功能。直到有一次，一架测绘飞机在山区转弯时突然掉高，差点撞山。从那以后，我对失速保护的态度就变成了「必须做，而且要做好」。

今天咱们就深入看看，ArduPilot和PX4这两个主流开源飞控，到底是怎么实现失速保护的。

4.1 失速保护的核心逻辑

先理清一个概念：失速保护不是「防止失速」，而是「在失速发生后，尽快恢复」。

你想想看，飞机一旦进入失速状态，升力骤降，高度开始掉。这时候如果飞控还傻傻地执行原来的航线指令，那后果就是螺旋坠机。所以失速保护要做的事情其实很简单：

  • 检测：判断飞机是否已经失速
  • 响应：自动执行改出动作
  • 恢复：回到正常飞行状态

嗯，听起来简单，但实现起来坑不少。我见过不少团队，失速检测做得太灵敏，结果飞机在正常转弯时就被触发了保护，反而造成了危险。

4.2 ArduPilot的失速保护实现

ArduPilot的失速保护，核心在 AP_AHRS 和 AP_TECS 这两个模块里。我个人习惯把失速保护分成三个层级：

层级 检测依据 响应动作
一级保护 空速低于阈值 增加油门，降低俯仰角
二级保护 迎角超过限制 强制推杆，解除横滚
三级保护 高度下降率过大 进入紧急改出模式

来看看核心代码片段。ArduPilot里失速检测的关键函数在 AP_TECS.cpp：

// ArduPilot 失速检测核心逻辑（简化版）
void TECS::update_stall_protection()
{
    // 1. 获取当前空速
    float airspeed = _ahrs->airspeed();
    
    // 2. 计算失速空速阈值（通常为1.3倍失速速度）
    float stall_threshold = _params.stall_speed * 1.3f;
    
    // 3. 判断是否接近失速
    if (airspeed < stall_threshold) {
        // 失速保护触发！
        // 降低目标俯仰角，增加目标空速
        _pitch_demand = constrain_float(_pitch_demand, -30.0f, 10.0f);
        _speed_demand = stall_threshold * 1.2f;
        
        // 记录触发事件
        gcs().send_text(MAV_SEVERITY_CRITICAL, "Stall protection triggered!");
    }
}

这段代码看起来简单，但实际调参时我踩过不少坑。比如 _params.stall_speed 这个参数，很多新手直接填了理论值，结果实际飞行时发现根本不对。

注意：失速速度不是固定值！它会随着飞机重量、载荷分布、飞行高度而变化。我建议在参数调优时，留出20%的余量。

4.3 PX4的失速保护实现

PX4的失速保护思路和ArduPilot不太一样。它更依赖状态机来管理失速保护的整个流程。我个人觉得PX4的设计更「优雅」一些，但调试起来也更复杂。

PX4的失速保护状态机大致是这样的：

// PX4 失速保护状态机（简化版）
enum class StallState {
    NORMAL,         // 正常飞行
    PRE_STALL,      // 接近失速（预警）
    STALL,          // 已失速
    RECOVERY,       // 改出中
    RECOVERED       // 已恢复
};

每个状态之间的转换条件，我整理成了表格：

当前状态 转换条件 下一状态
NORMAL 迎角 > 12° 且空速 < 1.2倍失速速度 PRE_STALL
PRE_STALL 迎角 > 15° 或 空速 < 失速速度 STALL
STALL 推杆改出，迎角 < 8° RECOVERY
RECOVERY 空速 > 1.5倍失速速度 持续3秒 RECOVERED
RECOVERED 正常飞行 持续10秒 NORMAL

为什么要设计这么复杂的状态机？

因为失速不是「非黑即白」的事情。飞机从正常飞行到完全失速，中间有一个渐变过程。如果只在「失速」和「不失速」之间切换，很容易出现频繁触发、频繁退出的问题。我在项目中就遇到过这种情况——飞机在湍流中飞行，空速波动大，结果失速保护反复触发，飞控一直在「保护-恢复-保护-恢复」之间震荡，最后飞机直接失控了。

我的经验：状态机设计时，一定要加入「滞回」逻辑。也就是说，进入失速的条件和退出失速的条件，要留出一定的差值。比如进入失速需要迎角>15°，但退出只需要迎角<8°。这样能有效避免震荡。

4.4 参数调优实战

参数调优这块，我踩过的坑最多。给你分享几个关键参数的调优经验：

4.4.1 失速速度（STALL_SPEED）

这个参数是所有失速保护的基础。怎么确定？

  • 理论计算：根据翼型、翼载荷、最大升力系数估算
  • 实际测试：在安全高度上，逐渐减速直到失速，记录当时的空速
  • 安全余量：最终取值 = 实测值 × 1.2

我曾经犯过一个错误：直接用理论值，结果实际失速速度比理论值高了15%。那次试飞差点出事，飞机在转弯时突然掉高，还好高度够，手动改出来了。

4.4.2 迎角限制（AOA_LIMIT）

迎角限制是失速保护的第二道防线。我建议这样设置：

  • 巡航状态：限制在10°-12°
  • 机动状态：可以放宽到15°-18°
  • 紧急状态：不超过20°

注意，迎角限制不能设得太死。我在做一款高速侦察机时，把迎角限制设在了8°，结果飞机在爬升时频繁触发保护，飞控一直在推杆，导致爬升率上不去。后来放宽到12°，问题就解决了。

4.4.3 改出速率（RECOVERY_RATE）

这个参数控制失速改出的快慢。太快了容易造成结构过载，太慢了高度损失太大。

我一般这样调：

  1. 先设一个保守值（比如每秒5°的俯仰变化）
  2. 在仿真中测试，观察高度损失
  3. 逐步增加速率，直到高度损失在可接受范围内
  4. 最终取值 = 测试值 × 0.8（留余量）

小技巧：改出时不要只推杆，还要配合油门。我习惯在失速保护触发时，把油门推到最大，同时推杆到-15°。这样能最快速度恢复空速。

4.5 失速保护与航线规划的联动

这一点很多人会忽略。失速保护不是孤立的，它和航线规划有密切关系。

举个例子：如果你的航线中有急转弯（转弯半径小于3倍翼展），那么在转弯过程中，飞机需要更大的迎角来维持升力。这时候失速保护如果太敏感，就会在转弯时频繁触发。

我的做法是：

  • 在航线规划时，自动检测急转弯，并提前通知飞控
  • 飞控在急转弯前，适当提高空速（比如增加10%）
  • 转弯过程中，临时放宽失速保护阈值

这样既保证了安全，又不会因为失速保护而打断航线任务。

4.6 失速保护测试流程

最后，分享一个我常用的测试流程。每次调完失速保护参数，我都会按这个流程走一遍：

  1. 仿真测试：在SITL中模拟失速场景，观察保护逻辑是否正确
  2. 高度测试：在200米以上高度，手动减速触发失速，验证改出效果
  3. 转弯测试：在不同坡度下测试失速保护，确保不会误触发
  4. 风扰测试：在阵风条件下测试，验证抗干扰能力
  5. 极限测试：在低空（50米）模拟失速，测试保护系统的响应速度

嗯，第5步是最刺激的。我一般会在安全区域做这个测试，旁边有安全员随时准备切手动。说实话，每次做低空失速测试，手心都会出汗。但正是这些测试，让我对失速保护系统有了充分的信心。

失速保护这东西，平时看起来「可有可无」，但真到了关键时刻，它就是飞机的最后一道防线。做好它，是对自己、对飞机、对地面人员的负责。

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

← 上一章 📖 返回目录 下一章 →