重力負荷インピーダンスアームの安全監視の反応

要点: 位置指令型アームの異常検知では、反応は「実測位置で保持」にする。トルクを抜くと自重で落ち、乖離した指令のまま止めると指令へ引っ張り続ける。どちらも検知後に危険を増やす。

前提#

MIT モード / インピーダンス制御のアームは、関節トルクを

$\tau =k_p(q_{cmd}-q)+k_d({\dot{q}}_{cmd}-\dot{q})+{\tau }_{ff}$

で出す。$q_{cmd}$ は指令 (setpoint)、$q$ は実測。異常検知は追従誤差 $|q_{cmd}-q|$ や電流 $|\tau |$ が閾値を継続超過した時に発報する。追従誤差トリップは 定義上、指令が実測から乖離した瞬間 に出る (衝突・拘束・脱調で実測が指令に追随できていない)。

3 つの反応と結果#

実測位置ホールドの実装#

1. 軌道生成器をリセットして目標を破棄。
2. 内部状態を実測にシード ($q_{cmd}=q$、${\dot{q}}_{cmd}=0$)。
3. 実測位置の setpoint を 1 回 publish。下流 (重力補償 → bridge) に latch され保持される。
4. ゲインと重力補償 FF は切らない。切ると落ちる。

ホールド後は setpoint が更新停止 → stale 判定で誤差監視が自動休止 → 再アーム。再試行で再拘束すれば再トリップする (自己制限)。

減速停止 (~/stop) との使い分け#

指令が実測をよく追っている通常運転の中断 (操作解除・一時停止) では、現在の setpoint からの減速停止でよい。乖離が無いので引っ張りは出ない。乖離が前提の異常検知だけ実測ホールドにする。両者を別サービス/別関数に分ける。

誤発報の抑制 (直交する対策)#

反応を安全にしても、閾値が固定だと速い移動 + 伝送遅れ (例: 片道 60–100 ms) で誤差を踏みやすい。閾値を指令速度に連動させる ($\text{thr}={\text{thr}}_0+T_d\cdot |{\dot{q}}_{cmd}|$)、または継続時間を上げる。ただし過度に鈍くすると実衝突の検知が遅れる。

関連#

• ギア付きインピーダンスの残留振動
• ジャーク制限モーション
• Host↔FW レイテンシの測定手順