国際宇宙ステーション（ISS）のロボットアーム「カナダアーム2」は、全長約17メートルの7自由度マニピュレータです。宇宙飛行士はこのアームを操作して、数トンの実験モジュールをミリメートル単位の精度でドッキングポートに接続します。このとき、アームの先端（エンドエフェクタ）が今どこにあり、どの方向を向いているかを正確に知る必要があります。
では、「アームの先端の位置と向き」をどのように数学的に表現すればよいのでしょうか？
位置だけであれば3次元ベクトル $(x, y, z)$ で十分です。しかし、向き（姿勢）を表現するには、もうひと工夫が必要です。たとえば、グリッパーが「上を向いている」のか「横を向いている」のかは、位置ベクトルだけでは記述できません。ここで登場するのが回転行列と同次変換行列です。
回転行列と同次変換行列を理解すると、以下のようなことが可能になります。
- ロボットアームの順運動学: 各関節の角度から、アーム先端の位置・姿勢を計算する
- 衛星の姿勢制御: 地球観測衛星のカメラがどの方向を向いているかを記述する
- 3Dコンピュータグラフィックス: ゲームやシミュレーションでオブジェクトを回転・移動させる
- 自動運転: LiDARやカメラの座標系を車体座標系に変換する
本記事の内容
- 回転行列の直感的な理解と数学的定義（SO(3)群）
- 基本回転行列 $\bm{R}_x, \bm{R}_y, \bm{R}_z$ の導出
- 回転行列の合成と順序依存性
- 同次変換行列の定義と使い方
- 同次変換行列の逆変換
- 連鎖的な座標変換（複数フレームの接続）
- Pythonでの実装と3D可視化
本記事は宇宙ロボティクスシリーズの第1回として、以降のすべての記事の数学的基盤となります。
前提知識
この記事を読む前に、以下の知識があると理解が深まります。
- 行列の積と逆行列 — 行列の基本演算
- ベクトルの内積と外積 — 3次元ベクトルの幾何学的意味
特に、行列とベクトルの積 $\bm{A}\bm{x}$ が「ベクトル $\bm{x}$ を線形変換する」という見方に慣れていると、本記事の内容がスムーズに理解できます。
座標系と剛体の姿勢
なぜ座標系が必要なのか
物体の位置や姿勢を語るとき、必ず「何に対して」という基準が必要です。たとえば、「ISSのロボットアームの先端がISSの重心から5メートル先にある」と言うとき、ISS本体に固定された座標系が基準になっています。一方、「地球の表面から400キロメートル上空にある」と言うとき、基準は地球に固定された座標系です。
ロボティクスでは、こうした基準となる座標系をフレーム（frame）と呼びます。3次元空間のフレームは、原点と3本の直交する軸（$x, y, z$）で定義されます。
剛体とフレーム
剛体とは、力を加えても変形しない理想的な物体です。剛体に固定されたフレームを考えると、剛体の運動はそのフレームの運動として記述できます。具体的には、剛体の運動は次の2つの要素に分解できます。
- 並進（translation）: フレームの原点の移動
- 回転（rotation）: フレームの軸の向きの変化
ロボットアームのリンク（各節の部品）は剛体として扱えるため、各リンクにフレームを固定し、隣り合うフレーム間の関係を「並進＋回転」で記述することが、ロボティクスの基本的なアプローチです。
ここで自然な疑問が生まれます。回転を数学的にどう表現するのか？ 行列を使えば、回転をきわめてエレガントに記述できます。
回転行列とは
回転の直感的な理解
まず2次元で考えてみましょう。紙の上に座標軸を描き、ベクトル $\bm{p} = (1, 0)^\top$ を反時計回りに角度 $\theta$ だけ回転させると、新しいベクトルは $(\cos\theta, \sin\theta)^\top$ になります。この操作を行列で書くと
$$ \begin{pmatrix} \cos\theta \\ \sin\theta \end{pmatrix} = \begin{pmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{pmatrix} \begin{pmatrix} 1 \\ 0 \end{pmatrix} $$
となります。左側の $2 \times 2$ 行列が2次元の回転行列です。この行列をベクトルに掛けると、ベクトルを角度 $\theta$ だけ回転させることができます。同じ考え方を3次元に拡張したものが、3次元の回転行列です。
3次元空間で物体を回転させるのは、2次元よりずっと複雑です。2次元では回転軸は1つ（紙面に垂直な軸）しかありませんが、3次元では無数の回転軸が取れます。それでも、任意の回転は $3 \times 3$ の回転行列で統一的に表現できるのが、この道具の強力な点です。
回転行列の数学的定義
3次元空間における回転行列 $\bm{R}$ は、以下の2つの条件を満たす $3 \times 3$ 実行列です。
$$ \bm{R}^\top \bm{R} = \bm{I}_3 \quad \text{（直交条件）} $$
$$ \det(\bm{R}) = 1 \quad \text{（右手系の保存）} $$
ここで $\bm{I}_3$ は $3 \times 3$ の単位行列、$\det$ は行列式です。
第1の条件 $\bm{R}^\top \bm{R} = \bm{I}_3$ は、$\bm{R}$ が直交行列であることを意味します。直交行列とは、列ベクトル同士が互いに直交し、かつ各列ベクトルの大きさが1であるような行列です。幾何学的に言えば、直交行列による変換は「長さと角度を保存する」変換です。回転は物体を伸縮させないので、直交条件は自然な要請です。
第2の条件 $\det(\bm{R}) = 1$ は、座標系の「利き手」が変わらないことを保証します。もし $\det(\bm{R}) = -1$ であれば、それは回転ではなく鏡映（反転）を含む変換になります。たとえば、右手座標系が左手座標系に変わってしまうような変換は回転ではありません。
SO(3)群
この2つの条件を満たす $3 \times 3$ 行列全体の集合を、特殊直交群 $\text{SO}(3)$ と呼びます。
$$ \text{SO}(3) = \{ \bm{R} \in \mathbb{R}^{3 \times 3} \mid \bm{R}^\top \bm{R} = \bm{I}_3, \; \det(\bm{R}) = 1 \} $$
$\text{SO}(3)$ は数学的に群（group）の構造を持ちます。これは以下の性質を意味します。
- 閉包性: 2つの回転行列の積は再び回転行列になる（回転を連続して行うとまた回転になる）
- 結合律: $(\bm{R}_1 \bm{R}_2) \bm{R}_3 = \bm{R}_1 (\bm{R}_2 \bm{R}_3)$
- 単位元の存在: 単位行列 $\bm{I}_3$ は「回転なし」に対応する
- 逆元の存在: 任意の回転 $\bm{R}$ に対して逆回転 $\bm{R}^{-1} = \bm{R}^\top$ が存在する
特に4番目の性質は重要です。直交行列の逆行列は転置行列に等しいため、逆回転の計算が非常に簡単です。一般の行列の逆行列を求めるには掃き出し法などの手間がかかりますが、回転行列の場合は転置するだけで済みます。
$$ \bm{R}^{-1} = \bm{R}^\top $$
この性質は、後ほど同次変換行列の逆変換を求める際にも活躍します。
ここまでで回転行列の一般的な定義と性質を理解しました。では、具体的な回転行列はどのように構成するのでしょうか？ 最も基本的な回転 — 座標軸まわりの回転 — から見ていきましょう。
基本回転行列の導出
3次元空間での回転の中で最もシンプルなのは、$x$ 軸、$y$ 軸、$z$ 軸のいずれかを回転軸とする回転です。これらを基本回転と呼び、対応する回転行列を $\bm{R}_x(\theta)$、$\bm{R}_y(\theta)$、$\bm{R}_z(\theta)$ と書きます。
z軸まわりの回転 $\bm{R}_z(\theta)$
まず、最もイメージしやすい $z$ 軸まわりの回転から導出します。$z$ 軸を回転軸として角度 $\theta$ だけ反時計回り（$z$ 軸の正の方向から見て）に回転させることを考えます。
$z$ 軸まわりの回転では、$z$ 座標は変化しません。$xy$ 平面内の点の動きは、先ほど見た2次元の回転そのものです。したがって、3次元の点 $\bm{p} = (p_x, p_y, p_z)^\top$ を $z$ 軸まわりに角度 $\theta$ 回転させた点 $\bm{p}’$ は
$$ \bm{p}’ = \begin{pmatrix} p_x \cos\theta – p_y \sin\theta \\ p_x \sin\theta + p_y \cos\theta \\ p_z \end{pmatrix} $$
と書けます。上の2成分は2次元の回転公式そのもので、第3成分は変化しません。
これを行列で表現すると
$$ \bm{R}_z(\theta) = \begin{pmatrix} \cos\theta & -\sin\theta & 0 \\ \sin\theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{pmatrix} $$
となります。第3行第3列が1で、それ以外の第3行・第3列の成分が0であることが、「$z$ 座標は変化しない」ことに対応しています。
x軸まわりの回転 $\bm{R}_x(\theta)$
同じ考え方を $x$ 軸まわりの回転に適用します。$x$ 軸を回転軸とすると、$x$ 座標は変化せず、$yz$ 平面内で2次元回転が起こります。$x$ 軸の正の方向から見て反時計回りに角度 $\theta$ 回転させると
$$ \begin{cases} p_x’ = p_x \\ p_y’ = p_y \cos\theta – p_z \sin\theta \\ p_z’ = p_y \sin\theta + p_z \cos\theta \end{cases} $$
となります。行列表現は
$$ \bm{R}_x(\theta) = \begin{pmatrix} 1 & 0 & 0 \\ 0 & \cos\theta & -\sin\theta \\ 0 & \sin\theta & \cos\theta \end{pmatrix} $$
です。今度は第1行第1列が1で、それ以外の第1行・第1列の成分が0です。
y軸まわりの回転 $\bm{R}_y(\theta)$
$y$ 軸まわりの回転では $y$ 座標が変化せず、$zx$ 平面内で回転が起こります。ただし、ここで注意が必要です。右手座標系において $y$ 軸の正の方向から見ると、$z$ 軸から $x$ 軸に向かう回転が正の回転（反時計回り）です。つまり、$zx$ 平面での2次元回転は $z$ → $x$ の順に回転するため
$$ \begin{cases} p_x’ = p_x \cos\theta + p_z \sin\theta \\ p_y’ = p_y \\ p_z’ = -p_x \sin\theta + p_z \cos\theta \end{cases} $$
となります。$\bm{R}_x$ や $\bm{R}_z$ と比べて $\sin\theta$ の符号が逆になっている点に注目してください。行列表現は
$$ \bm{R}_y(\theta) = \begin{pmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \end{pmatrix} $$
です。
基本回転行列のまとめ
3つの基本回転行列をまとめると
$$ \bm{R}_x(\theta) = \begin{pmatrix} 1 & 0 & 0 \\ 0 & \cos\theta & -\sin\theta \\ 0 & \sin\theta & \cos\theta \end{pmatrix}, \quad \bm{R}_y(\theta) = \begin{pmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \end{pmatrix}, \quad \bm{R}_z(\theta) = \begin{pmatrix} \cos\theta & -\sin\theta & 0 \\ \sin\theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{pmatrix} $$
これらは全て SO(3) の条件を満たすことを確認しておきましょう。たとえば $\bm{R}_z(\theta)$ について、$\bm{R}_z(\theta)^\top \bm{R}_z(\theta)$ を計算すると
$$ \bm{R}_z(\theta)^\top \bm{R}_z(\theta) = \begin{pmatrix} \cos\theta & \sin\theta & 0 \\ -\sin\theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{pmatrix} \begin{pmatrix} \cos\theta & -\sin\theta & 0 \\ \sin\theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{pmatrix} $$
(1,1)成分を計算すると $\cos^2\theta + \sin^2\theta = 1$、(1,2)成分を計算すると $-\cos\theta\sin\theta + \sin\theta\cos\theta = 0$ となり、同様に他の成分も計算すると
$$ \bm{R}_z(\theta)^\top \bm{R}_z(\theta) = \bm{I}_3 $$
が確認できます。行列式も $\det(\bm{R}_z(\theta)) = 1 \cdot (\cos^2\theta + \sin^2\theta) = 1$ です。$\bm{R}_x$、$\bm{R}_y$ についても同様に成り立ちます。
3つの基本回転行列が手に入りました。では、これらを組み合わせてより複雑な回転を表現するにはどうすればよいのでしょうか？ 次に、回転行列の合成とその重要な注意点を見ていきます。
回転行列の合成と順序依存性
回転の合成は行列の積
2つの回転を順番に行った結果は、対応する回転行列の積で表されます。たとえば、まず $\bm{R}_1$ で回転し、次に $\bm{R}_2$ で回転する場合、合成回転は
$$ \bm{R}_{\text{total}} = \bm{R}_2 \bm{R}_1 $$
です。注意すべきは、先に適用する回転が右側に来るということです。これは行列がベクトルに右から順に作用するためです。
$$ \bm{p}” = \bm{R}_2 (\bm{R}_1 \bm{p}) = (\bm{R}_2 \bm{R}_1) \bm{p} $$
順序依存性 — 回転は交換しない
3次元回転の最も重要な特徴の一つが、回転の順序を入れ替えると結果が変わるということです。数学的に言えば、一般に
$$ \bm{R}_1 \bm{R}_2 \neq \bm{R}_2 \bm{R}_1 $$
です。行列の積は一般に非可換であり、回転行列も例外ではありません。
これは日常経験からも納得できます。スマートフォンを手に持って、次の2つの操作を試してみてください。
操作A: まず $x$ 軸（横方向）まわりに90度回転 → 次に $z$ 軸（画面に垂直）まわりに90度回転
操作B: まず $z$ 軸まわりに90度回転 → 次に $x$ 軸まわりに90度回転
最終的なスマートフォンの向きが異なることがわかるでしょう。これを具体的に計算で確認します。
$\theta = 90° = \pi/2$ として、$\bm{R}_x(\pi/2)$ と $\bm{R}_z(\pi/2)$ を計算すると
$$ \bm{R}_x(\pi/2) = \begin{pmatrix} 1 & 0 & 0 \\ 0 & 0 & -1 \\ 0 & 1 & 0 \end{pmatrix}, \quad \bm{R}_z(\pi/2) = \begin{pmatrix} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 1 \end{pmatrix} $$
操作A（$x$ → $z$ の順）の場合、$\bm{R}_z \bm{R}_x$ を計算すると
$$ \bm{R}_z(\pi/2) \bm{R}_x(\pi/2) = \begin{pmatrix} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 1 \end{pmatrix} \begin{pmatrix} 1 & 0 & 0 \\ 0 & 0 & -1 \\ 0 & 1 & 0 \end{pmatrix} $$
(1,1)成分は $0 \cdot 1 + (-1) \cdot 0 + 0 \cdot 0 = 0$、(1,2)成分は $0 \cdot 0 + (-1) \cdot 0 + 0 \cdot 1 = 0$、(1,3)成分は $0 \cdot 0 + (-1) \cdot (-1) + 0 \cdot 0 = 1$ のように各成分を計算していくと
$$ \bm{R}_z(\pi/2) \bm{R}_x(\pi/2) = \begin{pmatrix} 0 & 0 & 1 \\ 1 & 0 & 0 \\ 0 & 1 & 0 \end{pmatrix} $$
操作B（$z$ → $x$ の順）の場合、$\bm{R}_x \bm{R}_z$ を計算すると
$$ \bm{R}_x(\pi/2) \bm{R}_z(\pi/2) = \begin{pmatrix} 1 & 0 & 0 \\ 0 & 0 & -1 \\ 0 & 1 & 0 \end{pmatrix} \begin{pmatrix} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 1 \end{pmatrix} $$
同様に各成分を計算すると
$$ \bm{R}_x(\pi/2) \bm{R}_z(\pi/2) = \begin{pmatrix} 0 & -1 & 0 \\ 0 & 0 & -1 \\ 1 & 0 & 0 \end{pmatrix} $$
明らかに
$$ \bm{R}_z(\pi/2) \bm{R}_x(\pi/2) \neq \bm{R}_x(\pi/2) \bm{R}_z(\pi/2) $$
です。このように、3次元回転では順序が結果に直接影響するため、ロボットの姿勢計算では回転の適用順序を常に意識する必要があります。
オイラー角と回転行列
任意の3次元回転は、3つの基本回転の積として表現できることが知られています。たとえば、ZYX オイラー角 $(\alpha, \beta, \gamma)$ を使えば
$$ \bm{R} = \bm{R}_z(\alpha) \bm{R}_y(\beta) \bm{R}_x(\gamma) $$
と書けます。$\gamma$（ロール）、$\beta$（ピッチ）、$\alpha$（ヨー）として、航空宇宙分野で広く使われる表現です。ロボティクスでも、関節角からリンクの姿勢を計算する際に、このような基本回転の合成が頻繁に登場します。
ただし、オイラー角にはジンバルロックと呼ばれる特異点の問題があります。たとえばZYXオイラー角では、ピッチ角 $\beta = \pm 90°$ のとき、ロールとヨーが区別できなくなり自由度が1つ失われます。この問題を回避するために、実際の衛星姿勢制御ではクォータニオン（四元数）がよく使われますが、それは別の記事で扱います。
回転行列によって物体の「向き」を表現できることがわかりました。しかし、ロボットアームの各リンクは回転するだけでなく、関節の構造によって次のリンクの原点位置もずれます。つまり、「回転」と「並進」を同時に扱う必要があります。これを一つの行列で統一的に記述するのが、同次変換行列です。
同次変換行列とは
回転＋並進の問題
ある座標系（フレームA）から見た点 $\bm{p}^A$ を、別の座標系（フレームB）から見た表現 $\bm{p}^B$ に変換することを考えます。フレームBの原点がフレームAの原点から見て位置 $\bm{d}$ にあり、フレームBの軸がフレームAに対して回転行列 $\bm{R}$ で回転しているとすると
$$ \bm{p}^A = \bm{R} \bm{p}^B + \bm{d} $$
という関係が成り立ちます。右辺は「まず $\bm{p}^B$ を回転させ、次に $\bm{d}$ だけ並進させる」という操作です。
この式には1つ問題があります。変換の合成が行列の積として書けないのです。たとえば、フレームA → フレームB → フレームC と2段階の変換がある場合
$$ \bm{p}^A = \bm{R}_1 \bm{p}^B + \bm{d}_1 $$
$$ \bm{p}^B = \bm{R}_2 \bm{p}^C + \bm{d}_2 $$
を合成すると
$$ \bm{p}^A = \bm{R}_1 (\bm{R}_2 \bm{p}^C + \bm{d}_2) + \bm{d}_1 = \bm{R}_1 \bm{R}_2 \bm{p}^C + \bm{R}_1 \bm{d}_2 + \bm{d}_1 $$
となり、回転部分 $\bm{R}_1 \bm{R}_2$ と並進部分 $\bm{R}_1 \bm{d}_2 + \bm{d}_1$ を別々に管理しなければなりません。ロボットアームのように5個も6個もフレームが連鎖すると、この書き方では煩雑になります。
同次座標の導入
この問題を解決するアイデアは、次元を1つ増やすことです。3次元ベクトル $\bm{p} = (p_x, p_y, p_z)^\top$ を、末尾に1を追加した4次元ベクトル
$$ \tilde{\bm{p}} = \begin{pmatrix} p_x \\ p_y \\ p_z \\ 1 \end{pmatrix} $$
に拡張します。この表現を同次座標（homogeneous coordinates）と呼びます。「同次」という名前は射影幾何学に由来しますが、ここではスケーリングファクタとして1を付加した便利な表記法と考えてください。
同次変換行列の定義
同次座標を使うと、回転 $\bm{R}$ と並進 $\bm{d}$ を1つの $4 \times 4$ 行列の積で表現できます。
$$ \bm{T} = \begin{pmatrix} \bm{R} & \bm{d} \\ \bm{0}^\top & 1 \end{pmatrix} = \begin{pmatrix} r_{11} & r_{12} & r_{13} & d_x \\ r_{21} & r_{22} & r_{23} & d_y \\ r_{31} & r_{32} & r_{33} & d_z \\ 0 & 0 & 0 & 1 \end{pmatrix} $$
ここで $\bm{R} \in \text{SO}(3)$ は $3 \times 3$ の回転行列、$\bm{d} = (d_x, d_y, d_z)^\top$ は並進ベクトル、$\bm{0}^\top = (0, 0, 0)$ はゼロ行ベクトルです。
この行列を同次座標に作用させると
$$ \begin{pmatrix} \bm{R} & \bm{d} \\ \bm{0}^\top & 1 \end{pmatrix} \begin{pmatrix} \bm{p}^B \\ 1 \end{pmatrix} = \begin{pmatrix} \bm{R} \bm{p}^B + \bm{d} \\ 1 \end{pmatrix} = \begin{pmatrix} \bm{p}^A \\ 1 \end{pmatrix} $$
上3成分を見ると、$\bm{p}^A = \bm{R} \bm{p}^B + \bm{d}$ が確かに再現されています。第4成分は常に1のまま保たれるため、同次座標の構造は破壊されません。
このように、同次変換行列は「回転してから並進する」という操作を、1回の行列の積で実行できるようにする道具です。
SE(3)群
同次変換行列全体の集合を特殊ユークリッド群 $\text{SE}(3)$ と呼びます。
$$ \text{SE}(3) = \left\{ \begin{pmatrix} \bm{R} & \bm{d} \\ \bm{0}^\top & 1 \end{pmatrix} \in \mathbb{R}^{4 \times 4} \;\middle|\; \bm{R} \in \text{SO}(3), \; \bm{d} \in \mathbb{R}^3 \right\} $$
$\text{SE}(3)$ も $\text{SO}(3)$ と同様に群の構造を持ちます。特に重要なのは、2つの同次変換行列の積がまた同次変換行列になることです。これにより、フレームの連鎖を行列の積として自然に記述できます。
同次変換行列の最大のメリットが見えてきました。では、その逆変換 — つまり座標変換を「逆方向」に行う行列 — はどのように求められるのでしょうか？
同次変換行列の逆変換
逆変換の必要性
ロボティクスでは、フレームAからフレームBへの変換 ${}^A\bm{T}_B$ が既知のとき、逆方向の変換 ${}^B\bm{T}_A$（フレームBからフレームAへ）も頻繁に必要になります。たとえば、アーム先端のカメラで物体を検出した場合、物体のカメラ座標系での位置をベース座標系に変換するには、逆変換が必要です。
逆変換の導出
同次変換行列
$$ \bm{T} = \begin{pmatrix} \bm{R} & \bm{d} \\ \bm{0}^\top & 1 \end{pmatrix} $$
の逆行列 $\bm{T}^{-1}$ を求めます。$\bm{T} \bm{T}^{-1} = \bm{I}_4$ を満たす行列を
$$ \bm{T}^{-1} = \begin{pmatrix} \bm{A} & \bm{b} \\ \bm{c}^\top & d \end{pmatrix} $$
と置きます。$\bm{T} \bm{T}^{-1} = \bm{I}_4$ を展開すると
$$ \begin{pmatrix} \bm{R} & \bm{d} \\ \bm{0}^\top & 1 \end{pmatrix} \begin{pmatrix} \bm{A} & \bm{b} \\ \bm{c}^\top & d \end{pmatrix} = \begin{pmatrix} \bm{I}_3 & \bm{0} \\ \bm{0}^\top & 1 \end{pmatrix} $$
左辺のブロック行列の積を計算すると
$$ \begin{pmatrix} \bm{R}\bm{A} + \bm{d}\bm{c}^\top & \bm{R}\bm{b} + d\bm{d} \\ \bm{c}^\top & d \end{pmatrix} = \begin{pmatrix} \bm{I}_3 & \bm{0} \\ \bm{0}^\top & 1 \end{pmatrix} $$
(2,2)ブロックから $d = 1$、(2,1)ブロックから $\bm{c}^\top = \bm{0}^\top$ が得られます。
$\bm{c}^\top = \bm{0}^\top$ を(1,1)ブロックに代入すると $\bm{R}\bm{A} = \bm{I}_3$ なので $\bm{A} = \bm{R}^{-1} = \bm{R}^\top$ です。
$d = 1$ を(1,2)ブロックに代入すると $\bm{R}\bm{b} + \bm{d} = \bm{0}$ なので $\bm{b} = -\bm{R}^{-1}\bm{d} = -\bm{R}^\top \bm{d}$ です。
以上をまとめると、同次変換行列の逆行列は
$$ \bm{T}^{-1} = \begin{pmatrix} \bm{R}^\top & -\bm{R}^\top \bm{d} \\ \bm{0}^\top & 1 \end{pmatrix} $$
と書けます。
この結果を直感的に理解しておきましょう。$\bm{T}$ は「$\bm{R}$ で回転してから $\bm{d}$ だけ並進する」という操作です。その逆操作は、「$\bm{d}$ だけ逆向きに並進してから、$\bm{R}^\top$ で逆回転する」です。これを1ステップで書くと、まず $-\bm{d}$ を $\bm{R}^\top$ で回転させてから適用する形になるため、並進部分が $-\bm{R}^\top \bm{d}$ となるのです。
一般の $4 \times 4$ 行列の逆行列を求めるには計算コストがかかりますが、同次変換行列の特殊な構造（回転部分が直交行列である）を利用することで、転置と行列ベクトル積だけで逆変換が求まります。これは計算の速さが求められるリアルタイム制御で大きな利点です。
逆変換が簡潔に求まることがわかりました。次に、同次変換行列の真価が発揮される場面 — 複数のフレームを連鎖的に接続する座標変換 — を見ていきましょう。
連鎖的な座標変換
フレームの連鎖とロボットアーム
ロボットアームは、複数のリンク（剛体）が関節で接続された構造です。ベース（台座）のフレームを $\{0\}$、第1リンクのフレームを $\{1\}$、第2リンクのフレームを $\{2\}$、…、先端（エンドエフェクタ）のフレームを $\{n\}$ とします。
隣り合うフレーム間の関係は、それぞれの同次変換行列で記述されます。
$$ {}^0\bm{T}_1, \quad {}^1\bm{T}_2, \quad {}^2\bm{T}_3, \quad \ldots, \quad {}^{n-1}\bm{T}_n $$
ここで ${}^{i-1}\bm{T}_i$ は、「フレーム $\{i\}$ で表された点をフレーム $\{i-1\}$ での表現に変換する行列」です。左上の添字が変換先、右下の添字が変換元を表します。
行列の連鎖積
ベースフレーム $\{0\}$ からエンドエフェクタフレーム $\{n\}$ までの変換は、各変換行列を順番に掛けるだけで得られます。
$$ {}^0\bm{T}_n = {}^0\bm{T}_1 \cdot {}^1\bm{T}_2 \cdot {}^2\bm{T}_3 \cdots {}^{n-1}\bm{T}_n $$
添字のルールは「隣り合う添字が消える」と覚えると間違いにくくなります。${}^0\bm{T}_{\cancel{1}} \cdot {}^{\cancel{1}}\bm{T}_{\cancel{2}} \cdot {}^{\cancel{2}}\bm{T}_3$ のように、右下と次の左上が一致して消えていきます。
具体例: 2リンクのロボットアーム
最も簡単な例として、2次元平面上の2リンクアームを考えましょう。第1リンクの長さを $L_1$、第2リンクの長さを $L_2$、各関節角を $\theta_1, \theta_2$ とします。
ベースフレーム $\{0\}$ から第1リンクフレーム $\{1\}$ への変換は、$z$ 軸まわりに $\theta_1$ 回転し、$x$ 軸方向に $L_1$ 並進です。
$$ {}^0\bm{T}_1 = \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} $$
第1リンクフレーム $\{1\}$ から第2リンクフレーム（先端）$\{2\}$ への変換は、同様に $\theta_2$ 回転して $L_2$ 並進です。
$$ {}^1\bm{T}_2 = \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 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{pmatrix} $$
ベースフレームからエンドエフェクタフレームへの変換は
$$ {}^0\bm{T}_2 = {}^0\bm{T}_1 \cdot {}^1\bm{T}_2 $$
この積を計算すると、エンドエフェクタの位置ベクトル（第4列の上3成分）は
$$ \bm{d} = \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} $$
となります。これは幾何学から直接導ける2リンクアームの先端位置の公式と一致します。同次変換行列を使えば、幾何学的な考察なしに、行列の掛け算だけで先端位置が求まるのです。この機械的な手順こそが、関節数が増えたときに威力を発揮します。
理論的な準備が整いました。ここからは、Pythonを使って回転行列と同次変換行列を実装し、3Dの座標系変換を実際に可視化してみましょう。
Pythonでの実装と3D可視化
基本回転行列の実装
まず、3つの基本回転行列を生成する関数を実装します。
import numpy as np
def rot_x(theta):
"""x軸まわりの回転行列"""
c, s = np.cos(theta), np.sin(theta)
return np.array([
[1, 0, 0],
[0, c, -s],
[0, s, c]
])
def rot_y(theta):
"""y軸まわりの回転行列"""
c, s = np.cos(theta), np.sin(theta)
return np.array([
[ c, 0, s],
[ 0, 1, 0],
[-s, 0, c]
])
def rot_z(theta):
"""z軸まわりの回転行列"""
c, s = np.cos(theta), np.sin(theta)
return np.array([
[c, -s, 0],
[s, c, 0],
[0, 0, 1]
])
この実装は、先ほど導出した数式をそのままコードに落としたものです。角度はラジアンで受け取ります。np.cos と np.sin は1回ずつしか呼ばないよう、事前に変数 c, s に格納しています。
回転行列の性質を検証
実装した回転行列がSO(3)の条件を満たすことを数値的に検証しましょう。
import numpy as np
def rot_x(theta):
c, s = np.cos(theta), np.sin(theta)
return np.array([[1,0,0],[0,c,-s],[0,s,c]])
def rot_y(theta):
c, s = np.cos(theta), np.sin(theta)
return np.array([[c,0,s],[0,1,0],[-s,0,c]])
def rot_z(theta):
c, s = np.cos(theta), np.sin(theta)
return np.array([[c,-s,0],[s,c,0],[0,0,1]])
# 任意の角度で検証
theta = np.deg2rad(37) # 37度
R = rot_z(theta)
# 直交条件: R^T R = I
print("R^T R =")
print(np.round(R.T @ R, 10))
# 行列式 = 1
print(f"\ndet(R) = {np.linalg.det(R):.10f}")
# 合成回転もSO(3)に属することを確認
R_composite = rot_z(np.deg2rad(30)) @ rot_y(np.deg2rad(45)) @ rot_x(np.deg2rad(60))
print(f"\n合成回転の det = {np.linalg.det(R_composite):.10f}")
print("合成回転の R^T R =")
print(np.round(R_composite.T @ R_composite, 10))
実行すると、$\bm{R}^\top \bm{R}$ が単位行列に、行列式が1に非常に近い値になることが確認できます。また、3つの基本回転行列を合成した行列も同様にSO(3)の条件を満たしています。浮動小数点の丸め誤差により厳密にゼロや1にはなりませんが、$10^{-15}$ 程度の微小な誤差に収まります。
回転の順序依存性の可視化
理論で確認した順序依存性を、Pythonでも確かめてみましょう。
import numpy as np
def rot_x(theta):
c, s = np.cos(theta), np.sin(theta)
return np.array([[1,0,0],[0,c,-s],[0,s,c]])
def rot_z(theta):
c, s = np.cos(theta), np.sin(theta)
return np.array([[c,-s,0],[s,c,0],[0,0,1]])
# 90度回転
angle = np.pi / 2
# 順序A: x軸回転 → z軸回転
R_A = rot_z(angle) @ rot_x(angle)
# 順序B: z軸回転 → x軸回転
R_B = rot_x(angle) @ rot_z(angle)
print("順序A (Rz @ Rx):")
print(np.round(R_A, 10))
print("\n順序B (Rx @ Rz):")
print(np.round(R_B, 10))
print("\n差分 (A - B):")
print(np.round(R_A - R_B, 10))
差分行列がゼロ行列にならないことから、回転の順序を入れ替えると結果が変わることが数値的にも確認できます。これは先ほど手計算で求めた結果と一致しています。
同次変換行列の実装
次に、同次変換行列を扱うユーティリティ関数を実装します。
import numpy as np
def make_transform(R, d):
"""回転行列Rと並進ベクトルdから同次変換行列を作成"""
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = d
return T
def inv_transform(T):
"""同次変換行列の逆行列（効率的な計算）"""
R = T[:3, :3]
d = T[:3, 3]
T_inv = np.eye(4)
T_inv[:3, :3] = R.T
T_inv[:3, 3] = -R.T @ d
return T_inv
make_transform は $3 \times 3$ の回転行列と $3 \times 1$ の並進ベクトルから $4 \times 4$ の同次変換行列を構成します。inv_transform は先ほど導出した逆変換の公式をそのまま実装しています。np.linalg.inv で汎用的な逆行列計算をするよりも、回転行列の直交性を利用した方が高速かつ数値的にも安定です。
逆変換の検証
逆変換が正しく機能することを数値的に確認します。
import numpy as np
def rot_y(theta):
c, s = np.cos(theta), np.sin(theta)
return np.array([[c,0,s],[0,1,0],[-s,0,c]])
def rot_z(theta):
c, s = np.cos(theta), np.sin(theta)
return np.array([[c,-s,0],[s,c,0],[0,0,1]])
def make_transform(R, d):
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = d
return T
def inv_transform(T):
R = T[:3, :3]
d = T[:3, 3]
T_inv = np.eye(4)
T_inv[:3, :3] = R.T
T_inv[:3, 3] = -R.T @ d
return T_inv
# 回転 + 並進の変換を作成
R = rot_z(np.deg2rad(45)) @ rot_y(np.deg2rad(30))
d = np.array([1.0, 2.0, 3.0])
T = make_transform(R, d)
# 逆変換を計算
T_inv = inv_transform(T)
# T @ T_inv が単位行列になることを確認
print("T @ T_inv =")
print(np.round(T @ T_inv, 10))
# np.linalg.inv との比較
T_inv_numpy = np.linalg.inv(T)
print("\n自作inv_transformとnp.linalg.invの差:")
print(np.round(T_inv - T_inv_numpy, 15))
$\bm{T} \bm{T}^{-1}$ が $4 \times 4$ の単位行列になっていること、また np.linalg.inv の結果と一致していることが確認できます。自作関数の方が構造を利用しているため、数値的な安定性がわずかに高い場合もあります。
3D座標系の可視化
ここからが本記事のハイライトです。同次変換行列によって座標フレームがどのように移動・回転するかを、3Dプロットで可視化します。
まず、座標フレームを描画するヘルパー関数を定義します。
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
def rot_x(theta):
c, s = np.cos(theta), np.sin(theta)
return np.array([[1,0,0],[0,c,-s],[0,s,c]])
def rot_y(theta):
c, s = np.cos(theta), np.sin(theta)
return np.array([[c,0,s],[0,1,0],[-s,0,c]])
def rot_z(theta):
c, s = np.cos(theta), np.sin(theta)
return np.array([[c,-s,0],[s,c,0],[0,0,1]])
def make_transform(R, d):
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = d
return T
def draw_frame(ax, T, label="", length=1.0, lw=2):
"""同次変換行列Tで定義されるフレームを描画"""
origin = T[:3, 3]
# 各軸の方向ベクトル（回転行列の列ベクトル）
x_axis = T[:3, 0] * length
y_axis = T[:3, 1] * length
z_axis = T[:3, 2] * length
# x軸: 赤, y軸: 緑, z軸: 青
ax.quiver(*origin, *x_axis, color='r', linewidth=lw, arrow_length_ratio=0.1)
ax.quiver(*origin, *y_axis, color='g', linewidth=lw, arrow_length_ratio=0.1)
ax.quiver(*origin, *z_axis, color='b', linewidth=lw, arrow_length_ratio=0.1)
if label:
ax.text(*(origin + 0.1), label, fontsize=12, fontweight='bold')
この draw_frame 関数は、同次変換行列 $\bm{T}$ から座標フレームの原点と3つの軸方向を読み取り、矢印として描画します。色の規則は RGB = XYZ に対応しています（赤=X, 緑=Y, 青=Z）。これは3Dグラフィックスやロボティクスで広く使われる慣習です。
回転前後のフレームの比較
基準フレームに対して、回転のみを適用したフレームを可視化してみましょう。
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
def rot_x(theta):
c, s = np.cos(theta), np.sin(theta)
return np.array([[1,0,0],[0,c,-s],[0,s,c]])
def rot_y(theta):
c, s = np.cos(theta), np.sin(theta)
return np.array([[c,0,s],[0,1,0],[-s,0,c]])
def rot_z(theta):
c, s = np.cos(theta), np.sin(theta)
return np.array([[c,-s,0],[s,c,0],[0,0,1]])
def make_transform(R, d):
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = d
return T
def draw_frame(ax, T, label="", length=1.0, lw=2):
origin = T[:3, 3]
x_axis = T[:3, 0] * length
y_axis = T[:3, 1] * length
z_axis = T[:3, 2] * length
ax.quiver(*origin, *x_axis, color='r', linewidth=lw, arrow_length_ratio=0.1)
ax.quiver(*origin, *y_axis, color='g', linewidth=lw, arrow_length_ratio=0.1)
ax.quiver(*origin, *z_axis, color='b', linewidth=lw, arrow_length_ratio=0.1)
if label:
ax.text(*(origin + 0.1), label, fontsize=12, fontweight='bold')
fig = plt.figure(figsize=(14, 5))
# --- (a) z軸まわり45度回転 ---
ax1 = fig.add_subplot(131, projection='3d')
T_base = np.eye(4)
R1 = rot_z(np.deg2rad(45))
T1 = make_transform(R1, np.array([0, 0, 0]))
draw_frame(ax1, T_base, label="{0}", length=1.5, lw=2)
draw_frame(ax1, T1, label="{1}", length=1.2, lw=2)
ax1.set_title("(a) Rz(45°)", fontsize=12)
ax1.set_xlim([-2, 2]); ax1.set_ylim([-2, 2]); ax1.set_zlim([-2, 2])
ax1.set_xlabel("X"); ax1.set_ylabel("Y"); ax1.set_zlabel("Z")
# --- (b) x軸まわり60度回転 ---
ax2 = fig.add_subplot(132, projection='3d')
R2 = rot_x(np.deg2rad(60))
T2 = make_transform(R2, np.array([0, 0, 0]))
draw_frame(ax2, T_base, label="{0}", length=1.5, lw=2)
draw_frame(ax2, T2, label="{1}", length=1.2, lw=2)
ax2.set_title("(b) Rx(60°)", fontsize=12)
ax2.set_xlim([-2, 2]); ax2.set_ylim([-2, 2]); ax2.set_zlim([-2, 2])
ax2.set_xlabel("X"); ax2.set_ylabel("Y"); ax2.set_zlabel("Z")
# --- (c) 合成回転 Rz(45°) Ry(30°) Rx(60°) ---
ax3 = fig.add_subplot(133, projection='3d')
R3 = rot_z(np.deg2rad(45)) @ rot_y(np.deg2rad(30)) @ rot_x(np.deg2rad(60))
T3 = make_transform(R3, np.array([0, 0, 0]))
draw_frame(ax3, T_base, label="{0}", length=1.5, lw=2)
draw_frame(ax3, T3, label="{1}", length=1.2, lw=2)
ax3.set_title("(c) Rz(45°)Ry(30°)Rx(60°)", fontsize=12)
ax3.set_xlim([-2, 2]); ax3.set_ylim([-2, 2]); ax3.set_zlim([-2, 2])
ax3.set_xlabel("X"); ax3.set_ylabel("Y"); ax3.set_zlabel("Z")
plt.tight_layout()
plt.savefig("rotation_frames.png", dpi=150, bbox_inches='tight')
plt.show()
3つのサブプロットを見てみましょう。(a) では $z$ 軸まわりに45度回転しているため、$x$ 軸（赤）と $y$ 軸（緑）が $xy$ 平面内で45度傾き、$z$ 軸（青）は変化していません。(b) では $x$ 軸まわりに60度回転しているため、$x$ 軸は変化せず、$y$ 軸と $z$ 軸が回転しています。(c) は3つの基本回転を合成した結果で、3つの軸すべてが元の方向から変化していることがわかります。このように、回転行列の列ベクトルが回転後の各軸の方向を表しています。
同次変換（回転＋並進）の可視化
次に、回転と並進の両方を含む同次変換を可視化します。
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
def rot_x(theta):
c, s = np.cos(theta), np.sin(theta)
return np.array([[1,0,0],[0,c,-s],[0,s,c]])
def rot_y(theta):
c, s = np.cos(theta), np.sin(theta)
return np.array([[c,0,s],[0,1,0],[-s,0,c]])
def rot_z(theta):
c, s = np.cos(theta), np.sin(theta)
return np.array([[c,-s,0],[s,c,0],[0,0,1]])
def make_transform(R, d):
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = d
return T
def draw_frame(ax, T, label="", length=1.0, lw=2):
origin = T[:3, 3]
x_axis = T[:3, 0] * length
y_axis = T[:3, 1] * length
z_axis = T[:3, 2] * length
ax.quiver(*origin, *x_axis, color='r', linewidth=lw, arrow_length_ratio=0.1)
ax.quiver(*origin, *y_axis, color='g', linewidth=lw, arrow_length_ratio=0.1)
ax.quiver(*origin, *z_axis, color='b', linewidth=lw, arrow_length_ratio=0.1)
if label:
ax.text(*(origin + 0.1), label, fontsize=12, fontweight='bold')
fig = plt.figure(figsize=(10, 8))
ax = fig.add_subplot(111, projection='3d')
# ベースフレーム（原点）
T_base = np.eye(4)
draw_frame(ax, T_base, label="{0} Base", length=1.0, lw=2.5)
# フレーム1: z軸まわり30度回転 + (3, 0, 0)並進
R1 = rot_z(np.deg2rad(30))
d1 = np.array([3, 0, 0])
T1 = make_transform(R1, d1)
draw_frame(ax, T1, label="{1}", length=0.8, lw=2)
# フレーム2: フレーム1からさらにy軸まわり45度回転 + (0, 2, 0)並進
R2 = rot_y(np.deg2rad(45))
d2 = np.array([0, 2, 0])
T_1to2 = make_transform(R2, d2)
T2 = T1 @ T_1to2 # 連鎖: {0} → {1} → {2}
draw_frame(ax, T2, label="{2}", length=0.8, lw=2)
# フレーム3: フレーム2からさらにx軸まわり60度回転 + (0, 0, 1.5)並進
R3 = rot_x(np.deg2rad(60))
d3 = np.array([0, 0, 1.5])
T_2to3 = make_transform(R3, d3)
T3 = T2 @ T_2to3 # 連鎖: {0} → {1} → {2} → {3}
draw_frame(ax, T3, label="{3}", length=0.8, lw=2)
# フレーム間の接続線（原点同士を結ぶ）
origins = np.array([T_base[:3,3], T1[:3,3], T2[:3,3], T3[:3,3]])
ax.plot(origins[:,0], origins[:,1], origins[:,2],
'k--', linewidth=1.5, alpha=0.5)
ax.set_xlim([-1, 5]); ax.set_ylim([-2, 4]); ax.set_zlim([-1, 4])
ax.set_xlabel("X"); ax.set_ylabel("Y"); ax.set_zlabel("Z")
ax.set_title("Chained Homogeneous Transforms: {0}→{1}→{2}→{3}", fontsize=13)
ax.view_init(elev=25, azim=-60)
plt.tight_layout()
plt.savefig("chained_transforms.png", dpi=150, bbox_inches='tight')
plt.show()
この3Dプロットでは、4つのフレームが連鎖的に接続されている様子が確認できます。ベースフレーム $\{0\}$ から出発し、各フレームは前のフレームに対して回転＋並進しています。破線は各フレームの原点を結んでおり、ロボットアームのリンク構造に対応しています。重要な点は、フレーム $\{2\}$ やフレーム $\{3\}$ の軸方向が、ベースフレームとは大きく異なっていることです。これは各段階での回転が累積的に適用されているためです。
2リンクロボットアームの順運動学
最後に、理論と実装を統合する応用例として、2リンクロボットアームの順運動学を実装し、関節角を変化させたときのアーム先端の軌跡を可視化します。
import numpy as np
import matplotlib.pyplot as plt
def rot_z(theta):
c, s = np.cos(theta), np.sin(theta)
return np.array([[c,-s,0],[s,c,0],[0,0,1]])
def make_transform(R, d):
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = d
return T
def forward_kinematics_2link(theta1, theta2, L1, L2):
"""2リンクアームの順運動学"""
# ベース → 第1関節: z軸まわりにtheta1回転、x方向にL1並進
R1 = rot_z(theta1)
d1 = R1 @ np.array([L1, 0, 0])
T01 = make_transform(R1, d1)
# 第1関節 → 先端: z軸まわりにtheta2回転、x方向にL2並進
R2 = rot_z(theta2)
d2 = R2 @ np.array([L2, 0, 0])
T12 = make_transform(R2, d2)
# 連鎖
T02 = T01 @ T12
return T01, T02
# パラメータ
L1, L2 = 2.0, 1.5
# --- (a) 特定の姿勢を描画 ---
fig, axes = plt.subplots(1, 2, figsize=(14, 6))
ax1 = axes[0]
theta1, theta2 = np.deg2rad(45), np.deg2rad(30)
T01, T02 = forward_kinematics_2link(theta1, theta2, L1, L2)
# リンクを描画
joints = np.array([[0, 0], T01[:2, 3], T02[:2, 3]])
ax1.plot(joints[:, 0], joints[:, 1], 'ko-', linewidth=3, markersize=8)
ax1.plot(joints[-1, 0], joints[-1, 1], 'rs', markersize=12, label='End effector')
ax1.plot(0, 0, 'k^', markersize=12, label='Base')
# 各フレームの軸を描画（xy平面のみ）
for i, T in enumerate([np.eye(4), T01, T02]):
origin = T[:2, 3]
x_dir = T[:2, 0] * 0.5
y_dir = T[:2, 1] * 0.5
ax1.annotate('', xy=origin+x_dir, xytext=origin,
arrowprops=dict(arrowstyle='->', color='r', lw=1.5))
ax1.annotate('', xy=origin+y_dir, xytext=origin,
arrowprops=dict(arrowstyle='->', color='g', lw=1.5))
ax1.set_xlim([-4, 4]); ax1.set_ylim([-4, 4])
ax1.set_aspect('equal')
ax1.grid(True, alpha=0.3)
ax1.set_xlabel("X [m]"); ax1.set_ylabel("Y [m]")
ax1.set_title(f"2-Link Arm: θ1={np.rad2deg(theta1):.0f}°, θ2={np.rad2deg(theta2):.0f}°", fontsize=12)
ax1.legend()
# --- (b) theta2を変化させたときの先端軌跡 ---
ax2 = axes[1]
theta1_fixed = np.deg2rad(30)
theta2_range = np.linspace(-np.pi, np.pi, 200)
end_positions = []
for th2 in theta2_range:
_, T02 = forward_kinematics_2link(theta1_fixed, th2, L1, L2)
end_positions.append(T02[:2, 3])
end_positions = np.array(end_positions)
ax2.plot(end_positions[:, 0], end_positions[:, 1], 'b-', linewidth=2, label='End effector trajectory')
# いくつかの姿勢を描画
for th2_deg in [-90, -45, 0, 45, 90, 135]:
th2 = np.deg2rad(th2_deg)
T01, T02 = forward_kinematics_2link(theta1_fixed, th2, L1, L2)
joints = np.array([[0, 0], T01[:2, 3], T02[:2, 3]])
ax2.plot(joints[:, 0], joints[:, 1], 'k-', linewidth=0.8, alpha=0.4)
ax2.plot(joints[-1, 0], joints[-1, 1], 'r.', markersize=6)
ax2.plot(0, 0, 'k^', markersize=12)
ax2.set_xlim([-4, 4]); ax2.set_ylim([-4, 4])
ax2.set_aspect('equal')
ax2.grid(True, alpha=0.3)
ax2.set_xlabel("X [m]"); ax2.set_ylabel("Y [m]")
ax2.set_title(f"End Effector Trajectory (θ1={np.rad2deg(theta1_fixed):.0f}° fixed)", fontsize=12)
ax2.legend()
plt.tight_layout()
plt.savefig("two_link_arm.png", dpi=150, bbox_inches='tight')
plt.show()
左図 (a) では、$\theta_1 = 45°$、$\theta_2 = 30°$ の特定姿勢における2リンクアームを描いています。黒い三角がベース、赤い四角がエンドエフェクタです。各関節に小さな座標フレーム（赤=X軸、緑=Y軸）が表示されており、フレームの向きが関節角に応じて回転していることが確認できます。
右図 (b) では、$\theta_1 = 30°$ に固定して $\theta_2$ を $-180°$ から $180°$ まで変化させたときのエンドエフェクタの軌跡を青線で示しています。軌跡は第1関節の先端を中心とする半径 $L_2 = 1.5$ の円になっています。これは当然の結果で、第2関節の回転はリンク2の先端を円弧上に動かすからです。薄い灰色の線はいくつかの代表的な姿勢を示しており、関節角の変化に応じてアーム全体がどのように動くかを視覚的に理解できます。
ワークスペースの可視化
もう一歩進めて、両方の関節角を変化させたときのワークスペース（エンドエフェクタが到達可能な全領域）も可視化してみましょう。
import numpy as np
import matplotlib.pyplot as plt
def rot_z(theta):
c, s = np.cos(theta), np.sin(theta)
return np.array([[c,-s,0],[s,c,0],[0,0,1]])
def make_transform(R, d):
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = d
return T
def forward_kinematics_2link(theta1, theta2, L1, L2):
R1 = rot_z(theta1)
d1 = R1 @ np.array([L1, 0, 0])
T01 = make_transform(R1, d1)
R2 = rot_z(theta2)
d2 = R2 @ np.array([L2, 0, 0])
T12 = make_transform(R2, d2)
T02 = T01 @ T12
return T01, T02
L1, L2 = 2.0, 1.5
# 両関節角を離散化
theta1_range = np.linspace(-np.pi, np.pi, 300)
theta2_range = np.linspace(-np.pi, np.pi, 300)
all_positions = []
for th1 in theta1_range:
for th2 in theta2_range:
_, T02 = forward_kinematics_2link(th1, th2, L1, L2)
all_positions.append(T02[:2, 3])
all_positions = np.array(all_positions)
fig, ax = plt.subplots(figsize=(8, 8))
ax.scatter(all_positions[:, 0], all_positions[:, 1],
s=0.1, c='steelblue', alpha=0.3)
# 到達限界の円を描画
theta_circle = np.linspace(0, 2*np.pi, 200)
# 外側の円: L1 + L2
ax.plot((L1+L2)*np.cos(theta_circle), (L1+L2)*np.sin(theta_circle),
'r--', linewidth=1.5, label=f'r = L1+L2 = {L1+L2}')
# 内側の円: |L1 - L2|
if abs(L1 - L2) > 0.01:
ax.plot(abs(L1-L2)*np.cos(theta_circle), abs(L1-L2)*np.sin(theta_circle),
'r:', linewidth=1.5, label=f'r = |L1-L2| = {abs(L1-L2)}')
ax.set_xlim([-4.5, 4.5]); ax.set_ylim([-4.5, 4.5])
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("Workspace of 2-Link Planar Arm", fontsize=14)
ax.legend(fontsize=11)
ax.plot(0, 0, 'k^', markersize=12)
plt.tight_layout()
plt.savefig("workspace.png", dpi=150, bbox_inches='tight')
plt.show()
ワークスペースは、内半径 $|L_1 – L_2| = 0.5$ m、外半径 $L_1 + L_2 = 3.5$ m の環状領域（アニュラス）になっています。外側の限界は、2つのリンクが一直線に伸びた状態（$\theta_2 = 0°$）に対応し、内側の限界はリンクが完全に折り畳まれた状態（$\theta_2 = \pm 180°$）に対応します。赤い破線と点線がそれぞれの境界円を示しており、散布図の領域とぴったり一致していることが確認できます。もし $L_1 = L_2$ であれば内半径が0になり、原点（ベース直上）にも到達可能になります。
このワークスペース解析は、宇宙ロボットアームの設計において「アームがどの領域に到達できるか」を評価する基本的な手法です。
まとめ
本記事では、宇宙ロボティクスの数学的基礎として、剛体の姿勢を表現する回転行列と、回転＋並進を統一的に扱う同次変換行列について解説しました。
ポイントを整理します。
- 回転行列は $3 \times 3$ の直交行列で、$\bm{R}^\top \bm{R} = \bm{I}$ かつ $\det(\bm{R}) = 1$ を満たす（SO(3)群の元）
- 基本回転行列 $\bm{R}_x, \bm{R}_y, \bm{R}_z$ は各座標軸まわりの回転を表し、これらの積で任意の回転を構成できる
- 回転の合成は非可換（$\bm{R}_1 \bm{R}_2 \neq \bm{R}_2 \bm{R}_1$）であり、順序を常に意識する必要がある
- 同次変換行列は $4 \times 4$ 行列で回転と並進を1つの行列に統合し、フレーム間の変換を行列積で連鎖的に記述できる
- 逆変換は $\bm{R}^\top$ と $-\bm{R}^\top \bm{d}$ を使って効率的に計算できる
- 連鎖的な座標変換により、ロボットアームの順運動学が行列の積として機械的に求まる
これらは、ロボットアームの運動学・動力学、衛星の姿勢制御、3D空間でのセンサデータ処理など、宇宙ロボティクスのあらゆる場面で使われる基礎です。
宇宙ロボティクスシリーズの次のステップとして、以下のトピックに進む予定です。
- DH（デナビット・ハルテンバーグ）パラメータ — ロボットアームの各関節間の同次変換行列を系統的に設定する標準的な方法
- 逆運動学 — アーム先端の目標位置・姿勢から各関節角を逆算する手法
- クォータニオンによる姿勢表現 — ジンバルロックを回避する姿勢表現法