マルチコプタ制御入門 — 第6章

PID制御の道具箱

教科書の式から、実機の運転まで

前章では PID を「使い」ました。この章では PID を分解します。 教科書に最初に出てくる式は3行で書けますが、そのまま実機に載せると たいてい痛い目に遭います——微分はノイズで暴れ、指令を変えるたびに出力が跳ね、 積分はいつの間にか育ちすぎて機体が言うことを聞かなくなる。 教科書の式と実機のコードの間にある「工夫の階段」を、 第4章で同定したモータを実験台にひとつずつ登っていきます。

6.1 教科書のPID — 3つの項、3つの時間

最もよく使われるのは並列形です。誤差 \(e = r - y\) （目標値 − 測定値）に対して:

$$u(t) = K_p\,e(t) + K_i \int_0^t e(\tau)\,d\tau + K_d\,\frac{de}{dt}$$

3つの項は、見ている「時間」が違います。 P は現在——いまの誤差に比例して押す。 I は過去——これまでの誤差の蓄積を返済するまで押し続ける。 D は未来——誤差の増える速さから先回りしてブレーキを掛ける。 ゲインを時間の単位でくくり直した時定数形もよく使われます:

$$u(t) = K_p\left(e + \frac{1}{T_i}\int e\,d\tau + T_d\,\dot{e}\right), \qquad T_i = \frac{K_p}{K_i},\quad T_d = \frac{K_d}{K_p}$$

\(T_i\)（積分時間）は「I が P に追いつくまでの時間」、\(T_d\)（微分時間）は 「D が何秒先を予測するか」と読めます。実機 StampFly のファームウェアは 時定数形で、前章のゲイン表の \(T_i\)・\(T_d\) がまさにこれです。 本章では式の見通しがよい並列形で進め、必要なときに換算します。

6.2 モータで体感する — P の限界、I の御利益、I の欲張りすぎ

実験台は第4章で同定したモータです。ホバリング点まわりで線形化すると、 Duty からプロペラ回転数への伝達はほぼ一次遅れになります:

$$G(s) = \frac{\Delta\omega}{\Delta u} \approx \frac{K_m}{\tau s + 1}, \qquad K_m \approx 4500\ \mathrm{rad/s\ (Duty1あたり)},\quad \tau \approx 16\ \mathrm{ms}$$

「回転数を 300 rad/s 上げろ」というステップ指令と、途中で入る負荷外乱 （プロペラが傷ついて負荷が増えた想定）に対して、6.1 の並列形 PID を そのまま実装しました。P・I・D すべて自由に触れます。 まず P と I で応答を整えて、それから \(K_d\) を 0 から少しずつ 上げてみてください——何が起きるでしょうか。

条件: 目標 +300 rad/s のステップ（t=0.05s）、負荷外乱（t=0.45s、Duty −0.05 相当）。 制御は 6.1 の並列形 PID をそのまま実装（微分は教科書どおりの理想微分・偏差入力）。 飽和なし・ノイズなし。

やってみると、教科書の1行ずつが体感になります:

6.3 理想微分は使えない — 不完全微分と η の意味

6.2 の暴走の犯人は、理想微分 \(D(s) = K_d\,s\) のゲインが周波数に比例して 無限に増えることです。ゲインが高い周波数で頭打ちにならないため、 制御周期1回分のわずかな遅れさえ許容できず、ある \(K_d\) を超えると ループごと不安定になります。仮に安定でも、センサの測定値には必ず高周波ノイズが 乗っているので、理想微分は「ノイズ専用の増幅器」として暴れます。 そこで一次遅れフィルタを抱き合わせた不完全微分を使います:

$$D(s) = \frac{K_d\,s}{\tau_f s + 1}, \qquad \tau_f = \eta\,T_d$$

低い周波数（\(\omega \ll 1/\tau_f\)）ではほぼ理想微分 \(K_d s\) として働き、 高い周波数ではゲインが頭打ちになります。その上限は

$$|D(j\omega)| \xrightarrow{\ \omega\to\infty\ } \frac{K_d}{\tau_f} = \frac{K_d}{\eta\,T_d} = \frac{K_p}{\eta}$$

つまり \(\eta\) は「微分項のゲインを \(K_p\) の何倍で打ち止めにするか」を 決める係数です。\(\eta = 0.125\)（実機の値）なら上限は \(K_p\) の 8 倍。 \(\eta\) を小さくするほど微分は理想に近づき鋭くなりますが、ノイズの増幅上限も \(1/\eta\) 倍に上がります。微分が「微分として」働く帯域は \(1/T_d\) から \(1/(\eta T_d)\) まで——幅にして \(1/\eta\) 倍。 \(\eta\) は 0.1〜0.2 が相場です。

