第 4 章：串級 PID —— 飛控的心臟

本章學習目標

• 用白話搞懂 PID 三兄弟（P / I / D）各做什麼
• 理解為什麼要「兩層」控制：外環角度 + 內環角速度
• 帶你算一次：看一筆指令怎麼一步步讓飛機擺到位（含完整數字變化）
• 看懂「內環數字變很快、外環數字變很慢」這個關鍵差別，以及它為什麼重要

📺 官方影片：PID 的原理以及程式實現（5.5）

---

4.1 問題：你推桿要它傾斜 10°，怎麼做到「又準又穩」？

你把搖桿往右推，心裡想的是「飛機給我向右傾 10 度」。 但飛機不會自己乖乖停在 10°——推太用力會衝過頭、晃來晃去；推太溫柔又半天到不了。 飛控要做的，就是持續地比較「現在幾度」和「想要幾度」，然後決定馬達該出多少力。 這個「比較 → 修正」的迴圈，就是 PID。

---

4.2 PID 三兄弟（先建立感覺）

想像你在開車要停到一條停止線：

• P（比例）：看「現在還差多遠」。差越遠踩越大力。簡單直接，但只有 P 容易衝過頭再彈回來。
• I（積分）：看「長期累積的偏差」。如果一直差一點點到不了（例如逆風），I 會慢慢把這點誤差補上。
• D（微分）：看「逼近的速度有多快」，像煞車。快到線了就先收力，避免衝過頭。

這台飛機的 PID 程式就是這一行（MATH/pid.c）：

void pidUpdate(PidObject* pid, const float dt)
{
    float error, deriv;

    error = pid->desired - pid->measured;        // 想要的 − 現在的 ＝ 誤差
    pid->integ += error * dt;                     // 誤差隨時間累積（積分項）
    deriv = (error - pid->prevError) / dt;        // 誤差變化的快慢（微分項）

    pid->out = pid->kp * error                    // P：比例
             + pid->ki * pid->integ               // I：積分
             + pid->kd * deriv;                   // D：微分

    pid->prevError = error;
}

一句話：輸出 = kp×（差多少）＋ ki×（累積差）＋ kd×（差變化得多快）。

---

4.3 為什麼要「兩層」？外環角度 vs 內環角速度

如果只用一層 PID 直接「角度差 → 給馬達」，飛機會很難調得又快又穩。 所以這台（和幾乎所有現代飛控）用串級（cascade）：把控制拆成外環和內環兩層，內環套在外環裡。

	外環（角度環）	內環（角速度環）
它在看什麼	現在傾斜幾度 vs 想要幾度	現在轉多快（°/s）vs 該轉多快
資料來源	姿態角 Angle.roll（第 3 章算出來的）	陀螺 gyroX（第 2 章直接讀的）
它的輸出	「該用多快的角速度去糾正」	「馬達該出多少力」
反應速度	慢（度，要時間累積）	快（度/秒，瞬間反映）

關鍵接法：外環的輸出，當成內環的目標。

🛹 生活化比喻：騎平衡車 你身體傾斜的角度（外環）決定你「想要前進多快」這個目標速度； 而輪子馬達（內環）負責即時把車速維持在那個目標、並抵抗路面顛簸。 你不會直接用「傾斜角度」去控制馬達電流——中間一定隔著「速度」這層。飛機也是。

---

4.4 程式裡的串接：FlightPidControl()

對著程式看這兩層怎麼串（HAL/control.c）：

pidRateX.measured = MPU6050.gyroX * Gyro_G;   // 內環測量值：陀螺，單位 °/s
// …
pidRoll.measured  = Angle.roll;               // 外環測量值：姿態角，單位 度

pidUpdate(&pidRoll, dt);                       // ① 先算外環（角度）
pidRateX.desired = pidRoll.out;                // ② 外環輸出 → 變成內環的目標角速度
pidUpdate(&pidRateX, dt);                      // ③ 再算內環（角速度），它的 out 送去馬達

就三步：算外環 → 把外環結果塞給內環當目標 → 算內環。馬達拿到的是內環的 out。

這台的增益（USER/INIT.c 的 pid_param_Init()）：

環	kp	ki	kd
外環 pidRoll / pidPitch（角度）	5.0	0	0
內環 pidRateX / pidRateY（角速度）	2.0	0	0.10

注意外環只有 P、內環多了 D（kd=0.10）——D 是內環的「煞車」，等下會看到它的作用。

---

4.5 帶你算一次：一筆「傾斜 10°」指令的完整旅程

假設你推桿要 roll = 10°，飛機從水平靜止開始（角度 0°、角速度 0°/s）。 我們一拍一拍看數字怎麼變（為了好懂，先忽略很小的 D 項）：

第 1 拍（剛推桿，angle=0°、gyro=0°/s）

• 外環：error = 10 − 0 = 10° → out = 5.0 × 10 = 50 → 內環目標 = 50°/s
• 內環：error = 50 − 0 = 50 → out ≈ 2.0 × 50 = 100 → 馬達用力把飛機往右轉

