第24章 - 順運動学、逆運動学、ヤコビアン

ステージ5 · 第24章 · 理論

24. 順運動学、逆運動学、ヤコビアン

Seeed Physical AI Beginner's Course 第24章 — 関節空間とデカルト空間、DHパラメータとURDFからの順運動学、逆運動学とその複数解／解なし、ヤコビアンと速度運動学、特異姿勢とダンピング付き最小二乗法、解析的IKと数値的IK。

Joint vs Cartesian Forward kinematics Inverse kinematics Jacobian

24.1 主なトピック

関節空間 vs. デカルト空間
順運動学 (FK)
逆運動学 (IK)
複数解、解なし、ワークスペース
関節リミット
ヤコビアン行列
速度運動学
特異姿勢
数値的IKと閉ループIK

24.2 学習目標

アームが関節角度からエンドエフェクタの姿勢をどのように計算するか、また目標姿勢が与えられたときに関節角度をどのように求めるかを説明できる。

24.3 関節空間 vs. デカルト空間

	関節空間	デカルト空間
説明	関節角度 $(q_1, q_2, \ldots, q_n)$	エンドエフェクタ姿勢 $(x, y, z, rx, ry, rz)$
次元	n（関節数）	6（位置3 + 姿勢3）
物理的意味	モータがどのように回るか	先端がどこにあり、どちらを向いているか
運動経路	関節速度一定 -> エンドエフェクタの軌跡は不規則	先端の直線／円弧経路 -> 関節運動は非線形
制御の難しさ	直接（そのままモータに送る）	間接（まずIKで関節角度を解き、それからモータに送る）
典型的な用途	自由動作、障害物回避、原点復帰	把持、溶接、塗装、経路追従

例1：文字を書くロボット

タスク	どちらの空間か
モータ1を30度、モータ2を45度回す	関節空間（角度を直接送る）
ペン先で文字のストロークを直線でなぞる	デカルト空間（各点の関節角度を計算する）

例2：カップを持ち上げる（経路は無視）

段階	どちらの空間か
ホームからカップの近くまで動かす	関節空間（単純・一定速度・安全）
カップの縁に正確に合わせる最後の数センチ	デカルト空間（XYZで接近する必要がある）
持ち上げて棚に置く	関節空間（直線経路の要求はない）

例3：自動車の溶接

タスク	どちらの空間か
溶接トーチが継ぎ目に沿って直線で動く	デカルト空間（直線 = 溶接線）
トーチを持ち上げて次の位置へ移動	関節空間（重要なのは速さ）

24.4 順運動学 vs. 逆運動学

	順運動学 (FK)	逆運動学 (IK)
既知	関節角度	エンドエフェクタ姿勢
求めるもの	エンドエフェクタ姿勢	関節角度
向き	関節 -> エンドエフェクタ	エンドエフェクタ -> 関節
解	一意	複数 / なし
計算	単純（式をそのまま適用）	複雑（方程式を解く／反復計算）

FK が使われる場面

可視化：関節角度をリアルタイムに3Dモデルとして描画（ROS RViz、ゲームキャラクタ）
検証：計算されたエンドエフェクタ姿勢が正しいか確認
キャリブレーション：理論上の姿勢と実際の姿勢を比較
単純な制御：あらかじめ決めた角度列に沿って動かす

IK が使われる場面

把持：カメラが物体を見る -> 関節角度を解く -> アームを制御
溶接／塗装：先端が指定された軌道に沿う必要がある
ヒューマノイド：足が指定された地面の点に着地しなければならない
「ある場所へ行く」タイプのあらゆるシナリオ

24.5 順運動学

関節角度からエンドエフェクタ姿勢を計算するプロセス

一文で言うと

ベースからエンドエフェクタまで、各関節のローカル変換を順番に 掛け合わせる。

T_{base}^{end} = T_{base}^{link_1} \cdot T_{link_1}^{link_2} \cdots T_{link_{n-1}}^{link_n} \cdot T_{link_n}^{tcp}

計算手順（6ステップ）

リンク座標系を定義：各関節にローカル座標系を構築（DHパラメータ／URDF）
各リンクの変換行列 $T_i^{i+1}$ を書き下す（一つ前の関節に対する並進 + 回転）
関節角度を代入 $q_i$ （回転は $\theta_i$ 、並進はDHテーブルから）
順番に掛け合わせる $T_1^2 \cdot T_2^3 \cdot \ldots \cdot T_n^{n+1}$
ツールオフセットを掛ける $T_{flange}^{tcp}$
最終的に得られる $T_{base}^{end}$ — 位置 $(x,y,z)$ と姿勢（ $R$ または $q$ またはオイラー角）を含む

関連する概念を1枚にまとめると

Joint angles (q)
   |
DH params / URDF  ← describe link geometry (a, alpha, d, theta)
   |