6.2 と同じ実験を、微分だけ不完全微分に差し替えてやり直します。 今度は \(K_d\) をいくら上げても暴走しません——上限が \(K_p/\eta\) で 頭打ちだからです。ノイズを入れて \(\eta\) を動かすと、 「切れ味」と「うるささ」のトレードオフが制御入力に現れます。

条件: 6.2 と同じ実験（目標 +300 rad/s・負荷外乱つき）。 微分だけが不完全微分（偏差入力）に変わった。 上段=回転数、下段=制御入力（Duty）。

6.4 微分キックと微分先行 — D は測定値に掛ける

もうひとつ、微分には問題があります。誤差は \(e = r - y\) なので \(\dot{e} = \dot{r} - \dot{y}\)。目標値 \(r\) がステップで変わると、 \(\dot{r}\) は撃力（一瞬だけ無限大）——微分項が出力を思い切り蹴飛ばします。 これが微分キックです。マルチコプタでは目標値はスティックから来ます。 12bit の AD 変換値は常に階段状に変わるので、キックは「たまに」ではなく 常時発生します。

対策は簡単で、D を誤差ではなく測定値 \(y\) だけに掛ける （\(\dot e\) の代わりに \(-\dot y\) を使う）。これを微分先行（PI-D） と呼びます。閉ループの分母（極）は変わらないので安定性・外乱応答はそのまま、 目標値の微分だけが式から消えます。 さらに P も測定値側に移した比例微分先行（I-PD）という形もあります ——目標値応答をいっそう滑らかにしたいプロセス制御などで使われる、 「そういうものもある」と知っておけば十分です。 StampFly は P は偏差・D は測定値の PI-D 構成です （ソースコード pid.hpp で確認済み）。

6.3 の実験の続きです。同じ条件のまま、D の掛け先だけを変えた2本 （偏差 vs 測定値）と、D なしの基準を重ねて描きます。

条件: 6.3 と同じ（\(K_p=10^{-3}, K_i=0.2, K_d=6\times10^{-6}, \eta=0.125\)、 目標 +300 rad/s・負荷外乱つき・ノイズなし）。 上段=回転数（3本）、下段=制御入力（D あり2本）。

種明かしをすると、一次遅れのプラントでは、回転数の変化に逆らう D は 「重り」として働きます（実効時定数が \(\tau + K_m K_d\) に伸びる）。 重りは外乱にも逆らうので落ち込みはわずかに浅くなりますが、 戻りは遅くなり、遅くなったプラントを I が急かすので目標応答は振動的になる—— 教科書の期待に反して、このプラントに D を入れる理由はありません。

では D はいつ効くのか — カスケードとの再会: 「速度に逆らう力」が減衰として本領を発揮するのは、 トルク→角速度→角度のように積分が重なった系です。 前章の姿勢制御がまさにそれ——そして思い出してください、 あの内側レートループこそ「測定した角速度に逆らう」構造でした。 外側 P ＋内側 P のカスケードは、展開すると角度誤差への P ＋角速度への フィードバック、つまり PD 制御と等価です。カスケードとは D 項の仕事を構造でやる方法であり、しかもジャイロが角速度を 直接くれるので、角度を微分してノイズと格闘する必要すらありません。 一方、一次遅れのモータには PI で十分——これも教科書の定番です。

6.5 積分の実装 — 地味だが、いちばん事故る場所

積分のコードは1行です。前進オイラー（毎周期、誤差×周期を足し込む）なら: 

// Forward Euler / 前進オイラー
integral += e * dt;

// Trapezoidal (Tustin) — the real firmware uses this for all terms
// 台形則（Tustin）— 実機は全項この形
integral += 0.5f * (e + e_prev) * dt;

400 Hz もあれば両者の差はわずかです。恐いのは離散化ではなく、 ワインドアップ（巻き上がり）のほうです。 モータの Duty は 0〜1 までしか出せません。出力が飽和している間も 積分器は「まだ誤差がある」と足し込み続け、際限なく育ちます。 誤差が反転しても、育った積分が吐き出されるまで出力は戻らない—— 大オーバーシュートや、最悪「しばらく言うことを聞かない」機体になります。

対策には段階があります:

コラム — やらかし博物館（積分編）: 積分まわりの失敗は制御実装の定番です。その1: 積分にリミットを かけ忘れ、値がオーバーフローして符号が反転——制御入力が逆向きに 振り切れて機体が暴走する。その2: 飽和中に積分が巻き上がり、 操作しても全然効かない機体になる（吐き出しが終わるまで数秒間）。 「飽和したら積分をやめる」と言うのは一行ですが、方向の判定・復帰の条件まで含めて 正しく動くコードを書くのは、実はそれなりに大変です。 ——余談: いまではこうした定型実装は AI がそつなく書いてくれます。それでも、 だからこそ、書かれたコードの正しさを検収できる目が要ります。 この章はその目を作るための章です。

