サーボを三段の PID で動かす ―― 位置・速度・電流のカスケードを波形で確かめる

前回は、コタツを Python で作って P → PI → PID の効き目を波形で見ました。温度は時定数が秒〜分の、ゆっくりした世界だったので、素朴な PID 一段でもちゃんと目標に届きました。

今回は、その続きで サーボモータの位置決め に踏み込みます。ここが温度と大きく違うのは、二つあります。ひとつは、速いこと。ミリ秒の単位で位置を合わせにいきます。もうひとつが厄介で、合わせたいのは「位置」なのに、モータに直接命じられるのは「電流（トルク）」だけ だということです。位置を電流でいきなり押そうとすると、間に速度と慣性が挟まって、行き過ぎたり振れたりしやすい。

そこで、内側から 電流 → 速度 → 位置 と、PID を三段に入れ子にします。これを カスケード制御 と呼びます。内側ほど速く、外側ほどゆっくり。外側のループが「こういう速度で回れ」と目標を作り、内側のループがそれを実現する、という役割分担です。今回も式は薄めに、全部を時間領域の差分（オイラー法）で回します。ラプラス変換は使いません。

モータを Python で作る

まず、制御される側 ―― ブラシ付き DC モータのモデルです。コタツのときと同じで、物理を二行の差分にするだけです。電気側（コイル）と機械側（回転）を分けて書きます。

# 電気側：コイルにかかった電圧の残りで電流が変わる（Ke·ω は逆起電力）
i     += dt/L * (V - R*i - Ke*omega)
# 機械側：トルク Kt·i から摩擦と負荷を引いて回す
omega += dt/J * (Kt*i - friction - load)
theta += dt * omega

Ke*omega が 逆起電力 です。モータは回ると発電機にもなるので、速く回るほど電流が入りにくくなる ―― これがあとで「高速域の頭打ち」として効いてきます。Kt*i が発生トルク。あとは慣性 J と摩擦で回り、その速度を積み上げると角度 theta になります。

味付けとして、実機で必ずぶつかる制限を入れておきます。電源電圧には上限（ここでは DC バス 24V）があり、電流にも上限（8A ＝ トルクの上限）があります。この二つが、次の波形の主役になります。

三段のカスケード ―― 内側ほど速く

制御側は、前回の PID をそのまま三段に重ねるだけです。位置ループの出力が速度の目標に、速度ループの出力が電流の目標に、電流ループの出力が電圧になります。

w_ref = pos.update(theta_target, theta_meas)   # 位置ループ → 速度の目標
i_ref = spd.update(w_ref,        speed_meas)   # 速度ループ → 電流の目標
V     = cur.update(i_ref,        i_meas)       # 電流ループ → 電圧

内側の電流ループはいちばん速く（ここでは 20 kHz）、速度と位置はその外側（2 kHz）で回します。三段といっても、どこもフルの PID でそろえる必要はありません。ここでは 位置＝P、速度＝PI、電流＝PI と配っています（なぜ位置だけ P で済むのかは、後で出てきます）。では、目標を 10 rad（約 1.6 回転）動かしてみます。

一枚に、サーボの動きがひととおり出ています。順に読むと、

• 加速中は電流が +8A で頭打ち（3段目）。これがトルクの上限で、モータは出せる最大のトルクで加速しています。
• 速度が乗ってくると、今度は電圧が 24V で頭打ち（4段目）。逆起電力が増えて、電源電圧では押し切れなくなる。ここが最高速を決めます。
• 目標に近づくと、電流が −8A に反転。これは減速のトルクです（このとき戻ってくる電力を、実機では回路構成しだいで回生させたり、熱として捨てたりします ―― その話は電力変換の回で）。行き過ぎないように、内側からトルクで止めにいっている。
• 結果、位置は行き過ぎ 0 で、約 91 ミリ秒で目標に収まりました（1段目）。
• 最後、350 ミリ秒で外から負荷トルクを乗せる と、電流が 3A の段に落ち着きます。これは「負荷 0.15 ÷ トルク定数 0.05 ＝ 3A」で、位置を保つのにちょうど要る保持電流です。

なぜ三段に分けるのか。冒頭でも触れたとおり、位置を電流で直接合わせにいくと、途中の速度と慣性のせいで、うまく止められません。内側の速い電流ループがトルクを確実に担い、その外の速度ループが速さを担い、いちばん外はただ「この速度で回れ」と目標を渡すだけ ―― こうやって役割を分けると、それぞれのループは前回のコタツと同じくらい素朴な PID で済みます。

ひとつ、地味ですが実際に手を焼いた点を。外側のループだけ更新を遅く（たとえば位置だけ 500 Hz）すると、その粗い階段状の目標が段差となって内側に伝わり、電流と電圧が細かく振れました。前回の「微分キック」の親戚です。ここでは位置ループも速度と同じ速さで回して収めています。

段差を滑らかに ―― 台形の指令とフィードフォワード

さきほどは目標を段差（ステップ）で与えました。でも実際の装置で「いきなり全力」はふつう避けます。代わりに、加速 → 巡航 → 減速 の台形の速度プロファイルを引いて、位置の目標をなめらかに動かします。加速度と最高速をあらかじめ決めておくので、急な動きになりません。

