宇宙ステーションの外壁を這うロボットアームが、接近してくるデブリを避けるために手先を素早く横方向に動かしたい場面を想像してください。このとき、各関節のモーターをどのような速度で回転させれば、手先が望む方向に望む速さで動くのでしょうか？ 順運動学で学んだ「関節角から手先位置への写像」だけでは、この問いに答えるには不十分です。位置ではなく速度の関係を結ぶ道具が必要になります。
その道具こそがヤコビ行列（Jacobian matrix）です。ヤコビ行列を理解すると、次のような問題に答えられるようになります。
- 速度制御: 宇宙ロボットがドッキング中にミリ単位の精度で手先速度を制御する
- 衝突回避: 手先だけでなくリンク上の任意の点の速度を計算し、安全な軌道を生成する
- 特異姿勢の解析: ロボットが「動けなくなる」姿勢を事前に予測する
- 力の伝達設計: 関節トルクと手先に発生する力・モーメントの関係を解析する
本記事の内容
- ヤコビ行列の直感的な理解と数学的定義
- 幾何学的ヤコビアンと解析的ヤコビアンの違い
- 回転関節・直動関節ごとのヤコビ列ベクトルの導出
- 2リンク平面マニピュレータでの具体計算
- 仮想仕事の原理に基づく力の伝達
- Pythonでの実装と手先速度ベクトルの可視化
前提知識
この記事を読む前に、以下の記事を読んでおくと理解が深まります。
順運動学の基本（関節角度から手先位置・姿勢を求める方法）を前提とします。また、偏微分と行列演算の基礎知識が必要です。
ヤコビ行列とは — 直感的理解
地図のスケールとしてのヤコビ行列
ヤコビ行列を最初に理解するために、地図のアナロジーを使いましょう。
地図には縮尺（スケール）があります。地図上で1cm動くと実際には100m動く、といった具合です。ただし、地図が等角図法（メルカトル図法など）で描かれている場合、場所によってスケールが変わります。赤道付近と極付近では、地図上の同じ1cmに対応する実際の距離が異なるのです。
ロボットのヤコビ行列は、まさにこの「場所によって変わるスケール」の多次元版です。
- 地図の場合: 地図上の微小移動 → 実際の微小移動（スケールは位置によって変わる）
- ロボットの場合: 関節空間の微小速度 → 作業空間の微小速度（ヤコビ行列は姿勢によって変わる）
つまり、ヤコビ行列は「関節空間と作業空間の間のローカルな変換倍率」を行列の形でまとめたものです。関節を微小に動かしたとき、手先がどの方向にどれだけ動くかを教えてくれます。
もう少し具体的に
2自由度のロボットアームを考えましょう。肩の関節角度 $\theta_1$ と肘の関節角度 $\theta_2$ を持ちます。順運動学により、手先位置 $(x, y)$ は $\theta_1, \theta_2$ の関数です。
ここで素朴な疑問が生まれます。「$\theta_1$ を少し速く回したら、手先は $x$ 方向と $y$ 方向にそれぞれどれだけ速く動くのか？」 この問いへの答えが、まさにヤコビ行列の各成分です。
$$ \begin{pmatrix} \dot{x} \\ \dot{y} \end{pmatrix} = \underbrace{\begin{pmatrix} \dfrac{\partial x}{\partial \theta_1} & \dfrac{\partial x}{\partial \theta_2} \\[8pt] \dfrac{\partial y}{\partial \theta_1} & \dfrac{\partial y}{\partial \theta_2} \end{pmatrix}}_{\text{ヤコビ行列 } \bm{J}} \begin{pmatrix} \dot{\theta}_1 \\ \dot{\theta}_2 \end{pmatrix} $$
行列の $(i, j)$ 成分は、「$j$ 番目の関節速度が $i$ 番目の手先速度成分にどれだけ影響するか」を表しています。しかも、この行列はロボットの姿勢（現在の $\theta_1, \theta_2$）によって値が変わります。腕を伸ばしきった姿勢と、肘を曲げた姿勢では、同じ関節速度に対して手先の動き方が全く異なるのです。
ヤコビ行列の直感を掴んだところで、次に数学的にきちんと定義していきましょう。
ヤコビ行列の数学的定義
順運動学から微分運動学へ
$n$ 自由度のロボットマニピュレータにおいて、順運動学は関節変数のベクトル $\bm{q} = (q_1, q_2, \dots, q_n)^T$ から手先の位置・姿勢を求める写像です。手先の位置を $\bm{p} = (x, y, z)^T$、姿勢を何らかの表現 $\bm{\phi}$（オイラー角など）で表すと、順運動学は次のように書けます。
$$ \bm{x} = \bm{f}(\bm{q}) $$
ここで $\bm{x}$ は手先の位置と姿勢をまとめた作業空間座標ベクトル（一般に $m$ 次元）です。
この写像 $\bm{f}$ の時間微分を取ると、連鎖律（チェインルール）により次の関係が得られます。
$$ \dot{\bm{x}} = \frac{\partial \bm{f}}{\partial \bm{q}} \dot{\bm{q}} $$
この偏微分行列こそがヤコビ行列です。形式的に定義すると次のようになります。
定義
ロボットマニピュレータのヤコビ行列 $\bm{J}(\bm{q})$ は、順運動学写像 $\bm{f}: \mathbb{R}^n \to \mathbb{R}^m$ の $\bm{q}$ におけるヤコビ行列として定義されます。
$$ \bm{J}(\bm{q}) = \frac{\partial \bm{f}}{\partial \bm{q}} = \begin{pmatrix} \dfrac{\partial f_1}{\partial q_1} & \dfrac{\partial f_1}{\partial q_2} & \cdots & \dfrac{\partial f_1}{\partial q_n} \\[8pt] \dfrac{\partial f_2}{\partial q_1} & \dfrac{\partial f_2}{\partial q_2} & \cdots & \dfrac{\partial f_2}{\partial q_n} \\[8pt] \vdots & \vdots & \ddots & \vdots \\[4pt] \dfrac{\partial f_m}{\partial q_1} & \dfrac{\partial f_m}{\partial q_2} & \cdots & \dfrac{\partial f_m}{\partial q_n} \end{pmatrix} $$
これにより、関節速度ベクトル $\dot{\bm{q}} \in \mathbb{R}^n$ と手先速度ベクトル $\dot{\bm{x}} \in \mathbb{R}^m$ の関係は次式で表されます。
$$ \dot{\bm{x}} = \bm{J}(\bm{q}) \, \dot{\bm{q}} $$
この式が微分運動学（differential kinematics）の基本方程式です。
ヤコビ行列の性質
ヤコビ行列について、いくつかの重要な性質を確認しておきましょう。
- サイズ: $\bm{J}$ は $m \times n$ 行列です。3次元空間で位置と姿勢の両方を考える場合、$m = 6$（並進3 + 回転3）です。
- 姿勢依存性: $\bm{J}(\bm{q})$ は関節変数 $\bm{q}$ の関数であり、ロボットの姿勢が変われば値も変わります。
- 線形近似: ヤコビ行列は、関節空間から作業空間への写像の「局所的な線形近似」を与えます。微小変位に対しては $\delta \bm{x} \approx \bm{J} \, \delta \bm{q}$ が成り立ちます。
- 特異点: $\bm{J}$ のランクが落ちる（正方行列の場合は行列式がゼロになる）姿勢を特異姿勢（singularity）と呼びます。特異姿勢では、ある方向への手先速度を生成できなくなります。
ここまでの定義では、手先の「位置と姿勢」を一つのベクトル $\bm{x}$ としてまとめて扱いました。しかし、回転の表現方法によっては微分の取り方に注意が必要です。次に、この点を詳しく見ていきましょう。
幾何学的ヤコビアン vs 解析的ヤコビアン
2種類のヤコビアンが存在する理由
3次元空間で動くロボットの手先速度は、並進速度 $\bm{v} = (\dot{x}, \dot{y}, \dot{z})^T$ と角速度 $\bm{\omega} = (\omega_x, \omega_y, \omega_z)^T$ の6成分で表されます。ところが、この「速度」の定義の仕方には2通りのアプローチがあり、それぞれに対応するヤコビアンが存在します。
幾何学的ヤコビアン（Geometric Jacobian）
幾何学的ヤコビアン $\bm{J}_g(\bm{q})$ は、手先の並進速度 $\bm{v}$ と角速度 $\bm{\omega}$ を直接、関節速度 $\dot{\bm{q}}$ に結びつけます。
$$ \begin{pmatrix} \bm{v} \\ \bm{\omega} \end{pmatrix} = \bm{J}_g(\bm{q}) \, \dot{\bm{q}} $$
ここで $\bm{v}$ は手先位置の時間微分、$\bm{\omega}$ は手先（エンドエフェクタ）に固定された座標系の角速度ベクトルです。幾何学的ヤコビアンは、ロボットの幾何学的構造（リンクの長さ、関節軸の方向など）から直接的に構成できます。姿勢の表現方法に依存しないという利点があります。
解析的ヤコビアン（Analytical Jacobian）
解析的ヤコビアン $\bm{J}_a(\bm{q})$ は、手先の位置 $\bm{p}$ と姿勢パラメータ $\bm{\phi}$（例えばオイラー角 $(\alpha, \beta, \gamma)^T$）をまとめた作業空間座標 $\bm{x} = (\bm{p}^T, \bm{\phi}^T)^T$ の時間微分を、関節速度に結びつけます。
$$ \dot{\bm{x}} = \begin{pmatrix} \dot{\bm{p}} \\ \dot{\bm{\phi}} \end{pmatrix} = \bm{J}_a(\bm{q}) \, \dot{\bm{q}} $$
解析的ヤコビアンは順運動学写像 $\bm{f}$ を直接偏微分して得られます。前節で定義したヤコビ行列そのものです。
2つのヤコビアンの関係
並進速度の部分は両者で同じです（$\dot{\bm{p}} = \bm{v}$）。違いが出るのは回転の部分です。角速度 $\bm{\omega}$ と姿勢パラメータの時間微分 $\dot{\bm{\phi}}$ は一般に一致しません。両者は変換行列 $\bm{T}(\bm{\phi})$ で結ばれます。
$$ \bm{\omega} = \bm{T}(\bm{\phi}) \, \dot{\bm{\phi}} $$
この変換行列 $\bm{T}$ は、姿勢の表現方法（ZYZオイラー角、RPYなど）に依存します。例えばZYZオイラー角 $(\alpha, \beta, \gamma)$ の場合、回転行列の時間微分を計算すると次のようになります。
$$ \bm{T}(\bm{\phi}) = \begin{pmatrix} 0 & -\sin\alpha & \cos\alpha \sin\beta \\ 0 & \cos\alpha & \sin\alpha \sin\beta \\ 1 & 0 & \cos\beta \end{pmatrix} $$
この関係を使うと、幾何学的ヤコビアンと解析的ヤコビアンの関係を書き下せます。ブロック行列の形で表すと次のようになります。
$$ \bm{J}_g = \begin{pmatrix} \bm{I}_3 & \bm{0} \\ \bm{0} & \bm{T}(\bm{\phi}) \end{pmatrix} \bm{J}_a $$
つまり両者は、姿勢パラメータに依存する変換行列で結ばれているのです。
どちらを使うべきか？
実用上は、以下のように使い分けるのが一般的です。
| 用途 | 推奨 | 理由 |
|---|---|---|
| 速度制御・逆運動学 | 幾何学的ヤコビアン | 角速度を直接扱え、姿勢表現の特異点（ジンバルロック等）に影響されない |
| 作業空間の軌道計画 | 解析的ヤコビアン | 作業空間座標の時間微分と直接対応する |
| 力の解析 | 幾何学的ヤコビアン | 仮想仕事の原理との整合性がよい |
宇宙ロボティクスでは、手先の角速度を直接制御する場面が多いため、幾何学的ヤコビアンが主に使われます。以降では、特に断りがない限り幾何学的ヤコビアンを扱います。
2種類のヤコビアンの違いを理解したところで、次に幾何学的ヤコビアンを実際にどう構成するかを見ていきましょう。特に、回転関節と直動関節で列ベクトルの形が異なる点が重要です。
回転関節と直動関節のヤコビ列ベクトル
幾何学的ヤコビアンの列ベクトル構成
幾何学的ヤコビアン $\bm{J}_g$ は $6 \times n$ 行列ですが、これを $n$ 本の列ベクトルに分解して考えると理解しやすくなります。
$$ \bm{J}_g = \begin{pmatrix} \bm{J}_{v} \\ \bm{J}_{\omega} \end{pmatrix} = \begin{pmatrix} \bm{j}_{v_1} & \bm{j}_{v_2} & \cdots & \bm{j}_{v_n} \\ \bm{j}_{\omega_1} & \bm{j}_{\omega_2} & \cdots & \bm{j}_{\omega_n} \end{pmatrix} $$
ここで $\bm{J}_v$ は並進速度に対応する上半分（$3 \times n$）、$\bm{J}_\omega$ は角速度に対応する下半分（$3 \times n$）です。
各列ベクトル $(\bm{j}_{v_i}^T, \bm{j}_{\omega_i}^T)^T$ は「$i$ 番目の関節だけを単位速度で動かしたとき、手先に生じる速度」を意味します。この列ベクトルの形は、関節が回転関節か直動関節かで異なります。
回転関節（Revolute Joint）の場合
$i$ 番目の関節が回転関節の場合を考えましょう。関節 $i$ の回転軸方向の単位ベクトルを $\bm{z}_{i-1}$（基準座標系で見た、リンク $i-1$ の $z$ 軸方向）、関節 $i$ の位置を $\bm{o}_{i-1}$、手先位置を $\bm{o}_n$ とします。
回転関節を角速度 $\dot{q}_i$ で回すと、手先には2つの速度成分が生じます。
角速度成分: 関節軸まわりの回転がそのまま手先の角速度に寄与します。
$$ \bm{j}_{\omega_i} = \bm{z}_{i-1} $$
並進速度成分: 手先は関節軸を中心に回転するため、回転運動の接線方向に並進速度が生じます。回転による接線速度は $\bm{\omega} \times \bm{r}$（角速度ベクトルと腕の長さベクトルの外積）で与えられるので、次のようになります。
$$ \bm{j}_{v_i} = \bm{z}_{i-1} \times (\bm{o}_n – \bm{o}_{i-1}) $$
$\bm{o}_n – \bm{o}_{i-1}$ は関節 $i$ から手先までの位置ベクトルです。外積を取ることで、回転軸に垂直な方向に速度が生じることが表現されています。
まとめると、回転関節 $i$ のヤコビ列ベクトルは次のようになります。
$$ \bm{j}_i^{(\text{R})} = \begin{pmatrix} \bm{z}_{i-1} \times (\bm{o}_n – \bm{o}_{i-1}) \\ \bm{z}_{i-1} \end{pmatrix} $$
直動関節（Prismatic Joint）の場合
$i$ 番目の関節が直動関節の場合はもっと単純です。直動関節はリンクをスライドさせるだけで、回転は起こしません。
角速度成分: 直動関節は回転を生じないので、角速度への寄与はゼロです。
$$ \bm{j}_{\omega_i} = \bm{0} $$
並進速度成分: スライド方向は関節軸 $\bm{z}_{i-1}$ の方向なので、手先にはその方向の並進速度が生じます。
$$ \bm{j}_{v_i} = \bm{z}_{i-1} $$
まとめると、直動関節 $i$ のヤコビ列ベクトルは次のようになります。
$$ \bm{j}_i^{(\text{P})} = \begin{pmatrix} \bm{z}_{i-1} \\ \bm{0} \end{pmatrix} $$
一般的な構成手順
以上をまとめると、$n$ 自由度マニピュレータの幾何学的ヤコビアンは次の手順で構成できます。
- 順運動学を解き、各関節の同次変換行列 $\bm{T}_0^i$（$i = 0, 1, \dots, n$）を求める
- 各 $\bm{T}_0^i$ から関節位置 $\bm{o}_i$ と $z$ 軸方向 $\bm{z}_i$ を抽出する
- 各関節 $i$ について、回転関節なら $\bm{j}_i^{(\text{R})}$、直動関節なら $\bm{j}_i^{(\text{P})}$ を列ベクトルとしてヤコビ行列を構成する
注目すべきは、この手順では偏微分を直接計算する必要がないという点です。順運動学の結果（各座標系の位置と姿勢）だけから幾何学的にヤコビ行列を構成できます。これが「幾何学的」ヤコビアンと呼ばれる理由です。
それでは、この構成手順を具体的な例に適用して理解を深めましょう。最もシンプルな2リンク平面マニピュレータで計算してみます。
具体例: 2リンク平面マニピュレータ
問題設定
2つの回転関節を持つ平面マニピュレータを考えます。リンク1の長さを $l_1$、リンク2の長さを $l_2$、関節角度をそれぞれ $\theta_1$, $\theta_2$ とします。
DHパラメータと順運動学で学んだように、順運動学は次のように与えられます。
$$ x = l_1 \cos\theta_1 + l_2 \cos(\theta_1 + \theta_2) $$
$$ y = l_1 \sin\theta_1 + l_2 \sin(\theta_1 + \theta_2) $$
平面問題なので $z = 0$ であり、姿勢は手先の向き $\phi = \theta_1 + \theta_2$ の1成分だけです。
解析的ヤコビアンの導出
まず、順運動学を直接偏微分する方法でヤコビ行列を求めてみましょう。手先速度は次のように書けます。
$$ \begin{pmatrix} \dot{x} \\ \dot{y} \end{pmatrix} = \bm{J}(\bm{\theta}) \begin{pmatrix} \dot{\theta}_1 \\ \dot{\theta}_2 \end{pmatrix} $$
$\dot{x}$ を求めるために、$x = l_1 \cos\theta_1 + l_2 \cos(\theta_1 + \theta_2)$ を $\theta_1$ で偏微分します。
$$ \frac{\partial x}{\partial \theta_1} = -l_1 \sin\theta_1 – l_2 \sin(\theta_1 + \theta_2) $$
次に $\theta_2$ で偏微分します。$l_1 \cos\theta_1$ は $\theta_2$ に依存しないので消えます。
$$ \frac{\partial x}{\partial \theta_2} = -l_2 \sin(\theta_1 + \theta_2) $$
同様に $y = l_1 \sin\theta_1 + l_2 \sin(\theta_1 + \theta_2)$ を偏微分します。
$\theta_1$ で偏微分すると、
$$ \frac{\partial y}{\partial \theta_1} = l_1 \cos\theta_1 + l_2 \cos(\theta_1 + \theta_2) $$
$\theta_2$ で偏微分すると、
$$ \frac{\partial y}{\partial \theta_2} = l_2 \cos(\theta_1 + \theta_2) $$
これらをまとめると、2リンク平面マニピュレータのヤコビ行列（位置成分のみ）は次のようになります。
$$ \bm{J}(\bm{\theta}) = \begin{pmatrix} -l_1 \sin\theta_1 – l_2 \sin(\theta_1 + \theta_2) & -l_2 \sin(\theta_1 + \theta_2) \\ l_1 \cos\theta_1 + l_2 \cos(\theta_1 + \theta_2) & l_2 \cos(\theta_1 + \theta_2) \end{pmatrix} $$
この行列の各成分の意味を確認してみましょう。
- $(1,1)$ 成分: $\theta_1$ を回すと手先 $x$ 座標が $-l_1 \sin\theta_1 – l_2 \sin(\theta_1+\theta_2)$ の速さで変化する
- $(1,2)$ 成分: $\theta_2$ を回すと手先 $x$ 座標が $-l_2 \sin(\theta_1+\theta_2)$ の速さで変化する
- $(2,1)$ 成分: $\theta_1$ を回すと手先 $y$ 座標が $l_1 \cos\theta_1 + l_2 \cos(\theta_1+\theta_2)$ の速さで変化する
- $(2,2)$ 成分: $\theta_2$ を回すと手先 $y$ 座標が $l_2 \cos(\theta_1+\theta_2)$ の速さで変化する
幾何学的ヤコビアンによる検証
同じ結果を、前節の幾何学的ヤコビアンの公式を使って確認しましょう。平面問題を3次元に拡張して考えます。全ての回転軸は $z$ 軸方向なので $\bm{z}_0 = \bm{z}_1 = (0, 0, 1)^T$ です。
各関節の位置は次のとおりです。
$$ \bm{o}_0 = \begin{pmatrix} 0 \\ 0 \\ 0 \end{pmatrix}, \quad \bm{o}_1 = \begin{pmatrix} l_1 \cos\theta_1 \\ l_1 \sin\theta_1 \\ 0 \end{pmatrix}, \quad \bm{o}_2 = \begin{pmatrix} l_1 \cos\theta_1 + l_2 \cos(\theta_1+\theta_2) \\ l_1 \sin\theta_1 + l_2 \sin(\theta_1+\theta_2) \\ 0 \end{pmatrix} $$
関節1（回転関節）の列ベクトルを求めます。まず並進速度成分 $\bm{z}_0 \times (\bm{o}_2 – \bm{o}_0)$ を計算します。
$$ \bm{z}_0 \times (\bm{o}_2 – \bm{o}_0) = \begin{pmatrix} 0 \\ 0 \\ 1 \end{pmatrix} \times \begin{pmatrix} l_1 c_1 + l_2 c_{12} \\ l_1 s_1 + l_2 s_{12} \\ 0 \end{pmatrix} $$
ここで表記を簡潔にするため、$c_1 = \cos\theta_1$, $s_1 = \sin\theta_1$, $c_{12} = \cos(\theta_1+\theta_2)$, $s_{12} = \sin(\theta_1+\theta_2)$ と略記します。
外積の公式 $\bm{a} \times \bm{b} = (a_2 b_3 – a_3 b_2, \; a_3 b_1 – a_1 b_3, \; a_1 b_2 – a_2 b_1)^T$ を適用すると、
$$ \bm{z}_0 \times (\bm{o}_2 – \bm{o}_0) = \begin{pmatrix} -(l_1 s_1 + l_2 s_{12}) \\ l_1 c_1 + l_2 c_{12} \\ 0 \end{pmatrix} $$
同様に関節2について、$\bm{z}_1 \times (\bm{o}_2 – \bm{o}_1)$ を計算します。
$$ \bm{o}_2 – \bm{o}_1 = \begin{pmatrix} l_2 c_{12} \\ l_2 s_{12} \\ 0 \end{pmatrix} $$
外積を取ると、
$$ \bm{z}_1 \times (\bm{o}_2 – \bm{o}_1) = \begin{pmatrix} 0 \\ 0 \\ 1 \end{pmatrix} \times \begin{pmatrix} l_2 c_{12} \\ l_2 s_{12} \\ 0 \end{pmatrix} = \begin{pmatrix} -l_2 s_{12} \\ l_2 c_{12} \\ 0 \end{pmatrix} $$
$xy$ 平面の成分だけ取り出すと、前節で偏微分から求めたヤコビ行列と完全に一致していることが確認できます。
ヤコビ行列の行列式と特異姿勢
この2リンクマニピュレータのヤコビ行列の行列式を計算してみましょう。$2 \times 2$ 行列の行列式は $\det(\bm{J}) = J_{11} J_{22} – J_{12} J_{21}$ ですので、各成分を代入します。
$$ \det(\bm{J}) = (-l_1 s_1 – l_2 s_{12})(l_2 c_{12}) – (-l_2 s_{12})(l_1 c_1 + l_2 c_{12}) $$
第1項を展開すると、
$$ -l_1 l_2 s_1 c_{12} – l_2^2 s_{12} c_{12} $$
第2項を展開すると、
$$ l_1 l_2 s_{12} c_1 + l_2^2 s_{12} c_{12} $$
両方を加えると $l_2^2$ の項が相殺され、次が残ります。
$$ \det(\bm{J}) = l_1 l_2 (s_{12} c_1 – s_1 c_{12}) $$
ここで三角関数の加法定理 $\sin(A – B) = \sin A \cos B – \cos A \sin B$ を思い出すと、括弧内は $\sin(\theta_1 + \theta_2 – \theta_1) = \sin\theta_2$ に他なりません。
$$ \det(\bm{J}) = l_1 l_2 \sin\theta_2 $$
この結果は非常に明快です。$l_1, l_2 > 0$ なので、行列式がゼロになるのは $\sin\theta_2 = 0$、すなわち $\theta_2 = 0$ または $\theta_2 = \pi$ のときだけです。
- $\theta_2 = 0$: 腕が完全に伸びきった姿勢（外部境界特異点）
- $\theta_2 = \pi$: 腕が完全に折り畳まれた姿勢（内部境界特異点）
これらの特異姿勢では、ヤコビ行列のランクが1に落ち、手先は1方向にしか動けなくなります。直感的にも、腕が伸びきった状態では半径方向に手先を動かすことができないことがわかります。特異姿勢の詳細な解析は、次の記事で扱います。
具体例で計算の流れが掴めたところで、ヤコビ行列のもう一つの重要な側面 — 力の伝達 — について見ていきましょう。
力の伝達 — 仮想仕事の原理との関係
関節トルクと手先力の関係
ヤコビ行列は速度の写像だけでなく、力の写像にも登場します。これは仮想仕事の原理から自然に導かれます。
仮想仕事の原理を直感的に述べると、「系が平衡状態にあるとき、任意の微小な仮想変位に対して、外力がする仕事の総和はゼロである」ということです。ロボットアームの場合、手先に作用する外力 $\bm{F}$（力とモーメントを含む6次元ベクトル）と、各関節のトルク $\bm{\tau}$（$n$ 次元ベクトル）の間で仮想仕事が釣り合う条件を考えます。
手先の微小仮想変位を $\delta \bm{x}$、対応する関節の微小仮想変位を $\delta \bm{q}$ とすると、微分運動学の関係から次が成り立ちます。
$$ \delta \bm{x} = \bm{J} \, \delta \bm{q} $$
手先の外力がする仮想仕事は $\bm{F}^T \delta \bm{x}$、関節トルクがする仮想仕事は $\bm{\tau}^T \delta \bm{q}$ です。仮想仕事の原理により、これらが等しいことを要求します。
$$ \bm{\tau}^T \delta \bm{q} = \bm{F}^T \delta \bm{x} $$
右辺に $\delta \bm{x} = \bm{J} \, \delta \bm{q}$ を代入すると、
$$ \bm{\tau}^T \delta \bm{q} = \bm{F}^T \bm{J} \, \delta \bm{q} $$
この式が任意の $\delta \bm{q}$ に対して成り立つためには、次の条件が必要です。
$$ \bm{\tau}^T = \bm{F}^T \bm{J} $$
両辺を転置すると、最終的に次の関係が得られます。
$$ \bm{\tau} = \bm{J}^T \bm{F} $$
速度と力の双対関係
この結果は非常に美しい双対関係を示しています。
| 量 | 関係式 | ヤコビ行列の役割 |
|---|---|---|
| 速度 | $\dot{\bm{x}} = \bm{J} \, \dot{\bm{q}}$ | 関節速度 → 手先速度（$\bm{J}$ を左から掛ける） |
| 力 | $\bm{\tau} = \bm{J}^T \bm{F}$ | 手先力 → 関節トルク（$\bm{J}^T$ を左から掛ける） |
速度の場合は $\bm{J}$ が関節空間から作業空間への写像を与え、力の場合は $\bm{J}^T$ が作業空間から関節空間への写像を与えます。方向が逆転しているのがポイントです。
この双対性は、エネルギー保存の観点からも理解できます。関節側のパワー $\bm{\tau}^T \dot{\bm{q}}$ と手先側のパワー $\bm{F}^T \dot{\bm{x}}$ は等しくなければなりません（理想的なロボット、つまりエネルギー損失がない場合）。
$$ \bm{\tau}^T \dot{\bm{q}} = \bm{F}^T \dot{\bm{x}} = \bm{F}^T \bm{J} \dot{\bm{q}} $$
これが任意の $\dot{\bm{q}}$ で成り立つことから $\bm{\tau} = \bm{J}^T \bm{F}$ が直ちに導かれます。
宇宙ロボットにおける力の伝達の重要性
宇宙ロボティクスでは、力の伝達は特に重要です。
ドッキング操作: 衛星とドッキングする際、手先に生じる接触力が各関節にどのようなトルクを要求するかを事前に計算する必要があります。関節モーターのトルク限界を超えないようにミッションを設計するためです。
フリーフライングロボット: 宇宙空間ではベースが固定されていないため、手先に力を加えると反作用でベース衛星自体も動きます。このとき、ヤコビ行列はベースの運動も含めた拡張形で定式化されます（一般化ヤコビアン）。
力制御: 手先にセンサがない場合でも、関節トルクの測定値と $\bm{J}^T$ の逆を使って手先力を推定できます（ただし $\bm{J}^T$ が逆行列を持つ場合に限ります）。
力の伝達まで理解できたところで、ここまでの理論をPythonで実装してみましょう。実際に数値計算をして、ヤコビ行列が姿勢によってどう変わるかを可視化します。
Pythonでの実装と可視化
2リンクマニピュレータのヤコビ行列計算
まず、2リンク平面マニピュレータのヤコビ行列を計算し、手先速度ベクトルを可視化する関数を実装します。
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import FancyArrowPatch
def forward_kinematics_2link(theta1, theta2, l1, l2):
"""2リンクマニピュレータの順運動学"""
x = l1 * np.cos(theta1) + l2 * np.cos(theta1 + theta2)
y = l1 * np.sin(theta1) + l2 * np.sin(theta1 + theta2)
return x, y
def jacobian_2link(theta1, theta2, l1, l2):
"""2リンクマニピュレータのヤコビ行列（2x2, 位置成分のみ）"""
s1 = np.sin(theta1)
c1 = np.cos(theta1)
s12 = np.sin(theta1 + theta2)
c12 = np.cos(theta1 + theta2)
J = np.array([
[-l1 * s1 - l2 * s12, -l2 * s12],
[ l1 * c1 + l2 * c12, l2 * c12]
])
return J
def draw_manipulator(ax, theta1, theta2, l1, l2, color='steelblue'):
"""マニピュレータの描画"""
# 関節位置
x0, y0 = 0, 0
x1 = l1 * np.cos(theta1)
y1 = l1 * np.sin(theta1)
x2, y2 = forward_kinematics_2link(theta1, theta2, l1, l2)
# リンク描画
ax.plot([x0, x1], [y0, y1], '-o', color=color, linewidth=3,
markersize=8, markerfacecolor='white', markeredgewidth=2)
ax.plot([x1, x2], [y1, y2], '-o', color=color, linewidth=3,
markersize=8, markerfacecolor='white', markeredgewidth=2)
ax.plot(x0, y0, 's', color='black', markersize=10) # ベース
ax.plot(x2, y2, 'o', color='red', markersize=10) # 手先
return x2, y2
このコードでは、2リンクマニピュレータの順運動学、ヤコビ行列の計算、そしてリンクの描画のための3つの関数を定義しています。ヤコビ行列の計算では、先ほど解析的に導出した式をそのまま実装しています。
関節速度に対する手先速度ベクトルの可視化
次に、ある姿勢における手先速度ベクトルを可視化します。各関節を単位角速度で個別に回転させたとき、手先がどの方向にどれだけ動くかを矢印で表示します。
import numpy as np
import matplotlib.pyplot as plt
def forward_kinematics_2link(theta1, theta2, l1, l2):
"""2リンクマニピュレータの順運動学"""
x = l1 * np.cos(theta1) + l2 * np.cos(theta1 + theta2)
y = l1 * np.sin(theta1) + l2 * np.sin(theta1 + theta2)
return x, y
def jacobian_2link(theta1, theta2, l1, l2):
"""2リンクマニピュレータのヤコビ行列（2x2, 位置成分のみ）"""
s1 = np.sin(theta1)
c1 = np.cos(theta1)
s12 = np.sin(theta1 + theta2)
c12 = np.cos(theta1 + theta2)
J = np.array([
[-l1 * s1 - l2 * s12, -l2 * s12],
[ l1 * c1 + l2 * c12, l2 * c12]
])
return J
# パラメータ設定
l1, l2 = 1.0, 0.8
theta1 = np.radians(45)
theta2 = np.radians(60)
# ヤコビ行列を計算
J = jacobian_2link(theta1, theta2, l1, l2)
print("ヤコビ行列 J =")
print(np.round(J, 4))
print(f"\ndet(J) = {np.linalg.det(J):.4f}")
print(f"l1 * l2 * sin(theta2) = {l1 * l2 * np.sin(theta2):.4f}")
# 手先位置
ex, ey = forward_kinematics_2link(theta1, theta2, l1, l2)
# 可視化
fig, ax = plt.subplots(1, 1, figsize=(8, 8))
# マニピュレータ描画
x0, y0 = 0, 0
x1 = l1 * np.cos(theta1)
y1 = l1 * np.sin(theta1)
ax.plot([x0, x1], [y0, y1], '-o', color='steelblue', linewidth=3,
markersize=8, markerfacecolor='white', markeredgewidth=2)
ax.plot([x1, ex], [y1, ey], '-o', color='steelblue', linewidth=3,
markersize=8, markerfacecolor='white', markeredgewidth=2)
ax.plot(x0, y0, 's', color='black', markersize=10)
ax.plot(ex, ey, 'o', color='red', markersize=10)
# ヤコビ行列の列ベクトル（= 各関節を単位速度で回したときの手先速度）
scale = 0.5 # 矢印のスケール
j1 = J[:, 0] # 第1列: theta1の影響
j2 = J[:, 1] # 第2列: theta2の影響
ax.annotate('', xy=(ex + scale * j1[0], ey + scale * j1[1]),
xytext=(ex, ey),
arrowprops=dict(arrowstyle='->', color='orangered', lw=2.5))
ax.annotate('', xy=(ex + scale * j2[0], ey + scale * j2[1]),
xytext=(ex, ey),
arrowprops=dict(arrowstyle='->', color='forestgreen', lw=2.5))
ax.text(ex + scale * j1[0] + 0.05, ey + scale * j1[1] + 0.05,
r'$\mathbf{j}_1$ ($\dot{\theta}_1$)', color='orangered', fontsize=12)
ax.text(ex + scale * j2[0] + 0.05, ey + scale * j2[1] - 0.08,
r'$\mathbf{j}_2$ ($\dot{\theta}_2$)', color='forestgreen', fontsize=12)
ax.set_xlim(-0.5, 2.2)
ax.set_ylim(-0.5, 2.2)
ax.set_aspect('equal')
ax.grid(True, alpha=0.3)
ax.set_xlabel('x [m]', fontsize=12)
ax.set_ylabel('y [m]', fontsize=12)
ax.set_title(r'Jacobian Column Vectors at $\theta_1=45°$, $\theta_2=60°$', fontsize=13)
plt.tight_layout()
plt.show()
このグラフでは、赤い矢印 $\bm{j}_1$ が「関節1を単位角速度で回したときの手先速度ベクトル」、緑の矢印 $\bm{j}_2$ が「関節2を単位角速度で回したときの手先速度ベクトル」を表しています。$\bm{j}_1$ は関節1（原点）からの腕の長さが大きいため $\bm{j}_2$ より長くなっていることがわかります。また、両方の矢印がリンクに垂直な方向を向いており、回転関節の特性（回転軸と腕のベクトルの外積方向に速度が生じる）が確認できます。行列式の値が l1 * l2 * sin(theta2) と一致していることも、理論の正しさを裏付けています。
姿勢変化に伴うヤコビ行列の変化
次に、$\theta_2$ を変化させたときにヤコビ行列の列ベクトルがどのように変わるかを複数姿勢で可視化します。特に特異姿勢に近づくと何が起きるかに注目しましょう。
import numpy as np
import matplotlib.pyplot as plt
def forward_kinematics_2link(theta1, theta2, l1, l2):
x = l1 * np.cos(theta1) + l2 * np.cos(theta1 + theta2)
y = l1 * np.sin(theta1) + l2 * np.sin(theta1 + theta2)
return x, y
def jacobian_2link(theta1, theta2, l1, l2):
s1 = np.sin(theta1)
c1 = np.cos(theta1)
s12 = np.sin(theta1 + theta2)
c12 = np.cos(theta1 + theta2)
J = np.array([
[-l1 * s1 - l2 * s12, -l2 * s12],
[ l1 * c1 + l2 * c12, l2 * c12]
])
return J
l1, l2 = 1.0, 0.8
theta1 = np.radians(30)
theta2_list = [np.radians(a) for a in [90, 45, 10, 0]]
labels = [r'$\theta_2 = 90°$', r'$\theta_2 = 45°$',
r'$\theta_2 = 10°$', r'$\theta_2 = 0°$ (singular)']
fig, axes = plt.subplots(2, 2, figsize=(12, 12))
axes = axes.flatten()
for idx, (th2, label) in enumerate(zip(theta2_list, labels)):
ax = axes[idx]
J = jacobian_2link(theta1, th2, l1, l2)
det_val = np.linalg.det(J)
# マニピュレータ描画
x0, y0 = 0, 0
x1 = l1 * np.cos(theta1)
y1 = l1 * np.sin(theta1)
ex, ey = forward_kinematics_2link(theta1, th2, l1, l2)
ax.plot([x0, x1], [y0, y1], '-o', color='steelblue', linewidth=3,
markersize=8, markerfacecolor='white', markeredgewidth=2)
ax.plot([x1, ex], [y1, ey], '-o', color='steelblue', linewidth=3,
markersize=8, markerfacecolor='white', markeredgewidth=2)
ax.plot(x0, y0, 's', color='black', markersize=10)
ax.plot(ex, ey, 'o', color='red', markersize=10)
# ヤコビ列ベクトル
scale = 0.4
j1 = J[:, 0]
j2 = J[:, 1]
ax.annotate('', xy=(ex + scale * j1[0], ey + scale * j1[1]),
xytext=(ex, ey),
arrowprops=dict(arrowstyle='->', color='orangered', lw=2.5))
ax.annotate('', xy=(ex + scale * j2[0], ey + scale * j2[1]),
xytext=(ex, ey),
arrowprops=dict(arrowstyle='->', color='forestgreen', lw=2.5))
ax.set_xlim(-1.0, 2.5)
ax.set_ylim(-0.5, 2.5)
ax.set_aspect('equal')
ax.grid(True, alpha=0.3)
ax.set_title(f'{label}\ndet(J) = {det_val:.4f}', fontsize=12)
ax.set_xlabel('x [m]')
ax.set_ylabel('y [m]')
plt.suptitle('Jacobian Column Vectors for Different Configurations', fontsize=14, y=1.01)
plt.tight_layout()
plt.show()
この4つのプロットから、極めて重要なことが読み取れます。
- $\theta_2 = 90°$ のとき: 2つの列ベクトル $\bm{j}_1$ と $\bm{j}_2$ は異なる方向を向いており、行列式は $l_1 l_2 \sin 90° = 0.8$ と最大値をとります。手先はあらゆる方向に自由に速度を生成できます。
- $\theta_2$ が小さくなるにつれて: 2つの列ベクトルの方向が近づいていきます。行列式の絶対値が小さくなり、ある方向への速度生成能力が低下します。
- $\theta_2 = 0°$（特異姿勢）のとき: 2つの列ベクトルが完全に同一方向を向き、行列式はゼロになります。手先は腕に垂直な方向にしか動けず、腕に沿った方向（半径方向）への速度を生成する手段がなくなります。
このように、ヤコビ行列の列ベクトルの幾何学的配置が特異姿勢の本質を直感的に説明してくれます。
手先速度楕円の可視化
ヤコビ行列が姿勢によってどう変わるかをさらに理解するために、速度楕円（velocity ellipsoid）を描いてみましょう。関節速度の大きさが一定（$\|\dot{\bm{q}}\| \le 1$）という制約のもとで、手先がどの方向にどれだけの速度を出せるかを表す楕円です。
import numpy as np
import matplotlib.pyplot as plt
def forward_kinematics_2link(theta1, theta2, l1, l2):
x = l1 * np.cos(theta1) + l2 * np.cos(theta1 + theta2)
y = l1 * np.sin(theta1) + l2 * np.sin(theta1 + theta2)
return x, y
def jacobian_2link(theta1, theta2, l1, l2):
s1 = np.sin(theta1)
c1 = np.cos(theta1)
s12 = np.sin(theta1 + theta2)
c12 = np.cos(theta1 + theta2)
J = np.array([
[-l1 * s1 - l2 * s12, -l2 * s12],
[ l1 * c1 + l2 * c12, l2 * c12]
])
return J
l1, l2 = 1.0, 0.8
theta1 = np.radians(45)
theta2_list = [np.radians(a) for a in [90, 60, 30, 5]]
labels = [r'$\theta_2 = 90°$', r'$\theta_2 = 60°$',
r'$\theta_2 = 30°$', r'$\theta_2 = 5°$']
fig, axes = plt.subplots(2, 2, figsize=(12, 12))
axes = axes.flatten()
# 単位円上の点（関節速度空間）
phi = np.linspace(0, 2 * np.pi, 200)
unit_circle = np.array([np.cos(phi), np.sin(phi)]) # (2, 200)
for idx, (th2, label) in enumerate(zip(theta2_list, labels)):
ax = axes[idx]
J = jacobian_2link(theta1, th2, l1, l2)
det_val = np.linalg.det(J)
# 手先位置
ex, ey = forward_kinematics_2link(theta1, th2, l1, l2)
# マニピュレータ描画
x1 = l1 * np.cos(theta1)
y1 = l1 * np.sin(theta1)
ax.plot([0, x1], [0, y1], '-o', color='steelblue', linewidth=3,
markersize=8, markerfacecolor='white', markeredgewidth=2)
ax.plot([x1, ex], [y1, ey], '-o', color='steelblue', linewidth=3,
markersize=8, markerfacecolor='white', markeredgewidth=2)
ax.plot(0, 0, 's', color='black', markersize=10)
ax.plot(ex, ey, 'o', color='red', markersize=10)
# 速度楕円: J @ unit_circle
ellipse = J @ unit_circle # (2, 200)
scale = 0.3
ax.plot(ex + scale * ellipse[0], ey + scale * ellipse[1],
'-', color='purple', linewidth=2, alpha=0.8)
ax.fill(ex + scale * ellipse[0], ey + scale * ellipse[1],
color='purple', alpha=0.1)
# 特異値を計算
U, sigma, Vt = np.linalg.svd(J)
ax.set_xlim(-1.0, 2.5)
ax.set_ylim(-0.5, 2.5)
ax.set_aspect('equal')
ax.grid(True, alpha=0.3)
ax.set_title(f'{label}\ndet(J) = {det_val:.3f}, '
+ r'$\sigma_1$=' + f'{sigma[0]:.3f}, '
+ r'$\sigma_2$=' + f'{sigma[1]:.3f}', fontsize=11)
ax.set_xlabel('x [m]')
ax.set_ylabel('y [m]')
plt.suptitle('Velocity Ellipses for Different Configurations', fontsize=14, y=1.01)
plt.tight_layout()
plt.show()
速度楕円のプロットから、以下のことが読み取れます。
- $\theta_2 = 90°$: 楕円は比較的円に近く、手先はどの方向にもバランスよく速度を出せます。2つの特異値 $\sigma_1, \sigma_2$ がともに大きく、その比も小さいです。
- $\theta_2$ が小さくなる: 楕円が細長くなっていきます。長軸方向には速い速度を出せますが、短軸方向の速度生成能力が急激に低下します。特異値の比 $\sigma_1 / \sigma_2$（条件数）が大きくなります。
- $\theta_2 = 5°$（ほぼ特異姿勢）: 楕円は極めて扁平な形状になり、事実上1方向にしか動けない状態に近づいています。$\sigma_2$ がほぼゼロに近くなり、短軸方向に速度をわずかでも出そうとすると、非常に大きな関節速度が必要になります。
速度楕円は、ロボットが特定の姿勢でどの方向にどれだけ「動きやすいか」を一目で把握できる強力なツールです。宇宙ロボットの軌道計画では、この楕円がなるべく等方的（円に近い）な姿勢を維持するように制御することが、安全な操作のために重要です。
力楕円の可視化
速度と力の双対性を確認するため、力楕円も描いてみましょう。関節トルクの大きさが一定（$\|\bm{\tau}\| \le 1$）という制約のもとで、手先にどの方向にどれだけの力を発生できるかを表す楕円です。
import numpy as np
import matplotlib.pyplot as plt
def forward_kinematics_2link(theta1, theta2, l1, l2):
x = l1 * np.cos(theta1) + l2 * np.cos(theta1 + theta2)
y = l1 * np.sin(theta1) + l2 * np.sin(theta1 + theta2)
return x, y
def jacobian_2link(theta1, theta2, l1, l2):
s1 = np.sin(theta1)
c1 = np.cos(theta1)
s12 = np.sin(theta1 + theta2)
c12 = np.cos(theta1 + theta2)
J = np.array([
[-l1 * s1 - l2 * s12, -l2 * s12],
[ l1 * c1 + l2 * c12, l2 * c12]
])
return J
l1, l2 = 1.0, 0.8
theta1 = np.radians(45)
theta2 = np.radians(60)
J = jacobian_2link(theta1, theta2, l1, l2)
ex, ey = forward_kinematics_2link(theta1, theta2, l1, l2)
# 単位円上の点
phi = np.linspace(0, 2 * np.pi, 200)
unit_circle = np.array([np.cos(phi), np.sin(phi)])
# 速度楕円: dot_x = J @ dot_q → 単位関節速度 → 手先速度
vel_ellipse = J @ unit_circle
# 力楕円: tau = J^T @ F → 単位トルク → 手先力
# F = J^{-T} @ tau なので: 単位トルクの像は J^{-T} @ unit_circle
J_inv_T = np.linalg.inv(J).T
force_ellipse = J_inv_T @ unit_circle
fig, axes = plt.subplots(1, 2, figsize=(14, 7))
# マニピュレータ描画（共通）
for ax in axes:
x1 = l1 * np.cos(theta1)
y1 = l1 * np.sin(theta1)
ax.plot([0, x1], [0, y1], '-o', color='steelblue', linewidth=3,
markersize=8, markerfacecolor='white', markeredgewidth=2)
ax.plot([x1, ex], [y1, ey], '-o', color='steelblue', linewidth=3,
markersize=8, markerfacecolor='white', markeredgewidth=2)
ax.plot(0, 0, 's', color='black', markersize=10)
ax.plot(ex, ey, 'o', color='red', markersize=10)
# 速度楕円
scale_v = 0.3
axes[0].plot(ex + scale_v * vel_ellipse[0], ey + scale_v * vel_ellipse[1],
'-', color='purple', linewidth=2)
axes[0].fill(ex + scale_v * vel_ellipse[0], ey + scale_v * vel_ellipse[1],
color='purple', alpha=0.15)
axes[0].set_title('Velocity Ellipse', fontsize=13)
# 力楕円
scale_f = 0.3
axes[1].plot(ex + scale_f * force_ellipse[0], ey + scale_f * force_ellipse[1],
'-', color='darkorange', linewidth=2)
axes[1].fill(ex + scale_f * force_ellipse[0], ey + scale_f * force_ellipse[1],
color='darkorange', alpha=0.15)
axes[1].set_title('Force Ellipse', fontsize=13)
for ax in axes:
ax.set_xlim(-0.5, 2.2)
ax.set_ylim(-0.5, 2.2)
ax.set_aspect('equal')
ax.grid(True, alpha=0.3)
ax.set_xlabel('x [m]', fontsize=12)
ax.set_ylabel('y [m]', fontsize=12)
plt.suptitle(r'Velocity vs Force Ellipses ($\theta_1=45°$, $\theta_2=60°$)',
fontsize=14)
plt.tight_layout()
plt.show()
速度楕円と力楕円を並べて比較すると、非常に興味深い関係が見えます。速度楕円の長軸方向と力楕円の短軸方向が一致しているのです。これは「速度を出しやすい方向では力が出しにくく、速度を出しにくい方向では力が出しやすい」という、速度と力のトレードオフを意味しています。
数学的には、速度楕円の主軸はヤコビ行列の特異値分解から得られる左特異ベクトルの方向で、その長さは特異値 $\sigma_i$ に比例します。一方、力楕円の主軸長は特異値の逆数 $1/\sigma_i$ に比例します。したがって、速度楕円が扁平な方向（$\sigma$ が小さい方向）では力楕円が膨らみ、逆もまた然りです。
このトレードオフはギア比のアナロジーで理解できます。低速ギアでは大きなトルクが出せますが速度は遅く、高速ギアでは速度は出るがトルクは小さい — これと同じ原理が、ロボットの姿勢依存的に方向ごとに生じているのです。
一般的なnリンクマニピュレータのヤコビ行列計算
最後に、より一般的な $n$ リンクマニピュレータに対応するヤコビ行列計算のコードを示します。DH パラメータから順運動学とヤコビ行列を計算する汎用関数です。
import numpy as np
def dh_transform(theta, d, a, alpha):
"""DHパラメータから同次変換行列を計算"""
ct, st = np.cos(theta), np.sin(theta)
ca, sa = np.cos(alpha), np.sin(alpha)
T = np.array([
[ct, -st * ca, st * sa, a * ct],
[st, ct * ca, -ct * sa, a * st],
[0, sa, ca, d ],
[0, 0, 0, 1 ]
])
return T
def compute_geometric_jacobian(dh_params, joint_types):
"""
幾何学的ヤコビアンを計算する汎用関数
Parameters
----------
dh_params : list of (theta, d, a, alpha)
各関節のDHパラメータ
joint_types : list of str
'R' (revolute) or 'P' (prismatic) for each joint
Returns
-------
J : ndarray, shape (6, n)
幾何学的ヤコビアン
T_all : list of ndarray
各フレームの同次変換行列（ベースから見た）
"""
n = len(dh_params)
# 各フレームの同次変換行列を計算
T_all = [np.eye(4)] # T_0^0 = I
T_current = np.eye(4)
for i in range(n):
T_i = dh_transform(*dh_params[i])
T_current = T_current @ T_i
T_all.append(T_current.copy())
# 手先位置
o_n = T_all[-1][:3, 3]
# ヤコビ行列の構成
J = np.zeros((6, n))
for i in range(n):
z_i = T_all[i][:3, 2] # i番目フレームのz軸方向
o_i = T_all[i][:3, 3] # i番目フレームの原点位置
if joint_types[i] == 'R':
# 回転関節
J[:3, i] = np.cross(z_i, o_n - o_i) # 並進速度
J[3:, i] = z_i # 角速度
elif joint_types[i] == 'P':
# 直動関節
J[:3, i] = z_i # 並進速度
J[3:, i] = 0 # 角速度なし
return J, T_all
この汎用関数を使って、3リンク平面マニピュレータのヤコビ行列を計算してみましょう。
import numpy as np
def dh_transform(theta, d, a, alpha):
ct, st = np.cos(theta), np.sin(theta)
ca, sa = np.cos(alpha), np.sin(alpha)
T = np.array([
[ct, -st * ca, st * sa, a * ct],
[st, ct * ca, -ct * sa, a * st],
[0, sa, ca, d ],
[0, 0, 0, 1 ]
])
return T
def compute_geometric_jacobian(dh_params, joint_types):
n = len(dh_params)
T_all = [np.eye(4)]
T_current = np.eye(4)
for i in range(n):
T_i = dh_transform(*dh_params[i])
T_current = T_current @ T_i
T_all.append(T_current.copy())
o_n = T_all[-1][:3, 3]
J = np.zeros((6, n))
for i in range(n):
z_i = T_all[i][:3, 2]
o_i = T_all[i][:3, 3]
if joint_types[i] == 'R':
J[:3, i] = np.cross(z_i, o_n - o_i)
J[3:, i] = z_i
elif joint_types[i] == 'P':
J[:3, i] = z_i
J[3:, i] = 0
return J, T_all
# 3リンク平面マニピュレータのDHパラメータ
l1, l2, l3 = 1.0, 0.8, 0.5
theta = [np.radians(30), np.radians(45), np.radians(-30)]
dh_params = [
(theta[0], 0, l1, 0),
(theta[1], 0, l2, 0),
(theta[2], 0, l3, 0),
]
joint_types = ['R', 'R', 'R']
J, T_all = compute_geometric_jacobian(dh_params, joint_types)
print("3リンク平面マニピュレータの幾何学的ヤコビアン (6x3):")
print(np.round(J, 4))
print(f"\n手先位置: {np.round(T_all[-1][:3, 3], 4)}")
print(f"\nJ_v (並進速度, 上3行):")
print(np.round(J[:3, :], 4))
print(f"\nJ_omega (角速度, 下3行):")
print(np.round(J[3:, :], 4))
# ランク確認
rank = np.linalg.matrix_rank(J)
print(f"\nヤコビ行列のランク: {rank}")
出力結果から、3リンク平面マニピュレータの幾何学的ヤコビアンの構造が確認できます。平面問題であるため、$J_v$ の第3行（$z$ 方向の並進速度）は全てゼロ、$J_\omega$ の第1行と第2行（$x, y$ 軸まわりの角速度）も全てゼロになっています。有効な成分は $J_v$ の $xy$ 成分（2行）と $J_\omega$ の $z$ 成分（1行）の計3行であり、3自由度に対応しています。これは平面マニピュレータの性質を正しく反映しています。
ヤコビ行列の応用 — 逆速度運動学への橋渡し
逆速度問題
ここまでで「関節速度 → 手先速度」の順方向の写像を扱ってきましたが、実用上は逆問題、つまり「望む手先速度を実現する関節速度」を求めたい場面のほうが多いです。
$$ \dot{\bm{q}} = \bm{J}^{-1}(\bm{q}) \, \dot{\bm{x}} $$
$\bm{J}$ が正方かつ正則（行列式がゼロでない）であれば、この逆行列が存在し、一意の解が得られます。2リンク平面マニピュレータの場合、$\bm{J}$ は $2 \times 2$ の正方行列なので、$\det(\bm{J}) = l_1 l_2 \sin\theta_2 \neq 0$ である限り逆行列が存在します。
冗長マニピュレータの場合
宇宙ステーションのロボットアーム（例えば7自由度のカナダアーム2）のように、関節数 $n$ が作業空間の次元 $m$ より大きいマニピュレータを冗長マニピュレータと呼びます。この場合、$\bm{J}$ は $m \times n$（$m < n$）の横長行列であり、逆行列は存在しません。
その代わりに、擬似逆行列（Moore-Penrose pseudo-inverse） $\bm{J}^+$ を使います。
$$ \dot{\bm{q}} = \bm{J}^+ \dot{\bm{x}} + (\bm{I} – \bm{J}^+ \bm{J}) \bm{z} $$
第1項 $\bm{J}^+ \dot{\bm{x}}$ は望む手先速度を実現する最小ノルム解、第2項 $(\bm{I} – \bm{J}^+ \bm{J}) \bm{z}$ はヤコビ行列の零空間への射影です。零空間成分は手先速度に影響を与えないため、障害物回避や関節限界の回避など、副次的な目的に利用できます。
宇宙ロボティクスでは、この冗長性を利用してベース衛星への反力を最小化することが重要な課題です。宇宙空間では反力がベースの姿勢を乱し、太陽電池パネルの向きや通信アンテナの指向に影響を与えるためです。
特異点の問題
$\bm{J}$ のランクが落ちる特異姿勢では、$\bm{J}^{-1}$ も $\bm{J}^+$ も数値的に不安定になります。特異点近傍では、わずかな手先速度を実現するために非常に大きな関節速度が必要になり、現実のモーターでは実現不可能な値を出力してしまいます。
この問題に対処するために、ダンプド最小二乗法（Damped Least Squares, DLS）が広く使われます。
$$ \dot{\bm{q}} = \bm{J}^T (\bm{J} \bm{J}^T + \lambda^2 \bm{I})^{-1} \dot{\bm{x}} $$
ダンピング係数 $\lambda$ を導入することで、特異点近傍での関節速度を抑制できます。$\lambda$ が大きいほど安定しますが、手先の追従精度は低下します。特異点の詳細な解析と可操作度については、次の記事で詳しく扱います。
まとめ
本記事では、ロボットマニピュレータのヤコビ行列と微分運動学について解説しました。
- ヤコビ行列は順運動学の偏微分として定義され、関節速度と手先速度の間のローカルな線形写像を与える
- 幾何学的ヤコビアンは角速度を直接扱い、ロボットの幾何学的構造から構成できる。解析的ヤコビアンは姿勢パラメータの時間微分を扱い、両者は変換行列で結ばれる
- 回転関節の列ベクトルは $\bm{z}_{i-1} \times (\bm{o}_n – \bm{o}_{i-1})$ と $\bm{z}_{i-1}$、直動関節は $\bm{z}_{i-1}$ と $\bm{0}$ で構成される
- 2リンクマニピュレータでは $\det(\bm{J}) = l_1 l_2 \sin\theta_2$ となり、腕が伸びきった/折り畳まれた姿勢で特異点となる
- 仮想仕事の原理により、手先力と関節トルクは $\bm{\tau} = \bm{J}^T \bm{F}$ で結ばれ、速度と力の美しい双対関係がある
- 速度楕円と力楕円は直交的な関係にあり、「速度を出しやすい方向は力が出しにくい」というトレードオフが存在する
ヤコビ行列は、速度制御、逆運動学の数値解法、力制御、可操作性解析など、ロボティクスのほぼ全ての応用分野で基盤となる道具です。特に宇宙ロボティクスでは、特異姿勢の回避とベース衛星への反力管理が安全な運用の鍵を握っており、ヤコビ行列の深い理解が不可欠です。
次の記事では、ヤコビ行列から自然に導かれる特異点解析と可操作度について詳しく解説します。特異姿勢の分類、可操作度楕円体、そして冗長マニピュレータにおける特異点回避戦略を扱います。