Homogeneous transform (4x4)  ← translation + rotation combined
   |
   +-- rotation matrix R (3x3, SO(3))
   |      +-- rotation representation: Euler / quaternion / axis-angle
   +-- translation vector t (3x1)
   |
Matrix chain multiplication (chain rule)
   |
End-effector pose T_base^end (SE(3))
   |
   +-- position (x, y, z)  ← FK output
   +-- orientation (R / q / rpy)  ← orientation representation
   |
Cartesian-space trajectory / Jacobian (velocity mapping)
   |
Visualization (RViz / simulation)

中核となる概念チェックリスト

概念	役割
DHパラメータ	各リンクの $a, \alpha, d, \theta$ を4つの数値として符号化
同次変換	並進 + 回転を4x4行列にまとめて連鎖しやすくする
連鎖則	多数の行列を掛け合わせる本質
回転の表現	姿勢をオイラー角／クォータニオン／回転行列として出力
三角関数	行列展開はすべて $\sin/\cos$
ヤコビアン	FKは姿勢を与え、Jは速度を与える（IKでは逆向きに利用）
URDF	実際のロボット記述ファイル；FKの数値の出どころ

小さな例（2関節）

\theta_1, \theta_2 = \text{関節角度}, \qquad l_1, l_2 = \text{リンク長}

T_1^2 = \begin{pmatrix} \cos\theta_1 & -\sin\theta_1 & 0 & l_1\cos\theta_1 \\ \sin\theta_1 & \cos\theta_1 & 0 & l_1\sin\theta_1 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{pmatrix} \qquad T_2^3 = \begin{pmatrix} \cos\theta_2 & -\sin\theta_2 & 0 & l_2\cos\theta_2 \\ \sin\theta_2 & \cos\theta_2 & 0 & l_2\sin\theta_2 \\ \vdots & \vdots & \vdots & \vdots \\ 0 & 0 & 0 & 1 \end{pmatrix}

T_1^3 = T_1^2 \, T_2^3 \quad\Longrightarrow\quad \text{エンドエフェクタ姿勢}

FK = DH記述 + 同次行列 + 連鎖乗算 により、「関節角度」を「先端がどこにあるか」に変換すること。

その途中で必ず押さえるべきもの：DH、4x4変換、行列の掛け算、回転表現（オイラー／クォータニオン）、URDF。

24.6 逆運動学

エンドエフェクタ姿勢から関節角度を計算するプロセス。 実際にはほとんど常に数値的に行われます：姿勢方程式を一度に解くのではなく、ソルバが徐々に目標へ近づいていきます。

棚の本に手を伸ばすことを考えてみてください：手が本からどれだけ離れているかを見て、どちらへ動かすかを決め、関節を少しだけ動かし、それを触れるまで繰り返します。ロボットもまったく同じことをします。

ループ — 各反復で3つのことを行います：

誤差を見る：エンドエフェクタが目標姿勢からまだどれだけ離れているか
方向を推定する：その誤差を減らすために各関節をどう動かすべきか
一歩進む：関節を一度動かし、再びステップ1に戻る

誤差が十分小さくなるまで繰り返します。

なぜ広く使われているのか

汎用的：同じループが6軸、7軸、ハンド、スネークアームすべてで機能する
式を導出する必要がない：紙一面の代数ではなく、プログラムである
精度を調整可能：より高精度が欲しければ、反復回数を増やせばよい

産業用途では、このループは実測された関節位置を用いて閉じられ、モデルが少し誤っていても、コントローラは誤差を監視し続けて補正します。この「動きながら見る」バージョンが CLIK（Closed-Loop IK） であり、工場のロボットアームにおける事実上の標準です。

一文で言うと： 数値的IK = 誤差を見ながら少しずつ目標に近づく — 汎用的で、有効かつ安定。

24.7 ヤコビアン行列

ヤコビアンは「姿勢を解く」問題を「速度を解く」問題に変換する。

姿勢方程式は非線形で、通常は閉形式解を持ちません：

f(q) = T_{target}

現在の $q$ でこれを微分するとヤコビアンが得られ、一次近似では問題は線形になります：

J(q) = \frac{\partial f}{\partial q}, \qquad v_{end} = J(q)\,\dot{q}, \qquad \Delta T \approx J(q)\,\Delta q, \qquad \dot{q} = J^{-1} v_{end}

反復ループ

現在の $q$ -> エンドエフェクタ姿勢 $T$ を計算
誤差： $\Delta T = T_{target} - T$
エンドエフェクタ速度: $v = \Delta T / dt$
関節速度: $\dot{q} = J^{-1} v$
更新: $q_{new} = q + \dot{q}\,dt$
$\Delta T$ が十分小さくなるまで 1 に戻る

一文で言うと： ヤコビ行列は非線形な IK 方程式を局所的に「平坦化」して線形化し，その速度を解いて積分することで目標姿勢に収束させる。

