无人机姿态控制与悬停的关键技术原理：从四元数到串级 PID，再到 INDI

上一篇讲了无线控制与图传协议------指令怎么送到机上。这一篇讲指令到了之后，飞控内部到底发生了什么：四个电机怎么协同，把一个天生不稳定的物体稳稳地按在空中一个点上。

四旋翼是典型的欠驱动、非线性、开环不稳定系统：4 个输入要控制 6 个自由度，撒手就翻。它能悬停，靠的是每秒上千次的"测量---解算---修正"闭环。本文按数据流顺序拆解这条闭环的每一环。

---

目录

• 一、为什么四旋翼必须靠电控才能飞
• 二、坐标系与姿态表示：欧拉角、旋转矩阵、四元数
• 三、动力学模型：从电机转速到机体运动
• [四、姿态解算：IMU 融合的三种路线](#四、姿态解算：IMU 融合的三种路线)
• [五、控制架构：串级 PID 的四层嵌套](#五、控制架构：串级 PID 的四层嵌套)
• [六、混控与输出：从期望力矩到 PWM](#六、混控与输出：从期望力矩到 PWM)
• 七、悬停：定高、定点与油门自学习
• 八、振动与滤波：飞控的头号杀手
• 九、调参方法论
• [十、进阶控制：LQR / MPC / INDI / ADRC](#十、进阶控制：LQR / MPC / INDI / ADRC)
• 十一、失效防护与安全逻辑
• 十二、排障速查表

---

一、为什么四旋翼必须靠电控才能飞

固定翼可以设计成静稳定的------扰动之后自己回来。四旋翼不行。

四个螺旋桨提供的总推力始终沿机体 z 轴 ，一旦机体倾斜，推力就带着水平分量，飞机加速偏离；而偏离产生的姿态误差没有任何气动阻尼把它拉回来。数学上讲，姿态子系统的极点在右半平面，开环不稳定。

同时它还是欠驱动 的：4 个控制量（总推力 + 三轴力矩）要管 6 个自由度（三轴位置 + 三轴姿态）。水平位置无法独立控制，必须通过倾斜机身来产生水平力。这就决定了控制结构必然是串级的：

想往前飞 ⇒ 需要前向加速度 ⇒ 需要低头 ⇒ 需要俯仰力矩 ⇒ 需要前后电机差速 \text{想往前飞} \Rightarrow \text{需要前向加速度} \Rightarrow \text{需要低头} \Rightarrow \text{需要俯仰力矩} \Rightarrow \text{需要前后电机差速} 想往前飞⇒需要前向加速度⇒需要低头⇒需要俯仰力矩⇒需要前后电机差速

这条链条反过来写，就是飞控的代码结构。

悬停附近做小角度线性化，水平加速度与姿态角近似成正比：

a x ≈ − g   θ , a y ≈ g   ϕ a_x \approx -g\,\theta,\qquad a_y \approx g\,\phi ax≈−gθ,ay≈gϕ

也就是说，1° 的姿态误差 ≈ 0.17 m/s² 的水平加速度漂移。姿态环 1° 的静差，5 秒后就是 2 米多的位置偏移。这解释了为什么悬停精度本质上是姿态精度问题。

---

二、坐标系与姿态表示：欧拉角、旋转矩阵、四元数

2.1 两个坐标系

坐标系	定义	用途
导航系（NED / ENU）	北-东-地 或 东-北-天，固连于地面	位置、速度、航线
机体系（FRD / FLU）	前-右-下 或 前-左-上，固连于机身	IMU 测量、力矩输出

混淆 NED 与 ENU 是新手最常见的坐标系事故（PX4 内部用 NED/FRD，ROS 生态用 ENU/FLU，MAVROS 中间要做转换）。动手前先把约定写在代码注释第一行。

2.2 三种姿态表示的取舍

表示	参数量	优点	缺点
欧拉角 (φ, θ, ψ)	3	直观、便于人读	万向锁（θ=±90° 退化）、三角函数开销大、不便于插值
旋转矩阵 R	9	无奇异、直接用于向量变换	冗余、需正交化、存储大
四元数 q	4	无奇异、计算高效、便于积分与插值	不直观、有双覆盖（q 与 −q 表示同一姿态）

飞控内部一律用四元数，只在 OSD 显示和地面站给人看的时候转成欧拉角。

四元数 q = q w , q x , q y , q z T q = q_w, q_x, q_y, q_z^T q=qw,qx,qy,qzT， ∥ q ∥ = 1 \|q\| = 1 ∥q∥=1。姿态运动学方程：

q ˙ = 1 2 q ⊗ 0 ω b \dot{q} = \frac{1}{2} q \otimes \begin{bmatrix} 0 \\ \boldsymbol{\omega}_b \end{bmatrix} q˙=21q⊗0ωb

展开为矩阵形式便于离散积分：

q ˙ = 1 2 0 − ω x − ω y − ω z ω x 0 ω z − ω y ω y − ω z 0 ω x ω z ω y − ω x 0 q \dot{q} = \frac{1}{2} \begin{bmatrix} 0 & -\omega_x & -\omega_y & -\omega_z \\ \omega_x & 0 & \omega_z & -\omega_y \\ \omega_y & -\omega_z & 0 & \omega_x \\ \omega_z & \omega_y & -\omega_x & 0 \end{bmatrix} q q˙=21 0ωxωyωz−ωx0−ωzωy−ωyωz0−ωx−ωz−ωyωx0 q

struct Quat { float w, x, y, z; };

// 一阶积分 + 归一化，dt 为采样周期（秒），gx/gy/gz 单位 rad/s
inline void quat_integrate(Quat& q, float gx, float gy, float gz, float dt)
{
    const float hdt = 0.5f * dt;
    Quat d;
    d.w = (-q.x * gx - q.y * gy - q.z * gz) * hdt;
    d.x = ( q.w * gx + q.y * gz - q.z * gy) * hdt;
    d.y = ( q.w * gy - q.x * gz + q.z * gx) * hdt;
    d.z = ( q.w * gz + q.x * gy - q.y * gx) * hdt;

    q.w += d.w;  q.x += d.x;  q.y += d.y;  q.z += d.z;

    // 归一化：抑制数值漂移，必须每周期做
    float n = 1.0f / sqrtf(q.w*q.w + q.x*q.x + q.y*q.y + q.z*q.z);
    q.w *= n;  q.x *= n;  q.y *= n;  q.z *= n;
}

高动态场景（陀螺 > 1000 °/s）一阶积分会有明显误差，可用二阶龙格库塔或旋转矢量法（rotation vector / 圆锥误差补偿）。一般消费级平台 1 kHz 采样下一阶足够。

2.3 姿态误差怎么算

用四元数算误差比用欧拉角作差正确得多（欧拉角作差在大角度下完全错误）：

q e r r = q s p − 1 ⊗ q q_{err} = q_{sp}^{-1} \otimes q qerr=qsp−1⊗q

取误差四元数的矢量部分，乘 2 得到近似的轴角误差（弧度）：

// 返回机体系下的三轴角度误差（rad），可直接送给姿态环 P 控制器
inline void attitude_error(const Quat& q, const Quat& q_sp, float e[3])
{
    // q_err = q_sp^-1 * q  （q_sp 为单位四元数，逆即共轭）
    Quat qe;
    qe.w =  q_sp.w*q.w + q_sp.x*q.x + q_sp.y*q.y + q_sp.z*q.z;
    qe.x =  q_sp.w*q.x - q_sp.x*q.w - q_sp.y*q.z + q_sp.z*q.y;
    qe.y =  q_sp.w*q.y + q_sp.x*q.z - q_sp.y*q.w - q_sp.z*q.x;
    qe.z =  q_sp.w*q.z - q_sp.x*q.y + q_sp.y*q.x - q_sp.z*q.w;

    // 双覆盖处理：保证走最短路径，否则会绕远路转 350° 而不是 -10°
    float s = (qe.w >= 0.0f) ? 2.0f : -2.0f;
    e[0] = s * qe.x;
    e[1] = s * qe.y;
    e[2] = s * qe.z;
}

双覆盖那两行是重点。 漏掉它，飞机偶尔会在小角度纠正时突然翻一整圈------这是四元数控制器最经典的 bug。

---

三、动力学模型：从电机转速到机体运动

3.1 单桨的推力与反扭矩

T i = C T ρ n i 2 D 4 ≈ k f ω i 2 , Q i = C Q ρ n i 2 D 5 ≈ k m ω i 2 T_i = C_T \rho n_i^2 D^4 \approx k_f \omega_i^2, \qquad Q_i = C_Q \rho n_i^2 D^5 \approx k_m \omega_i^2 Ti=CTρni2D4≈kfωi2,Qi=CQρni2D5≈kmωi2

两个关键结论：

1. 推力与转速平方成正比。 所以油门-推力是非线性的，中油门附近的"每 1% 油门带来的推力增量"远小于高油门。飞控里的**推力线性化（thrust curve）**参数就是在补这条曲线。
2. 每个桨还产生一个反扭矩 Q i Q_i Qi，方向与旋转方向相反。四旋翼正是靠"两正两反"抵消反扭矩，并通过打破这个平衡实现偏航。

顺带一提：偏航是四旋翼最弱的通道。滚转俯仰靠推力差（力臂长、响应快），偏航靠反扭矩差（量级小一个数量级）。所以偏航响应慢、容易饱和，调参时不要奢望它跟横滚一样快。

3.2 刚体动力学

平动（导航系）：

m v ˙ = m g + R ( q ) 0 0 − T − D ( v ) m\dot{\mathbf{v}} = m\mathbf{g} + \mathbf{R}(q)\begin{bmatrix}0\\0\\-T\end{bmatrix} - \mathbf{D}(\mathbf{v}) mv˙=mg+R(q) 00−T −D(v)

转动（机体系，欧拉方程）：

J ω ˙ + ω × ( J ω ) = τ − τ g y r o \mathbf{J}\dot{\boldsymbol{\omega}} + \boldsymbol{\omega} \times (\mathbf{J}\boldsymbol{\omega}) = \boldsymbol{\tau} - \boldsymbol{\tau}_{gyro} Jω˙+ω×(Jω)=τ−τgyro

其中 ω × J ω \boldsymbol{\omega} \times \mathbf{J}\boldsymbol{\omega} ω×Jω 是陀螺耦合项：快速滚转时会诱发俯仰扰动，这就是为什么三轴 PID 参数不能完全独立看待。小型四旋翼上这项通常被 PID 的鲁棒性吃掉了，但在大惯量机型或高速机动时必须显式补偿（前馈或用 INDI）。

τ g y r o \boldsymbol{\tau}_{gyro} τgyro 是螺旋桨转子的陀螺效应，高转速大桨机型不可忽略。

3.3 控制分配（Mixer）

以 X 型四旋翼为例，约定：

• M1 前右（CCW ）、M2 后左（CCW ）、M3 前左（CW ）、M4 后右（CW）
• 滚转右倾为正、抬头为正、右偏航为正

$$

\begin{bmatrix} u_1 \ u_2 \ u_3 \ u_4 \end{bmatrix}

\begin{bmatrix}

1 & -1 & -1 & +1 \

1 & +1 & +1 & +1 \

1 & +1 & -1 & -1 \

1 & -1 & +1 & -1

\end{bmatrix}

\begin{bmatrix} u_T \ u_\phi \ u_\theta \ u_\psi \end{bmatrix}

$$

// 混控 + 饱和处理（roll/pitch 优先级高于 yaw 与油门）
void mixer_quad_x(float thr, float roll, float pitch, float yaw, float out[4])
{
    out[0] = thr - roll - pitch + yaw;   // 前右 CCW
    out[1] = thr + roll + pitch + yaw;   // 后左 CCW
    out[2] = thr + roll - pitch - yaw;   // 前左 CW
    out[3] = thr - roll + pitch - yaw;   // 后右 CW

    // --- 关键：饱和去饱和（desaturation）---
    float mn = out[0], mx = out[0];
    for (int i = 1; i < 4; i++) {
        if (out[i] < mn) mn = out[i];
        if (out[i] > mx) mx = out[i];
    }
    float range = mx - mn;
    if (range > 1.0f) {
        // 姿态需求超出可用范围：等比缩放姿态分量，保住姿态控制权
        float scale = 1.0f / range;
        for (int i = 0; i < 4; i++)
            out[i] = (out[i] - mn) * scale;
    } else {
        // 整体平移，让所有输出落进 [0,1]，牺牲油门保姿态（Air Mode）
        float offset = 0.0f;
        if (mn < 0.0f)      offset = -mn;
        else if (mx > 1.0f) offset = 1.0f - mx;
        for (int i = 0; i < 4; i++) out[i] += offset;
    }
}

这段"去饱和"逻辑就是 Betaflight 的 Air Mode 、PX4 的 mixer desaturation 的核心思想：

当控制量打满时，优先保证姿态力矩的相对关系，宁可牺牲总油门。

不这么做的后果是：油门收到底做翻滚时电机全部到 0，姿态完全失控------这就是早期飞控"低油门翻机"的原因。

---

四、姿态解算：IMU 融合的三种路线

4.1 传感器各自的毛病

传感器	测什么	优点	致命缺点
陀螺仪	角速度	短期精度极高、带宽高、不受加速度干扰	有零偏且随温度漂移，积分误差随时间发散
加速度计	比力（重力 + 运动加速度）	长期可观测重力方向，无漂移	机动时分不清重力和运动加速度；对振动极敏感
磁力计	地磁方向	提供绝对航向	被电机电流、机架铁磁材料、环境钢结构严重干扰
气压计	气压→高度	无累积误差	分辨率约 10 cm、受风与螺旋桨下洗影响、温漂
GNSS	位置速度	绝对、无漂移	5~10 Hz、室内不可用、多径

融合的本质：用陀螺提供高频短期姿态，用加速度计/磁力计在低频上校正它的漂移。

4.2 路线一：互补滤波（Mahony / Madgwick）

计算量极小，适合 MCU 资源紧张的场合。Mahony 的思路很优雅：把"加速度计测得的重力方向"和"当前姿态估计推出的重力方向"做叉乘，得到误差向量，再用 PI 控制器把这个误差喂回陀螺积分。

e = a ^ m e a s × g ^ e s t , ω c o r r = ω g y r o + K p e + K i ∫ e   d t \mathbf{e} = \hat{\mathbf{a}}{meas} \times \hat{\mathbf{g}}{est}, \qquad \boldsymbol{\omega}{corr} = \boldsymbol{\omega}{gyro} + K_p \mathbf{e} + K_i \int \mathbf{e}\,dt e=a^meas×g^est,ωcorr=ωgyro+Kpe+Ki∫edt

妙处在于：K i K_i Ki 那一项估计出来的就是陀螺零偏。

// Mahony 姿态解算（仅六轴），ax..az 无需归一化前的单位，gx..gz 单位 rad/s
static float ex_int = 0, ey_int = 0, ez_int = 0;

void mahony_update(Quat& q, float gx, float gy, float gz,
                   float ax, float ay, float az, float dt,
                   float Kp = 1.0f, float Ki = 0.02f)
{
    float norm = sqrtf(ax*ax + ay*ay + az*az);
    if (norm > 1e-6f) {
        ax /= norm; ay /= norm; az /= norm;

        // 由当前四元数推算的重力方向在机体系下的投影
        float vx = 2.0f * (q.x*q.z - q.w*q.y);
        float vy = 2.0f * (q.w*q.x + q.y*q.z);
        float vz = q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z;

        // 误差 = 测量重力 × 估计重力
        float ex = ay*vz - az*vy;
        float ey = az*vx - ax*vz;
        float ez = ax*vy - ay*vx;

        ex_int += ex * Ki * dt;   // 积分项即陀螺零偏估计
        ey_int += ey * Ki * dt;
        ez_int += ez * Ki * dt;

        gx += Kp * ex + ex_int;
        gy += Kp * ey + ey_int;
        gz += Kp * ez + ez_int;
    }
    // 加速度模长严重偏离 1g 时（大机动/撞击），跳过校正只做积分
    quat_integrate(q, gx, gy, gz, dt);
}

norm 那个判断要升级成 加速度门限 ：当 ∣   ∥ a ∥ − g   ∣ > 0.3 g |\,\|a\| - g\,| > 0.3g ∣∥a∥−g∣>0.3g 时应当降低甚至关闭 K p K_p Kp，否则加速飞行时飞机会"以为"自己倾斜了，产生众所周知的加速抬头现象。

4.3 路线二：EKF / ESKF

现代飞控（PX4 ekf2、ArduPilot EKF3）都用扩展卡尔曼滤波，状态量通常包含：

姿态(4 或 误差角3) | 速度3 | 位置3 | 陀螺零偏3 | 加计零偏3 | 地磁3 | 磁偏差3 | 风速2

相比互补滤波的优势：

• 零偏、尺度、外参可以被在线估计；
• 多源观测（GPS、气压、光流、测距、视觉里程计）可以按各自的噪声协方差自动加权；
• 提供协方差，能判断"当前估计有多可信"，进而驱动飞行模式降级。

工程上更推荐 ESKF（误差状态卡尔曼） ：名义状态用四元数正常积分，滤波器只估计小量的误差状态（用 3 维旋转矢量表示），这样避免了四元数的约束问题，雅可比也更简洁、线性化误差更小。

实践提醒：EKF 不健康 ≠ 传感器坏了。 最常见的原因是振动导致加计噪声超出模型假设、或者磁干扰使航向创新量（innovation）持续超限。看日志要看 innovation 和 innovation test ratio，而不是只看最终输出的角度好不好看。

4.4 路线三：不解算全姿态

竞速机（Betaflight ACRO 模式）其实不需要精确的绝对姿态------角速率环直接用陀螺原始数据，姿态只在自稳模式下才用。这提示我们一个重要原则：

姿态解算的精度要求，取决于你用它做什么。 只做角速率控制，陀螺够了；要自稳，需要重力方向；要定点，才需要完整的位置速度姿态融合。不要一上来就上最重的方案。

---

五、控制架构：串级 PID 的四层嵌套

5.1 整体结构

#mermaid-svg-mng4jFdiJTc4A7p3{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-mng4jFdiJTc4A7p3 .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-mng4jFdiJTc4A7p3 .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-mng4jFdiJTc4A7p3 .error-icon{fill:#552222;}#mermaid-svg-mng4jFdiJTc4A7p3 .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-mng4jFdiJTc4A7p3 .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-mng4jFdiJTc4A7p3 .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-mng4jFdiJTc4A7p3 .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-mng4jFdiJTc4A7p3 .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-mng4jFdiJTc4A7p3 .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-mng4jFdiJTc4A7p3 .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-mng4jFdiJTc4A7p3 .marker{fill:#333333;stroke:#333333;}#mermaid-svg-mng4jFdiJTc4A7p3 .marker.cross{stroke:#333333;}#mermaid-svg-mng4jFdiJTc4A7p3 svg{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-mng4jFdiJTc4A7p3 p{margin:0;}#mermaid-svg-mng4jFdiJTc4A7p3 .label{font-family:"trebuchet ms",verdana,arial,sans-serif;color:#333;}#mermaid-svg-mng4jFdiJTc4A7p3 .cluster-label text{fill:#333;}#mermaid-svg-mng4jFdiJTc4A7p3 .cluster-label span{color:#333;}#mermaid-svg-mng4jFdiJTc4A7p3 .cluster-label span p{background-color:transparent;}#mermaid-svg-mng4jFdiJTc4A7p3 .label text,#mermaid-svg-mng4jFdiJTc4A7p3 span{fill:#333;color:#333;}#mermaid-svg-mng4jFdiJTc4A7p3 .node rect,#mermaid-svg-mng4jFdiJTc4A7p3 .node circle,#mermaid-svg-mng4jFdiJTc4A7p3 .node ellipse,#mermaid-svg-mng4jFdiJTc4A7p3 .node polygon,#mermaid-svg-mng4jFdiJTc4A7p3 .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-mng4jFdiJTc4A7p3 .rough-node .label text,#mermaid-svg-mng4jFdiJTc4A7p3 .node .label text,#mermaid-svg-mng4jFdiJTc4A7p3 .image-shape .label,#mermaid-svg-mng4jFdiJTc4A7p3 .icon-shape .label{text-anchor:middle;}#mermaid-svg-mng4jFdiJTc4A7p3 .node .katex path{fill:#000;stroke:#000;stroke-width:1px;}#mermaid-svg-mng4jFdiJTc4A7p3 .rough-node .label,#mermaid-svg-mng4jFdiJTc4A7p3 .node .label,#mermaid-svg-mng4jFdiJTc4A7p3 .image-shape .label,#mermaid-svg-mng4jFdiJTc4A7p3 .icon-shape .label{text-align:center;}#mermaid-svg-mng4jFdiJTc4A7p3 .node.clickable{cursor:pointer;}#mermaid-svg-mng4jFdiJTc4A7p3 .root .anchor path{fill:#333333!important;stroke-width:0;stroke:#333333;}#mermaid-svg-mng4jFdiJTc4A7p3 .arrowheadPath{fill:#333333;}#mermaid-svg-mng4jFdiJTc4A7p3 .edgePath .path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-mng4jFdiJTc4A7p3 .flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-mng4jFdiJTc4A7p3 .edgeLabel{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-mng4jFdiJTc4A7p3 .edgeLabel p{background-color:rgba(232,232,232, 0.8);}#mermaid-svg-mng4jFdiJTc4A7p3 .edgeLabel rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-mng4jFdiJTc4A7p3 .labelBkg{background-color:rgba(232, 232, 232, 0.5);}#mermaid-svg-mng4jFdiJTc4A7p3 .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-mng4jFdiJTc4A7p3 .cluster text{fill:#333;}#mermaid-svg-mng4jFdiJTc4A7p3 .cluster span{color:#333;}#mermaid-svg-mng4jFdiJTc4A7p3 div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:12px;background:hsl(80, 100%, 96.2745098039%);border:1px solid #aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-mng4jFdiJTc4A7p3 .flowchartTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-mng4jFdiJTc4A7p3 rect.text{fill:none;stroke-width:0;}#mermaid-svg-mng4jFdiJTc4A7p3 .icon-shape,#mermaid-svg-mng4jFdiJTc4A7p3 .image-shape{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-mng4jFdiJTc4A7p3 .icon-shape p,#mermaid-svg-mng4jFdiJTc4A7p3 .image-shape p{background-color:rgba(232,232,232, 0.8);padding:2px;}#mermaid-svg-mng4jFdiJTc4A7p3 .icon-shape .label rect,#mermaid-svg-mng4jFdiJTc4A7p3 .image-shape .label rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-mng4jFdiJTc4A7p3 .label-icon{display:inline-block;height:1em;overflow:visible;vertical-align:-0.125em;}#mermaid-svg-mng4jFdiJTc4A7p3 .node .label-icon path{fill:currentColor;stroke:revert;stroke-width:revert;}#mermaid-svg-mng4jFdiJTc4A7p3 :root{--mermaid-font-family:"trebuchet ms",verdana,arial,sans-serif;} 陀螺
IMU融合
GNSS/光流
GNSS/气压
位置期望
位置环 P
速度期望
速度环 PID
加速度期望 → 期望姿态+推力
姿态环 P
角速率期望
角速率环 PID
期望力矩
混控器
电调/电机
机体

各环的典型运行频率：

控制环	频率	主控制器类型	被控对象带宽
角速率环	1 ~ 8 kHz（PX4 常见 400 Hz ~ 1 kHz）	PID + 前馈	最快（几十 ms）
姿态角环	250 ~ 1000 Hz	纯 P（或 P + 前馈）	快
速度环	50 ~ 250 Hz	PID	中
位置环	20 ~ 100 Hz	纯 P	慢（秒级）

串级设计的铁律：内环带宽至少是外环的 3 ~ 5 倍。 否则外环的指令内环跟不上，两环互相"打架"，表现为低频振荡。这也是为什么永远先调内环（角速率），内环没调好调外环纯属浪费时间。

另一个常被问的问题：为什么姿态环和位置环用纯 P 就够？ 因为它们的输出是下一环的"速率期望"，本身已经带有积分特性（角度是角速率的积分）。再加 I 项容易引入不必要的相位滞后和超调。

5.2 角速率环 PID 的每一项

τ = K p e + K i ∫ e   d t + K d d e d t + K f f   ω ˙ s p \tau = K_p e + K_i \int e\,dt + K_d \frac{de}{dt} + K_{ff}\,\dot{\omega}_{sp} τ=Kpe+Ki∫edt+Kddtde+Kffω˙sp

项	物理意义	调大的表现	调过头
P	当前误差的比例响应，决定"跟随力度"	响应更跟手、更硬	高频抖动、电机发烫、桨叶噪声变尖
I	消除稳态误差（补偿重心偏移、桨叶损伤、风）	抗风好、不会持续偏一边	慢速摇摆（低频振荡）、bounce back
D	阻尼，抑制超调	更干脆、无过冲	放大噪声，电机高频抽搐、发烫
FF	前馈，直接响应指令变化	指令跟随快，无需等误差累积	过冲、指令突变时抽动

工程实现要点（下面这段是能直接用的）：

struct RatePID {
    float kp, ki, kd, kff;
    float i_term      = 0.0f;
    float prev_meas   = 0.0f;
    float d_lpf       = 0.0f;
    float i_limit     = 0.3f;   // 积分限幅（归一化力矩单位）
    float d_lpf_alpha = 0.2f;   // D 项低通系数

    float update(float sp, float meas, float sp_dot, float dt, bool saturated)
    {
        float error = sp - meas;

        // --- 积分：抗饱和（conditional integration）---
        // 输出已饱和且误差还在同向推 → 停止积分，否则积分会一路涨到失控
        if (!saturated) {
            i_term += ki * error * dt;
            if (i_term >  i_limit) i_term =  i_limit;
            if (i_term < -i_limit) i_term = -i_limit;
        }

        // --- 微分：对"测量值"求导，而非对误差求导 ---
        // 避免 setpoint 阶跃时 D 项产生巨大尖峰（derivative kick）
        float d_raw = -(meas - prev_meas) / dt;
        prev_meas = meas;
        d_lpf += d_lpf_alpha * (d_raw - d_lpf);   // D 项必须低通，否则纯放大噪声

        return kp * error + i_term + kd * d_lpf + kff * sp_dot;
    }
};

三个细节值得单独强调：

1. D 项对测量值求导，不对误差求导。 否则遥控器摇杆一动，setpoint 阶跃，D 项瞬间产生一个巨大脉冲（derivative kick），电机猛抽一下。
2. D 项必须低通滤波。 陀螺噪声经微分后被频率放大，不滤波的 D 项等于把机架振动直接送进电机------电机烫、电流大、还会激发更多振动，形成正反馈。
3. 积分抗饱和必须做。 起飞前如果没做 arm 时清零和地面积分锁定，飞机在地上被摁住时 I 项会一路积到限幅，一松手立刻翻。

5.3 从加速度期望到姿态期望

速度环输出的是期望加速度 a s p \mathbf{a}_{sp} asp，需要转成"期望姿态 + 期望推力"：

F s p = m ( a s p − g ) , z b , s p = F s p ∥ F s p ∥ , T s p = ∥ F s p ∥ \mathbf{F}{sp} = m(\mathbf{a}{sp} - \mathbf{g}), \qquad \mathbf{z}{b,sp} = \frac{\mathbf{F}{sp}}{\|\mathbf{F}{sp}\|}, \qquad T{sp} = \|\mathbf{F}_{sp}\| Fsp=m(asp−g),zb,sp=∥Fsp∥Fsp,Tsp=∥Fsp∥

期望机体 z 轴确定后，再结合期望航向 ψ s p \psi_{sp} ψsp 就能唯一确定期望姿态四元数。这一步就是欠驱动的数学体现------你不能任意指定姿态和加速度，姿态由加速度需求反推得到。

---

六、混控与输出：从期望力矩到 PWM

6.1 推力线性化与电压补偿

由 T ∝ ω 2 T \propto \omega^2 T∝ω2，若直接把归一化的力矩需求线性映射到油门，会得到"低油门段迟钝、高油门段过冲"的手感。补偿方法是求逆：

// thrust_factor ∈ [0,1]：0 表示完全线性，1 表示完全按平方律反解
float thrust_linearize(float u, float thrust_factor)
{
    if (thrust_factor < 1e-3f) return u;
    float a = thrust_factor;
    float b = 1.0f - thrust_factor;
    // 解 a*x^2 + b*x = u
    return (-b + sqrtf(b*b + 4.0f*a*u)) / (2.0f*a);
}

电压补偿同样重要：同样的 PWM 占空比，满电 4.2 V/cell 和低电 3.5 V/cell 产生的推力差 20% 以上。不补偿的后果是"电池打完一半，飞机就变软了，PID 手感全变"。做法是按实测电压对 PID 输出整体缩放：

u o u t = u ⋅ V n o m i n a l V m e a s u r e d u_{out} = u \cdot \frac{V_{nominal}}{V_{measured}} uout=u⋅VmeasuredVnominal

6.2 电调协议（承接上一篇的协议话题）

协议	类型	更新率	分辨率	双向遥测
PWM	模拟脉宽	50 ~ 490 Hz	受限于脉宽精度	❌
OneShot125	模拟脉宽（压缩）	~ 4 kHz	---	❌
MultiShot	模拟脉宽	~ 32 kHz	---	❌
DShot300/600/1200	数字	对应 kHz 级	11 bit（0~2047）	✅ 双向 DShot 可回传转速

双向 DShot 的意义远不止于"能看转速"------它让飞控实时知道每个电机的实际 RPM，从而可以精确地把陷波滤波器对准当前的桨叶通过频率。这是近年飞行品质提升最大的单项技术，见下一节。

---

七、悬停：定高、定点与油门自学习

悬停 = 三个方向的位置闭环。三个方向的难度完全不同。

7.1 定高（Z 轴）

传感器	有效范围	精度	问题
气压计	无限	±0.3 ~ 1 m	受风、下洗、开关门气压突变影响
超声波	0 ~ 5 m	±2 cm	软表面吸声、电机噪声干扰
激光 TOF	0 ~ 12 m（室外受阳光影响）	±1 cm	强光下失效、玻璃/水面误测
GNSS 高度	无限	±3 ~ 5 m	比气压计还差，一般只作辅助
RTK	无限	±2 cm	需基站与差分链路

气压计有两个经典坑：

1. 必须做物理防护。 裸露的气压计被螺旋桨下洗气流或机身内气流一吹，读数会跳几米。标准做法是盖一小块开孔海绵，并遮光（BMP280 这类芯片对光敏感）。
2. 必须与加速度计融合。 气压计单独用噪声太大、延迟太高。标准做法是用加计的 z 向积分提供高频、气压计提供低频，做互补滤波或塞进 EKF。

7.2 悬停油门自学习

维持悬停所需的油门（hover throttle）随电池电量、载荷、气压密度变化，不可能是常数。现代飞控用在线学习：

// 当垂直速度接近 0 且姿态平稳时，把当前油门缓慢学习为悬停油门
if (fabsf(vz) < 0.1f && fabsf(az_body + 9.81f) < 0.5f && tilt_deg < 10.0f) {
    hover_thr += 0.0005f * (current_thr - hover_thr);   // 极慢的一阶滤波
    hover_thr = clampf(hover_thr, 0.15f, 0.75f);
}

学到的 hover_thr 用作高度控制器的前馈基准：控制器只需输出"相对悬停点的增量"，而不是从零开始积分。这让起飞瞬间不再需要等 I 项慢慢爬升，高度响应立刻变干脆。

7.3 定点（XY 轴）

方案	精度	适用环境	说明
GNSS	1 ~ 3 m	室外开阔	基础方案，受多径影响
GNSS + RTK	2 ~ 5 cm	室外有基站	精准作业
光流 + 测距	5 ~ 20 cm	室内、低空、地面有纹理	便宜可靠，但依赖地面纹理
VIO / VSLAM	5 ~ 30 cm	室内外通用	算力需求高，动态场景易漂
UWB	10 ~ 30 cm	需布基站	室内定位常用

光流的关键细节 ：光流传感器测的是图像的角速度，包含了机体旋转引起的部分。必须减掉陀螺量再乘高度才能得到真实水平速度：

v x = h ⋅ ( ω f l o w , y − ω g y r o , y ) , v y = − h ⋅ ( ω f l o w , x − ω g y r o , x ) v_x = h \cdot (\omega_{flow,y} - \omega_{gyro,y}), \qquad v_y = -h \cdot (\omega_{flow,x} - \omega_{gyro,x}) vx=h⋅(ωflow,y−ωgyro,y),vy=−h⋅(ωflow,x−ωgyro,x)

所以光流必须配测距 （高度 h h h 是尺度因子），且高度误差会直接按比例污染速度估计。光流在这些场景会失效：纯色地板、水面、镜面、光照过暗、飞得太高（超出测距量程后 h h h 无效）。

7.4 悬停时的扰动来源

扰动	表现	对策
风	持续单向偏移	位置环 I 项 + EKF 风速估计前馈
地效（离地 < 1 倍桨径）	起降时推力异常增大，"弹起来"	起飞阶段限制爬升率、降落用专门的着陆检测
下洗回流（室内墙角）	高度和位置乱跳	避免贴墙悬停；提高测距权重
重心偏移	恒定倾斜	角速率 I 项吸收，或做 trim 校准
涡环状态（快速垂直下降）	突然失去升力、剧烈晃动	限制下降率（一般 < 3 m/s），下降时带一点前向速度

涡环（VRS）值得单独警告 ：垂直快速下降时飞机进入自身下洗气流形成的涡环，升力骤降且加油门无效（加油门反而加强涡环）。这是多旋翼最危险的飞行状态之一，唯一的脱出方法是建立水平速度飞出涡环。飞控层面的防护就是硬性限制垂直下降速率。

---

八、振动与滤波：飞控的头号杀手

绝大多数"PID 调不好"的问题，根源是振动，不是参数。

链条是这样的：桨叶不平衡/电机轴承磨损 → 机架共振 → 陀螺读到振动 → D 项放大 → 电机高频响应 → 加剧振动 → 正反馈。表现为电机异常发烫、续航暴跌、有时会突然"抽风"甚至炸机。

8.1 频率在哪

主要噪声源是桨叶通过频率：

f = RPM 60 × N b l a d e f = \frac{\text{RPM}}{60} \times N_{blade} f=60RPM×Nblade

例：8000 RPM 的双叶桨，基频 = 8000/60 × 2 ≈ 267 Hz，还有二次、三次谐波。

8.2 三层防御

1. 物理层（最有效） ：平衡桨、换掉磨损电机、软胶减震座、避免飞控刚性直连碳板。能在物理上解决的振动，不要留给滤波去解决------每一级滤波都要付出相位延迟的代价。
2. 静态低通 ：对陀螺输出做一阶/二阶低通（如 PT1/PT2/Biquad）。截止频率越低越干净，但相位滞后越大，滞后会吃掉相位裕度，导致必须降低 PID 增益。这是一个硬性折中。
3. 动态陷波（RPM Filter）：这是目前的最优解。用双向 DShot 拿到实时转速，为每个电机的基频和谐波各放一个窄陷波器，随转速实时移动中心频率。

相比宽低通：只挖掉噪声所在的窄带，保留了绝大部分相位裕度 \text{相比宽低通：只挖掉噪声所在的窄带，保留了绝大部分相位裕度} 相比宽低通：只挖掉噪声所在的窄带，保留了绝大部分相位裕度

低通滤波：  |----------通带----------\____衰减____   ← 相位滞后大
动态陷波：  |----------通带-------- V ---通带-----   ← 只挖一个坑，相位影响小
                                   ↑ 随 RPM 移动

调滤波的诊断方法是看 黑盒日志的 FFT 频谱（Betaflight Blackbox Explorer / PIDtoolbox 都能画）。目标是让陀螺频谱在 80 Hz 以上尽量干净，同时保证 D 项输出的频谱没有明显尖峰。

---

九、调参方法论

9.1 顺序

1. 先解决振动（看频谱，不看手感）
2. 调角速率环 P：逐步加大到出现高频抖动，回退 20~30%
3. 加 D：抑制 P 引起的超调，加到电机开始发烫或出现高频噪声就停
4. 加 I：消除稳态误差，加到抗风好但不出现低频摇摆
5. 调 FF：改善指令跟随，主要影响"手感"而非稳定性
6. 姿态环 P：决定自稳的"硬度"
7. 速度/位置环：最后调，且必须在前面都稳定之后

9.2 怎么判断"调好了"

不要只靠手感，看数据：

指标	从哪看	健康值
阶跃响应	黑盒日志中 setpoint vs gyro	超调 < 10%，无持续振荡，上升时间几十 ms
跟踪误差	gyro − setpoint	快速机动时误差小且能迅速收敛
电机输出频谱	motor output FFT	无集中的高频能量尖峰
电机温度	手摸 / 温度传感器	落地后温热不烫手
电流	电流计	悬停电流与理论值接近，无异常偏高

电机烫是最好用的免费诊断工具。 悬停几分钟后电机烫手，几乎一定是 D 项过大或振动过大，跟"电机功率不够"没关系。

9.3 自动调参

PX4 的 Autotune、ArduPilot 的 AUTOTUNE 模式本质上是在做系统辨识：给系统注入激励（扫频或阶跃），采集响应，拟合出传递函数，再按目标相位裕度/增益裕度反解 PID 参数。

自动调参给出的通常是保守但安全的参数，作为基线很好，追求极致手感还需要手工微调。

---

十、进阶控制：LQR / MPC / INDI / ADRC

PID 之外的方案，各有各的适用面：

方法	核心思想	优点	代价
LQR	基于线性模型，最小化二次型代价	多变量统一设计，天然考虑耦合	依赖精确模型，对参数变化不鲁棒
MPC	在线滚动优化，显式处理约束	能处理执行器饱和、避障约束、可预测未来	算力需求大，实时性挑战
INDI（增量非线性动态逆）	用实测角加速度代替模型预测，只对增量做动态逆	对模型误差极不敏感，扰动抑制强，响应干脆	需要高质量角加速度估计（对陀螺噪声敏感）、需知道控制效能矩阵
ADRC（自抗扰）	用扩张状态观测器（ESO）把所有未建模动态和扰动统一估计成"总扰动"再抵消	几乎不需要模型，抗扰强，参数少	观测器带宽与噪声的折中，理论分析较难
L1 自适应	在线估计不确定性并快速补偿	载荷/参数变化时表现好	实现复杂

为什么 INDI 在无人机圈子里越来越流行？ 因为它把"我需要多大力矩"这个依赖精确惯量、桨效、气动模型的难题，转化成了"相对于现在，我需要多加多少力矩"：

u = u 0 + G − 1 ( ω ˙ d e s − ω ˙ m e a s ) \mathbf{u} = \mathbf{u}0 + \mathbf{G}^{-1}\left( \dot{\boldsymbol{\omega}}{des} - \dot{\boldsymbol{\omega}}_{meas} \right) u=u0+G−1(ω˙des−ω˙meas)

只要控制效能矩阵 G \mathbf{G} G 的量级大致正确，模型的其他部分（惯量、气动阻尼、重心偏移、甚至掉了一片桨）都被实测角加速度自动"吃掉"了。这跟 ADRC 的哲学是一致的：不追求把模型建准，而是实时观测并抵消模型误差。

实际工程里，PID + 良好的滤波 + 前馈 + 电压补偿，已经能覆盖 95% 的应用。上进阶方法之前，先确认你的瓶颈真的是控制律，而不是振动、延迟或标定。

---

十一、失效防护与安全逻辑

姿态控制系统必须假定一切都会出错：

故障	检测方法	处置
遥控失联	控制帧超时（见上一篇的看门狗）	悬停 → 返航 → 降落，分级触发
GNSS 丢失/精度下降	卫星数、HDOP、EKF 位置方差	降级为定高模式，禁止位置控制
EKF 不健康	innovation test ratio 超限	降级模式 + 告警，严重时强制降落
IMU 失效	多 IMU 交叉一致性检查	切换到备份 IMU（冗余设计）
姿态发散	倾角超过阈值（如 > 60°）持续一定时间	立即切断电机（防止飞机在地上疯转伤人）
电机/桨失效	转速反馈异常、控制误差持续大	四轴基本只能迫降；六轴/八轴可以重构控制分配继续飞
低电量	电压 + 电流积分 + 内阻补偿	分级：告警 → 返航 → 就地降落

一个反复出现的教训：failsafe 逻辑必须在真实条件下测试过。 参数设了但从没验证过，等于没有。做法是在开阔场地、低空、有人监护的条件下，主动制造失联/低电，观察飞机是不是真的按预期动作。

---

十二、排障速查表

现象	最可能的原因
高频抖动、电机发烫、声音尖锐	D 项过大 / 陀螺滤波不足 / 机架振动
慢速大幅摇摆（1~3 Hz）	I 项过大 或 外环增益过高（内外环带宽冲突）
打杆后回中有反弹（bounce back）	I 项过大或 I 项抗饱和缺失
加速前飞时机头上仰	加速度计校正未做加速度门限（把运动加速度当成了重力）
一直往一个方向漂	加计未水平校准 / 重心偏移 / 磁偏航偏差 / 有恒定风
悬停高度缓慢上下起伏	气压计未防风防光 / 高度环 I 项与气压噪声共振
起飞瞬间弹起或砸地	悬停油门估计不准 / 地效 / 起飞前 I 项未清零
偏航慢、打杆无力	正常现象（偏航靠反扭矩），检查是否已进入输出饱和
航向缓慢自转	磁力计干扰或未校准 / 陀螺 z 轴零偏未收敛
低油门做机动时失控翻滚	未启用 Air Mode / 混控未做去饱和
快速下降时突然失控晃动	涡环状态，限制下降率并建立水平速度

---

结语

四旋翼能悬停，靠的不是某一个"关键算法"，而是一整条链路每一环都不出错：

低振动的机械 → 干净的陀螺信号 → 合理滤波（少丢相位）→ 准确的姿态估计 → 带宽匹配的串级控制 → 不饱和失控的混控 → 可靠的失效防护。

任何一环拖后腿，后面所有的精调都会被浪费掉。这也是为什么真正的飞控调试功夫，往往花在看日志频谱和拧螺丝上，而不是改 PID 数字。

下一篇如果大家有兴趣，可以聊聊多传感器融合（EKF）的工程实现细节 ，或者从飞控视角看无人机的自主导航与路径规划。评论区告诉我你更想看哪个。

---

系列上一篇：《无线控制与图传协议全景：从 S.BUS 到 WebRTC》