ロボット工学12
note Item Type Metadata
note
# 2自由度アームのヤコビ行列
## 順運動学と逆運動学の比較
### 順運動学（Forward Kinematics）
**定義**：関節角から手先位置・姿勢を求める問題
$$
\text{入力：関節角} \quad \boldsymbol{\theta} = [\theta_1, \theta_2, \ldots, \theta_n]^T
$$
$$
\text{出力：手先位置・姿勢} \quad \mathbf{p} = [x, y, z, \phi_x, \phi_y, \phi_z]^T
$$
```mermaid
flowchart LR
A["関節角<br/>θ = [θ₁, θ₂, ..., θₙ]"] -->|順運動学| B["手先位置・姿勢<br/>p = [x, y, z, φ]"]
direction LR
```
**特徴**：
- **解析的に求めることができる**（機械的に）
- Denavit-Hartenberg記法などの手法を用いて、同次変換行列の連結により計算可能
- **解が必ず1つ存在する**（一意解）
**方法**：
- Denavit-Hartenberg（D-H）記法
- 同次変換行列の連結
- 幾何学的計算
### 逆運動学（Inverse Kinematics）
**定義**：手先位置・姿勢から関節角を求める問題
$$
\text{入力：手先位置・姿勢} \quad \mathbf{p} = [x, y, z, \phi_x, \phi_y, \phi_z]^T
$$
$$
\text{出力：関節角} \quad \boldsymbol{\theta} = [\theta_1, \theta_2, \ldots, \theta_n]^T
$$
```mermaid
flowchart LR
A["手先位置・姿勢<br/>p = [x, y, z, φ]"] -->|逆運動学| B["関節角<br/>θ = [θ₁, θ₂, ..., θₙ]"]
direction LR
```
**問題点**：
- **求めることが難しい場合がある**
- 解が存在しない場合がある
- 解が複数存在する場合がある（多値性）
- 解析的な解が得られない場合がある
**解決策**：**ヤコビ行列の利用**
## ヤコビ行列の基本
### 1変数関数の場合
ヤコビ行列の基本概念を理解するために、まず1変数関数の例から考えます。
関数$y = f(x)$を考えます。点$P(x_0, f(x_0))$が与えられたとき、**ここから少しズレした点の座標を求めたい**とします。
```mermaid
graph LR
A["点P<br/>x₀, f(x₀)"] -->|少しズレ| B["新しい点<br/>x₀ + Δx, ?"]
direction LR
```
**問題**：$x$が$x_0$から$\Delta x$だけ変化したとき、対応する$y$座標は？
**正確な値**：
点$P'$の座標は：
$$
P'(x_0 + \Delta x, f(x_0 + \Delta x))
$$
- $X$座標：$x_0 + \Delta x$
- $Y$座標：$f(x_0 + \Delta x)$（正確な値）
```mermaid
graph TB
P["点P<br/>x₀, f(x₀)"] -->|Δxだけ変化| P2["点P'<br/>x₀ + Δx, f(x₀ + Δx)"]
P2 -->|正確な値| Exact["f(x₀ + Δx)"]
```
### 微分の表記法と接線の傾き
関数$y = f(x)$を微分すると、以下のような表記法があります：
$$
y' \quad \text{又は} \quad f'(x) \quad \text{又は} \quad \frac{dy}{dx} \quad \text{又は} \quad \frac{df(x)}{dx}
$$
これらはすべて同じ意味で、**接線の傾きがわかる**ことを示しています。
```mermaid
graph TB
A["関数 y = f(x)"] -->|微分| B["y' または f' x または dy/dx"]
B --> C["接線の傾きがわかる"]
D["元の関数は曲線であることが多い"] --> E["接線は直線<br/>シンプル"]
E --> F["これで元の関数を近似<br/>局所線形化"]
```
**接線の性質**：
- 元の関数は曲線であることが多い
- 接線は**直線**（シンプル）
- これで元の関数を近似できる（**局所線形化**）
### 拡大してみる：接線による近似
点$P$の周辺を拡大して見ると、接線による近似の仕組みが理解できます。
点$P(x_0, f(x_0))$における接線の傾きは$f'(x_0)$です。
```mermaid
graph TB
P["点P<br/>x: x₀<br/>y: f(x₀)"] -->|接線| T["接線<br/>傾き f' x₀"]
T -->|Δxだけ移動| P2["点P'<br/>x: x₀ + Δx<br/>y ≈ f(x₀) + f' x₀ · Δx"]
P2 -->|実際の曲線上の点| P3["点P''<br/>x: x₀ + Δx<br/>y: f(x₀ + Δx)"]
```
**三角形の高さ**：
接線上の点$P'$と点$P$の間の$y$座標の差は、三角形の高さとして計算できます：
$$
\text{三角形の高さ} = \text{傾き} \times \text{底辺} = f'(x_0) \cdot \Delta x
$$
したがって、点$P'$の$y$座標の近似値は：
$$
P' \text{の} y \text{座標} = P \text{の} y \text{座標} + \text{三角形の高さ} = f(x_0) + f'(x_0) \cdot \Delta x
$$
### 別解：微分を用いた近似解
**微分を用いて近似解を求める方法もある**。
$\Delta x$が十分小さい場合、テイラー展開の1次項までで近似できます：
$$
f(x_0 + \Delta x) \approx f(x_0) + f'(x_0) \cdot \Delta x
$$
ここで、$f'(x_0) = \frac{df}{dx}\Big|_{x=x_0}$は点$x_0$における微分係数（傾き）です。
したがって、点$P'$の$Y$座標の**近似値**は：
$$
f(x_0 + \Delta x) \approx f(x_0) + f'(x_0) \cdot \Delta x
$$
$y$の変化量$\Delta y$は：
$$
\Delta y = f(x_0 + \Delta x) - f(x_0) \approx f'(x_0) \cdot \Delta x
$$
つまり：
$$
\Delta y = \frac{df}{dx}\Big|_{x=x_0} \cdot \Delta x
$$
```mermaid
flowchart LR
A["入力の微小変化<br/>Δx"] -->|微分係数<br/>f' x₀| B["出力の微小変化<br/>Δy ≈ f' x₀ · Δx"]
direction LR
```
**正確な値と近似解の比較**：
| 方法 | $Y$座標の値 | 特徴 |
|------|------------|------|
| **正確な値** | $f(x_0 + \Delta x)$ | 厳密だが、関数$f$の値を直接計算する必要がある |
| **近似解** | $f(x_0) + f'(x_0) \cdot \Delta x$ | 微分係数さえ分かれば計算可能。$\Delta x$が小さいほど精度が高い |
### 近似誤差について
点$P''$（曲線上の実際の点）と点$P'$（接線上の近似点）の座標の値は**少しずれている**（近似値）。
**誤差の性質**：
- $\Delta x$が小さければ、この誤差も**小さくなる**
- 接線は直線であり、元の関数（曲線）を近似している
- これは**局所線形化**と呼ばれる
```mermaid
graph LR
A["Δxが小さい"] --> B["近似誤差も小さい"]
C["接線は直線"] --> D["元の関数（曲線）を近似"]
D --> E["局所線形化"]
```
### 逆問題の例
**例**：点$P''$の$Y$座標がわかっている時に、$P''$の$X$座標を求めたい。
#### 正解（正確な値）
$$
x = f^{-1}(y)
$$
**逆関数**を用いる方法ですが、**難しいときもある**。
- 逆関数が存在しない場合がある
- 逆関数が複数値になる場合がある
- 逆関数の解析的な式が得られない場合がある
#### 近似解
**接線の傾きを利用**する方法。
点$P$の$x$座標と$y$座標は両方わかっているとします。点$P'$の$y$座標はわかっているが、$x$座標を知りたいとします。
**考え方**：$P'$の接線の傾きを、$P$の傾きで代用してみる（少し誤差はある）。
```mermaid
graph TB
P["点P<br/>xもyもわかっている"] -->|y座標はわかっている| P2["点P'<br/>↓x座標を知りたい"]
P -->|接線の傾き f' x₀| T["Pの接線"]
T -.->|代用| P2T["P'の接線の傾き<br/>（不明）"]
P2T -->|近似| P2
```
### 逆問題の近似解：$\Delta y$から$\Delta x$を求める
**問題**：$\Delta y$がわかっている時に、$\Delta x$を求めたい。
**傾きの定義**：
$$
\text{傾き} = \frac{\Delta y}{\Delta x} = f'(x_0)
$$
この式を変形すると：
$$
\Delta x = \frac{1}{\text{傾き}} \times \Delta y = \frac{1}{f'(x_0)} \cdot \Delta y
$$
したがって、新しい$x$座標の近似値は：
$$
x_1 = x_0 + \frac{\Delta y}{f'(x_0)}
$$
**利点**：
- **シンプルな式**で逆問題を**近似値計算**できる
- 接線は直線なので、計算が簡単
- 局所線形化により、逆関数を直接求める必要がない
```mermaid
flowchart LR
A["Δy<br/>わかっている"] -->|1/f' x₀| B["Δx<br/>知りたい"]
C["傾き f' x₀<br/>わかっている"] --> B
B --> D["x₁ = x₀ + Δy/f' x₀"]
```
**逆問題における利点**：
- **逆問題がとても簡単になる**
- 接線は直線なので、計算がシンプル
- これで元の関数を近似できる（局所線形化）
- 逆関数を直接求める必要がない
**1変数関数の場合の「ヤコビ行列」**：
- 1変数関数では、ヤコビ行列は1×1行列（スカラー）となり、それは微分係数$f'(x_0)$そのものです
- $\Delta y = f'(x_0) \cdot \Delta x$という関係式が、多変数関数におけるヤコビ行列の関係式の原型です
- 逆問題では、$\Delta x = \frac{1}{f'(x_0)} \cdot \Delta y$という関係式が、多変数関数における逆ヤコビ行列の関係式の原型です
### 多変数関数への拡張
#### 1変数関数と多変数関数の比較
**1変数関数**：
- $y = f(x)$
- 変数：**1つだけ**
**ロボット工学における多変数関数**：
ロボット工学では**多変数関数**が使われます。
**順運動学**：
```mermaid
flowchart LR
A["関節角<br/>θ₁, θ₂, ..., θₘ<br/>複数（n個の）"] -->|順運動学| B["手先<br/>x, y, z<br/>φ, θ, ψ"]
B --> C["こちらも複数ある"]
B --> D["こちらは1つずつ考えてもよい"]
```
- **入力**：複数の関節角 $\theta_1, \theta_2, \ldots, \theta_n$（$n$個の関節角）
- **出力**：手先の位置・姿勢
- 位置：$x, y, z$（こちらも複数ある）
- 姿勢：$\phi, \theta, \psi$（こちらは1つずつ考えてもよい）
#### たくさんの「傾き」が存在する
多変数関数では、**たくさんの「傾き」が存在する**。
- $\theta_1$以外は変化させず、$\theta_1$だけを変化させたときの傾き
- $\theta_2$以外は変化させず、$\theta_2$だけを変化させたときの傾き
- $\theta_3$以外は変化させず、$\theta_3$だけを変化させたときの傾き
- $\ldots$
- $\theta_n$以外は変化させず、$\theta_n$だけを変化させたときの傾き
これらは**偏微分を用いて求める**ことができます。
#### 偏微分の定義
**偏微分**とは：
- **着目する変数以外は変化させない**状況で微分を行う
- 着目する変数以外は**定数とみなせる**
- （定数の微分はゼロ）
記号：$\frac{\partial}{\partial \theta_i}$は「ラウンド」と読み、**偏微分を表す記号**です。
```mermaid
graph TB
A["多変数関数"] --> B["偏微分"]
B --> C["着目する変数以外は<br/>変化させない"]
C --> D["定数とみなせる"]
D --> E["定数の微分はゼロ"]
```
#### 多変数関数のヤコビ行列
多変数関数$\mathbf{y} = \mathbf{f}(\mathbf{x})$の場合、1変数の微分係数の代わりに**ヤコビ行列**が使われます：
$$
\Delta \mathbf{y} = \mathbf{J}(\mathbf{x}_0) \Delta \mathbf{x}
$$
ここで、$\mathbf{J}(\mathbf{x}_0)$は点$\mathbf{x}_0$におけるヤコビ行列です。
## ヤコビ行列による逆運動学の解法
### なぜヤコビ行列を使うのか？
順運動学の式を微小変化で線形化することで、逆運動学を解くことができる。
### 基本的な考え方
順運動学の関係式：
$$
\mathbf{p} = \mathbf{f}(\boldsymbol{\theta})
$$
この式を微小変化で線形化すると：
$$
\delta \mathbf{p} = \mathbf{J}(\boldsymbol{\theta}) \delta \boldsymbol{\theta}
$$
ここで、$\mathbf{J}(\boldsymbol{\theta})$は**ヤコビ行列**である。
**記号について**：
- $\delta$：微小変化を表す記号（変位・変分）
- $\Delta$：有限の変化量を表す記号
- 本資料では、微小変化には$\delta$、有限変化には$\Delta$を使用しています
### 速度の関係
両辺を時間$dt$で割ると：
$$
\dot{\mathbf{p}} = \mathbf{J}(\boldsymbol{\theta}) \dot{\boldsymbol{\theta}}
$$
ここで：
- $\dot{\mathbf{p}}$：手先速度
- $\dot{\boldsymbol{\theta}}$：関節角速度
### 逆速度運動学
ヤコビ行列の逆行列が存在する場合（正則な場合）：
$$
\dot{\boldsymbol{\theta}} = \mathbf{J}^{-1}(\boldsymbol{\theta}) \dot{\mathbf{p}}
$$
#### 特異姿勢での対処法：擬似逆行列
特異姿勢や$\det(\mathbf{J})$が非常に小さい場合、逆行列$\mathbf{J}^{-1}$が存在しないか、数値的に不安定になります。
このような場合、**擬似逆行列**（Moore-Penrose pseudo-inverse）$\mathbf{J}^{+}$を使用することができます：
$$
\dot{\boldsymbol{\theta}} = \mathbf{J}^{+}(\boldsymbol{\theta}) \dot{\mathbf{p}}
$$
**擬似逆行列の性質**：
- 常に存在する（特異姿勢でも計算可能）
- $\mathbf{J}$が正則な場合、$\mathbf{J}^{+} = \mathbf{J}^{-1}$となる
- 最小二乗解を提供する
**注意**：擬似逆行列は近似解であり、特異姿勢では完全な解が得られない場合があります。
```mermaid
flowchart TB
subgraph FK["順運動学"]
θ["関節角 θ"] -->|解析的<br/>一意解| P["手先位置・姿勢 p"]
end
subgraph IK["逆運動学"]
P2["手先位置・姿勢 p"] -->|困難<br/>解が0個/1個/複数個| θ2["関節角 θ"]
end
subgraph JAC["ヤコビ行列による解法"]
P3["手先速度 ṗ"] -->|"J⁻¹(θ)"| θdot["関節角速度 θ̇"]
θdot -->|積分| θ3["関節角 θ"]
end
FK -.->|微小変化で線形化| JAC
IK -.->|代替手法| JAC
```
**ヤコビ行列の利点**：
- 速度レベルでの逆変換が可能
- 数値的に解くことができる
- 特異姿勢を検出できる
## 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}$は、各出力変数を各入力変数で偏微分した行列：
$$
\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}
$$
#### 偏微分の計算方法
**重要な原則**：$\theta_1$で偏微分するときは、$\theta_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}
$$
ここで、$l_1$と$l_2$は**定数**（リンクの長さ）です。
#### $\frac{\partial x}{\partial \theta_1}$の計算
$$
\begin{aligned}
\frac{\partial x}{\partial \theta_1} &= \frac{\partial}{\partial \theta_1} \left( l_1 \cos\theta_1 + l_2 \cos(\theta_1 + \theta_2) \right) \\
&= \frac{\partial}{\partial \theta_1} (l_1 \cos\theta_1) + \frac{\partial}{\partial \theta_1} \left( l_2 \cos(\theta_1 + \theta_2) \right)
\end{aligned}
$$
第一項：$l_1$は定数なので、
$$
\frac{\partial}{\partial \theta_1} (l_1 \cos\theta_1) = l_1 \frac{\partial \cos\theta_1}{\partial \theta_1} = -l_1 \sin\theta_1
$$
第二項：**合成関数の微分**（チェインルール）を適用します。
$l_2$は定数なので、まず$l_2$を外に出します：
$$
\frac{\partial}{\partial \theta_1} \left( l_2 \cos(\theta_1 + \theta_2) \right) = l_2 \frac{\partial}{\partial \theta_1} \cos(\theta_1 + \theta_2)
$$
合成関数の微分により：
$$
\frac{\partial \cos(\theta_1 + \theta_2)}{\partial \theta_1} = \frac{\partial \cos(\theta_1 + \theta_2)}{\partial (\theta_1 + \theta_2)} \cdot \frac{\partial (\theta_1 + \theta_2)}{\partial \theta_1}
$$
ここで：
- $\frac{\partial \cos(\theta_1 + \theta_2)}{\partial (\theta_1 + \theta_2)} = -\sin(\theta_1 + \theta_2)$
- $\frac{\partial (\theta_1 + \theta_2)}{\partial \theta_1} = 1$（$\theta_2$は定数とみなすため）
したがって：
$$
\begin{aligned}
\frac{\partial}{\partial \theta_1} \left( l_2 \cos(\theta_1 + \theta_2) \right) &= l_2 \cdot (-\sin(\theta_1 + \theta_2)) \cdot 1 \\
&= -l_2 \sin(\theta_1 + \theta_2)
\end{aligned}
$$
**結果**：
$$
\frac{\partial x}{\partial \theta_1} = -l_1 \sin\theta_1 - l_2 \sin(\theta_1 + \theta_2)
$$
#### $\frac{\partial x}{\partial \theta_2}$の計算
$\theta_2$で偏微分するときは、$\theta_1$は**定数とみなす**。
$$
\begin{aligned}
\frac{\partial x}{\partial \theta_2} &= \frac{\partial}{\partial \theta_2} \left( l_1 \cos\theta_1 + l_2 \cos(\theta_1 + \theta_2) \right) \\
&= \frac{\partial}{\partial \theta_2} (l_1 \cos\theta_1) + \frac{\partial}{\partial \theta_2} \left( l_2 \cos(\theta_1 + \theta_2) \right)
\end{aligned}
$$
第一項：$l_1 \cos\theta_1$は$\theta_2$を含まないため、定数として扱われます：
$$
\frac{\partial}{\partial \theta_2} (l_1 \cos\theta_1) = 0
$$
第二項：合成関数の微分を適用：
$$
\begin{aligned}
\frac{\partial}{\partial \theta_2} \left( l_2 \cos(\theta_1 + \theta_2) \right) &= l_2 \frac{\partial \cos(\theta_1 + \theta_2)}{\partial (\theta_1 + \theta_2)} \cdot \frac{\partial (\theta_1 + \theta_2)}{\partial \theta_2} \\
&= l_2 \cdot (-\sin(\theta_1 + \theta_2)) \cdot 1 \\
&= -l_2 \sin(\theta_1 + \theta_2)
\end{aligned}
$$
**結果**：
$$
\frac{\partial x}{\partial \theta_2} = -l_2 \sin(\theta_1 + \theta_2)
$$
#### $\frac{\partial y}{\partial \theta_1}$の計算
$\theta_1$で偏微分するときは、$\theta_2$は**定数とみなす**。
$$
\begin{aligned}
\frac{\partial y}{\partial \theta_1} &= \frac{\partial}{\partial \theta_1} \left( l_1 \sin\theta_1 + l_2 \sin(\theta_1 + \theta_2) \right) \\
&= \frac{\partial}{\partial \theta_1} (l_1 \sin\theta_1) + \frac{\partial}{\partial \theta_1} \left( l_2 \sin(\theta_1 + \theta_2) \right)
\end{aligned}
$$
第一項：$l_1$は定数なので、
$$
\frac{\partial}{\partial \theta_1} (l_1 \sin\theta_1) = l_1 \frac{\partial \sin\theta_1}{\partial \theta_1} = l_1 \cos\theta_1
$$
第二項：**合成関数の微分**（チェインルール）を適用します。
$l_2$は定数なので、まず$l_2$を外に出します：
$$
\frac{\partial}{\partial \theta_1} \left( l_2 \sin(\theta_1 + \theta_2) \right) = l_2 \frac{\partial}{\partial \theta_1} \sin(\theta_1 + \theta_2)
$$
合成関数の微分により：
$$
\frac{\partial \sin(\theta_1 + \theta_2)}{\partial \theta_1} = \frac{\partial \sin(\theta_1 + \theta_2)}{\partial (\theta_1 + \theta_2)} \cdot \frac{\partial (\theta_1 + \theta_2)}{\partial \theta_1}
$$
ここで：
- $\frac{\partial \sin(\theta_1 + \theta_2)}{\partial (\theta_1 + \theta_2)} = \cos(\theta_1 + \theta_2)$
- $\frac{\partial (\theta_1 + \theta_2)}{\partial \theta_1} = 1$（$\theta_2$は定数とみなすため）
したがって：
$$
\begin{aligned}
\frac{\partial}{\partial \theta_1} \left( l_2 \sin(\theta_1 + \theta_2) \right) &= l_2 \cdot \cos(\theta_1 + \theta_2) \cdot 1 \\
&= l_2 \cos(\theta_1 + \theta_2)
\end{aligned}
$$
**結果**：
$$
\frac{\partial y}{\partial \theta_1} = l_1 \cos\theta_1 + l_2 \cos(\theta_1 + \theta_2)
$$
#### $\frac{\partial y}{\partial \theta_2}$の計算
$\theta_2$で偏微分するときは、$\theta_1$は**定数とみなす**。
$$
\begin{aligned}
\frac{\partial y}{\partial \theta_2} &= \frac{\partial}{\partial \theta_2} \left( l_1 \sin\theta_1 + l_2 \sin(\theta_1 + \theta_2) \right) \\
&= \frac{\partial}{\partial \theta_2} (l_1 \sin\theta_1) + \frac{\partial}{\partial \theta_2} \left( l_2 \sin(\theta_1 + \theta_2) \right)
\end{aligned}
$$
第一項：$l_1 \sin\theta_1$は$\theta_2$を含まないため、定数として扱われます：
$$
\frac{\partial}{\partial \theta_2} (l_1 \sin\theta_1) = 0
$$
第二項：合成関数の微分を適用：
$$
\begin{aligned}
\frac{\partial}{\partial \theta_2} \left( l_2 \sin(\theta_1 + \theta_2) \right) &= l_2 \frac{\partial \sin(\theta_1 + \theta_2)}{\partial (\theta_1 + \theta_2)} \cdot \frac{\partial (\theta_1 + \theta_2)}{\partial \theta_2} \\
&= l_2 \cdot \cos(\theta_1 + \theta_2) \cdot 1 \\
&= l_2 \cos(\theta_1 + \theta_2)
\end{aligned}
$$
**結果**：
$$
\frac{\partial y}{\partial \theta_2} = l_2 \cos(\theta_1 + \theta_2)
$$
#### ヤコビ行列の最終形
各要素を計算すると：
$$
\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}
$$
### 速度の関係式
手先速度と関節角速度の関係：
$$
\begin{bmatrix} \dot{x} \\ \dot{y} \end{bmatrix} = \mathbf{J} \begin{bmatrix} \dot{\theta}_1 \\ \dot{\theta}_2 \end{bmatrix}
$$
逆変換（$\det(\mathbf{J}) \neq 0$の場合）：
$$
\begin{bmatrix} \dot{\theta}_1 \\ \dot{\theta}_2 \end{bmatrix} = \mathbf{J}^{-1} \begin{bmatrix} \dot{x} \\ \dot{y} \end{bmatrix}
$$
### 特異姿勢
#### 特異姿勢とは
ヤコビ行列の行列式が0になる姿勢を**特異姿勢**（singular configuration）と呼びます。
$$
\det(\mathbf{J}) = 0
$$
特異姿勢では、ヤコビ行列の逆行列が存在しないため、逆速度運動学を直接解くことができません。
#### 2自由度アームの特異姿勢の具体例
2自由度アームのヤコビ行列の行列式を計算すると：
$$
\begin{aligned}
\det(\mathbf{J}) &= \left(-l_1 \sin\theta_1 - l_2 \sin(\theta_1 + \theta_2)\right) \cdot l_2 \cos(\theta_1 + \theta_2) \\
&\quad - \left(-l_2 \sin(\theta_1 + \theta_2)\right) \cdot \left(l_1 \cos\theta_1 + l_2 \cos(\theta_1 + \theta_2)\right) \\
&= l_1 l_2 \sin\theta_2
\end{aligned}
$$
したがって、$\det(\mathbf{J}) = 0$となる条件は：
$$
\sin\theta_2 = 0
$$
これは、$\theta_2 = 0$または$\theta_2 = \pi$（180度）のときです。
**具体例**：
1. **完全に伸びた状態**（$\theta_2 = 0$）
- 2つのリンクが一直線上に並んだ状態
- この姿勢では、手先をリンクの延長方向に動かすことができない
2. **完全に折りたたまれた状態**（$\theta_2 = \pi$）
- 2つのリンクが完全に重なった状態
- この姿勢でも、特定の方向に動かすことができない
```mermaid
graph TB
A["通常姿勢<br/>det J ≠ 0"] -->|sin θ₂ = 0| B["特異姿勢<br/>det J = 0"]
B --> C["θ₂ = 0<br/>完全に伸びた状態"]
B --> D["θ₂ = π<br/>完全に折りたたまれた状態"]
C --> E["特定方向に<br/>動かせない"]
D --> E
```
#### 特異姿勢の検出方法
特異姿勢を検出するには、ヤコビ行列の行列式を計算します：
$$
\det(\mathbf{J}) = l_1 l_2 \sin\theta_2
$$
- $\det(\mathbf{J}) \neq 0$：通常姿勢（逆行列が存在する）
- $\det(\mathbf{J}) = 0$：特異姿勢（逆行列が存在しない）
**注意**：数値計算では、$\det(\mathbf{J})$が非常に小さい値（例：$10^{-6}$以下）の場合も、実質的に特異姿勢として扱うことがあります。
## まとめ
| 項目 | 順運動学 | 逆運動学 |
|------|----------|----------|
| **入力** | 関節角 | 手先位置・姿勢 |
| **出力** | 手先位置・姿勢 | 関節角 |
| **解の性質** | 一意解（必ず1つ） | 解が0個/1個/複数個 |
| **解法** | 解析的（D-H記法など） | 困難な場合がある |
| **代替手法** | - | **ヤコビ行列の利用** |
**ヤコビ行列の役割**：
- 逆運動学の困難さを解決する手段
- 速度レベルでの変換を可能にする
- 数値的な解法を提供する
**実装時の注意点**：
- **数値誤差**：微小変化の計算では、数値誤差が累積する可能性がある。適切な数値精度を確保する
- **反復計算**：位置制御では、速度から位置を求めるために積分が必要。反復計算により誤差が蓄積する可能性がある
- **特異姿勢の回避**：特異姿勢に近づいた場合、動作を変更するか、擬似逆行列を使用する
- **計算効率**：ヤコビ行列の計算は関節数が増えると計算コストが高くなる。効率的な実装を検討する
Collection
Citation
unjuno, “ロボット工学12,” unjuno'sResearchLibrary, accessed October 6, 2026, https://archive.unjuno.org/items/show/158.
コメント