24.8 特異姿勢

特異姿勢では，アームはエンドエフェクタをある方向（または複数方向）に動かす能力を失う — 関節をどう動かしても，その方向には動かせない。

アームが完全に伸び切った状態: これ以上外側に届かない；外向きの方向が失われる
2 つの手首関節が同一直線上: 2 つの回転軸が一致し，自由度が 1 つ消える

特異姿勢で何が起こるか

状況	症状
ヤコビ行列が「破綻」する	関節からエンドへの写像がもはや逆行列を持たない
IK 解が発散する	計算された関節速度が無限大に向かう
関節のビクつき	モータが激しく振動する
制御の発振	エンドが前後に飛び跳ねる

本質的には，その写像のいくつかの成分が $0/0$ になってしまう — 単に大きいのではなく未定義になる。

検出方法 — ヤコビ行列が退化するとき，ある 1 つの数値がゼロに近づく：

\det J = 0, \qquad \sigma_{\min}(J) = 0, \qquad \operatorname{rank} J < n

その数値を監視し，ゼロに近づいたらアラームを上げる。

対処方法

対策	考え方
ダンピング付き最小二乗法 (DLS)	解が発散しないように「摩擦」を加える: $\dot{q} = J^\top (J J^\top + \lambda^2 I)^{-1} v$
経路を変える	特異領域を回避するように事前に経路を計画する
減速する	特異姿勢付近で減速し，対処する時間的余裕を持たせる
姿勢を変える	同じ目標姿勢に対して，非特異な解を選ぶ

2 種類の特異姿勢

作業空間の境界: 最遠点まで伸び切っており，いくつかの方向が失われる
作業空間の内部: 手首が一直線 / 肘が伸び切り；自由度が減少する

24.9 解析的 IK と数値的 IK

解析的 IK は方程式を直接解き，数値的 IK は解に向かって反復する。同じタスク — $25 \times 4$ の計算 — を両方の方法で行うと：

	解析的	数値的
考え方	"×4 は ×2 を 2 回" と考え，まず $25 \times 2 = 50$ を求め，次に $\times 2 = 100$ とする	90 と当ててみて 10 足りないと気づき，102 と当ててから 100 にする — 一致するまで調整する
結果	1 ステップで厳密解	数回の反復で十分近い値

本質的な違い

観点	解析的 IK	数値的 IK
アプローチ	方程式を直接解く	反復的な近似
速度	最速（マイクロ秒）	遅い（ms〜s）
精度	厳密	近似（調整可能）
汎用性	低い	高い
出力	閉形式の式	数値解

解析的 IK — 方程式を一度解いてしまえば，次のような公式が得られる：

\theta_1 = \operatorname{atan2}(\ldots), \qquad \theta_2 = \operatorname{acos}(\ldots), \qquad \vdots

長所	短所
1 行のコードで計算できる；反復誤差がない；すべての解を列挙できる	すべてのロボットに閉形式解があるわけではない；関節数が多く構造が複雑だと方程式が解けない；ロボットが変わるたびにすべての式を書き直す必要がある

数値的 IK — 公式は一切使わず，「関節を少し曲げて，エンドが近づいたか確認し，ダメならもう少し曲げる」という操作を，十分近づくまで繰り返すだけである。

長所	短所
どんなロボットにも使える；導出すべき方程式がない；制約を簡単に追加できる	遅い（反復が必要）；誤った解にハマることがある；特異姿勢で発散する

適用シナリオ

シナリオ	選択
2 関節，3 関節，特殊な幾何構造	解析的（高速・厳密）
6 軸の標準的な産業用アーム	どちらも可，シナリオ次第
7 軸の冗長アーム	数値的（解析解なし）
閉ループ / パラレル	数値的（解析解の導出が困難）
リアルタイム制御 (kHz)	解析的またはオフライン生成した解析解
ビジュアルサーボ (30 Hz)	数値的

重要ポイントのまとめ

関節空間はモータが理解する世界であり，デカルト空間はタスクが定義される世界である。
FK は一意で，計算コストが低く，常に解ける；IK には複数解，単一解，あるいは解なしの場合がある。
ヤコビ行列は関節速度をエンドエフェクタ速度に変換し，実際に IK を解くための手段である。
特異姿勢とはヤコビ行列の階数が落ちる姿勢であり；それらを検出（最小特異値）し，ダンピング（DLS）で抑える。
次の章では：姿勢を生成できるようになっても，そこへ滑らかかつ安全に移動しなければならない。

24. 順運動学、逆運動学、ヤコビアン

24.1 主なトピック​

24.2 学習目標​

24.3 関節空間 vs. デカルト空間​

24.4 順運動学 vs. 逆運動学​

24.5 順運動学​

24.6 逆運動学​

24.7 ヤコビアン行列​

24.8 特異姿勢​

24.9 解析的 IK と数値的 IK​