ところが、台形の目標を P だけの位置ループ で追うと、困ったことが起きます。

赤い線がそれです。動いているあいだ中、実際の位置が指令からずっと遅れています。ピークで 2.4 rad（0.4 回転ぶん近く）も引きずられました。この遅れを 追従誤差 と呼びます。P は「今ずれている分」しか押さないので、目標が動き続けるかぎり、遅れも残り続けるのです。

対策は、フィードフォワード（先読み） です。フィードバックが「ずれてから直す」のに対し、フィードフォワードは「これから必要な分を、先に足しておく」考え方です。

w_ref = pos.update(theta_ref, theta_meas) + v_traj          # 速度FF
i_ref = spd.update(w_ref,     speed_meas) + (J/Kt) * a_traj  # トルクFF

まず 速度フィードフォワード。軌道が要求している速度 v_traj を、位置ループの出力に先に足しておきます（橙の線）。これだけで追従誤差は 2.4 rad から 0.09 rad まで縮みました。さらに トルク（加速）フィードフォワード。加速に要る電流は「慣性 J × 加速度 ÷ トルク定数 Kt」で先に計算できるので、それを電流の目標に足します（青の線）。ここで先読みするのは慣性ぶんだけで、摩擦や外からの負荷までは織り込んでいません。追従誤差は 0.036 rad まで下がり、わずかに出ていた行き過ぎもほぼ消えました。

ここで、実際に私がはまったポイントが二つありました。

ひとつ。フィードフォワードは「出せる範囲の遅れ」しか消せません。加速度を欲張って、必要な電流が 8A の制限を超えると、モータは物理的にそれ以上加速できず、先読みしても遅れは埋まりません。だから軌道は、モータが追える範囲に引いておく ―― これが台形プロファイルを引くことの意味でもあります。

もうひとつ。位置ループには D を入れませんでした（P のみ）。前回は D でブレーキをかけましたが、カスケードでは制動役は内側の速度ループがすでに担っています。位置ループにも D を入れると、測定値の微分が速度フィードフォワードと打ち消し合って、かえって追従が悪くなりました。実際、試しに位置ループへ D を足してみると（リポジトリの Kd_p を上げるだけです）、ステップの整定が 91 → 189 ミリ秒と、むしろ遅くなりました。制動は内側に任せて、位置は P だけ ―― これが素直でした。

エンコーダという現実 ―― 位置しか測れない

ここまで、速度を当たり前のように使ってきました。でもエンコーダ式のサーボで直接得られる基本量は、「位置」 です。速度は、その位置を差分して作り出します。そして、ここに落とし穴があります。

エンコーダは角度を、決まった刻み（counts）でしか返せません。上段のように、なめらかな真の角度が階段状に量子化されます。この階段をそのまま差分して速度を作ると ―― 下段の赤い線のように、量子化の段差が拡大されて、速度に真値からおよそ ±20〜25 rad/s もの量子化ノイズが乗ってしまいます。この荒れた速度を速度ループに入れると、電流の指令が乱れて、モータが小刻みに震えます。

ここで効くのが、前回も試した工夫です。微分は測定値そのものから取り、さらに 一次のローパスフィルタ で均す。差分で作った生の速度に 200 Hz のフィルタを通したのが青い線で、真の速度（灰）にちゃんと乗ってきます。「傾きはノイズを拾いやすいから、そのまま使わず均す」―― コタツの微分キックのときと、根っこは同じ話です。

v_raw = (theta_meas - theta_prev) / dt   # 差分で速度を作る（量子化で荒れる）
v_est += alpha * (v_raw - v_est)         # 一次ローパスで均す

おわりに ―― 次は「回る座標」と「電圧の作り方」

カスケードで、位置決めの骨格ができました。ブラシ付き DC モータでは「電流 ＝ トルク」が素直だったので、内側の電流ループが握りやすかった。台形の指令とフィードフォワードでなめらかに追従し、エンコーダの荒れはフィルタで均す ―― ここまでで、そこそこ実機に近い形になっています。

ただ、モータがブラシレス（三相交流）になると、話が一段変わります。電流の向きがローターと一緒に回ってしまい、そのままでは狙いを定めにくい。そこで「回る座標に私たちも乗ってしまう」という dq 変換 / FOC の出番です。回る座標に乗ると、交流が直流に見えて、今回と同じ感覚で電流を押せるようになる ―― 次回はそこを、やはり波形で確かめてみます。

さらにその先には、制御が出した「電圧指令」を、実際にどうやって本物の電流にするか、という話があります。電源の交流を整流して直流にし、それを FET のスイッチングで作り直してモータへ ―― インバータと電力変換の世界です。ここも、いつか回してみたいと思っています。

コード一式（モータのモデル、三段カスケード、台形指令とフィードフォワード、エンコーダと速度推定、3 枚の図の生成）は、そのまま動く形で置いてあります。python servo_dc.py で本文の数値が、python plot_servo.py で 3 枚の図が出ます。

• リポジトリ: logicia32/servo-lab