PX4 MulticopterPositionControl 主循环详解

发表于 更新于 分类于 ， ， 阅读次数： 本文字数： 1k 阅读时长 ≈ 3 分钟
深入解析 PX4 MulticopterPositionControl.cpp 的主循环 Run()，包括状态预处理、多级滤波原理、EKF 重置鲁棒补偿、起飞状态机与推力斜坡、故障回退策略。

一、Run() 主循环结构

MulticopterPositionControl 运行在 wq:nav_and_controllers WorkQueue 上，由 vehicle_local_position 话题到达时触发（数据驱动），同时设置 100 ms 看门狗定时调度（防止 EKF 停发时模块锁死）：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
void MulticopterPositionControl::Run() {
    // 1. 生命周期检查
    if (should_exit()) { ... return; }
    ScheduleDelayed(100_ms);  // 看门狗

    // 2. 时钟对齐与 dt 计算
    vehicle_local_position_s vl_pos;
    if (!_local_pos_sub.update(&vl_pos)) return;
    const float dt = math::constrain(
        (vl_pos.timestamp_sample - _time_stamp_last_loop) * 1e-6f,
        0.002f, 0.04f);  // 约束在 [2ms, 40ms]，即 [25Hz, 500Hz]
    _time_stamp_last_loop = vl_pos.timestamp_sample;

    // 3. 参数更新
    parameters_update(false);

    // 4. 模式管理与悬停推力自适应
    // （见 §2）

    // 5. 状态预处理（滤波）
    PositionControlStates states = set_vehicle_states(vl_pos, dt);

    // 6. Goto 平滑桥接（若有 goto_setpoint）
    if (_goto_control.checkForSetpoint(...)) _goto_control.update(...);

    // 7. 读取轨迹设定点
    _trajectory_setpoint_sub.update(&_setpoint);

    // 8. EKF 重置补偿
    adjustSetpointForEKFResets(vl_pos, _setpoint);

    // 9. 失效保护初始激活
    // 10. 起飞约束与状态机处理

    // 11. 核心控制更新
    _control.setInputSetpoint(_setpoint);
    _control.setState(states);
    _control.update(dt);

    // 12. 发布输出
    publish_local_pos_sp(); publish_attitude_sp();
}

dt 使用物理采样时间戳（timestamp_sample）而非 CPU 接收时间，消除调度抖动对微分项的影响。

二、模式管理与悬停推力自适应

1
2
3
4
5
6
7
8
9
10
11
12
13
14
// 检测位置控制使能状态切换
if (!previous_enabled && now_enabled) {
    _time_position_control_enabled = _vehicle_control_mode.timestamp;
} else if (previous_enabled && !now_enabled) {
    _setpoint = PositionControl::empty_trajectory_setpoint;  // 清除旧设定点
}

// HTE 悬停推力自适应
if (_param_mpc_use_hte.get()) {
    hover_thrust_estimate_s hte;
    if (_hover_thrust_estimate_sub.update(&hte) && hte.valid) {
        _control.updateHoverThrust(hte.hover_thrust);
    }
}

HTE（Hover Thrust Estimate）：EKF2 通过加速度和推力的比较估计实时悬停推力，补偿电池电压下降（推力随电压下降而降低）或载荷变化（挂载掉落导致质量减小），提高 Z 轴位置保持精度。

三、set_vehicle_states() — 多级滤波数据清洗

EKF2 发布的速度数据包含两类典型噪声：

  1. 机架共振尖峰：机臂/桨叶在特定频率产生强振动（如 120 Hz）
  2. 高频白噪声：传感器和电调开关噪声，分布广泛

处理策略：先陷波（精确消除共振峰），再低通（整体压制高频）

1
2
3
4
5
6
// 水平速度处理
states.velocity.xy() = _vel_xy_lp_filter.update(
                        _vel_xy_notch_filter.apply(velocity_xy));
// 加速度由速度差分估计（再低通）
states.acceleration.xy() = _vel_deriv_xy_lp_filter.update(
                            (current_vel_lp - prev_vel_lp) / dt);

3.1 陷波滤波器数学

二阶陷波（带阻）滤波器传递函数：

在共振频率处 ，精确消除该频率的振动分量，其他频率几乎不受影响。关键参数：

  • MPC_VEL_NF_FRQ：陷波中心频率（Hz），需与实际机架共振频率匹配
  • MPC_VEL_NF_BW：陷波带宽（Hz），决定 ζ，太窄则对频率漂移敏感

3.2 一阶低通滤波器（EMA）

离散实现为指数移动平均（EMA）：

MPC_VEL_LP 参数设置截止频率 （Hz）。

3.3 处理流程

