モータ制御の位相遅れ切り分け手順

FF 追従で観測される位相遅れの原因を「安い測定 → 高い測定」で特定する。traj rate 掃引 → fb rate 掃引 → step 応答 fit → CAN 改善 → FW 化の順

要点: FF 制御で観測される位相遅れは 8+ の経路の合算。安い測定から順に潰すことで犯人 (traj sampling / fb sampling / CAN / 機械 / 電気) を切り分ける。1 時間半で 8-9 割の内訳が判明する。

位相遅れの経路 (合算構造)#

#	経路	典型値	支配的な条件
1	Host DDS → CAN 送信	1-5 ms	Host 側
2	CAN 転送	< 1 ms	fibril_can, ~5 ms w/ 旧 gs_usb
3	FW 1kHz tick 待機	0-1 ms	± 1 tick jitter
4	PID + FF 計算	< 0.1 ms	無視
5	電気時定数 (L/R)	0.5-2 ms	モータ依存
6	機械時定数	v_p × 2π/(kp·Kt/J)	慣性 + 摩擦
7	Encoder → FW tick	< 1 ms	無視
8	FW → CAN fb 送信	最大 10 ms (100 Hz aging)	支配的
9	Host DDS 受信	1-5 ms	Host 側

traj publish rate aging: 100 Hz なら平均 5 ms

合計: 15-30 ms が典型。

前提: FF 校正が正常であること #

壊れた FF (viscous < 0, accel_ff_gain < 0 等) は測定結果を汚す。まず sanity check 通過の校正で走らせる。

手順 (安い → 高い)#

Step 0: ベースライン測定 #

現状の位相遅れを数値化 (bag 取って fit)。**「削るべき target」**を確定させる。

Step 1: traj publish rate 掃引 (30 分)#

Publish rate を 100 → 200 → 500 Hz で同じ trajectory を流す。

判定:

rate ↑ で位相遅れが階段状に減る → traj sampling aging が主犯
変わらない → traj aging 無罪

Step 2: feedback rate 掃引 (10 分)#

Feedback rate を 100 → 500 Hz。

判定:

rate ↑ で位相遅れ ↓ → fb sampling aging が主犯
変わらない → fb 無罪

Step 3: step 応答で dead time 分離 (20 分)#

Target に velocity step (+1.0 rad/s など) を送って、feedback.velocity を bag に取る。Python (scipy.optimize.curve_fit) で 一次遅れ + 純遅延 モデルに fit:

$v (t) = K \cdot (1 - e^{- (t - τ) / T}) \cdot u (t - τ)$

τ (dead time) = 通信 + サンプリング aging → 15 ms 級なら通信支配
T (時定数) = 機械 + 電気 → 5 ms 以下なら健全

これで 通信系 (τ) と機械系 (T) が数値で分離。

Step 4: 判定 & 次の一手 #

Step 1-3 の結果で:

症状	犯人	対策
Step 1 で rate ↑ で 5 ms 減	traj sampling aging	publish rate を 500 Hz に (無コスト)
Step 2 で rate ↑ で 5 ms 減	fb sampling aging	fb rate を 500 Hz に (無コスト)
Step 1/2 変わらず、Step 3 で τ 大	CAN 通信	fibril_can に移行 (別 task)
Step 1/2 変わらず、Step 3 で T 大	機械時定数	PID gain 上げ or FF 精度改善
全部残る 5-10 ms	FW tick + 電気	FW 軌道生成で除外

Step 5 (option): CAN スタック改善後に再測 #

新 CAN スタック (fibril_can 等) に移行したら Step 1-3 を再実行。差分が CAN スタック改善の実効値。

Step 6 (最終手段): FW 側 trajectory 生成 #

Step 1-5 やっても目標未達の場合のみ。

実装コスト ~1 週間 (Ruckig-lite を FW に移す)
通信経路 (traj publish 含む) がゼロになる
残るは真の機械/電気/FW tick のみ

所要時間 #

Step	時間	得られるもの
0. ベースライン	5 分	削るべき target 数値
1. traj rate 掃引	30 分	通信 vs 機械の 1 次切り分け
2. fb rate 掃引	10 分	fb aging の効き
3. step 応答 fit	20 分	dead time / 時定数の分離
4. 判定	10 分	犯人特定
5. CAN 改善後再測	40 分	CAN 改善の実効値
6. FW trajectory	1 週間	通信系ゼロ

Step 1-4 で 1 時間半で 8-9 割の犯人が分かる。それ以上は投資対効果が悪化するので慎重に。

位相遅れの目標 (参考)#

ロボットアーム trajectory 追従 (産業用 Franka, UR): < 10 ms
産業用 direct drive servo: < 5 ms
一般的な gearbox + PID + FF: 10-25 ms が妥当ライン

24 ms は「よくある値」だが、高速動作 (1 s 周期の sin) では明らかに削りたい。5-10 ms が実用ライン。

落とし穴 #

feedback timestamp が測定時刻でない: header.stamp = 受信時刻。FW が測定したのは 5-10 ms 前。位相遅れ計算に含めるかどうかは目的次第
FF が壊れてる状態で測定: viscous 負や accel_ff_gain 負が入ってると測定値が汚れる
traj_probe のような Python 生成 trajectory: 発行ジッタが混じる → C++ で厳密 rate 制御した trajectory の方が測定精度上がる
Bag 内 timestamp の使い分け: publish time vs receive time. header.stamp は publisher 依存
温度依存: モータが冷えてる時と暖まってる時で電気時定数が変わる