ロボットアームの順運動学・逆運動学（FK / IK）

多関節ロボットアームの「関節角 ↔ エンドエフェクタ位置・姿勢」を相互変換する運動学の理論。SO101（6自由度）を題材に、同次変換・Rodrigues の公式・ヤコビアン・減衰最小二乗法を整理する。応用としての軌跡データ拡張は so101-trajectory-augmentation を参照。

順運動学（Forward Kinematics, FK）

関節角からエンドエフェクタの位置・姿勢を求める。各ジョイントの同次変換行列を順次積算する:

$T_{ee} = \prod_{i = 1}^{n} T_{i} (θ_{i})$

各ジョイント変換は URDF から構成される 3 要素の積:

$T_{j o in t} = T_{t r an s l a t i o n} \times R_{f i x e d} \times R_{j o in t}$

T_translation: ジョイント原点の並進 (xyz)
R_fixed: 固定回転。URDF の RPY（固定軸 XYZ 規約） から $R_{f i x e d} = R_{z} (yaw) R_{y} (pitch) R_{x} (roll)$
R_joint: 関節角による回転

単位行列から始めて T = T @ T_joint を連鎖し、T[:3,3] が位置、T[:3,:3] が姿勢。

任意軸回転（Rodrigues の公式）

回転軸 $k = (k_{x}, k_{y}, k_{z})$ 、角度 $θ$ に対し、歪対称行列 $K$ を用いて:

$R = I + sin θ K + (1 - cos θ) K^{2}, K = 0 k_{z} - k_{y} - k_{z} 0 k_{x} k_{y} - k_{x} 0$

逆運動学（Inverse Kinematics, IK）

目標エンドエフェクタ位置から必要な関節角を逆算する。一般に非線形最適化問題で、閉形式解を持たないことが多い。

減衰最小二乗法（Damped Least Squares / Levenberg-Marquardt）

目標 $p_{t a r g e t}$ と現在 $p_{c u r r e n t}$ の誤差 $e$ を、関節更新 $Δ q$ のノルム正則化込みで最小化:

$min_{Δ q} ∥ e - J Δ q ∥^{2} + λ^{2} ∥Δ q ∥^{2} \Rightarrow Δ q = J^{⊤} (J J^{⊤} + λ^{2} I)^{- 1} e$

$J$ : ヤコビアン（位置のみなら 3×n）。解析解でなく有限差分で数値計算することも多い。
減衰係数 $λ$ : 大きいほど安定だが収束が遅い／小さいほど速いが特異点近傍で不安定。実装例では $λ = 0.1$ 。正則化項 $λ^{2} I$ が逆行列の悪条件化（特異点）を回避する。

この減衰最小二乗の構造は newtons-method-cpp の準ニュートン法（ヘッセ近似を解く線形系）と同様、「ヤコビアン/ヘッセ系の正則化付き線形求解を反復する」という最適化の共通形に属する。

正規化値とラジアン

データセットの関節角は正規化されている（SO101: アーム関節 1–5 が [-100,100]、グリッパー 6 が [0,100]）ため、線形写像でラジアンと相互変換する。

グリッパーオフセット補正

FK/IK が扱うのは gripperframe の位置で、実把持点はそこからオフセット（例 [0.040,-0.058,0.030] m）がある。FK 後に姿勢回転込みで actual = fk_pos + R @ GRIPPER_OFFSET と補正する。

SO101 の構成

6 自由度・全 revolute（Z 軸）: shoulder_pan / shoulder_lift / elbow_flex / wrist_flex / wrist_roll / gripper。キネマティックチェーンは base → shoulder → upper_arm → lower_arm → wrist → gripper → moving_jaw → gripperframe。

Quartz 5

Explorer

ロボットアームの順運動学・逆運動学