1
2
3
4
5
6
7
flowchart LR
    EKF["EKF2<br/>vz（含机架共振+白噪声）"] --> NF["陷波滤波<br/>_vel_z_notch_filter<br/>消除共振尖峰"]
    NF --> LP["低通滤波<br/>_vel_z_lp_filter<br/>压制高频白噪声"]
    LP --> STATE["states.velocity(2)<br/>干净的速度信号"]
    LP --> DIFF["差分估计<br/>(vel_now - vel_prev) / dt"]
    DIFF --> LP2["加速度低通<br/>_vel_deriv_z_lp_filter"]
    LP2 --> ACC["states.acceleration(2)<br/>用于 PID D 项"]

有效性保护：若 EKF 数据无效（v_z_valid == false），对应状态写入 NAN 并重置所有滤波器，防止传感器恢复时出现加速度尖峰。

四、adjustSetpointForEKFResets() — EKF 重置鲁棒补偿

当 EKF 发生坐标系重置（如 GPS 回环修正、SLAM 重定位，产生 1 m 量级的瞬时跳变）时，若不处理，控制器会看到巨大的瞬时误差 ，导致满舵响应。

解决方案：维持控制误差的连续性：

当观测值发生跳变 时，同步修正设定点：

PX4 通过 reset_counter（计数器）机制实现：

1
2
3
4
5
6
7
8
9
// 检测 counter 是否变化（EKF 发生了重置）
if (vl_pos.xy_reset_counter != _xy_reset_counter) {
    setpoint.position[0] += vl_pos.delta_xy[0];  // 同步修正设定点
    setpoint.position[1] += vl_pos.delta_xy[1];
    // 同步重置速度滤波器内部状态
    _vel_xy_lp_filter.reset(_vel_xy_lp_filter.getState() + Vector2f(vl_pos.delta_vxy));
    _vel_xy_notch_filter.reset();
}
_xy_reset_counter = vl_pos.xy_reset_counter;  // 更新计数器

五种独立的 reset_counter：xy、z、vxy、vz、heading，各自独立处理。

五、TakeoffHandling — 起飞状态机与保护

5.1 起飞状态机

1
2
3
4
5
6
7
stateDiagram-v2
    [*] --> disarmed
    disarmed --> spoolup : armed
    spoolup --> ready_for_takeoff : 电机预热完成
    ready_for_takeoff --> rampup : want_takeoff == true
    rampup --> flight : 离地高度达到
    flight --> [*]

5.2 推力斜坡（rampup 阶段）

避免离地瞬间推力突变和姿态抖动：

MPC_TKO_RAMP_T 控制斜坡时间（默认 3 s）。

5.3 关键保护机制

  1. 地面压制：在 not_taken_off 或触地（flying_but_ground_contact）状态时，设置 100 m/s² 向下加速度，使 PID 输出饱和在最小推力，确保不因传感器抖动而蹦跳
  2. 积分清零：地面阶段清空 _control.resetIntegral()，防止高度误差积分导致离地时突然弹射
  3. 倾角限制：起飞前限制最大倾角（MPC_TILTMAX_LND，默认 12°），离地后恢复正常（MPC_TILTMAX_AIR）
  4. 悬停推力重置：未起飞时强制用参数预设值，不使用 HTE 估计（避免地面状态下 HTE 估计偏差影响起飞）
1
2
3
4
// 100 m/s² 下压的控制论解释
// T_cmd = SAT(PID_output + Kff * A_sp)
// A_sp = 100 → 无论 PID 产生多少向上修正，结果都被钳位在 T_min
Vector3f(0.f, 0.f, 100.f).copyTo(_setpoint.acceleration);

六、故障回退 generateFailsafeSetpoint()

当 trajectory_setpoint 无效或 PositionControl::update() 返回失败时：

1
2
3
4
5
6
7
8
9
10
flowchart TD
    A["控制更新失败"] --> B{"上次有效设定点<br/>是否在 200ms 内?"}
    B -->|是| C["使用上次有效设定点<br/>重试 update"]
    B -->|否| D{"XY 速度是否有效?"}
    C --> E{"重试成功?"}
    E -->|否| D
    D -->|是| F["生成 stop-and-wait<br/>水平速度设为0，等待原地"]
    D -->|否| G{"Z 速度是否有效?"}
    G -->|是| H["blind land<br/>缓慢垂直下降"]
    G -->|否| I["blind descent<br/>最大下降速度直至落地"]

三级故障回退确保在任何传感器失效情况下都有安全的下降策略。

七、小结

MulticopterPositionControl.cpp 的核心价值在于：

  1. 数据清洗：多级滤波（陷波+低通）为 PID D 项提供干净信号，直接影响飞行稳定性
  2. EKF 重置保护：误差连续性维护机制，防止传感器重定位时的满舵响应
  3. 起飞安全：状态机 + 斜坡 + 积分清零 + 倾角限制，处理最危险的起飞阶段
  4. 故障容错：三级回退策略，保证任何情况下都有安全的降落路径

上一篇：mc_pos_control 总览与控制链路
下一篇：PositionControl 串级控制算法
系列总目录：PX4 v1.16 源码解读总目录