跳轉到

第 3 章：姿態解算 —— 從感測器到「現在歪幾度」

本章學習目標

  • 理解為什麼「光有陀螺」或「光有加速度」都算不準角度
  • 看懂互補濾波怎麼把兩者的優點合起來（含帶你算一次）
  • 認識真實程式用的升級版：四元數 + Mahony 互補濾波（對應 MATH/imu.c）
  • 用上位機看 roll/pitch 波形，親眼看到「漂移」與「修正」

搭配官方影片：第 1 章已嵌入的 4.2 飛控程式工作流程，本章對應其中 GetAngle() 那一步。

3.1 兩個感測器，各有脾氣

第 2 章我們拿到了乾淨的陀螺和加速度。現在要回答飛機最關心的問題：「我現在傾斜幾度？」 你可能會想，那兩個感測器各算各的不就好了？問題是兩個都有缺點：

  • 陀螺儀像一個反應超快、但講越久越離譜的人。 它量的是「轉動速度」，把速度對時間積分就能得到角度——瞬間很準、反應飛快。 但只要有一丁點零偏沒扣乾淨，積分會把它一直累加，幾秒後角度就「越飄越偏」（這叫漂移 drift）。

  • 加速度計像一個誠實、但手會抖的人。 靜止時它只感受到地心引力，所以能算出「哪邊是下」，長期不會漂。 但飛機一震動、一加速，它就被干擾得亂跳，短期雜訊很大，沒辦法單獨用。

一句話：陀螺短期準、長期漂；加速度長期準、短期抖。 它倆的缺點剛好相反——這就是合作的契機。

3.2 互補濾波：讓兩人「互補」

互補濾波的點子很單純：

大部分時間相信反應快的陀螺，但每一次都偷偷把答案往誠實的加速度挪一點點。

這樣既保有陀螺的靈敏，又靠加速度「長期不漂」的特性，慢慢把陀螺的漂移拉回來。寫成公式：

\[ \theta_k = \underbrace{\alpha\,(\theta_{k-1} + \omega\,\Delta t)}_{\text{信任陀螺：上一刻角度＋這段時間轉的量}} + \underbrace{(1-\alpha)\,\theta_{acc}}_{\text{往加速度算出的角度挪一點}} \]

其中 α 介於 0~1，越接近 1 越信任陀螺。它由一個「時間常數」τ 決定：\( \alpha = \dfrac{\tau}{\tau + \Delta t} \)。

帶你算一次

用這台飛機真實的姿態任務週期 Δt = 0.006s（第 1 章的 Duty_6ms），取 τ = 0.5s：

\[ \alpha = \frac{0.5}{0.5 + 0.006} = 0.9881 \]

假設上一刻角度 θ = 10.00°、陀螺現在量到 ω = 20°/s、加速度反推的角度 θ_acc = 9.5°：

  1. 陀螺預測：10.00 + 20 × 0.006 = 10.12°
  2. 融合：0.9881 × 10.12 + (1 − 0.9881) × 9.5 = 9.9996 + 0.1131 = 10.11°

結果幾乎跟著陀螺（10.12，反應快），但被加速度往 9.5 拉回了 0.01°。 一次只拉 0.01° 好像沒用？ 但這個 6ms 任務每秒做 167 次，這「每次一點點」累積起來， 就足以持續抵消陀螺的漂移——這就是互補濾波的精髓。

把 α 改小會怎樣？

若 α 從 0.988 降到 0.9（更信加速度），同樣數字算出來是 0.9×10.12 + 0.1×9.5 = 10.06°， 更靠近加速度、修正更快——但也更容易被加速度的抖動帶歪。這就是調參的取捨。

3.3 真實程式更聰明：四元數 + Mahony

上面的一維互補濾波是為了讓你抓到直覺。真實飛機是 3D 的（同時有 roll/pitch/yaw）， 直接用「歐拉角」算會遇到一個叫萬向鎖（Gimbal Lock）的麻煩，數學上也不漂亮。 所以這份韌體（MATH/imu.c 的 GetAngle()）用的是工業界常見的做法：

用「四元數」表示姿態，再用「Mahony 互補濾波」更新它。

別被名詞嚇到——它的精神跟 3.2 節一模一樣：相信陀螺積分，再用加速度量到的「重力方向誤差」去修正。 我們對著程式看這個對應關係：

