弾性たわみ補償

関節ねじれとリンク bending を分けて考え、関節剛性 K を実測 fit して forward 補償するのが実装最短。

要点: 指令 vs 実 TCP の系統ずれは 関節ねじれ (τ/K) + リンク bending の合成。K を実測 fit してフォワード補償する arm 制御ノード に inline で追加するのが実装コスト最小。

発生源 (寄与順)#

関節ねじれ — gearbox torsional compliance + 出力軸ねじれ。Δq = τ/K
リンクたわみ — 片持ち梁曲げ。δ = F·L³/(3·E·I)
平行 4 節リンクのねじれ
base mount 変形 (通常無視)

モデル化 4 手法 #

手法	精度	パラメータ数	一般化	実装コスト
① 解析 (梁理論)	中	E, I 各リンク	○	中 (CAD 要)
② 経験 (実測 fit)	高 (測った範囲)	姿勢グリッド	×	低
③ 物理 + 較正 (関節剛性)	高	K × 関節数	○	中
④ 視覚 servo (online)	最高	動的	◎	高 (perception 依存)

お勧め: ③ を forward 補償で。少数 param・物理的解釈可・payload 変化に強い。

③ 物理モデル #

各関節 j:

Δq_j = τ_j(q, payload) / K_j
Δp   = J(q) · Δq

τ_j: 静的保持トルク (pinocchio の rnea で計算)
K_j: 関節剛性 [N·m/rad]、実測 fit で決める
J(q): Jacobian

フォワード補償の実装 (要点コード)#

Eigen::VectorXd q_ik   = solve_ik(target_tcp, tool_yaw, tool_pitch);
Eigen::VectorXd tau    = rnea(model, data, q_ik, 0, 0);
tau                   += external_wrench_torque(q_ik, payload_kg);
Eigen::VectorXd dq_e   = tau.cwiseQuotient(joint_stiffness_);  // 弾性ずれ
joint_target           = q_ik + dq_e;                          // 逆に押し込んで補償

較正手順 #

TCP に AprilTag (or 定盤 + 目視)、外側から実位置測定
N=20〜30 姿勢: 3〜4 半径 × 3 高さ × payload {空 / 1缶 / 2缶}
Python で: minimize_K Σ ‖measured - (cmd + J(q)·τ(q)/K)‖²
K を YAML に保存、node が起動時に読む

検証 #

未測定 5 姿勢で mm 単位精度: 補正前 vs 補正後
運用中 ManualAdjust の delta が減れば OK

出典 #

Zhang, "Stiffness Analysis of Serial Manipulators", 2015
Reinhart & Zäh, "Deflection Compensation for Industrial Robots"

最終更新: 2026年9月13日