ニュートン法とIK（Inverse Kinematics）

1. IKの基本概念

Inverse Kinematics（IK）は、ロボットアームなどの関節角度を、目標とするエンドエフェクタの位置・姿勢から逆算する手法です。これは非線形最適化問題として定式化でき、様々な数値解法で解くことができます。

目標位置を $\mathbf{x}_{\text{target}}$、関節角度ベクトルを $\boldsymbol{\theta}$ とし、順運動学を $\mathbf{f}(\boldsymbol{\theta})$ とすると、IKは以下の最小化問題として表現できます：

\min_{\boldsymbol{\theta}} \|\mathbf{f}(\boldsymbol{\theta}) - \mathbf{x}_{\text{target}}\|^2

2. 最適化手法の階層的理解

2.1 ニュートン法（完全な二次近似）

ニュートン法は目的関数の二次近似を完全な形で用います：

E(\boldsymbol{\theta} + \Delta\boldsymbol{\theta}) \approx E(\boldsymbol{\theta}) + \nabla E^T\Delta\boldsymbol{\theta} + \frac{1}{2}\Delta\boldsymbol{\theta}^T H \Delta\boldsymbol{\theta}

この二次近似を最小化する更新式は：

\boldsymbol{\theta}_{k+1} = \boldsymbol{\theta}_k - H^{-1}\nabla E

ここでヘッセ行列 $H$ は：

H = J^T J + \sum_{i=1}^n r_i \nabla^2 r_i

2.2 ガウス・ニュートン法（ヘッセ行列の近似）

ガウス・ニュートン法は、ヘッセ行列の二次微分項を無視することで計算を簡略化します：

H \approx J^T J

この近似により、更新式は：

\boldsymbol{\theta}_{k+1} = \boldsymbol{\theta}_k - (J^T J)^{-1}J^T\mathbf{r}

この近似は、残差 $r_i$ が小さい場合や、システムがほぼ線形の場合に特によく機能します。

2.3 ヤコビ法（さらなる近似）

ヤコビ法は、ガウス・ニュートン法をさらに近似し、$J^T J$ の逆行列計算を回避します：

(J^T J)^{-1} \approx \alpha I

この近似により、更新式は：

\boldsymbol{\theta}_{k+1} = \boldsymbol{\theta}_k - \alpha J^T\mathbf{r}

この階層的な近似の関係は以下のようにまとめられます：

1. ニュートン法：完全な二次近似を使用
2. ガウス・ニュートン法：ヘッセ行列から二次微分項を除外
3. ヤコビ法：さらに $(J^T J)^{-1}$ を定数行列で近似

各手法の特徴：

• ニュートン法：最も正確だが計算コストが高い
• ガウス・ニュートン法：良好な収束性と実用的な計算コスト
• ヤコビ法：最も単純で計算が軽いが、収束が遅い可能性がある

3. レーベンバーグ・マルカート法

3.1 アルゴリズムの導入

レーベンバーグ・マルカート法は、ガウス・ニュートン法に正則化項を加えることで、より安定した収束を実現します：

(J^T J + \lambda I)\Delta \boldsymbol{\theta} = -J^T \mathbf{r}

ここで：

• $\lambda > 0$ は正則化パラメータ（ダンピング係数）
• $I$ は適切なサイズの単位行列
• $\mathbf{r} = \mathbf{f}(\boldsymbol{\theta}) - \mathbf{x}_{\text{target}}$ は残差ベクトル

3.2 収束性と安定性の解析

$\lambda$ の値によって、アルゴリズムの振る舞いが変化します：

1. $\lambda \to 0$ の場合：

\Delta \boldsymbol{\theta} \approx -(J^T J)^{-1}J^T \mathbf{r}

ガウス・ニュートン法に近づき、収束が速くなりますが、不安定になる可能性があります。

1. $\lambda \to \infty$ の場合：

\Delta \boldsymbol{\theta} \approx -\frac{1}{\lambda}J^T \mathbf{r}

最急降下法（ヤコビ法）に近づき、より安定しますが、収束が遅くなります。

3.3 適応的な $\lambda$ の調整

レーベンバーグ・マルカート法では、各反復で以下のように $\lambda$ を調整します：

1. 誤差が減少した場合：$\lambda \leftarrow \lambda/\nu$ ($\nu > 1$)
2. 誤差が増加した場合：$\lambda \leftarrow \lambda\nu$

これにより：

• 解に近い場合は小さい $\lambda$ でガウス・ニュートン法的な速い収束
• 解から遠い場合は大きい $\lambda$ で最急降下法的な安定した収束

が実現されます。

6. 解析的解法：2リンクアームの場合

6.1 ガウス・ニュートン法の展開

2リンクアームの場合、順運動学は：

\mathbf{f}(\boldsymbol{\theta}) = \begin{bmatrix}
l_1\cos(\theta_1) + l_2\cos(\theta_1 + \theta_2) \\
l_1\sin(\theta_1) + l_2\sin(\theta_1 + \theta_2)
\end{bmatrix}

ヤコビ行列の各要素は以下の偏微分で表されます：

J_{ij} = \frac{\partial f_i}{\partial \theta_j}

これを計算すると：

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}

$J^T J$ を計算すると：

J^T J = \begin{bmatrix}
(\frac{\partial x}{\partial \theta_1})^2 + (\frac{\partial y}{\partial \theta_1})^2 & \frac{\partial x}{\partial \theta_1}\frac{\partial x}{\partial \theta_2} + \frac{\partial y}{\partial \theta_1}\frac{\partial y}{\partial \theta_2} \\
\frac{\partial x}{\partial \theta_1}\frac{\partial x}{\partial \theta_2} + \frac{\partial y}{\partial \theta_1}\frac{\partial y}{\partial \theta_2} & (\frac{\partial x}{\partial \theta_2})^2 + (\frac{\partial y}{\partial \theta_2})^2
\end{bmatrix}