仕上げの実験です。飽和が長く続く大きなステップで、ワインドアップの被害と 条件付き積分の効果を確かめてください。

条件: PI 制御（\(K_p=0.001,\ K_i=0.05\)——飽和がなければオーバーシュートしない おとなしい設計）。目標 +1800 rad/s、Duty の飽和 ±0.5。 上段=回転数、下段=制御入力（Duty）。

6.6 実装の形 — 時定数形、そして PI-D、むだ時間

仕上げに、実機 StampFly の PID を読みます。ゲインは 6.1 で出てきた 時定数形（\(K_p, T_i, T_d\)）です。まず、時定数形＋不完全微分を 「1本の制御器を誤差に掛ける」形で書くと:

$$C(s) = K_p\left(1 + \frac{1}{T_i s} + \frac{T_d s}{\eta T_d s + 1}\right), \qquad u = C(s)\,e$$

この式のまま、モータで運転してみましょう。ツマミが \(K_i, K_d\) から \(T_i, T_d\) に変わると操作感がどう変わるか—— 6.2〜6.3 の並列形と比べて味わってください。

条件: 6.2 と同じ実験（目標 +300 rad/s・負荷外乱つき）。時定数形・誤差入力・ 不完全微分。\(T_i\) のスライダーは対数刻み（2 ms〜1 s）。 読み出しに並列形への換算を表示。

ただし——実機の実装は上の式ではありません。6.4 で学んだとおり、 D は誤差ではなく測定値に掛けるのが実機の流儀です。StampFly の実装を 式に忠実に書くと:

$$u = K_p\,e + \frac{K_p}{T_i s}\,e - K_p\,\frac{T_d s}{\eta T_d s + 1}\,y$$

目標値と測定値で通り道が違うため、もう「1本の \(C(s)\) を誤差に掛ける」形には 書けません（2自由度制御と呼ばれる構造の一種です）。

では、\(T_d\) はいつ意味を持つのでしょうか。ここまでのモータには むだ時間がありませんでした。実機には必ずあります—— センサの読み出し、フィルタ、制御周期、PWM が反映されるまでの待ち。 そこで、同じモータにむだ時間 \(L\) を足して、 実装の形（PI-D）で運転してみます。

条件: 6.2 と同じモータ＋むだ時間 \(L\)（初期値 8 ms）。制御は実装の形 （P・I=誤差、D=測定値、η=0.125）。目標 +300 rad/s・負荷外乱つき。

6.7 実機を運転する — Web の中の StampFly

部品が揃いました。時定数形のゲイン、PI-D の構造、むだ時間—— 全部組み上げると、本物の StampFly のロール・レートループが このページの中に再現できます。プラントは第4・5章で実測した同定値そのまま （\(I_{xx}=9.16\times10^{-6}\ \mathrm{kg\,m^2}\)、モータ遅れ 20 ms、 むだ時間 12 ms）、制御器は実際に飛んでいるファームウェアと同じ構造 （PI-D・\(\eta=0.125\)）、そして初期値は飛行実績のあるゲイン そのものです。スライダーを動かすことは、実機にパラメータコマンドで ゲインを書き込む行為に相当します——違いは、失敗してもブラウザを リロードすれば済むことだけ。実機の値の前後にスライドして、この設計が 崖からどのくらいの距離に立っているかを確かめてください。

なぜ Web ページで「実機」が運転できるのか: 第4章でプラントの中身——慣性・時定数・むだ時間——を全部実測したからです。 数字が架空の教材シミュレータと違い、ここで動いている応答は実機の飛行ログと 同じ物理・同じ数字です。パラメータを測った者だけが手にできる配当です。

条件: ロールレート指令 +3 rad/s のステップ（t=0.05s）＋トルク外乱（t=0.45s、 \(10^{-4}\) N·m）。初期値は実機の飛行実績ゲイン \(K_p=9.8\times10^{-4},\ T_i=0.7,\ T_d=0.010\)（η=0.125）。 上段=ロールレート、下段=トルク指令。

最後に pid.hpp（1軸あたり約80行）の設計をまとめます:

教科書の3行と実機の80行の差は、この章で登った階段——不完全微分・微分先行・ 条件付き積分・初回の仕込み——そのものです。

まとめ

道具箱は揃いました。次章では視点を時間から周波数へ移します——実機 StampFly の ゲインを実際に決めているループ整形（位相余裕・交差周波数）と、 第5章で見た「崖」の種明かしです。

本章の対話記録: notes/qa_log.md の Q-PID-1〜Q-PID-4。実機実装の出典: stampfly_ecosystem firmware/vehicle/components/sf_controller_pid/include/pid.hpp （P=偏差・D=測定値・η=0.125・全項Tustin・条件付き積分を原文で確認）。 プラント数値は第4章の同定値（ホバ点線形化）。