FF スタック

位置・速度カスケード PID に velocity FF・加速度 FF・重力補償・摩擦補償を加える構成

要点: 位置制御の出力に軌道速度を加算し、速度制御の出力に加速度 FF・重力補償・摩擦補償を加算する構成。摩擦は FW、それ以外は Host で計算するのが遅延と計算量のトレードオフから妥当。

構成 #

target_pos = pos(t)                     ← 0階 (指令位置)
r_v   = kp_p·(pos_target - pos) + v(t)  ← 1階 velocity_ff
duty  = kp_v·(r_v - v_fb) + duty_ff     ← 2階 accel_ff + 重力補償
        + coulomb·sign(r_v)             ← クーロン摩擦
        + viscous·r_v                   ← 粘性摩擦 + EMF 補償

各項の役割 #

項	消す誤差	計算場所	由来
kp_p (position P)	定常位置誤差	FW	fb 位置
velocity_ff	Type-1 追従誤差	Host	軌道生成器
kp_v (velocity P)	定常速度誤差	FW	fb 速度
duty_ff (accel FF)	加減速時の遅れ	Host	軌道 acc × J/Kt
duty_ff (重力)	保持 duty	Host	pos → g(pos)
coulomb friction	静止摩擦	FW	r_v の符号
viscous friction	粘性 + EMF	FW	r_v

FW / Host の分担 #

変数が更新される場所と同じ場所で計算する。

情報	場所	更新周期
r_v (position PID の出力)	FW	1 kHz
feedback_speed	FW	1 kHz
position (エンコーダ)	両方 (FW→host 100Hz)	1 kHz (FW)
pos/vel/acc (軌道)	Host (Ruckig 等)	100 Hz
HSM の積載状態	Host	事象駆動

判定:

r_v 依存 → FW (摩擦の sign chatter を避けるため 10 ms 遅れは致命的)
軌道由来 → Host (FW は軌道を知らない)
位置のみ依存で積載切り替えあり → Host (HSM 状態に紐付け)

Host 計算の遅延コスト #

Host が t0 の軌道サンプルを送信 → FW 到着 t0+δ (δ = 2-5 ms、gs_usb の場合)。FF が δ だけ後追いになる。

v_ff = 10 rad/s、δ = 5 ms なら 50 mrad の追従誤差。

対策は gs_usb タイムスタンプ参照。

PID の役割 #

FF が揃うと PID はモデル誤差の修正のみを担う。

P だけで足りるケースが多い (cascade P-P)
velocity ki を少量 (0.1-0.3) 加えて FF ズレを吸収
position ki は入れない (重力は FF 側で補償)
D は不要 (cascade velocity feedback が微分項の役割)

FF 項の測定 #

一度測れば決まる。順番に連鎖する:

等速 sweep (±0.5, ±1.0, ±2.0 rad/s 各方向) → クーロン係数と粘性係数を LS fit
自由減衰 (duty=0 で ω(t)) + 摩擦既知 → J = -friction / (dω/dt)
J と Kt から accel_ff_gain = J/Kt
上下 2 方向保持 → gravity duty = 平均、静止摩擦 = 差/2

30-40 秒の 1 シーケンスで FF スタックの全パラメータが決まる。

PID gain の設計 (モデル同定 → 極配置)#

velocity 帯域 ω_v を決めれば gain が計算で出る:

kp_v = J·ω_v / Kt
kp_p = ω_v / 5      # cascade で position 帯域は 1/5 が定石
ki_v = kp_v × ω_v / 10   # 実測で微調整

チューニングノブは velocity 帯域を何 rad/s にするかの 1 個だけ。