Control
FF スタック
velocity_ff / duty_ff / 摩擦 / 重力 の設計と PID の役割
要点: 位置制御の出力 (r_v) に軌道速度を足し、速度制御の出力 (duty) に加速度 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 + 重力補償 (host 計算)
+ coulomb·sign(r_v) ← クーロン摩擦 (FW 計算)
+ viscous·r_v ← 粘性摩擦 + EMF 補償 (FW 計算)各項の役割
| 項 | 何を消すか | どこで計算 | 由来 |
|---|---|---|---|
| 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) callback |
| 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 を避けるため 10ms 遅れは致命的)
- 軌道由来 → Host (FW は軌道を知らない)
- 位置のみ依存で積載切り替えあり → Host (HSM 状態に紐付け)
Host 側計算の遅延コスト
Host が t0 の軌道サンプルを送信 → FW 到着 t0+δ (δ = 2-5ms)。FF が δ だけ後追い。
- v_ff = 10 rad/s なら 50 mrad の誤差
- 対策: gs_usb タイムスタンプ 参照
PID の役割は「小さくする」
FF が揃うと PID は「モデル誤差の修正」だけになる。
- P だけで十分なケースが多い (cascade P-P)
- velocity ki を少量 (0.1-0.3) で FF ズレを吸収
- position ki は入れない (重力は setGravityModel で受ける)
- D は不要 (cascade velocity feedback が既に微分項)
FF 項の測定 (auto-tune)
一度測れば決まる。順番:
- 等速 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 秒のシーケンスで全部埋まる。
PID gain の目安 (モデル同定 → 極配置)
velocity 帯域 ω_v を決める (例: 100 rad/s)
kp_v = J·ω_v / Kt
kp_p = ω_v / 5 # cascade で position 帯域は 1/5 が定石
ki_v = kp_v × ω_v / 10 # 実測で微調整チューニングノブは "velocity 帯域を何 rad/s にするか" の 1 個だけになる。
実装済み
- CanMotorMbed feature/ff-stack (V7 FW): CMD_SET_VELOCITY_FF / CMD_SET_DUTY_FF / FRICTION_*
- ros2_can_toolbox feature/ff-stack-api: sendTrajectoryPoint / setGravityModel / setFrictionCompensation / setAccelFFGain
- can_md_controller: friction.coulomb / friction.viscous param 転送 (bb8109b)