MATH/imu.c（節錄，已將 GBK 註解翻成正體中文）
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
// ① 用「目前的四元數姿態」推算出機體感受到的重力方向
Gravity.x = 2*(NumQ.q1*NumQ.q3 - NumQ.q0*NumQ.q2);
Gravity.y = 2*(NumQ.q0*NumQ.q1 + NumQ.q2*NumQ.q3);
Gravity.z = 1 - 2*(NumQ.q1*NumQ.q1 + NumQ.q2*NumQ.q2);

// ② 把「加速度計實際量到的重力方向」和「①推算的方向」做叉積 → 得到誤差 AccGravity
//    （兩個方向差多少，叉積就多大；這就是 3.2 節『往加速度挪』的 3D 版）

// ③ 誤差的積分項（對應互補濾波裡長期修正，Ki 很小）
GyroIntegError.x += AccGravity.x * KiDef;   // KiDef = 0.0003

// ④ 用「陀螺角速度 + Kp×誤差 + 積分項」當作修正後的角速度
Gyro.x = pMpu->gyroX * Gyro_Gr + KpDef * AccGravity.x + GyroIntegError.x;  // KpDef = 0.8

// ⑤ 用修正後的角速度去積分四元數（HalfTime = dt/2）
q0_t = (-NumQ.q1*Gyro.x - NumQ.q2*Gyro.y - NumQ.q3*Gyro.z) * HalfTime;
// … q1_t, q2_t, q3_t 同理 …
NumQ.q0 += q0_t;  /* … */

// ⑥ 正規化四元數（用快速平方根倒數 Q_rsqrt），最後用 atan2/asin 轉回我們看得懂的歐拉角
pAngE->roll = atan2f(vecyZ, veczZ) * RtA;   // RtA：弧度轉角度

把這六步對回 3.2 的直覺：

互補濾波（1D 直覺） Mahony（程式實作）
「加速度算的角度 θ_acc」 ① + ② 用四元數推重力、和加速度比，得到方向誤差
「往加速度挪一點」的比例 (1−α) ④ 裡的 Kp = 0.8（比例修正）
長期把漂移拉回 ③ 的 Ki = 0.0003 積分項（慢慢修零偏）
「上一刻角度 + 陀螺轉的量」 ⑤ 用陀螺積分四元數

所以四元數版到底好在哪？

  1. 沒有萬向鎖，任何姿態都能正確表示（飛特技翻滾也不會算爆）。
  2. 用叉積自然地在 3D 裡算出「重力方向誤差」，比逐軸算歐拉角乾淨。
  3. Kp / Ki 就是你調「信陀螺 vs 信加速度」的旋鈕，跟 1D 的 α 是同一回事。

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

  • 開上位機，畫出 roll、pitch。把飛控慢慢傾斜到約 45° 再回正，看數值是否跟著動、回正歸零。
  • 快速晃一下再放回水平：看姿態角會不會「跟得上又不亂跳」——這就是 Kp/Ki 調得好的表現。
  • 觀察 yaw（航向）：讓飛控靜止不動，過一兩分鐘看 yaw 有沒有慢慢漂。 會漂是正常的——因為這台沒有磁力計，yaw 只能靠陀螺積分，長期一定漂（正好印證 3.1 節）。

3.5 思考題

  • 把 KpDef 從 0.8 調大到 3.0，姿態角的反應會更快還是更慢？可能帶來什麼壞處？（對照 3.2 的「α 改小」）
  • 為什麼 roll/pitch 可以靠加速度修正、不會長期漂，yaw 卻會？（提示：地心引力能幫你定義「上下」，但能幫你定義「東南西北」嗎？）
  • 第 2 章若校準沒做好、陀螺零偏沒扣乾淨，對應到本章 ⑤ 的積分，會看到什麼現象？

3.6 本章對應的原始碼

檔案 看什麼
MATH/imu.c GetAngle()：四元數 + Mahony 互補濾波，本章重點
HAL/attitude_process.c 姿態相關的上層處理
MATH/myMath.c Q_rsqrt() 快速平方根倒數、atan2/asin 等工具

下一章：有了準確的姿態角，飛機怎麼「把自己擺回想要的角度」？ 這就要進入飛控的心臟——串級 PID 控制（對應官方影片 5.5）。