国際宇宙ステーション（ISS）には全長約17mの巨大なロボットアーム「カナダアーム2」が取り付けられています。宇宙飛行士はコントロールパネルで各関節の角度を操作しますが、実際に知りたいのは「アームの先端（エンドエフェクタ）が宇宙空間のどこにあるか」です。7つの関節がそれぞれ異なる角度で曲がっているとき、先端は座標系のどこに位置し、どの方向を向いているのか — この問いに答えるのが順運動学（forward kinematics）です。
順運動学は、ロボット工学で最も基本的かつ重要な計算です。関節角度の組（$\theta_1, \theta_2, \dots, \theta_n$）が与えられたとき、エンドエフェクタの位置と姿勢を求めます。しかし、関節が増えるほど座標系の変換は複雑になります。6自由度のアームなら6つの座標系変換を順番に重ねなければなりません。これを体系的かつ機械的に処理するための枠組みが、DHパラメータ（Denavit-Hartenberg パラメータ）です。
DHパラメータと順運動学を理解すると、以下のことが可能になります。
- 宇宙ロボットアームの制御: 各関節角度からアーム先端の3次元位置・姿勢をリアルタイムに計算し、宇宙飛行士やオペレータに表示する
- 産業ロボットのプログラミング: 溶接ロボットや組立ロボットのティーチングにおいて、ツール先端の軌跡を計算する
- 逆運動学への基盤: 「先端をこの位置に動かしたい」という問題（逆運動学）を解くためには、まず順運動学が正しく定式化されていなければならない
本記事の内容
- 順運動学の基本概念 — 関節空間と作業空間の関係
- DHパラメータの4つの量（$\theta_i, d_i, a_i, \alpha_i$）の幾何学的意味
- リンクごとにDH座標系を設定する手順
- DH変換行列の導出 — 4つの基本変換の積
- 具体例1: 2リンク平面マニピュレータの順運動学（手計算 + Python）
- 具体例2: 3リンク空間マニピュレータの順運動学
- Pythonによる汎用的な順運動学計算と3D可視化
前提知識
この記事を読む前に、以下の記事を読んでおくと理解が深まります。
特に、同次変換行列（$4 \times 4$ 行列）による座標系の並進・回転の表現方法を理解していることを前提とします。
順運動学とは — 関節空間から作業空間への写像
2つの空間
ロボットアームを記述するとき、2つの異なる「空間」を意識する必要があります。
関節空間（joint space）は、各関節の角度（回転関節の場合）や変位（直動関節の場合）をまとめたベクトルで表される空間です。$n$ 自由度のロボットなら、関節変数ベクトル $\bm{q} = (q_1, q_2, \dots, q_n)^T$ は $n$ 次元空間の一点に対応します。ロボットの内部的な状態、いわば「関節の角度計が示す値の組」がこの空間です。
作業空間（task space / Cartesian space）は、エンドエフェクタの位置と姿勢で表される空間です。3次元空間中の位置 $(x, y, z)$ と姿勢（ロール・ピッチ・ヨーなどで表す3つの角度）を合わせて、最大6次元のベクトルになります。これは、ロボットの先端が「外の世界のどこにどの向きで存在するか」を表します。
人間の腕で考えてみましょう。肩を30度回し、肘を45度曲げ、手首を10度ひねった — これが関節空間での記述です。そのとき指先がテーブルの上の座標 $(30\,\text{cm}, 50\,\text{cm}, 80\,\text{cm})$ にあり、手のひらが下を向いている — これが作業空間での記述です。
順運動学の定義
順運動学とは、関節空間から作業空間への写像を求める問題です。数学的に書くと、関節変数ベクトル $\bm{q}$ が与えられたとき、エンドエフェクタの位置 $\bm{p}$ と姿勢 $\bm{R}$（回転行列）を求める写像
$$ (\bm{p}, \bm{R}) = f(\bm{q}) $$
を構築することです。ここで $\bm{p} \in \mathbb{R}^3$ は位置、$\bm{R} \in SO(3)$ は $3 \times 3$ の回転行列です。
前記事で学んだ同次変換行列を使えば、この写像を一つの $4 \times 4$ 行列で表現できます。
$$ \bm{T}_0^n = \bm{T}_0^1 \bm{T}_1^2 \cdots \bm{T}_{n-1}^n $$
ここで $\bm{T}_{i-1}^i$ はリンク $i-1$ の座標系からリンク $i$ の座標系への同次変換行列です。この行列の積が、ベース座標系（$0$）からエンドエフェクタ座標系（$n$）への変換を与えます。
問題は、各 $\bm{T}_{i-1}^i$ をどう体系的に定めるかです。ロボットアームのリンクと関節の形状は千差万別ですが、適切な座標系の設定ルールを決めれば、全ての $\bm{T}_{i-1}^i$ をたった4つのパラメータで記述できます。それがDHパラメータです。
DHパラメータの幾何学的意味
DHパラメータの誕生
1955年、Jacques DenavitとRichard S. Hartenbergは、リンク機構を記述するための体系的な方法を提案しました。彼らのアイデアは「全ての関節とリンクを、たった4つの幾何学的パラメータで記述できる」という驚くべきものでした。以来、この方法はロボット工学の標準的な記法として世界中で使われています。
DHパラメータがなぜ4つで十分なのか、直感的に考えてみましょう。隣接する2つの関節軸（直線）の相対的な配置を決めるには、一般に次の4つの情報が必要です。
- 2つの軸の間の距離（最短距離）
- 2つの軸の相対的な傾き
- 一方の軸に沿った相対的な位置（オフセット）
- 一方の軸まわりの回転角度
これら4つの幾何学的な量が、DHパラメータの $a_i$, $\alpha_i$, $d_i$, $\theta_i$ にそれぞれ対応します。
4つのパラメータ
DHパラメータは次の4つの量から構成されます。各パラメータの幾何学的意味を、順に解説していきます。
リンク長 $a_i$（link length） — 関節軸 $i$ と関節軸 $i+1$ の間の共通法線の長さ（最短距離）です。$z_{i-1}$ 軸と $z_i$ 軸に共に垂直な線分の長さに等しく、常に $a_i \geq 0$ です。イメージとしては、「リンク $i$ の物理的な腕の長さ」に相当します。2本の平行でない軸の間には、一意な共通法線が定まります。2本の軸が平行な場合は共通法線が無数に存在しますが、そのときは慣習的な選び方で一意に定めます。
リンクねじれ角 $\alpha_i$（link twist） — 関節軸 $i$ と関節軸 $i+1$ のなす角です。共通法線を軸にして $z_{i-1}$ 軸を $z_i$ 軸まで回転させたときの角度です。右ねじの規約に従います。イメージとしては、「リンク $i$ の端で軸がどれだけねじれているか」を表します。平面マニピュレータ（全ての関節軸が平行）では $\alpha_i = 0$ です。
リンクオフセット $d_i$（link offset） — $z_{i-1}$ 軸に沿った、リンク $i-1$ の共通法線からリンク $i$ の共通法線までの距離です。$z_{i-1}$ 軸方向の「ずれ」を表します。直動関節（prismatic joint）の場合、$d_i$ が関節変数になります。回転関節の場合は定数です。
関節角度 $\theta_i$（joint angle） — $z_{i-1}$ 軸まわりの回転角度で、$x_{i-1}$ 軸から $x_i$ 軸への角度です。$z_{i-1}$ 軸方向から見て反時計回りを正とします。回転関節（revolute joint）の場合、$\theta_i$ が関節変数になります。直動関節の場合は定数です。
これらを表にまとめると次のようになります。
| パラメータ | 記号 | 意味 | 測定方向 |
|---|---|---|---|
| リンク長 | $a_i$ | 軸 $z_{i-1}$ と軸 $z_i$ の共通法線の長さ | $x_i$ 軸方向 |
| リンクねじれ角 | $\alpha_i$ | 軸 $z_{i-1}$ と軸 $z_i$ のなす角 | $x_i$ 軸まわり |
| リンクオフセット | $d_i$ | 共通法線間の $z_{i-1}$ 軸方向の距離 | $z_{i-1}$ 軸方向 |
| 関節角度 | $\theta_i$ | $x_{i-1}$ 軸から $x_i$ 軸への回転角 | $z_{i-1}$ 軸まわり |
重要なポイントは、回転関節では $\theta_i$ が変数（他の3つは定数）、直動関節では $d_i$ が変数（他の3つは定数）であるということです。つまり、1つの関節あたり自由度は1で、残りの3つのパラメータはロボットの構造（リンクの寸法や配置）によって決まる定数です。
DHパラメータの幾何学的意味がわかったところで、次に各リンクにどのように座標系を設定するかのルールを見ていきましょう。
DH座標系の設定手順
座標系設定のルール
DHパラメータを使うためには、各リンクに座標系を「正しい規約に従って」設定する必要があります。この規約がDH規約（Denavit-Hartenberg convention）です。座標系の設定ルールを一つずつ確認しましょう。
なお、DH規約には「標準DH規約」と「修正DH規約（Craig の規約）」の2種類が存在します。本記事では、教科書で最も広く使われている標準DH規約を採用します。
ステップ1: 関節軸の同定
まず、ロボットの全関節を番号付けします。ベース側から $1, 2, \dots, n$ と番号をふります。各関節 $i$ の回転軸（または直動方向）を特定し、これを $z_{i-1}$ 軸とします。ここが少し紛らわしいのですが、「関節 $i$ の軸」が「座標系 $i-1$ の $z$ 軸」に対応する点に注意してください。これは、関節 $i$ がリンク $i-1$ とリンク $i$ を繋いでおり、座標系 $i-1$ がリンク $i-1$ の端に固定されているためです。
ステップ2: 原点の配置
座標系 $i$ の原点 $O_i$ は、$z_{i-1}$ 軸と $z_i$ 軸の共通法線が $z_i$ 軸と交わる点に配置します。2つの軸が交差する場合は交点を原点とし、平行な場合は慣習的に決めます。
ステップ3: $x$ 軸の設定
$x_i$ 軸は、$z_{i-1}$ 軸と $z_i$ 軸の共通法線の方向に設定します。共通法線の向きは $z_{i-1}$ から $z_i$ に向かう方向を正とします。2つの軸が交差する場合は $x_i = z_{i-1} \times z_i / \|z_{i-1} \times z_i\|$ で定まります。平行な場合は自由度が残るため、簡便な方向を選びます。
ステップ4: $y$ 軸の設定
$y_i$ 軸は右手系の規約に従い、$y_i = z_i \times x_i$ で自動的に定まります。
ステップ5: ベース座標系とエンドエフェクタ座標系
ベース座標系（座標系 $0$）は $z_0$ 軸が関節1の軸と一致するように配置します。$x_0$ と $y_0$ は自由に選べますが、通常は都合のよい方向を選びます。
エンドエフェクタ座標系（座標系 $n$）は、通常はツール先端に配置します。最後の関節の先にリンクがないため、$z_n$ 軸方向と原点の位置にはある程度の自由度があります。一般的には $z_n$ は $z_{n-1}$ と平行にとるか、ツールの方向に合わせます。
設定手順のまとめ
以上をフローチャート風にまとめると次のようになります。
- 全ての関節軸（$z_0, z_1, \dots, z_{n-1}$）を特定する
- 隣接する $z_{i-1}$ 軸と $z_i$ 軸の共通法線を求める
- 共通法線と $z_i$ 軸の交点を原点 $O_i$ とする
- 共通法線の方向を $x_i$ 軸とする
- $y_i = z_i \times x_i$ で $y_i$ 軸を決める
- 4つのDHパラメータ $(\theta_i, d_i, a_i, \alpha_i)$ を読み取る
この手順は初見では抽象的に感じるかもしれませんが、具体的なロボットで実際にやってみると、すぐに慣れます。後のセクションで2リンク・3リンクの具体例を通じて実践します。
座標系が設定できれば、次はいよいよDH変換行列を導出します。各リンクの座標変換を、4つの基本変換の積として体系的に表現します。
DH変換行列の導出
座標系 $i-1$ から座標系 $i$ への変換
DH規約の本質は、隣接する2つの座標系の間の変換を、4つの基本的な変換（回転と並進）の積で表現できるという点にあります。
座標系 $i-1$ から座標系 $i$ への変換を、次の4つのステップに分解して考えます。各ステップは、一つのDHパラメータに対応しています。
変換1: $z_{i-1}$ 軸まわりに $\theta_i$ 回転
まず、$z_{i-1}$ 軸まわりに角度 $\theta_i$ だけ回転します。この回転で $x_{i-1}$ 軸が共通法線の方向を向くようになります。同次変換行列は
$$ \bm{R}(z_{i-1}, \theta_i) = \begin{pmatrix} \cos\theta_i & -\sin\theta_i & 0 & 0 \\ \sin\theta_i & \cos\theta_i & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{pmatrix} $$
です。
変換2: $z_{i-1}$ 軸方向に $d_i$ 並進
次に、$z_{i-1}$ 軸に沿って $d_i$ だけ平行移動します。これにより原点が $z_{i-1}$ 軸上の共通法線の足元に移ります。
$$ \bm{T}(z_{i-1}, d_i) = \begin{pmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & d_i \\ 0 & 0 & 0 & 1 \end{pmatrix} $$
変換3: $x_i$ 軸方向に $a_i$ 並進
続いて、共通法線の方向（回転後の $x$ 軸方向）に $a_i$ だけ平行移動します。これで原点が座標系 $i$ の原点に到達します。
$$ \bm{T}(x_i, a_i) = \begin{pmatrix} 1 & 0 & 0 & a_i \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{pmatrix} $$
変換4: $x_i$ 軸まわりに $\alpha_i$ 回転
最後に、$x_i$ 軸まわりに $\alpha_i$ だけ回転します。これで $z_{i-1}$ 軸方向が $z_i$ 軸方向に一致します。
$$ \bm{R}(x_i, \alpha_i) = \begin{pmatrix} 1 & 0 & 0 & 0 \\ 0 & \cos\alpha_i & -\sin\alpha_i & 0 \\ 0 & \sin\alpha_i & \cos\alpha_i & 0 \\ 0 & 0 & 0 & 1 \end{pmatrix} $$
DH変換行列の完成
これら4つの変換を順番に掛け合わせると、リンク $i$ のDH変換行列 $\bm{T}_{i-1}^i$ が得られます。
$$ \bm{T}_{i-1}^i = \bm{R}(z_{i-1}, \theta_i) \cdot \bm{T}(z_{i-1}, d_i) \cdot \bm{T}(x_i, a_i) \cdot \bm{R}(x_i, \alpha_i) $$
行列の積を計算していきましょう。まず最初の2つの変換を掛け合わせます。$z_{i-1}$ 軸まわりの回転と $z_{i-1}$ 軸方向の並進を合成すると
$$ \bm{R}(z, \theta_i) \cdot \bm{T}(z, d_i) = \begin{pmatrix} \cos\theta_i & -\sin\theta_i & 0 & 0 \\ \sin\theta_i & \cos\theta_i & 0 & 0 \\ 0 & 0 & 1 & d_i \\ 0 & 0 & 0 & 1 \end{pmatrix} $$
となります。回転行列が並進成分に影響しないのは、並進が回転軸方向だからです。
次に、この結果に $x$ 軸方向の並進を掛けます。右から $\bm{T}(x, a_i)$ を掛けると、$(1,4)$ 成分に $a_i \cos\theta_i$、$(2,4)$ 成分に $a_i \sin\theta_i$ が加わります。
$$ \begin{pmatrix} \cos\theta_i & -\sin\theta_i & 0 & a_i\cos\theta_i \\ \sin\theta_i & \cos\theta_i & 0 & a_i\sin\theta_i \\ 0 & 0 & 1 & d_i \\ 0 & 0 & 0 & 1 \end{pmatrix} $$
最後に、$x_i$ 軸まわりの回転 $\bm{R}(x, \alpha_i)$ を右から掛けます。この回転は第2列と第3列に作用します。計算結果をまとめると、DH変換行列は次のようになります。
$$ \bm{T}_{i-1}^i = \begin{pmatrix} \cos\theta_i & -\sin\theta_i\cos\alpha_i & \sin\theta_i\sin\alpha_i & a_i\cos\theta_i \\ \sin\theta_i & \cos\theta_i\cos\alpha_i & -\cos\theta_i\sin\alpha_i & a_i\sin\theta_i \\ 0 & \sin\alpha_i & \cos\alpha_i & d_i \\ 0 & 0 & 0 & 1 \end{pmatrix} $$
この $4 \times 4$ の行列がDH変換行列です。左上の $3 \times 3$ 部分が座標系 $i$ の姿勢（回転行列）を座標系 $i-1$ で表したもの、右上の $3 \times 1$ 部分が座標系 $i$ の原点の位置を座標系 $i-1$ で表したものです。
この行列の美しさは、4つのDHパラメータ $(\theta_i, d_i, a_i, \alpha_i)$ さえ決まれば、リンクの形状がどんなに複雑でも、隣接座標系間の変換が完全に記述できるという点にあります。
全体の順運動学
$n$ 自由度のロボットアームについて、ベース座標系からエンドエフェクタ座標系への変換は、各リンクのDH変換行列を順に掛け合わせることで得られます。
$$ \bm{T}_0^n = \bm{T}_0^1 \cdot \bm{T}_1^2 \cdots \bm{T}_{n-1}^n = \prod_{i=1}^n \bm{T}_{i-1}^i $$
この積の結果として得られる $4 \times 4$ 行列から、エンドエフェクタの位置と姿勢を読み取ることができます。
$$ \bm{T}_0^n = \begin{pmatrix} \bm{R}_0^n & \bm{p}_0^n \\ \bm{0} & 1 \end{pmatrix} $$
ここで $\bm{R}_0^n \in \mathbb{R}^{3 \times 3}$ はエンドエフェクタの姿勢（回転行列）、$\bm{p}_0^n \in \mathbb{R}^3$ はエンドエフェクタの位置です。
DH変換行列の一般形が得られたので、具体的なロボットアームに適用してみましょう。まずは最もシンプルな2リンク平面マニピュレータから始めます。
具体例1: 2リンク平面マニピュレータ
問題設定
最初の例として、平面内で動く2リンクのロボットアーム（2R マニピュレータ）を考えます。これは最もシンプルな多関節ロボットで、順運動学の本質を理解するのに最適です。
2リンク平面マニピュレータは次のような構成です。
- 関節1: ベースに固定された回転関節（角度 $\theta_1$）
- リンク1: 長さ $l_1$ の剛体
- 関節2: リンク1の先端に取り付けられた回転関節（角度 $\theta_2$）
- リンク2: 長さ $l_2$ の剛体
- エンドエフェクタ: リンク2の先端
全ての関節軸は $z$ 軸方向（紙面に垂直な方向）を向いており、ロボットは $xy$ 平面内でのみ動きます。
DHパラメータの設定
DH座標系を設定し、パラメータを読み取りましょう。
座標系0（ベース）: 原点をベース関節の位置に置き、$z_0$ 軸を紙面に垂直（上向き）にとります。$x_0$ 軸は水平右向きにとります。
座標系1: $z_0$ と $z_1$ は平行（ともに紙面垂直方向）です。原点は関節2の位置に置きます。$x_1$ 軸は $z_0$ と $z_1$ の共通法線の方向で、リンク1に沿った方向です。
座標系2: 同様に、原点はエンドエフェクタの位置に置きます。
この設定でDHパラメータを読み取ると
| リンク $i$ | $\theta_i$ | $d_i$ | $a_i$ | $\alpha_i$ |
|---|---|---|---|---|
| 1 | $\theta_1$ | 0 | $l_1$ | 0 |
| 2 | $\theta_2$ | 0 | $l_2$ | 0 |
となります。全ての関節軸が平行なため $\alpha_i = 0$、全ての関節軸が同一平面上にあるため $d_i = 0$ です。これは平面マニピュレータの特徴です。
手計算による順運動学
リンク1のDH変換行列を、一般形に $\theta_1, d_1=0, a_1=l_1, \alpha_1=0$ を代入して求めます。$\cos 0 = 1$, $\sin 0 = 0$ より
$$ \bm{T}_0^1 = \begin{pmatrix} \cos\theta_1 & -\sin\theta_1 \cdot 1 & \sin\theta_1 \cdot 0 & l_1\cos\theta_1 \\ \sin\theta_1 & \cos\theta_1 \cdot 1 & -\cos\theta_1 \cdot 0 & l_1\sin\theta_1 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{pmatrix} = \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} $$
同様に、リンク2のDH変換行列は $\theta_2, d_2=0, a_2=l_2, \alpha_2=0$ を代入して
$$ \bm{T}_1^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} $$
です。全体の順運動学は $\bm{T}_0^2 = \bm{T}_0^1 \cdot \bm{T}_1^2$ です。この積を計算しましょう。
まず $(1,4)$ 成分（エンドエフェクタの $x$ 座標）を計算します。同次変換行列の積の並進成分は、回転行列が右側の並進ベクトルに作用した結果に、左側の並進ベクトルを足したものです。
$$ p_x = l_1\cos\theta_1 + l_2(\cos\theta_1\cos\theta_2 – \sin\theta_1\sin\theta_2) $$
三角関数の加法定理 $\cos\theta_1\cos\theta_2 – \sin\theta_1\sin\theta_2 = \cos(\theta_1 + \theta_2)$ を使うと
$$ p_x = l_1\cos\theta_1 + l_2\cos(\theta_1 + \theta_2) $$
と簡潔になります。同様に $y$ 座標は
$$ p_y = l_1\sin\theta_1 + l_2\sin(\theta_1 + \theta_2) $$
です。これらの式は幾何学的にも明らかです。関節1が角度 $\theta_1$ をなすとき、リンク1の先端は $(l_1\cos\theta_1, l_1\sin\theta_1)$ にあります。そこから、水平に対して角度 $\theta_1 + \theta_2$ の方向にリンク2が伸びるので、エンドエフェクタは $l_2$ だけ先の位置にあります。
全体の変換行列を書き下すと
$$ \bm{T}_0^2 = \begin{pmatrix} \cos(\theta_1+\theta_2) & -\sin(\theta_1+\theta_2) & 0 & l_1\cos\theta_1 + l_2\cos(\theta_1+\theta_2) \\ \sin(\theta_1+\theta_2) & \cos(\theta_1+\theta_2) & 0 & l_1\sin\theta_1 + l_2\sin(\theta_1+\theta_2) \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{pmatrix} $$
となります。左上の $2 \times 2$ ブロックから、エンドエフェクタの姿勢は水平方向に対して角度 $\theta_1 + \theta_2$ であることがわかります。これは「2つの関節角度の和」が先端の向きを決めるという直感的な結果です。
数値例
具体的な数値で確認しましょう。$l_1 = 1.0\,\text{m}$、$l_2 = 0.8\,\text{m}$、$\theta_1 = 45°$、$\theta_2 = 30°$ とします。
$$ p_x = 1.0\cos 45° + 0.8\cos 75° = 1.0 \times 0.7071 + 0.8 \times 0.2588 = 0.7071 + 0.2071 = 0.9142\,\text{m} $$
$$ p_y = 1.0\sin 45° + 0.8\sin 75° = 1.0 \times 0.7071 + 0.8 \times 0.9659 = 0.7071 + 0.7727 = 1.4798\,\text{m} $$
エンドエフェクタは $(0.914, 1.480)$ の位置にあり、水平に対して $75°$ の方向を向いていることになります。Pythonで検算し、可視化してみましょう。
import numpy as np
import matplotlib.pyplot as plt
def dh_transform(theta, d, a, alpha):
"""DHパラメータから同次変換行列を計算する"""
ct, st = np.cos(theta), np.sin(theta)
ca, sa = np.cos(alpha), np.sin(alpha)
return np.array([
[ct, -st * ca, st * sa, a * ct],
[st, ct * ca, -ct * sa, a * st],
[0, sa, ca, d ],
[0, 0, 0, 1 ]
])
# 2リンク平面マニピュレータのパラメータ
l1, l2 = 1.0, 0.8
theta1 = np.radians(45)
theta2 = np.radians(30)
# DH変換行列の計算
T01 = dh_transform(theta1, 0, l1, 0)
T12 = dh_transform(theta2, 0, l2, 0)
T02 = T01 @ T12
# エンドエフェクタの位置
print("=== 2リンク平面マニピュレータの順運動学 ===")
print(f"theta1 = {np.degrees(theta1):.1f}°, theta2 = {np.degrees(theta2):.1f}°")
print(f"リンク長: l1 = {l1}, l2 = {l2}")
print(f"\nT_0^2 =\n{np.round(T02, 4)}")
print(f"\nエンドエフェクタ位置: ({T02[0,3]:.4f}, {T02[1,3]:.4f})")
print(f"エンドエフェクタ姿勢角: {np.degrees(np.arctan2(T02[1,0], T02[0,0])):.1f}°")
# 解析解との比較
px_analytic = l1 * np.cos(theta1) + l2 * np.cos(theta1 + theta2)
py_analytic = l1 * np.sin(theta1) + l2 * np.sin(theta1 + theta2)
print(f"\n解析解: ({px_analytic:.4f}, {py_analytic:.4f})")
# 可視化
fig, ax = plt.subplots(1, 1, figsize=(8, 8))
# 各関節の位置を計算
origin = np.array([0, 0])
joint1 = np.array([T01[0, 3], T01[1, 3]])
end_effector = np.array([T02[0, 3], T02[1, 3]])
# リンクの描画
joints_x = [origin[0], joint1[0], end_effector[0]]
joints_y = [origin[1], joint1[1], end_effector[1]]
ax.plot(joints_x, joints_y, 'o-', color='#2196F3', linewidth=3,
markersize=10, markerfacecolor='#FF5722', markeredgecolor='white',
markeredgewidth=2, label='Robot arm')
# 座標系の描画
ax.annotate('Base (0,0)', xy=origin, xytext=(0.05, -0.1),
fontsize=10, color='gray')
ax.annotate(f'Joint 2\n({joint1[0]:.3f}, {joint1[1]:.3f})',
xy=joint1, xytext=(joint1[0]+0.08, joint1[1]-0.12),
fontsize=9, color='gray')
ax.annotate(f'End Effector\n({end_effector[0]:.3f}, {end_effector[1]:.3f})',
xy=end_effector, xytext=(end_effector[0]+0.08, end_effector[1]+0.05),
fontsize=9, color='gray')
# 角度の弧
angle_arc1 = np.linspace(0, theta1, 30)
ax.plot(0.25 * np.cos(angle_arc1), 0.25 * np.sin(angle_arc1),
'g-', linewidth=1.5)
ax.text(0.3 * np.cos(theta1/2), 0.3 * np.sin(theta1/2),
f'θ₁={np.degrees(theta1):.0f}°', fontsize=9, color='green')
# ワークスペースの外枠（到達範囲）
theta_range = np.linspace(0, 2*np.pi, 200)
outer_r = l1 + l2
inner_r = abs(l1 - l2)
ax.plot(outer_r * np.cos(theta_range), outer_r * np.sin(theta_range),
'--', color='lightgray', linewidth=1, label=f'Outer reach (r={outer_r})')
ax.plot(inner_r * np.cos(theta_range), inner_r * np.sin(theta_range),
'--', color='lightgray', linewidth=1, label=f'Inner reach (r={inner_r})')
ax.set_xlim(-2.2, 2.2)
ax.set_ylim(-2.2, 2.2)
ax.set_aspect('equal')
ax.grid(True, alpha=0.3)
ax.legend(loc='lower right')
ax.set_xlabel('x [m]')
ax.set_ylabel('y [m]')
ax.set_title('2-Link Planar Manipulator: Forward Kinematics')
plt.tight_layout()
plt.savefig('2link_planar_fk.png', dpi=150, bbox_inches='tight')
plt.show()
このコードでは、dh_transform 関数を定義してDH変換行列を計算し、2リンクマニピュレータの順運動学を解いています。出力結果から、行列計算で求めたエンドエフェクタ位置 $(0.9142, 1.4798)$ が、先ほどの手計算の解析解と完全に一致していることが確認できます。
グラフでは、青い線でリンク、オレンジの点で関節を表しています。灰色の破線は到達可能範囲の外周（$r = l_1 + l_2 = 1.8$、完全に伸ばしたとき）と内周（$r = |l_1 – l_2| = 0.2$、完全に折り畳んだとき）を示しています。エンドエフェクタはこのドーナツ状の領域内のどこにでも到達できます。
2リンクの結果を確認できました。次は空間的に関節軸の方向が変わる、より現実的な3リンクマニピュレータに挑戦します。
具体例2: 3リンク空間マニピュレータ
問題設定
次に、3つの回転関節を持つ空間マニピュレータを考えます。平面マニピュレータとの決定的な違いは、関節軸の方向が異なる（$\alpha_i \neq 0$ の場合がある）ことです。これにより、アームが3次元空間内を動けるようになります。
具体的に、以下の構成のロボットアームを考えましょう。
- 関節1: ベースに固定された回転関節。回転軸は鉛直方向（$z_0$ 軸）。ベースの回転（旋回）を担当
- 関節2: 肩に相当する回転関節。回転軸は水平方向。リンク1を前後に傾ける
- 関節3: 肘に相当する回転関節。回転軸は関節2と平行。リンク2を上下に曲げる
- エンドエフェクタ: リンク2の先端
これは産業用ロボットや宇宙ロボットアームで基本的な「旋回 + 肩 + 肘」の構成を模したものです。
DHパラメータの設定
座標系を設定し、DHパラメータを読み取ります。
座標系0（ベース）: 原点をベース関節に置き、$z_0$ を鉛直上向きにとります。
座標系1: 関節2の回転軸が $z_1$ です。関節1の回転軸（$z_0$, 鉛直）と関節2の回転軸（$z_1$, 水平）は直交しているため、$\alpha_1 = -90° = -\pi/2$ です。リンク1の高さ分のオフセット $d_1$ が存在します。
座標系2: 関節3の回転軸が $z_2$ で、$z_1$ と平行です。$\alpha_2 = 0$。原点は関節3の位置にあり、リンク長 $a_2$ はリンク1の長さです。
座標系3（エンドエフェクタ）: $z_2$ と $z_3$ は平行で $\alpha_3 = 0$。リンク長 $a_3$ はリンク2の長さです。
以上をまとめると
| リンク $i$ | $\theta_i$ | $d_i$ | $a_i$ | $\alpha_i$ |
|---|---|---|---|---|
| 1 | $\theta_1$ | $d_1$ | 0 | $-\pi/2$ |
| 2 | $\theta_2$ | 0 | $a_2$ | 0 |
| 3 | $\theta_3$ | 0 | $a_3$ | 0 |
ここで $d_1$ はベースから肩関節までの高さ、$a_2$ は上腕の長さ、$a_3$ は前腕の長さです。
DH変換行列の計算
各リンクのDH変換行列を計算します。
リンク1は $\alpha_1 = -\pi/2$ のため、$\cos(-\pi/2) = 0$, $\sin(-\pi/2) = -1$ を代入します。また $a_1 = 0$ なので
$$ \bm{T}_0^1 = \begin{pmatrix} \cos\theta_1 & 0 & -\sin\theta_1 & 0 \\ \sin\theta_1 & 0 & \cos\theta_1 & 0 \\ 0 & -1 & 0 & d_1 \\ 0 & 0 & 0 & 1 \end{pmatrix} $$
$\alpha_1 = -\pi/2$ のため、$-\sin\theta_1\cos\alpha_1 = -\sin\theta_1 \cdot 0 = 0$、$\sin\theta_1\sin\alpha_1 = \sin\theta_1 \cdot (-1) = -\sin\theta_1$ となっていることを確認してください。
リンク2は平面リンク（$\alpha_2 = 0$, $d_2 = 0$）なので
$$ \bm{T}_1^2 = \begin{pmatrix} \cos\theta_2 & -\sin\theta_2 & 0 & a_2\cos\theta_2 \\ \sin\theta_2 & \cos\theta_2 & 0 & a_2\sin\theta_2 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{pmatrix} $$
リンク3も同様に
$$ \bm{T}_2^3 = \begin{pmatrix} \cos\theta_3 & -\sin\theta_3 & 0 & a_3\cos\theta_3 \\ \sin\theta_3 & \cos\theta_3 & 0 & a_3\sin\theta_3 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{pmatrix} $$
エンドエフェクタ位置の解析
全体の変換行列 $\bm{T}_0^3 = \bm{T}_0^1 \cdot \bm{T}_1^2 \cdot \bm{T}_2^3$ の計算は手動では煩雑ですが、構造を理解するためにエンドエフェクタの位置成分だけを導出してみましょう。
$\bm{T}_0^1$ の回転部分を見ると、座標系1では「$z_1$ 軸が水平方向」になっています。関節1の角度 $\theta_1$ はベースの旋回角で、関節2以降の動きは旋回後の鉛直平面内で行われます。
この構造から、エンドエフェクタ位置は次の形になることが予想できます。鉛直平面内での腕の伸びを $r$ とし、高さを $h$ とすると
$$ r = a_2\cos\theta_2 + a_3\cos(\theta_2 + \theta_3) $$
$$ h = a_2\sin\theta_2 + a_3\sin(\theta_2 + \theta_3) + d_1 $$
これを関節1の旋回角 $\theta_1$ で $xy$ 平面上に射影すると
$$ p_x = r\cos\theta_1 = [a_2\cos\theta_2 + a_3\cos(\theta_2 + \theta_3)]\cos\theta_1 $$
$$ p_y = r\sin\theta_1 = [a_2\cos\theta_2 + a_3\cos(\theta_2 + \theta_3)]\sin\theta_1 $$
$$ p_z = a_2\sin\theta_2 + a_3\sin(\theta_2 + \theta_3) + d_1 $$
この結果は直感的にも理解できます。関節2と関節3は「鉛直平面内の2リンクアーム」として動き、その全体を関節1が水平面内で旋回させています。つまり、3リンク空間マニピュレータは「旋回する平面マニピュレータ」として理解できるのです。
数値例
$d_1 = 0.5\,\text{m}$, $a_2 = 1.0\,\text{m}$, $a_3 = 0.8\,\text{m}$、$\theta_1 = 30°$, $\theta_2 = 45°$, $\theta_3 = -30°$ として計算してみます。
まず鉛直平面内の成分を計算します。
$$ r = 1.0\cos 45° + 0.8\cos(45° – 30°) = 0.7071 + 0.8\cos 15° = 0.7071 + 0.7727 = 1.4798\,\text{m} $$
$$ h = 1.0\sin 45° + 0.8\sin 15° + 0.5 = 0.7071 + 0.2071 + 0.5 = 1.4142\,\text{m} $$
旋回角 $\theta_1 = 30°$ で射影すると
$$ p_x = 1.4798\cos 30° = 1.4798 \times 0.8660 = 1.2812\,\text{m} $$
$$ p_y = 1.4798\sin 30° = 1.4798 \times 0.5 = 0.7399\,\text{m} $$
$$ p_z = 1.4142\,\text{m} $$
この数値もPythonで検証します。解析式と行列計算の両方で同じ結果が得られることを確認しましょう。
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)
return np.array([
[ct, -st * ca, st * sa, a * ct],
[st, ct * ca, -ct * sa, a * st],
[0, sa, ca, d ],
[0, 0, 0, 1 ]
])
# 3リンク空間マニピュレータのパラメータ
d1 = 0.5
a2, a3 = 1.0, 0.8
theta1 = np.radians(30)
theta2 = np.radians(45)
theta3 = np.radians(-30)
# DH変換行列の計算
T01 = dh_transform(theta1, d1, 0, -np.pi/2)
T12 = dh_transform(theta2, 0, a2, 0)
T23 = dh_transform(theta3, 0, a3, 0)
T02 = T01 @ T12
T03 = T02 @ T23
# エンドエフェクタの位置
print("=== 3リンク空間マニピュレータの順運動学 ===")
print(f"θ1={np.degrees(theta1):.0f}°, θ2={np.degrees(theta2):.0f}°, θ3={np.degrees(theta3):.0f}°")
print(f"d1={d1}, a2={a2}, a3={a3}")
print(f"\nT_0^3 =\n{np.round(T03, 4)}")
print(f"\nエンドエフェクタ位置: ({T03[0,3]:.4f}, {T03[1,3]:.4f}, {T03[2,3]:.4f})")
# 解析解との比較
r = a2 * np.cos(theta2) + a3 * np.cos(theta2 + theta3)
h = a2 * np.sin(theta2) + a3 * np.sin(theta2 + theta3) + d1
px = r * np.cos(theta1)
py = r * np.sin(theta1)
pz = h
print(f"解析解: ({px:.4f}, {py:.4f}, {pz:.4f})")
print(f"\n誤差: {np.linalg.norm([T03[0,3]-px, T03[1,3]-py, T03[2,3]-pz]):.2e}")
この出力から、行列計算の結果と解析的に導いた式の結果が一致し、誤差がほぼゼロ（浮動小数点精度レベル）であることが確認できます。DH変換行列による体系的な方法と、幾何学的な直感に基づく手計算が整合することは、理論の正しさを裏付けるものです。
ここまでの2つの具体例で、DHパラメータの設定から順運動学の計算までの一連の流れを体験しました。次のセクションでは、任意の自由度のロボットアームに対応する汎用的なPythonコードを実装し、3D可視化を行います。
Pythonでの汎用的な順運動学計算と3D可視化
汎用順運動学クラスの設計
これまでは個別のマニピュレータに対してDH変換行列を直接計算しましたが、実用的にはDHパラメータのテーブルを入力として受け取り、任意のロボットの順運動学を計算できる汎用的なコードが便利です。
設計方針は次の通りです。
- DHパラメータテーブル（各リンクの $\theta_i, d_i, a_i, \alpha_i$ と関節タイプ）を入力
- 関節変数を受け取り、各リンクの変換行列を計算
- 全リンクの変換行列を累積して各関節位置とエンドエフェクタの位置・姿勢を返す
import numpy as np
class ForwardKinematics:
"""DHパラメータに基づく順運動学計算クラス"""
def __init__(self, dh_params):
"""
Parameters
----------
dh_params : list of dict
各リンクのDHパラメータ。各辞書は以下のキーを持つ:
- 'a': リンク長
- 'alpha': リンクねじれ角 [rad]
- 'd': リンクオフセット
- 'theta': 関節角度 [rad] (回転関節のデフォルト値)
- 'joint_type': 'revolute' or 'prismatic'
"""
self.dh_params = dh_params
self.n_joints = len(dh_params)
@staticmethod
def dh_matrix(theta, d, a, alpha):
"""単一リンクのDH変換行列を計算する"""
ct, st = np.cos(theta), np.sin(theta)
ca, sa = np.cos(alpha), np.sin(alpha)
return np.array([
[ct, -st * ca, st * sa, a * ct],
[st, ct * ca, -ct * sa, a * st],
[0, sa, ca, d ],
[0, 0, 0, 1 ]
])
def compute(self, q):
"""
順運動学を計算する
Parameters
----------
q : array-like, shape (n_joints,)
関節変数のベクトル
Returns
-------
T_list : list of ndarray, shape (n_joints+1, 4, 4)
ベースから各関節・エンドエフェクタまでの累積変換行列
positions : ndarray, shape (n_joints+1, 3)
各関節とエンドエフェクタの3次元位置
"""
q = np.asarray(q, dtype=float)
T_list = [np.eye(4)] # ベース座標系
for i, params in enumerate(self.dh_params):
if params['joint_type'] == 'revolute':
theta_i = q[i]
d_i = params['d']
else: # prismatic
theta_i = params['theta']
d_i = q[i]
T_i = self.dh_matrix(theta_i, d_i, params['a'], params['alpha'])
T_list.append(T_list[-1] @ T_i)
positions = np.array([T[:3, 3] for T in T_list])
return T_list, positions
このクラスは、DHパラメータテーブルを初期化時に受け取り、compute メソッドで関節変数ベクトル $\bm{q}$ を渡すと、各座標系の累積変換行列と各関節の3次元位置を返します。joint_type を指定することで、回転関節と直動関節の両方に対応しています。
2リンク平面マニピュレータの検証
先ほど手計算で求めた2リンク平面マニピュレータの結果を、汎用クラスで再現できるか確認します。
import numpy as np
# ForwardKinematicsクラスは上のセルで定義済みとする
# 2リンク平面マニピュレータのDHパラメータ
dh_2link = [
{'a': 1.0, 'alpha': 0, 'd': 0, 'theta': 0, 'joint_type': 'revolute'},
{'a': 0.8, 'alpha': 0, 'd': 0, 'theta': 0, 'joint_type': 'revolute'},
]
fk = ForwardKinematics(dh_2link)
q = [np.radians(45), np.radians(30)]
T_list, positions = fk.compute(q)
print("=== 2リンクマニピュレータ（汎用クラス検証） ===")
for i, pos in enumerate(positions):
label = "Base" if i == 0 else f"Joint {i}" if i < len(positions)-1 else "End Effector"
print(f"{label}: ({pos[0]:.4f}, {pos[1]:.4f}, {pos[2]:.4f})")
print(f"\nエンドエフェクタ変換行列:\n{np.round(T_list[-1], 4)}")
出力を確認すると、エンドエフェクタ位置が $(0.9142, 1.4798, 0.0)$ となり、手計算の結果と完全に一致します。$z$ 成分が $0$ であることも、平面マニピュレータとして正しい結果です。
3リンク空間マニピュレータの3D可視化
いよいよ3D可視化です。3リンク空間マニピュレータを立体的に描画し、関節角度を変化させたときの姿勢を視覚的に確認します。
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
class ForwardKinematics:
"""DHパラメータに基づく順運動学計算クラス"""
def __init__(self, dh_params):
self.dh_params = dh_params
self.n_joints = len(dh_params)
@staticmethod
def dh_matrix(theta, d, a, alpha):
ct, st = np.cos(theta), np.sin(theta)
ca, sa = np.cos(alpha), np.sin(alpha)
return np.array([
[ct, -st * ca, st * sa, a * ct],
[st, ct * ca, -ct * sa, a * st],
[0, sa, ca, d ],
[0, 0, 0, 1 ]
])
def compute(self, q):
q = np.asarray(q, dtype=float)
T_list = [np.eye(4)]
for i, params in enumerate(self.dh_params):
if params['joint_type'] == 'revolute':
theta_i = q[i]
d_i = params['d']
else:
theta_i = params['theta']
d_i = q[i]
T_i = self.dh_matrix(theta_i, d_i, params['a'], params['alpha'])
T_list.append(T_list[-1] @ T_i)
positions = np.array([T[:3, 3] for T in T_list])
return T_list, positions
# 3リンク空間マニピュレータのDHパラメータ
dh_3link = [
{'a': 0, 'alpha': -np.pi/2, 'd': 0.5, 'theta': 0, 'joint_type': 'revolute'},
{'a': 1.0, 'alpha': 0, 'd': 0, 'theta': 0, 'joint_type': 'revolute'},
{'a': 0.8, 'alpha': 0, 'd': 0, 'theta': 0, 'joint_type': 'revolute'},
]
fk = ForwardKinematics(dh_3link)
# 複数の関節角度パターンで描画
configs = [
([np.radians(0), np.radians(45), np.radians(-30)], 'Config A: θ=(0°, 45°, -30°)'),
([np.radians(30), np.radians(45), np.radians(-30)], 'Config B: θ=(30°, 45°, -30°)'),
([np.radians(60), np.radians(30), np.radians(-60)], 'Config C: θ=(60°, 30°, -60°)'),
([np.radians(90), np.radians(60), np.radians(0)], 'Config D: θ=(90°, 60°, 0°)'),
]
fig = plt.figure(figsize=(14, 10))
colors = ['#2196F3', '#FF5722', '#4CAF50', '#9C27B0']
for idx, (q, label) in enumerate(configs):
ax = fig.add_subplot(2, 2, idx + 1, projection='3d')
T_list, positions = fk.compute(q)
# リンクの描画
ax.plot(positions[:, 0], positions[:, 1], positions[:, 2],
'o-', color=colors[idx], linewidth=3, markersize=8,
markerfacecolor='white', markeredgecolor=colors[idx],
markeredgewidth=2)
# ベースを強調
ax.scatter(*positions[0], color='black', s=100, zorder=5, marker='s')
# エンドエフェクタを強調
ax.scatter(*positions[-1], color='red', s=80, zorder=5, marker='*')
# 座標軸（エンドエフェクタの姿勢を表示）
T_ee = T_list[-1]
origin_ee = T_ee[:3, 3]
axis_len = 0.2
for j, (color_ax, name) in enumerate(zip(['r', 'g', 'b'], ['x', 'y', 'z'])):
direction = T_ee[:3, j]
ax.quiver(*origin_ee, *direction * axis_len,
color=color_ax, arrow_length_ratio=0.2, linewidth=1.5)
ax.set_xlim([-2, 2])
ax.set_ylim([-2, 2])
ax.set_zlim([0, 2.5])
ax.set_xlabel('X [m]')
ax.set_ylabel('Y [m]')
ax.set_zlabel('Z [m]')
ax.set_title(label, fontsize=10)
ax.view_init(elev=25, azim=-60)
plt.suptitle('3-Link Spatial Manipulator: Forward Kinematics', fontsize=14, y=1.01)
plt.tight_layout()
plt.savefig('3link_spatial_fk.png', dpi=150, bbox_inches='tight')
plt.show()
4つの異なる関節角度パターンで3リンク空間マニピュレータを可視化しています。各サブプロットでは、黒い四角がベース、白抜きの丸が関節、赤い星がエンドエフェクタを示しています。エンドエフェクタには座標軸（赤=x, 緑=y, 青=z）が描画されており、先端の姿勢がわかります。
Config Aとconfig Bを比較すると、$\theta_1$（旋回角）だけが $0° \to 30°$ に変化しており、アーム全体が水平面内で回転していることが読み取れます。一方、config Cでは $\theta_3 = -60°$ と肘を大きく曲げているため、エンドエフェクタがベースに近い位置に来ています。Config Dでは $\theta_3 = 0°$ で肘が伸びきった状態なので、アームが最も長く伸びた姿勢になっています。
ワークスペースの可視化
順運動学を使えば、ロボットアームのワークスペース（作業空間、到達可能な領域）を可視化できます。全ての関節角度を網羅的に変化させ、エンドエフェクタが到達できる点の集合をプロットします。
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
class ForwardKinematics:
"""DHパラメータに基づく順運動学計算クラス"""
def __init__(self, dh_params):
self.dh_params = dh_params
self.n_joints = len(dh_params)
@staticmethod
def dh_matrix(theta, d, a, alpha):
ct, st = np.cos(theta), np.sin(theta)
ca, sa = np.cos(alpha), np.sin(alpha)
return np.array([
[ct, -st * ca, st * sa, a * ct],
[st, ct * ca, -ct * sa, a * st],
[0, sa, ca, d ],
[0, 0, 0, 1 ]
])
def compute(self, q):
q = np.asarray(q, dtype=float)
T_list = [np.eye(4)]
for i, params in enumerate(self.dh_params):
if params['joint_type'] == 'revolute':
theta_i = q[i]
d_i = params['d']
else:
theta_i = params['theta']
d_i = q[i]
T_i = self.dh_matrix(theta_i, d_i, params['a'], params['alpha'])
T_list.append(T_list[-1] @ T_i)
positions = np.array([T[:3, 3] for T in T_list])
return T_list, positions
# 3リンク空間マニピュレータのDHパラメータ
dh_3link = [
{'a': 0, 'alpha': -np.pi/2, 'd': 0.5, 'theta': 0, 'joint_type': 'revolute'},
{'a': 1.0, 'alpha': 0, 'd': 0, 'theta': 0, 'joint_type': 'revolute'},
{'a': 0.8, 'alpha': 0, 'd': 0, 'theta': 0, 'joint_type': 'revolute'},
]
fk = ForwardKinematics(dh_3link)
# ワークスペースの計算
n_samples = 20 # 各関節の分割数
theta1_range = np.linspace(0, 2 * np.pi, n_samples)
theta2_range = np.linspace(-np.pi/2, np.pi/2, n_samples)
theta3_range = np.linspace(-2*np.pi/3, 2*np.pi/3, n_samples)
workspace_points = []
for t1 in theta1_range:
for t2 in theta2_range:
for t3 in theta3_range:
_, positions = fk.compute([t1, t2, t3])
workspace_points.append(positions[-1])
workspace_points = np.array(workspace_points)
# 可視化
fig = plt.figure(figsize=(14, 6))
# 3Dワークスペース
ax1 = fig.add_subplot(121, projection='3d')
ax1.scatter(workspace_points[:, 0], workspace_points[:, 1],
workspace_points[:, 2], c=workspace_points[:, 2],
cmap='viridis', s=1, alpha=0.3)
ax1.set_xlabel('X [m]')
ax1.set_ylabel('Y [m]')
ax1.set_zlabel('Z [m]')
ax1.set_title('3D Workspace')
ax1.view_init(elev=20, azim=-45)
# XZ平面への射影（theta1=0の断面）
ax2 = fig.add_subplot(122)
workspace_slice = []
for t2 in np.linspace(-np.pi/2, np.pi/2, 50):
for t3 in np.linspace(-2*np.pi/3, 2*np.pi/3, 50):
_, positions = fk.compute([0, t2, t3])
workspace_slice.append(positions[-1])
workspace_slice = np.array(workspace_slice)
ax2.scatter(workspace_slice[:, 0], workspace_slice[:, 2],
c='#2196F3', s=1, alpha=0.3)
ax2.set_xlabel('X [m]')
ax2.set_ylabel('Z [m]')
ax2.set_title('Workspace Cross-Section (θ₁ = 0°)')
ax2.set_aspect('equal')
ax2.grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('workspace_3link.png', dpi=150, bbox_inches='tight')
plt.show()
print(f"ワークスペースの点数: {len(workspace_points)}")
print(f"X範囲: [{workspace_points[:,0].min():.2f}, {workspace_points[:,0].max():.2f}]")
print(f"Y範囲: [{workspace_points[:,1].min():.2f}, {workspace_points[:,1].max():.2f}]")
print(f"Z範囲: [{workspace_points[:,2].min():.2f}, {workspace_points[:,2].max():.2f}]")
左の3Dプロットでは、エンドエフェクタが到達可能な全ての点を色つきで表示しています。色は高さ（$z$ 座標）に対応しており、ワークスペースが球殻状の3次元領域を形成していることがわかります。関節1の旋回により、$xy$ 平面内では軸対称な形状となっています。
右の断面図は $\theta_1 = 0°$ に固定した場合の $xz$ 平面における到達可能領域です。これは本質的に2リンク平面マニピュレータのワークスペースに対応しており、ドーナツ状の断面が観察されます。ベースの高さ $d_1 = 0.5\,\text{m}$ 分だけ上方にシフトしていることも読み取れます。
順運動学のアニメーション
最後に、関節角度を滑らかに変化させたときのアームの動きをフレームごとに描画するコードを示します。
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
class ForwardKinematics:
"""DHパラメータに基づく順運動学計算クラス"""
def __init__(self, dh_params):
self.dh_params = dh_params
self.n_joints = len(dh_params)
@staticmethod
def dh_matrix(theta, d, a, alpha):
ct, st = np.cos(theta), np.sin(theta)
ca, sa = np.cos(alpha), np.sin(alpha)
return np.array([
[ct, -st * ca, st * sa, a * ct],
[st, ct * ca, -ct * sa, a * st],
[0, sa, ca, d ],
[0, 0, 0, 1 ]
])
def compute(self, q):
q = np.asarray(q, dtype=float)
T_list = [np.eye(4)]
for i, params in enumerate(self.dh_params):
if params['joint_type'] == 'revolute':
theta_i = q[i]
d_i = params['d']
else:
theta_i = params['theta']
d_i = q[i]
T_i = self.dh_matrix(theta_i, d_i, params['a'], params['alpha'])
T_list.append(T_list[-1] @ T_i)
positions = np.array([T[:3, 3] for T in T_list])
return T_list, positions
# 3リンク空間マニピュレータのDHパラメータ
dh_3link = [
{'a': 0, 'alpha': -np.pi/2, 'd': 0.5, 'theta': 0, 'joint_type': 'revolute'},
{'a': 1.0, 'alpha': 0, 'd': 0, 'theta': 0, 'joint_type': 'revolute'},
{'a': 0.8, 'alpha': 0, 'd': 0, 'theta': 0, 'joint_type': 'revolute'},
]
fk = ForwardKinematics(dh_3link)
# 時刻に沿った関節角度の軌跡（sin波で滑らかに変化）
n_frames = 8
t = np.linspace(0, 2 * np.pi, n_frames, endpoint=False)
fig = plt.figure(figsize=(16, 8))
trajectory = []
for idx, ti in enumerate(t):
q = [
np.radians(60) * np.sin(ti), # 旋回: ±60°
np.radians(45) + np.radians(30) * np.sin(2 * ti), # 肩: 15°~75°
np.radians(-30) + np.radians(20) * np.sin(3 * ti), # 肘: -50°~-10°
]
T_list, positions = fk.compute(q)
trajectory.append(positions[-1].copy())
ax = fig.add_subplot(2, 4, idx + 1, projection='3d')
# リンクの描画
ax.plot(positions[:, 0], positions[:, 1], positions[:, 2],
'o-', color='#2196F3', linewidth=2.5, markersize=6,
markerfacecolor='white', markeredgecolor='#2196F3',
markeredgewidth=2)
# ベースを強調
ax.scatter(*positions[0], color='black', s=60, zorder=5, marker='s')
# エンドエフェクタを強調
ax.scatter(*positions[-1], color='red', s=50, zorder=5, marker='*')
# 軌跡を描画
if len(trajectory) > 1:
traj = np.array(trajectory)
ax.plot(traj[:, 0], traj[:, 1], traj[:, 2],
'--', color='red', linewidth=1, alpha=0.5)
ax.set_xlim([-2, 2])
ax.set_ylim([-2, 2])
ax.set_zlim([0, 2.5])
ax.set_xlabel('X', fontsize=7)
ax.set_ylabel('Y', fontsize=7)
ax.set_zlabel('Z', fontsize=7)
ax.set_title(f'Frame {idx+1}', fontsize=9)
ax.view_init(elev=25, azim=-60)
ax.tick_params(labelsize=6)
plt.suptitle('3-Link Manipulator Motion Sequence', fontsize=14)
plt.tight_layout()
plt.savefig('3link_motion_sequence.png', dpi=150, bbox_inches='tight')
plt.show()
8フレームのモーションシーケンスとして、3リンクマニピュレータが各関節を正弦波状に動かしている様子を描画しています。赤い破線はエンドエフェクタの軌跡を示しており、3つの関節角度が異なる周波数で振動するため、先端は3次元空間内で複雑な曲線を描きます。
フレームが進むにつれて軌跡が伸びていく様子から、順運動学が「各時刻の関節角度から先端位置を計算する」という操作を時系列で繰り返していることが視覚的に理解できます。実際のロボット制御では、この計算を毎制御周期（数ms）ごとに行い、エンドエフェクタの現在位置をリアルタイムに把握しています。
宇宙ロボットアームへの拡張
ここまでの3リンクモデルは、宇宙ロボットアームの簡略化モデルです。実際のカナダアーム2（SSRMS）は7自由度で、DHパラメータテーブルは7行になります。しかし計算原理は全く同じで、ForwardKinematics クラスの dh_params に7つのリンクの情報を追加するだけで対応できます。
宇宙ロボットに特有の順運動学の課題として、以下の点があります。
浮遊ベースの問題: 地上の産業用ロボットはベースが固定されていますが、宇宙ステーションのロボットアームはベースとなる宇宙ステーション自体が微小重力環境で浮いています。アームの動きがステーション本体に反作用を及ぼすため、厳密にはベースの6自由度の運動も考慮した拡張順運動学が必要です。
冗長自由度の活用: 7自由度のアームは6自由度の作業空間に対して1自由度の冗長性を持ちます。同じ先端位置・姿勢を実現する関節角度が無限に存在するため、障害物回避や関節リミット回避に冗長自由度を活用できます。この冗長性の管理は逆運動学の課題です。
極低温環境での精度: 宇宙空間では太陽に照らされた面と影の面で100度以上の温度差が生じ、リンクが熱膨張・収縮します。DHパラメータ（特にリンク長 $a_i$）が温度依存になるため、高精度な順運動学には温度補正が必要です。
まとめ
本記事では、ロボットアームの順運動学をDHパラメータに基づいて体系的に解く方法を解説しました。
- 順運動学は関節変数ベクトル $\bm{q}$ からエンドエフェクタの位置と姿勢を求める写像であり、ロボット工学の最も基本的な計算である
- DHパラメータ（$\theta_i, d_i, a_i, \alpha_i$）はリンクと関節の幾何学的配置をたった4つの量で記述し、座標系設定のルールを標準化する
- DH変換行列は4つの基本変換（$z$ 軸回転 $\to$ $z$ 軸並進 $\to$ $x$ 軸並進 $\to$ $x$ 軸回転）の積で導出される
- 2リンク平面マニピュレータでは $p_x = l_1\cos\theta_1 + l_2\cos(\theta_1+\theta_2)$, $p_y = l_1\sin\theta_1 + l_2\sin(\theta_1+\theta_2)$ という見通しのよい結果が得られる
- 3リンク空間マニピュレータは「旋回する2リンクアーム」として直感的に理解でき、DHパラメータの体系的な方法で同じ結果が再現できることを確認した
- Pythonの汎用クラスにより、任意の自由度のロボットアームの順運動学を計算・可視化できる
順運動学は「関節角度が決まれば先端位置が一意に決まる」という問題です。しかし実際のロボット制御では、逆の問題 — 「先端をこの位置に動かしたいとき、各関節をどの角度にすればよいか」— がより重要です。これが逆運動学（inverse kinematics）であり、次の記事で扱います。
次のステップとして、以下の記事も参考にしてください。