第 2 拍（飛機開始轉了，angle=2°、gyro=30°/s）

• 外環：error = 10 − 2 = 8° → out = 40 → 內環目標降到 40°/s（快到位了，不用那麼急）
• 內環：error = 40 − 30 = 10 → out ≈ 20 → 馬達力道變小

第 3 拍（快到了，angle=9°、gyro=8°/s）

• 外環：error = 10 − 9 = 1° → out = 5 → 內環目標只剩 5°/s
• 內環：error = 5 − 8 = −3 → out ≈ −6 → 馬達反向輕推（煞車），避免衝過 10°

第 4 拍（到位，angle≈10°、gyro≈0）

• 外環 error≈0 → 內環目標≈0；內環 error≈0 → out≈0 → 穩穩停在 10°

把這四拍連起來看，你就看到 PID 怎麼「先用力、接近時收力、到位前煞車、最後穩住」。

---

4.6 重點：內外環「數字變化」差在哪

這是你問的核心。把上面四拍的數字攤開比較：

	外環角度誤差	內環角速度誤差
第 1 拍	10°	50°/s
第 2 拍	8°	10°/s
第 3 拍	1°	−3°/s
第 4 拍	~0°	~0°/s
變化特性	慢慢縮小（度）	跳動很大、瞬間就變（度/秒）

為什麼一個慢、一個快？因為角度是角速度的「累積」——要轉了一段時間，角度才看得出變化。 所以外環的數字（度）總是溫吞地變，內環的數字（度/秒）一瞬間就能暴衝或反轉。

為什麼這個差別這麼重要？看一陣風

假設飛機已經穩穩停在 10°（外環 error≈0、內環目標≈0）。突然一陣風把它往右帶：

• 這一瞬間，角度幾乎還沒變（還是 ~10°），所以外環根本還沒反應過來。
• 但陀螺立刻測到飛機被吹得開始轉，例如 gyro 衝到 +40°/s。
• 內環馬上算：error = 0 − 40 = −40 → out = 2.0 × (−40) = −80 → 立刻反推，在角度被帶歪之前就先擋住！

💡 一句話總結： 內環是「第一線快速反應」——在飛機被擾動帶歪之前，就先憑角速度把它壓住； 外環是「確保最終擺到你要的角度」——它慢，但負責長期的準確。 兩層合起來：又穩（內環擋擾動）又準（外環定角度）。這就是串級的威力。

---

4.7 D 項與調參直覺

• 內環的 kd=0.10 就是 4.5 第 3 拍那個「煞車」：當誤差快速縮小時，D 會踩一下，抑制過衝。
• 調太大 → 對雜訊敏感，飛機會高頻抖動（還記得第 2.9 節嗎？感測器噪聲大時 D 項會放大雜訊——這正是換 ICM-42688 低噪 IMU 的好處之一）。
• 調太小 → 沒煞車，容易過衝、來回晃。

調 PID 的順序通常是：先調內環（讓角速度跟得又穩又不抖），再調外環（讓角度乾脆地到位）。

---

4.8 動手驗證（用 ANO 上位機看波形）

• 畫出內環的 pidRateX.desired（目標角速度）和 pidRateX.measured（陀螺實測）。 手動快速轉動飛控，看實測能不能緊跟著目標——兩條線越貼合，內環越健康。
• 推桿給一個角度，看 Angle.roll 是否乾脆地到位、不來回晃（外環表現）。
• 把內環 kp 調大一點點再飛，感受「更跟手」與「開始抖」之間的界線——這就是調參的手感。

調參務必拆槳或在安全場地

調大增益若過頭，飛機會劇烈震盪甚至翻機。先拆槳在手上感受馬達反應，確認方向正確再小心試飛。

---

4.9 思考題

• 用 4.5 的方法，假設目標是 20°、第 1 拍 angle=0/gyro=0，算出外環 out 和內環 out 各是多少。
• 一陣風讓 gyro 瞬間到 −25°/s（往左被吹），而角度幾乎沒變。內環這一拍的 out 是多少？方向對嗎？
• 為什麼調 PID 要「先內環後外環」？（提示：外環的目標是建立在「內環已經能可靠跟隨角速度」之上）
• 特技翻滾時，程式會跳過外環、直接給內環一個很大的目標角速度（第 9 章）。為什麼翻滾不能用「角度外環」來控制？

---

4.10 本章對應的原始碼

檔案	看什麼
MATH/pid.c	pidUpdate() 的 P/I/D 公式、CascadePID() 串級樣板
HAL/control.c	FlightPidControl()：姿態內外環的實際串接
USER/INIT.c	pid_param_Init()：所有 PID 增益（內外環、高度、光流）

---

下一章：內環算出的 out 還只是「要轉多少」，怎麼變成四顆馬達各自的轉速？ 這就是 馬達混控（motor mixing） 與 PWM 輸出。