ロボット工学１１
note Item Type Metadata
note
# 同次変換行列とヤコビ行列
## 導入：2自由度から3自由度への拡張
前回の内容では、2自由度アームの順運動学を平面上で図解により式を求めました。
3自由度アーム（立体的）の場合、図解でも式を立てることは可能ですが、複雑でわかりにくくなります。そこで、**関節リンクパラメータ**を使った表現方法が用いられます。
### 同次変換行列の必要性
3次元空間でのロボットアームの位置と姿勢を表現する方法として、**同次変換行列**（4×4行列）が広く使われます。
#### なぜ同次変換行列を使うのか？
- **回転**と**平行移動**を一つの行列で表現できる
- 式を単純化し、理解を助けることができる
別の方法として、回転を3×3行列$\mathbf{R}$、平行移動を3次元ベクトル$\mathbf{P}$で別々に表現する方法もありますが、同次変換行列を使うことで座標変換の式がシンプルになります。
## 同次変換行列の定義
### 数学的定義
同次変換行列$\mathbf{T}$は以下のように定義されます：
$$
\mathbf{T} = \begin{bmatrix}
\mathbf{R} & \mathbf{P} \\
\mathbf{0}^T & 1
\end{bmatrix} = \begin{bmatrix}
r_{11} & r_{12} & r_{13} & p_x \\
r_{21} & r_{22} & r_{23} & p_y \\
r_{31} & r_{32} & r_{33} & p_z \\
0 & 0 & 0 & 1
\end{bmatrix}
$$
ここで：
- $\mathbf{R}$：回転行列（3×3の直交行列）
- $\mathbf{P}$：平行移動ベクトル（3次元ベクトル）
- $\mathbf{0}^T$：零ベクトルの転置（$[0, 0, 0]$）
- 最後の行$[0, 0, 0, 1]$は次元合わせのため。これによりベクトルとの積が定義できる
```mermaid
graph TB
subgraph T["同次変換行列 T (4×4)"]
R["回転行列 R<br/>(3×3)"]
P["平行移動ベクトル P<br/>(3×1)"]
Z["零ベクトル 0^T<br/>(1×3)"]
O["1<br/>(スカラー)"]
end
T --> R
T --> P
T --> Z
T --> O
```
**同次変換行列の構造**：回転と平行移動を統一的に表現
### 回転行列の性質
回転行列$\mathbf{R}$は以下の性質を持ちます：
1. **直交性**：$\mathbf{R}^T \mathbf{R} = \mathbf{I}$（単位行列）
2. **行列式**：$\det(\mathbf{R}) = 1$
3. **逆行列**：$\mathbf{R}^{-1} = \mathbf{R}^T$
## 平行移動と回転の定義
### 平行移動
**平行移動**とは、物体の位置を向きを変えずに一定の方向に移動させることです。
- 姿勢が変化しない
- 3次元ベクトル$\mathbf{P} = [p_x, p_y, p_z]^T$で表現される
### 回転
**回転**とは、位置を変えずに向きだけを変えることです。
- 回転は向きを回転させる = 姿勢を回転させる
- 回転とは、3次元ベクトルを合同関係のまま軸で回転させることともいえる
## 回転の表現方法
3次元空間での回転を表現する方法には、主に以下の3つがあります。
### 1. ロール・ピッチ・ヨー（Roll, Pitch, Yaw）法
主に航空機やドローンなど、航空分野でよく使われる回転角の表現方法です。3つの軸周りの回転を順番に指定します。
- **ロール（Roll）**：機体の前後軸（X軸、進行方向）を中心にした回転。飛行機が「バンク」する動作（左右に傾く）
- **ピッチ（Pitch）**：機体の左右横軸（Y軸）を中心にした回転。機首上げ・下げの動作
- **ヨー（Yaw）**：機体の上下縦軸（Z軸）を中心にした回転。機体が左右を向く滑走路旋回の動作
この順序で3次元の回転を表せます。ロボットアームの軸順表現にも使われることがあります。
### 2. オイラー角
天文分野やロボット工学でも使われる伝統的な回転表現方法です。
- 3つの座標軸または移動座標軸に対して、順番に回転を適用します
- オイラー角の回転順序例（ZYX順等）が複数存在し、手法ごとに回転の意味や範囲が変わります
- 任意の回転を、「1つ目の軸」→「新しい2つ目の軸」→「新しい3つ目の軸」として表現できます
- 使いやすいが、「ジンバルロック」（特定の姿勢で2軸の向きが一致して自由度が減る現象）が起こる場合があります
### 3. 単位クォータニオン（Quaternion）
4元数とも言い、回転専用に設計された数学的な表現です。
- ベクトル$(x, y, z)$成分とスカラー成分$w$で構成される計算手法です
- 任意の回転軸とその軸周りの回転角$\theta$を「軸+角度」で簡潔に記述できます
- 回転の合成や、連続運動の補間（スムーズな回転）に適します
- コンピュータグラフィックスやロボットのアニメーションによく使われます
- ジンバルロック問題が発生しません
### 表現方法の比較
1と2について、回転軸が重なることがあるので精度をよくしたい場合は3（クォータニオン）を使うことが多いです。
これらの表現方法に対応する回転行列$\mathbf{R}$が存在します。
## 座標変換の理論
### 同次変換行列による座標変換
同次変換行列は、**姿勢の回転**と**平行移動**を一緒にまとめて表現することで、座標変換の式がシンプルになります。
### 2次元座標変換の例
ある直交座標系$\Sigma_1$と$\Sigma_2$があるとします。
ある点$P$を座標系$\Sigma_1$で表示すると$P(x_1, y_1)$、座標系$\Sigma_2$で表示すると$P(x_2, y_2)$となる場合、この差が平行移動量$\Delta \mathbf{P}$であり、これだけ平行移動したと定義できます。
### 座標変換の一般化
回転も同じように考えることができます。**姿勢**とは、この考え方から、自分の見る位置（座標系）が変わると変わるが、そのものの絶対座標が変わらないことを示しているにすぎません。
座標変換は以下のように表現されます：
$$
\begin{bmatrix} \mathbf{p}' \\ 1 \end{bmatrix} = \mathbf{T} \begin{bmatrix} \mathbf{p} \\ 1 \end{bmatrix}
$$
ここで、$\mathbf{p}$は変換前の位置ベクトル、$\mathbf{p}'$は変換後の位置ベクトルです。
```mermaid
flowchart LR
A["座標系 Σ₁<br/>点P: (x₁, y₁)"] -->|同次変換行列 T| B["座標系 Σ₂<br/>点P: (x₂, y₂)"]
C["回転 R"] --> T
D["平行移動 P"] --> T
```
**座標変換の流れ**：回転と平行移動を組み合わせて座標を変換
## 多関節ロボットアームへの応用
### 座標系の設定
多関節型ロボットアームでは、2つの接続する関節を見た時、それぞれの関節$\theta_i$に座標系を設定します。
- 各座標系$\Sigma_i$ごとに座標系ごとの軸が固定されて計算される
- このことによって複雑なものを単純化して計算ができる
### 同次変換行列の連結
それらを組み合わせると、それぞれの関節に対して同次変換行列を用いて座標変換を表現できます。
多軸の場合、その軸の数$n$と同じ数だけ同次変換行列が存在するとみなします。
3軸の場合を例にすると：
- $T_1$：関節0から関節1への変換
- $T_2$：関節1から関節2への変換
- $T_3$：関節2から関節3への変換
これを任意の軸の座標系ということを示すと、以下のように書けます：
- ${}^0\mathbf{T}_1$：座標系0から見た座標系1への変換
- ${}^1\mathbf{T}_2$：座標系1から見た座標系2への変換
- ${}^2\mathbf{T}_3$：座標系2から見た座標系3への変換
これはある軸から見たものを表示しているので、線形代数を使うと以下のように表示できます：
$$
{}^0\mathbf{T}_3 = {}^0\mathbf{T}_1 \cdot {}^1\mathbf{T}_2 \cdot {}^2\mathbf{T}_3
$$
結果的に、この数式で同次変換行列に相当するものを使うことができます。
```mermaid
flowchart LR
Σ0["座標系 Σ₀<br/>(ベース)"] -->|T₁| Σ1["座標系 Σ₁<br/>(関節1)"]
Σ1 -->|T₂| Σ2["座標系 Σ₂<br/>(関節2)"]
Σ2 -->|T₃| Σ3["座標系 Σ₃<br/>(手先)"]
Σ0 -.->|T₃ = T₁·T₂·T₃| Σ3
```
**多関節アームの座標系連結**：各関節の座標系を順次変換して手先位置を計算
### 順運動学と逆運動学
1自由度アームの例：
- **順運動学**：解が必ず1つ存在する（関節角から手先位置を求める）
- **逆運動学**：解が存在しないとき、解が複数ある、解が1つある（手先位置から関節角を求める）
各関節に相当する同次変換行列を連結することで、順運動学を計算できます。
```mermaid
flowchart TB
subgraph FK["順運動学 Forward Kinematics"]
θ["関節角<br/>θ = [θ₁, θ₂, ..., θₙ]"] -->|同次変換行列の連結| P["手先位置・姿勢<br/>P = [x, y, z, φ]"]
direction LR
end
subgraph IK["逆運動学 Inverse Kinematics"]
P2["手先位置・姿勢<br/>P = [x, y, z, φ]"] -->|数値解法・解析解法| θ2["関節角<br/>θ = [θ₁, θ₂, ..., θₙ]"]
direction LR
end
```
**順運動学と逆運動学の関係**：順運動学は一意解、逆運動学は解が0個・1個・複数個の可能性
## Denavit-Hartenberg記法
### 概要
同次変換行列の作り方として、**Denavit-Hartenberg記法**（D-H記法）が広く使われています。
1つの関節について4つのパラメータで表現します。
### 4つのパラメータ
D-H記法では、隣接する2つのリンク間の関係を以下の4つのパラメータで記述します：
1. **$a_i$**（リンク長）：隣接する2つの関節軸間の最短距離（共通垂線の長さ）
2. **$d_i$**（リンクオフセット）：前の関節軸から次の関節軸への共通垂線に沿った距離
3. **$\alpha_i$**（リンクねじれ角）：前の関節軸から次の関節軸への共通垂線周りの回転角
4. **$\theta_i$**（関節角）：前の関節軸から次の関節軸への共通垂線に垂直な軸周りの回転角
```mermaid
graph TB
subgraph DH["Denavit-Hartenberg記法の4パラメータ"]
a["aᵢ: リンク長<br/>共通垂線の長さ<br/>(定数)"]
d["dᵢ: リンクオフセット<br/>共通垂線に沿った距離<br/>(回転関節: 定数<br/>直動関節: 変数)"]
α["αᵢ: リンクねじれ角<br/>共通垂線周りの回転角<br/>(定数)"]
θ["θᵢ: 関節角<br/>共通垂線に垂直な軸周りの回転角<br/>(回転関節: 変数<br/>直動関節: 定数)"]
end
a --> T["同次変換行列<br/>Tᵢ"]
d --> T
α --> T
θ --> T
```
**D-H記法のパラメータ**：4つのパラメータから同次変換行列を生成
### パラメータの定数/変数の判定
- **回転関節**の場合：$\theta_i$が変数、$a_i, d_i, \alpha_i$が定数
- **直動関節**の場合：$d_i$が変数、$a_i, \alpha_i, \theta_i$が定数
### 同次変換行列への変換
D-Hパラメータから同次変換行列${}^{i-1}\mathbf{T}_i$は以下のように求められます：
$$
{}^{i-1}\mathbf{T}_i = \begin{bmatrix}
\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{bmatrix}
$$
### 参考資料
詳細については、以下の資料を参照してください：
- https://qiita.com/takeshiD/items/7bd5cf88a66a3ff59358
## ヤコビ行列と偏微分
### 速度ヤコビ行列の定義
ロボットアームの手先速度と関節角速度の関係を表す行列を**速度ヤコビ行列**（または単に**ヤコビ行列**）と呼びます。
手先位置を$\mathbf{p} = [x, y, z]^T$、関節角を$\boldsymbol{\theta} = [\theta_1, \theta_2, \ldots, \theta_n]^T$とすると、手先速度$\dot{\mathbf{p}}$と関節角速度$\dot{\boldsymbol{\theta}}$の関係は：
$$
\dot{\mathbf{p}} = \mathbf{J}(\boldsymbol{\theta}) \dot{\boldsymbol{\theta}}
$$
ここで、$\mathbf{J}(\boldsymbol{\theta})$が速度ヤコビ行列です。
```mermaid
flowchart LR
jointvel["関節角速度 θ̇ = [θ̇₁, θ̇₂, ..., θ̇ₙ]"] -->|ヤコビ行列 J| tcpvel["手先速度 ṗ = [ẋ, ẏ, ż]"]
Jmat["J(θ) = ∂p/∂θ (3×n行列)"] --> tcpvel
subgraph Calc["計算方法"]
FK["順運動学: p = f(θ)"] --> Diff["偏微分: Jᵢⱼ = ∂pᵢ/∂θⱼ"]
Diff --> Jmat
end
```
**ヤコビ行列の関係**：関節角速度から手先速度への変換
### 偏微分による導出
ヤコビ行列は、順運動学の式を各関節角で偏微分することで求められます。
手先位置が関節角の関数として$\mathbf{p} = \mathbf{f}(\boldsymbol{\theta})$と表されるとき、ヤコビ行列の各要素は：
$$
\mathbf{J}_{ij} = \frac{\partial p_i}{\partial \theta_j} = \frac{\partial f_i(\boldsymbol{\theta})}{\partial \theta_j}
$$
つまり、ヤコビ行列は以下のように表されます：
$$
\mathbf{J}(\boldsymbol{\theta}) = \begin{bmatrix}
\frac{\partial x}{\partial \theta_1} & \frac{\partial x}{\partial \theta_2} & \cdots & \frac{\partial x}{\partial \theta_n} \\
\frac{\partial y}{\partial \theta_1} & \frac{\partial y}{\partial \theta_2} & \cdots & \frac{\partial y}{\partial \theta_n} \\
\frac{\partial z}{\partial \theta_1} & \frac{\partial z}{\partial \theta_2} & \cdots & \frac{\partial z}{\partial \theta_n}
\end{bmatrix}
$$
### 2自由度アームの例
2自由度アームの場合、順運動学は：
$$
\begin{aligned}
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)
\end{aligned}
$$
ヤコビ行列は各要素を偏微分することで：
$$
\mathbf{J} = \begin{bmatrix}
\frac{\partial x}{\partial \theta_1} & \frac{\partial x}{\partial \theta_2} \\
\frac{\partial y}{\partial \theta_1} & \frac{\partial y}{\partial \theta_2}
\end{bmatrix} = \begin{bmatrix}
-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{bmatrix}
$$
### 特異点
ヤコビ行列の行列式が0になる点を**特異点**と呼びます。
$$
\det(\mathbf{J}) = 0
$$
特異点では：
- ヤコビ行列が正則でなくなる（逆行列が存在しない）
- 手先の特定の方向に動かすことができない
- 逆運動学の解が不安定になる
```mermaid
stateDiagram-v2
[*] --> 通常状態: det(J) ≠ 0
通常状態 --> 特異点: det(J) = 0
特異点 --> 通常状態: det(J) ≠ 0
通常状態: 逆行列が存在<br/>手先を任意方向に動かせる
特異点: 逆行列が存在しない<br/>特定方向に動かせない<br/>解が不安定
note right of 特異点
例：2自由度アームが
完全に伸びきった状態
または完全に折りたたまれた状態
end note
```
**特異点の状態遷移**：ヤコビ行列の行列式が0になると特異点となる
### 逆ヤコビ行列と逆運動学
逆運動学では、手先速度から関節角速度を求める必要があります。
ヤコビ行列が正則（逆行列が存在する）場合：
$$
\dot{\boldsymbol{\theta}} = \mathbf{J}^{-1}(\boldsymbol{\theta}) \dot{\mathbf{p}}
$$
ただし、特異点では逆行列が存在しないため、擬似逆行列$\mathbf{J}^+$を使うなどの方法が用いられます。
```mermaid
flowchart TB
PDOT["手先速度<br/>$\dot{\mathbf{p}} = [\dot{x}, \dot{y}, \dot{z}]$"]
PDOT --> CHECK
CHECK{"det(J) = 0?"}
CHECK -- "No: 正則" --> INV["逆行列 $J^{-1}$<br/>$\dot{\theta} = J^{-1} \dot{\mathbf{p}}$"]
CHECK -- "Yes: 特異点" --> PSEUDO["擬似逆行列 $J^{+}$<br/>$\dot{\theta} = J^{+} \dot{\mathbf{p}}$"]
INV --> OUT1["関節角速度<br/>$\dot{\theta}$"]
PSEUDO --> OUT2["関節角速度<br/>$\dot{\theta}$(近似解)"]
```
**逆ヤコビ行列による逆運動学**：正則な場合は逆行列、特異点では擬似逆行列を使用
### 位置ヤコビ行列
手先位置だけでなく、姿勢（向き）も考慮する場合、**位置ヤコビ行列**が用いられます。
手先の位置と姿勢を合わせた6次元ベクトル$\mathbf{x} = [\mathbf{p}^T, \boldsymbol{\phi}^T]^T$（$\boldsymbol{\phi}$は姿勢表現）に対して：
$$
\dot{\mathbf{x}} = \mathbf{J}_x(\boldsymbol{\theta}) \dot{\boldsymbol{\theta}}
$$
ここで、$\mathbf{J}_x$は6×$n$の位置ヤコビ行列です。
Collection
Citation
unjuno, “ロボット工学１１,” unjuno'sResearchLibrary, accessed October 7, 2026, https://archive.unjuno.org/items/show/144.
コメント