多旋翼无人机 ADRC 姿态控制实机工程落地：参数整定与外场抗扰实测

很多飞控工程师做多旋翼开发，姿态环往往最先想到串级 PID。PID 结构简单，在气流平稳、载荷固定的工况下确实够用。但在做吊舱抛投、植保药箱液面晃动，或者在 5 级阵风下悬停时，传统 PID 的抗扰能力往往捉襟见肘：积分项给大了容易引起超调甚至发散，微分项对陀螺仪高频噪声又极度敏感。

过去半年，我们在基于 PX4 架构的 10 kg 级巡检四旋翼上测试了二阶线性自抗扰控制（LADRC）在角速度环的应用。今天聊聊实机落地的工程经验和炸机教训。

一、为什么在角速度环上用 LADRC

做工程先看性价比。韩京清先生提出的非线性 ADRC 参数太多，调参维度高，实机上很难排查故障。工程落地首选高志强老师提出的 LADRC（线性自抗扰控制）。

LADRC 的核心在于将系统内部未建模动态（如气动阻尼、转动惯量变化）和外部扰动（如阵风、负载晃动）统一定义为“总扰动”，利用扩张状态观测器（ESO）实时估计并在线补偿。

对于四旋翼单轴（Roll 或 Pitch）角速度环，系统可以简化为：

$$ \dot{\omega} = b_0 u + f $$

其中 $\omega$ 是角速度，$u$ 是归一化电机油门指令，$f$ 包含总扰动，$b_0$ 是控制输入增益。

引入状态变量 $x_1 = \omega$，$x_2 = f$，离散化后只需要调三个参数：

1. $b_0$：控制输入增益（与推重比、电机响应、机体转动惯量直接相关）。
2. $\omega_o$：观测器带宽（ESO 的响应速度）。
3. $\omega_c$：控制器带宽（闭环极点配置）。

二、核心算法工程实现

以下是运行在 400 Hz 任务循环下的单轴 LADRC 角速度控制器核心代码（基于 C++ 风格，已做防积分饱和与微分滤波简化）：

class LADRC_RateController {
public:
    void init(float b0, float wc, float wo, float dt) {
        _b0 = b0;
        _wc = wc;
        _wo = wo;
        _dt = dt;
        
        // 极点配置计算 ESO 增益
        _beta1 = 2.0f * _wo;
        _beta2 = _wo * _wo;
        
        // 控制器增益
        _kp = _wc;
        
        _z1 = 0.0f; // 估计的角速度
        _z2 = 0.0f; // 估计的总扰动
    }

    float update(float setpoint, float feedback, float u_prev) {
        // 1. ESO 状态更新 (采用显式欧拉离散化)
        float err = _z1 - feedback;
        _z1 += (_z2 - _beta1 * err + _b0 * u_prev) * _dt;
        _z2 += (-_beta2 * err) * _dt;

        // 扰动限幅，防止观测器失步导致发散
        _z2 = math::constrain(_z2, -30.0f, 30.0f);

        // 2. 状态误差反馈律 (SEF)
        float e = setpoint - _z1;
        float u0 = _kp * e;

        // 3. 扰动补偿与控制输出
        float u = (u0 - _z2) / _b0;

        // 输出限幅 (归一化到 [-1.0, 1.0])
        return math::constrain(u, -1.0f, 1.0f);
    }

private:
    float _b0, _wc, _wo, _dt;
    float _beta1, _beta2, _kp;
    float _z1, _z2;
};

三、参数整定流程（实飞顺序）

调 LADRC 千万不要三个参数一起拧，按照下面的顺序调，最不容易炸机：

1. 粗估 $b_0$： $b_0$ 决定了控制量的基准缩放。可以通过给阶跃输入记录角加速度粗算： $$ b_0 \approx \frac{\Delta \dot{\omega}}{\Delta u} $$ 如果算不准，先往大估。$b_0$ 设大了系统偏软，设小了会剧烈震颤。

2. 设定控制器带宽 $\omega_c$： 先将观测器增益关掉或置低（$\omega_o$ 取较小值），此时算法退化为带前馈的比例控制。逐步提高 $\omega_c$，直到角速度跟踪响应变快但未出现高频抖动。通常取值在 15 到 30 rad/s 之间。

3. 提高观测器带宽 $\omega_o$： 经验公式是 $\omega_o = (3 \sim 5) \times \omega_c$。实测时慢慢上调 $\omega_o$。调大时能明显看到机身抗风阻力增强，但如果机体振动大，$\omega_o$ 过高会把高频噪声引入扰动项 $z_2$，导致电机尖叫发热。

四、外场实测数据对比

我们在轴距 850 mm、搭载 21 寸桨、自重 9.8 kg 的测试机上进行了对比。实测当天阵风 4 级（平均风速约 6.8 m/s）。

测试动作：悬停模式下施加 30 deg/s 的 Pitch 轴阶跃扰动，记录姿态角与角速度跟踪表现。

从 Log 来看，LADRC 的抗风表现非常硬朗，尤其在侧风突变时，ESO 对扰动 $z_2$ 的估计延迟在 20 ms 以内，基本在机身刚产生倾角趋势时，控制端就完成了前馈补偿。

五、炸机复盘：一次观测器发散导致的烧电调事故

在早期的室内系留测试中，我们曾烧毁过一组 60A 电调。

• 故障现象：在平稳悬停 30 秒后，机臂突然爆发 80 Hz 左右的高频自激震颤，持续不到 3 秒，单侧两颗电机严重过热退磁，电调 MOS 管击穿。
• Log 分析： 查阅陀螺仪与控制量输出日志，发现问题出在动平衡上。当天测试机更换了一根动平衡较差的折叠桨，在特定转速下机臂产生微小共振。 当时飞控底层的 Gyro LPF 截止频率设在 60 Hz，而我们将 ESO 带宽 $\omega_o$ 推到了 120 rad/s。共振噪声被 ESO 误判为外部气动力矩扰动，$z_2$ 项输出剧烈的高频正弦波，直接打满 PWM，导致电机在极短时间内反复堵转刹车，母线瞬时电流击穿 MOS。
• 避坑教训：
  1. 低通与陷波滤波必须走在前面：进 ESO 的反馈数据必须经过干净的数字滤波（Notch Filter + 二阶巴特沃斯 LPF），否则观测器就是“噪声放大器”。
  2. 扰动输出必须做饱和限幅：代码中必须对 $z_2$ 的更新幅度做限幅（Clamp），不能任由未建模高频震动把控制输出推向饱和。
  3. 参数平滑过渡：实机切模式调参时，不要在空中突变 $\omega_o$，先做斜坡过渡。

六、总结

ADRC 在多旋翼姿态控制上确实有其优越性，特别是对于受风面积大、载荷经常变化的行业机型，它用极少的调参成本换取了很强的外扰抑制能力。

但工程没有银弹。LADRC 把抗扰的重担交给了观测器，也就意味着它对反馈通道的延迟和噪声极度敏感。在实际部署前，先把机架动平衡做好、飞控减震调实、滤波器配准，再去推高观测器带宽，才是稳妥的工程路径。