残差ベクトル $\mathbf{r} = \mathbf{f}(\boldsymbol{\theta}) - \mathbf{x}_{\text{target}}$ との積 $J^T\mathbf{r}$ は：

J^T\mathbf{r} = \begin{bmatrix}
\frac{\partial x}{\partial \theta_1}(x - x_{\text{target}}) + \frac{\partial y}{\partial \theta_1}(y - y_{\text{target}}) \\
\frac{\partial x}{\partial \theta_2}(x - x_{\text{target}}) + \frac{\partial y}{\partial \theta_2}(y - y_{\text{target}})
\end{bmatrix}

これらの偏微分を具体的に計算し、整理すると、更新式は以下の形になります：

\begin{bmatrix}
\Delta \theta_1 \\
\Delta \theta_2
\end{bmatrix} = -(J^T J)^{-1}J^T\mathbf{r}

この式を解くと、最終的に余弦定理：

r^2 = l_1^2 + l_2^2 - 2l_1l_2\cos(\pi - \theta_2)

と等価な関係式が得られます。これは、2リンクアームの場合、ガウス・ニュートン法による反復計算が解析解に収束することを示しています。

ただし、3リンク以上の場合は行列の次元が大きくなり、このような簡単な形には帰着しないため、数値的な反復計算が必要となります。

7. 速度レベルのIK

7.1 基本式と最適化問題

エンドエフェクタの速度 $\dot{\mathbf{x}}$ と関節角速度 $\dot{\boldsymbol{\theta}}$ の関係は、ヤコビ行列を用いて以下のように表されます：

\dot{\mathbf{x}} = J(\boldsymbol{\theta})\dot{\boldsymbol{\theta}}

この関係は、順運動学 $\mathbf{x} = \mathbf{f}(\boldsymbol{\theta})$ を時間で微分することで得られます：

\dot{\mathbf{x}} = \frac{d\mathbf{f}(\boldsymbol{\theta})}{dt} = \frac{\partial \mathbf{f}}{\partial \boldsymbol{\theta}}\frac{d\boldsymbol{\theta}}{dt} = J(\boldsymbol{\theta})\dot{\boldsymbol{\theta}}

ここで $J(\boldsymbol{\theta}) = \frac{\partial \mathbf{f}}{\partial \boldsymbol{\theta}}$ はヤコビ行列であり、各要素は順運動学の偏微分で表されます。例えば2リンクアームの場合：

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}

このように、ヤコビ行列は関節角度の微小変化がエンドエフェクタの位置にどのように影響するかを表す感度行列としても解釈できます。

目標速度 $\dot{\mathbf{x}}_{\text{target}}$ が与えられたとき、以下の二次形式の最小化問題として定式化できます：

\min_{\dot{\boldsymbol{\theta}}} \|\dot{\mathbf{x}}_{\text{target}} - J\dot{\boldsymbol{\theta}}\|^2

7.2 ガウス・ニュートン法による解法

この最小化問題に対して、2.1節のニュートン法を適用します。目的関数を展開すると：

E(\dot{\boldsymbol{\theta}}) = \|\dot{\mathbf{x}}_{\text{target}} - J\dot{\boldsymbol{\theta}}\|^2 = (\dot{\mathbf{x}}_{\text{target}} - J\dot{\boldsymbol{\theta}})^T(\dot{\mathbf{x}}_{\text{target}} - J\dot{\boldsymbol{\theta}})

この勾配は：

\nabla E = -J^T(\dot{\mathbf{x}}_{\text{target}} - J\dot{\boldsymbol{\theta}})

ヘッセ行列は：

H = \frac{\partial^2 E}{\partial \dot{\boldsymbol{\theta}}^2} = J^TJ

となります。位置レベルのIKと異なり、速度レベルではヤコビ行列 $J$ が $\dot{\boldsymbol{\theta}}$ に依存しないため、ヘッセ行列は厳密に $J^TJ$ となります。つまり、この場合のガウス・ニュートン法は近似を含まない厳密な解法となります。

これを用いて解くと：

\dot{\boldsymbol{\theta}} = (J^TJ)^{-1}J^T\dot{\mathbf{x}}_{\text{target}}

この解は従来「疑似逆行列」として知られる解と同一です。

7.3 レーベンバーグ・マルカート法による安定化

位置制御と同様に、特異点近傍での計算の安定化のため、3.1節で導入したレーベンバーグ・マルカート法を適用します：

(J^TJ + \lambda I)\dot{\boldsymbol{\theta}} = J^T\dot{\mathbf{x}}_{\text{target}}

これは以下の形でも書けます：

\dot{\boldsymbol{\theta}} = J^T(JJ^T + \lambda I)^{-1}\dot{\mathbf{x}}_{\text{target}}

この式は、$\lambda$ の値によって：

• $\lambda \to 0$ のとき：ガウス・ニュートン法による解
• $\lambda$ が大きいとき：より安定だが応答が遅い解
  となり、3.2節で説明した特性と同様の性質を持ちます。

7.4 加速度制限と実装

実際のロボットシステムでは、関節の加速度制限を考慮する必要があります：

|\ddot{\theta}_i| \leq \ddot{\theta}_{\text{max},i}

これを満たすため、以下の手順で軌道を生成します：

1. 位置レベルのIKで目標姿勢を求める
2. 現在の姿勢から目標姿勢までの経路を生成
3. 加速度制限を考慮して時間軸上でパラメータ化
4. 各時刻での速度指令値を計算

この方法により、滑らかで実現可能な軌道を生成できます。