国際宇宙ステーション（ISS）に搭載されたロボットアーム「カナダアーム2」が、接近してくる補給船を掴みに行く場面を想像してみてください。オペレータが指定するのは「手先をこの位置・姿勢に動かしてほしい」という目標座標です。しかしロボットアームを実際に動かすには、各関節のモータに「何度回転させるか」を指令する必要があります。つまり、手先の目標位置・姿勢から、各関節の角度を逆算する必要があるのです。この逆算こそが逆運動学（Inverse Kinematics, IK）です。
順運動学（Forward Kinematics）が「関節角度 $\to$ 手先位置」という順方向の計算であるのに対し、逆運動学は「手先位置 $\to$ 関節角度」という逆方向の計算です。一見すると単に順運動学の逆を解くだけに見えますが、実際には非線形性、多解性、解の不存在など、厄介な問題が次々と現れます。
逆運動学を理解すると、以下のような応用が広がります。
- 宇宙ロボットアーム: ISS上のカナダアーム2やJEMリモートマニピュレータの制御。軌道上でのドッキング・組立作業
- 産業用ロボット: 工場の溶接・塗装ロボットが目標経路をなぞるための軌道計画
- CGアニメーション: キャラクターの手足を目標位置に配置する際にIKで自然な関節姿勢を算出
- 医療ロボット: 手術ロボットのアームを患部の正確な位置に誘導
本記事の内容
- 逆運動学の直感的理解と順運動学との関係
- 逆運動学が困難な3つの理由 — 非線形性・多解性・解なし
- 2リンク平面マニピュレータの解析的逆運動学（幾何学的方法と代数的方法）
- 多解性と「肘上げ/肘下げ」の問題
- 数値的逆運動学 — ヤコビ行列を用いたニュートン・ラフソン法
- ヤコビ転置法・擬似逆行列法・減衰最小二乗法（DLS）
- 特異点近傍での振る舞いと対策
- Pythonでの実装と可視化
前提知識
この記事を読む前に、以下の記事を読んでおくと理解が深まります。
- 剛体の姿勢表現と同次変換行列 — 回転行列と同次変換行列の基礎
- DHパラメータと順運動学 — DH法に基づく順運動学の導出
逆運動学とは
順運動学と逆運動学の関係
前記事で学んだ順運動学は、ロボットアームの各関節角度 $\bm{q} = (q_1, q_2, \dots, q_n)^T$ が与えられたとき、手先（エンドエフェクタ）の位置・姿勢 $\bm{x}$ を計算する問題でした。これは同次変換行列の積として一意に書ける、いわば「一方通行の計算」です。
$$ \bm{x} = f(\bm{q}) $$
ここで $f$ は順運動学の写像です。たとえば2次元平面の2リンクアームなら、手先の座標 $(x, y)$ は次のように計算できました。
$$ \begin{align} x &= l_1 \cos q_1 + l_2 \cos(q_1 + q_2) \\ y &= l_1 \sin q_1 + l_2 \sin(q_1 + q_2) \end{align} $$
逆運動学はこの逆問題です。目標とする手先の位置・姿勢 $\bm{x}_d$ が与えられたとき、
$$ \bm{x}_d = f(\bm{q}) $$
を満たす関節角度 $\bm{q}$ を求めます。日常のたとえで言えば、「テーブルの上のコップの位置はわかっている。では、肩と肘をどの角度にすれば手がコップに届くか？」というのが逆運動学の問題です。
なぜ逆が難しいのか — 直感的な理解
順運動学は三角関数の和で書ける明示的な式なので、関節角度を代入すれば即座に手先位置が得られます。しかし、逆を解こうとすると $\cos$ や $\sin$ が入り組んだ非線形方程式系を解くことになります。これは一般に、解の公式を書き下せません。
人間の腕で考えるとわかりやすいでしょう。テーブルの上の物体に手を伸ばすとき、同じ場所に手を置くにも「肘を上に突き出す」姿勢と「肘を下に垂らす」姿勢の両方が可能です。つまり、同じ手先位置に対して複数の関節角度の組が存在するのです。さらに、肩の自由度を考えれば無数の姿勢が可能になります。
この直感が、逆運動学の3つの根本的な困難 —「非線形性」「多解性」「解の不存在」— につながります。次のセクションで一つずつ見ていきましょう。
逆運動学の3つの困難
逆運動学が順運動学ほど単純にいかない理由は、大きく分けて3つあります。
困難1: 非線形性
順運動学の写像 $\bm{x} = f(\bm{q})$ は三角関数で構成されるため、本質的に非線形です。2リンクアームの例を再掲すると、
$$ \begin{align} x &= l_1 \cos q_1 + l_2 \cos(q_1 + q_2) \\ y &= l_1 \sin q_1 + l_2 \sin(q_1 + q_2) \end{align} $$
この方程式系を $q_1, q_2$ について閉じた形（解析解）で解こうとすると、加法定理を使って三角関数を展開・整理する必要があります。2リンクなら頑張れば解けますが、リンク数が増えると手に負えなくなります。6自由度以上のロボットアームでは、特殊な構造（手首3軸交差など）がない限り、解析解は一般に得られません。
困難2: 多解性
同じ手先の位置・姿勢に対して、関節角度の解が複数存在することがあります。2リンク平面アームでは最大2つの解（肘上げ・肘下げ）が存在し、6自由度の産業用ロボットでは最大16個の解が存在する場合があります。
さらに、関節数が手先の自由度（タスク空間の次元）を上回る冗長マニピュレータの場合、解は無限に存在します。たとえば、人間の腕は肩3自由度 + 肘1自由度 + 手首3自由度の計7自由度を持ちますが、手先の位置・姿勢は6自由度で記述されるため、1自由度の冗長性があります。この冗長性により、手先を同じ位置に保ったまま肘の位置だけを変える「ヌルスペース運動」が可能になります。
困難3: 解の不存在
目標位置がロボットアームの作業空間（ワークスペース）の外にある場合、当然ながら解は存在しません。2リンクアームで言えば、目標点が原点から $l_1 + l_2$ より遠い場合や $|l_1 – l_2|$ より近い場合がこれに該当します。
また、目標として位置だけでなく姿勢も指定する場合、位置は作業空間内にあっても、その位置で指定された姿勢を実現できないことがあります。こうした場合にどう対処するか（たとえば「位置は正確に、姿勢は可能な限り近づける」など）も逆運動学の重要なトピックです。
これら3つの困難を踏まえた上で、まずは最もシンプルなケースである2リンク平面マニピュレータで解析的に逆運動学を解いてみましょう。
解析的逆運動学 — 2リンク平面マニピュレータ
解析的逆運動学とは、順運動学の方程式を数学的に変形して、関節角度を目標位置の関数として閉じた形（closed-form）で表現する方法です。方程式の数と未知数の数が等しく、かつマニピュレータの構造がシンプルな場合に適用できます。
ここでは2リンク平面マニピュレータ（2つの回転関節 + 2つのリンク）を題材に、幾何学的方法と代数的方法の2つのアプローチを紹介します。
問題設定
2リンク平面マニピュレータは、原点（ベース）に第1関節があり、リンク長 $l_1$ の第1リンクの先に第2関節、さらにリンク長 $l_2$ の第2リンクの先に手先があります。
- $q_1$: 第1関節の角度（x軸正方向からの偏角）
- $q_2$: 第2関節の角度（第1リンクからの相対角度）
- 目標手先位置: $(x_d, y_d)$
順運動学は、
$$ \begin{align} x_d &= l_1 \cos q_1 + l_2 \cos(q_1 + q_2) \\ y_d &= l_1 \sin q_1 + l_2 \sin(q_1 + q_2) \end{align} $$
です。この連立方程式を $q_1, q_2$ について解きます。
幾何学的方法
幾何学的方法は、マニピュレータの幾何学的関係（三角形の辺と角の関係）を直接利用して解く方法です。図形的な直感が得られるため、まずこちらから紹介します。
ステップ1: $q_2$ を求める（余弦定理）
ベース（原点）、第2関節（肘）、手先の3点は三角形を形成します。この三角形の3辺は、
- ベースから手先までの距離: $r = \sqrt{x_d^2 + y_d^2}$
- 第1リンクの長さ: $l_1$
- 第2リンクの長さ: $l_2$
です。余弦定理を適用します。ベースから手先までの距離 $r$ に対して、
$$ r^2 = l_1^2 + l_2^2 – 2 l_1 l_2 \cos(\pi – q_2) $$
ここで $\pi – q_2$ は三角形の肘側の内角です。$\cos(\pi – q_2) = -\cos q_2$ を使うと、
$$ r^2 = l_1^2 + l_2^2 + 2 l_1 l_2 \cos q_2 $$
$r^2 = x_d^2 + y_d^2$ を代入して $\cos q_2$ について解くと、
$$ \begin{equation} \cos q_2 = \frac{x_d^2 + y_d^2 – l_1^2 – l_2^2}{2 l_1 l_2} \end{equation} $$
この値を $c_2$ と略記します。$|c_2| \leq 1$ であれば解が存在し、$|c_2| > 1$ であれば目標位置が作業空間外であることを意味します。
$\cos q_2 = c_2$ から $q_2$ を求めるには $\sin q_2$ も必要です。$\sin^2 q_2 + \cos^2 q_2 = 1$ より、
$$ \sin q_2 = \pm \sqrt{1 – c_2^2} $$
符号の $\pm$ が2つの解（肘上げ/肘下げ）に対応します。atan2 を使って、
$$ \begin{equation} q_2 = \text{atan2}(\pm\sqrt{1 – c_2^2},\ c_2) \end{equation} $$
と求められます。ここで $\text{atan2}(y, x)$ は4象限に対応した逆正接関数で、$\text{atan2}$ を使うことで $q_2$ の象限を正しく決定できます。
ステップ2: $q_1$ を求める
$q_2$ が定まると、$q_1$ を幾何学的に求められます。手先位置 $(x_d, y_d)$ の偏角を $\alpha$ とすると、
$$ \alpha = \text{atan2}(y_d, x_d) $$
次に、ベースから手先方向を見たとき、第1リンクと手先方向のなす角を $\beta$ とします。この角度は三角形の辺の関係から求まります。正弦定理、あるいは直接的に次のように計算できます。
$$ \beta = \text{atan2}(l_2 \sin q_2,\ l_1 + l_2 \cos q_2) $$
この式は、手先方向を向いたベクトルを第1リンク方向（長さ $l_1$）と第2リンクの寄与分に分解したときの角度ずれを表しています。
最終的に、
$$ \begin{equation} q_1 = \alpha – \beta = \text{atan2}(y_d, x_d) – \text{atan2}(l_2 \sin q_2,\ l_1 + l_2 \cos q_2) \end{equation} $$
と求まります。$q_2$ の符号（肘上げ/肘下げ）によって $\beta$ の値も変わるため、$q_1$ も連動して2つの解が得られます。
代数的方法
代数的方法は、三角関数の恒等式を使って方程式を代数的に操作し、解を導く方法です。幾何学的方法と結果は同じですが、手続きが系統的であり、より複雑な構造への拡張に適しています。
ステップ1: $q_2$ を求める
順運動学の式の両辺を二乗して足し合わせます。
$$ x_d^2 + y_d^2 = \left(l_1 \cos q_1 + l_2 \cos(q_1 + q_2)\right)^2 + \left(l_1 \sin q_1 + l_2 \sin(q_1 + q_2)\right)^2 $$
右辺を展開します。$\cos^2 \theta + \sin^2 \theta = 1$ を繰り返し使うと、
$$ x_d^2 + y_d^2 = l_1^2 + l_2^2 + 2 l_1 l_2 \left[\cos q_1 \cos(q_1 + q_2) + \sin q_1 \sin(q_1 + q_2)\right] $$
角括弧内に加法定理の逆 $\cos(A – B) = \cos A \cos B + \sin A \sin B$ を適用すると、
$$ \cos q_1 \cos(q_1 + q_2) + \sin q_1 \sin(q_1 + q_2) = \cos\left((q_1 + q_2) – q_1\right) = \cos q_2 $$
したがって、
$$ x_d^2 + y_d^2 = l_1^2 + l_2^2 + 2 l_1 l_2 \cos q_2 $$
これを $\cos q_2$ について解くと、幾何学的方法と同じ結果が得られます。
$$ \cos q_2 = \frac{x_d^2 + y_d^2 – l_1^2 – l_2^2}{2 l_1 l_2} $$
ステップ2: $q_1$ を求める
順運動学の式に加法定理を適用して展開します。$c_1 = \cos q_1$, $s_1 = \sin q_1$, $c_{12} = \cos(q_1 + q_2)$, $s_{12} = \sin(q_1 + q_2)$ と略記すると、
$$ \begin{align} x_d &= l_1 c_1 + l_2 (c_1 \cos q_2 – s_1 \sin q_2) \\ &= (l_1 + l_2 \cos q_2) c_1 – (l_2 \sin q_2) s_1 \end{align} $$
同様に $y_d$ についても、
$$ \begin{align} y_d &= l_1 s_1 + l_2 (s_1 \cos q_2 + c_1 \sin q_2) \\ &= (l_2 \sin q_2) c_1 + (l_1 + l_2 \cos q_2) s_1 \end{align} $$
ここで $A = l_1 + l_2 \cos q_2$, $B = l_2 \sin q_2$ とおくと、連立方程式は次のようにまとまります。
$$ \begin{pmatrix} x_d \\ y_d \end{pmatrix} = \begin{pmatrix} A & -B \\ B & A \end{pmatrix} \begin{pmatrix} c_1 \\ s_1 \end{pmatrix} $$
この行列の行列式は $A^2 + B^2 = l_1^2 + l_2^2 + 2 l_1 l_2 \cos q_2 = r^2 > 0$（$r \neq 0$ のとき）なので、逆行列が存在します。逆行列を左からかけると、
$$ \begin{pmatrix} c_1 \\ s_1 \end{pmatrix} = \frac{1}{A^2 + B^2} \begin{pmatrix} A & B \\ -B & A \end{pmatrix} \begin{pmatrix} x_d \\ y_d \end{pmatrix} $$
$c_1$ と $s_1$ が求まったので、
$$ q_1 = \text{atan2}(s_1, c_1) $$
として $q_1$ が得られます。結果は幾何学的方法と一致します。
代数的方法の利点は、三角形の図形的関係に頼らず連立方程式を行列形式で系統的に処理できる点です。3リンク以上の場合や、位置だけでなく姿勢も含めた逆運動学に拡張しやすくなります。
解析解が得られたところで、次はこの解が複数存在するという「多解性」の問題を詳しく見てみましょう。
多解性と肘上げ/肘下げ
2つの解の幾何学的意味
先ほど $\sin q_2 = \pm \sqrt{1 – c_2^2}$ の符号が2つの解を生むことを見ました。この2つの解は、肘関節（第2関節）の位置によって区別されます。
- 肘上げ（elbow-up）: $q_2 > 0$（肘が「上側」に曲がる姿勢）。$\sin q_2 > 0$ に対応
- 肘下げ（elbow-down）: $q_2 < 0$（肘が「下側」に曲がる姿勢）。$\sin q_2 < 0$ に対応
どちらの姿勢を採用するかは、応用文脈に依存します。産業用ロボットでは障害物を避けられる姿勢を選びますし、宇宙ロボットアームでは太陽電池パネルやアンテナとの干渉を避ける姿勢が優先されます。
特殊な場合
解の数は目標位置によって変わります。
- 2解: 一般的な位置（$|l_1 – l_2| < r < l_1 + l_2$）。肘上げと肘下げの2通り
- 1解（特異姿勢）: $r = l_1 + l_2$（完全に伸びきった姿勢）または $r = |l_1 – l_2|$（完全に折り畳まれた姿勢）。このとき $|c_2| = 1$ で解が1つに縮退
- 0解: $r > l_1 + l_2$（遠すぎて届かない）または $r < |l_1 - l_2|$（近すぎて届かない）
完全に伸びきった姿勢 $q_2 = 0$ や完全に折り畳まれた姿勢 $q_2 = \pm\pi$ は、後述する特異点に相当し、数値解法でも問題を引き起こします。
6自由度ロボットの多解性
産業用ロボットで標準的な6自由度マニピュレータでは、手先の位置（3自由度）と姿勢（3自由度）をすべて指定すると、一般に最大16個の解が存在します。Pieper の条件（最後の3軸が1点で交わる構造）を満たす場合は、位置と姿勢を分離して解くことができ、最大8個の解が得られます。多くの産業用ロボット（KUKA, ABB, FANUC など）はこの条件を意図的に満たすように設計されています。
解析解は2リンクや特殊構造のロボットには有効ですが、一般の多自由度マニピュレータでは適用困難です。そこで次のセクションでは、任意のマニピュレータに適用できる数値的な手法を見ていきましょう。
数値的逆運動学 — ヤコビ行列を用いた反復法
基本的なアイデア
解析解が得られない場合、逆運動学を反復的な数値最適化で解きます。基本的な考え方は、以下のとおりです。
- 関節角度の初期推定 $\bm{q}_0$ からスタート
- 順運動学で現在の手先位置 $\bm{x}_k = f(\bm{q}_k)$ を計算
- 目標位置 $\bm{x}_d$ との誤差 $\bm{e}_k = \bm{x}_d – f(\bm{q}_k)$ を計算
- 誤差を減らす方向に $\bm{q}_k$ を更新
- 誤差が十分小さくなるまで繰り返す
このアルゴリズムの核心は「ステップ4でどのように更新するか」にあります。ここでヤコビ行列が登場します。
ヤコビ行列の役割
ヤコビ行列 $\bm{J}(\bm{q})$ は、関節角度の微小変化 $\delta \bm{q}$ が手先位置の微小変化 $\delta \bm{x}$ にどう影響するかを線形近似する行列です。
$$ \delta \bm{x} = \bm{J}(\bm{q}) \, \delta \bm{q} $$
ここで $\bm{J}(\bm{q})$ は $m \times n$ 行列（$m$: 手先の自由度, $n$: 関節の数）で、各成分は、
$$ J_{ij} = \frac{\partial x_i}{\partial q_j} $$
です。たとえば2リンク平面アーム（$m = 2, n = 2$）のヤコビ行列は、
$$ \bm{J}(\bm{q}) = \begin{pmatrix} -l_1 \sin q_1 – l_2 \sin(q_1 + q_2) & -l_2 \sin(q_1 + q_2) \\ l_1 \cos q_1 + l_2 \cos(q_1 + q_2) & l_2 \cos(q_1 + q_2) \end{pmatrix} $$
ヤコビ行列は「関節空間からタスク空間への写像の局所的な線形近似」を与えます。順運動学 $\bm{x} = f(\bm{q})$ という非線形写像を、現在の $\bm{q}_k$ の近傍で1次テイラー展開したものです。
$$ f(\bm{q}_k + \delta \bm{q}) \approx f(\bm{q}_k) + \bm{J}(\bm{q}_k) \, \delta \bm{q} $$
ニュートン・ラフソン法
目標位置 $\bm{x}_d$ との誤差 $\bm{e}_k = \bm{x}_d – f(\bm{q}_k)$ をゼロにする $\delta \bm{q}$ を求めたいので、
$$ \bm{x}_d \approx f(\bm{q}_k) + \bm{J}(\bm{q}_k) \, \delta \bm{q} $$
両辺から $f(\bm{q}_k)$ を引くと、
$$ \bm{e}_k = \bm{J}(\bm{q}_k) \, \delta \bm{q} $$
$\bm{J}$ が正方かつ正則（$m = n$ かつ $\det \bm{J} \neq 0$）の場合、逆行列をとって、
$$ \begin{equation} \delta \bm{q} = \bm{J}(\bm{q}_k)^{-1} \, \bm{e}_k \end{equation} $$
更新式は、
$$ \bm{q}_{k+1} = \bm{q}_k + \delta \bm{q} = \bm{q}_k + \bm{J}(\bm{q}_k)^{-1} \, \bm{e}_k $$
これはまさにニュートン・ラフソン法（Newton-Raphson method）です。非線形方程式 $f(\bm{q}) – \bm{x}_d = \bm{0}$ の根を反復的に求めるアルゴリズムであり、初期値が解の近くにあれば2次収束（反復ごとに精度の有効桁数がおよそ倍増）します。
しかし、ニュートン・ラフソン法にはいくつかの限界があります。
- $\bm{J}$ が正方でない場合（冗長マニピュレータ: $n > m$、または劣駆動: $n < m$）は逆行列が存在しない
- $\bm{J}$ が特異（$\det \bm{J} = 0$）の場合は逆行列が計算できない（特異点問題）
- 初期値が解から遠いと収束しない可能性がある
これらの限界に対処するために、次のセクションでヤコビ行列の擬似逆行列法や減衰最小二乗法を紹介します。
ヤコビ転置法と擬似逆行列法
ヤコビ転置法
最もシンプルな代替手法がヤコビ転置法（Jacobian Transpose method）です。ヤコビ行列の逆行列の代わりに転置行列を使います。
$$ \begin{equation} \delta \bm{q} = \alpha \, \bm{J}(\bm{q}_k)^T \, \bm{e}_k \end{equation} $$
ここで $\alpha > 0$ はステップサイズ（学習率に相当）です。
なぜ転置行列でうまくいくのでしょうか。直感的には、$\bm{J}^T \bm{e}$ は「手先の誤差を関節空間に写し戻したベクトル」であり、この方向に関節角度を動かせば手先が目標に近づくことが保証されます。
より正確に言えば、$\bm{J}^T \bm{e}$ はタスク空間の誤差ノルム $\|\bm{e}\|^2 = \|\bm{x}_d – f(\bm{q})\|^2$ を関節空間で微分した勾配の方向（の反対向き）に対応しています。
$$ \frac{\partial}{\partial \bm{q}} \|\bm{e}\|^2 = -2 \bm{J}^T \bm{e} $$
したがって $\delta \bm{q} = \alpha \bm{J}^T \bm{e}$ は最急降下法（勾配降下法）と等価です。
ヤコビ転置法の利点は、逆行列の計算が不要で、特異点でも発散しない点です。一方、収束が遅い（1次収束）のが欠点で、実用的には後述する擬似逆行列法やDLS法が好まれます。
擬似逆行列法
擬似逆行列法（Pseudoinverse method）は、ヤコビ行列のムーア・ペンローズ擬似逆行列 $\bm{J}^+$ を使います。
$\bm{J}$ が $m \times n$ 行列のとき、$\bm{J}^+$ は以下のように定義されます。
$n \geq m$（冗長系: 関節数 $\geq$ タスク自由度）で $\bm{J}$ が行フルランクの場合、
$$ \begin{equation} \bm{J}^+ = \bm{J}^T (\bm{J} \bm{J}^T)^{-1} \end{equation} $$
$n \leq m$（劣駆動系）で $\bm{J}$ が列フルランクの場合、
$$ \bm{J}^+ = (\bm{J}^T \bm{J})^{-1} \bm{J}^T $$
擬似逆行列法の更新式は、
$$ \begin{equation} \delta \bm{q} = \bm{J}^+ \bm{e}_k \end{equation} $$
この解は、冗長系（$n > m$）の場合に $\|\delta \bm{q}\|$ を最小にする解を与えます。つまり、手先の誤差をゼロにする $\delta \bm{q}$ の中で、関節角度の変位が最も小さいものを選んでくれます。これは「必要最小限の関節運動で目標に到達する」という合理的な基準です。
擬似逆行列法はニュートン・ラフソン法の自然な一般化であり、$m = n$ かつ $\bm{J}$ が正則のときはニュートン・ラフソン法と一致します。冗長マニピュレータにも劣駆動系にも適用でき、多くのロボティクスソフトウェアで標準的に実装されています。
しかし、$\bm{J}$ が特異（ランク落ち）に近いとき、$(\bm{J} \bm{J}^T)^{-1}$ の成分が非常に大きくなり、$\delta \bm{q}$ が爆発的に大きくなるという問題があります。これが特異点問題です。
特異点問題と減衰最小二乗法（DLS）
特異点とは何か
マニピュレータの特異点（singularity）とは、ヤコビ行列 $\bm{J}$ のランクが落ちる（正方なら $\det \bm{J} = 0$ となる）姿勢のことです。特異点では、ある方向への手先の微小移動が不可能になります。
2リンク平面アームの場合、ヤコビ行列の行列式は、
$$ \det \bm{J} = l_1 l_2 \sin q_2 $$
したがって $q_2 = 0$（完全に伸びきった姿勢）または $q_2 = \pm\pi$（完全に折り畳まれた姿勢）で特異点になります。
特異点の物理的な意味を考えましょう。アームが完全に伸びきった状態（$q_2 = 0$）では、手先は原点から最も遠い位置 $r = l_1 + l_2$ にあります。このとき、手先をさらに遠ざける方向に動かすことは不可能です。つまり、特異点ではタスク空間の一部の方向への移動能力が失われます。
特異点近傍での問題
特異点の「ちょうど上」でなくても、特異点の近傍で問題が発生します。$\bm{J}$ の最小特異値 $\sigma_{\min}$ が小さくなると、擬似逆行列の成分が $1/\sigma_{\min}$ に比例して大きくなります。その結果、手先のわずかな誤差を解消するために、関節角度に巨大な変化が要求されます。
これは物理的には、特異点近傍では「手先のわずかな移動でも大きな関節運動が必要」であることを反映しています。実際のロボットでは関節速度に上限があるため、特異点近傍で所望の手先速度を実現できなくなります。
減衰最小二乗法（Damped Least Squares, DLS）
減衰最小二乗法（DLS法, Levenberg-Marquardt法とも呼ばれる）は、特異点問題を解決するための最も実用的な手法です。
基本的なアイデアは、「手先の誤差を正確にゼロにすること」だけでなく、「関節角度の変化を小さく保つこと」も同時に考慮する、というものです。具体的には、次の目的関数を最小化します。
$$ \begin{equation} \min_{\delta \bm{q}} \left\{ \|\bm{J} \, \delta \bm{q} – \bm{e}\|^2 + \lambda^2 \|\delta \bm{q}\|^2 \right\} \end{equation} $$
第1項はタスク空間での誤差、第2項は関節空間での変化の大きさです。$\lambda > 0$ は減衰係数（ダンピングファクタ）で、両者のバランスを制御します。
この最適化問題の解を求めましょう。目的関数を $\delta \bm{q}$ で微分してゼロとおきます。
$$ \bm{J}^T (\bm{J} \, \delta \bm{q} – \bm{e}) + \lambda^2 \delta \bm{q} = \bm{0} $$
$\delta \bm{q}$ について整理すると、
$$ (\bm{J}^T \bm{J} + \lambda^2 \bm{I}) \, \delta \bm{q} = \bm{J}^T \bm{e} $$
$\bm{J}^T \bm{J} + \lambda^2 \bm{I}$ は $\lambda > 0$ である限り常に正定値（したがって正則）なので、安全に逆行列を取れます。
$$ \begin{equation} \delta \bm{q} = (\bm{J}^T \bm{J} + \lambda^2 \bm{I})^{-1} \bm{J}^T \bm{e} \end{equation} $$
あるいは等価な形として、
$$ \delta \bm{q} = \bm{J}^T (\bm{J} \bm{J}^T + \lambda^2 \bm{I})^{-1} \bm{e} $$
とも書けます。この2つの形式は数学的に等価であり、行列サイズに応じて計算効率の良い方を選びます（$n < m$ なら前者、$n > m$ なら後者）。
DLS法の特異値分解による解釈
DLS法の挙動を理解するには、特異値分解（SVD）が役立ちます。ヤコビ行列を特異値分解すると、
$$ \bm{J} = \bm{U} \bm{\Sigma} \bm{V}^T $$
ここで $\bm{U}$ と $\bm{V}$ は直交行列、$\bm{\Sigma}$ は特異値 $\sigma_1 \geq \sigma_2 \geq \cdots \geq \sigma_r > 0$ を対角成分に持つ行列です。
擬似逆行列は各特異値の逆数 $1/\sigma_i$ で構成されます。
$$ \bm{J}^+ = \bm{V} \bm{\Sigma}^+ \bm{U}^T, \quad \sigma_i^+ = \frac{1}{\sigma_i} $$
$\sigma_i$ が小さいと $1/\sigma_i$ が爆発し、これが特異点近傍での発散の原因です。
DLS法では、$1/\sigma_i$ の代わりに次のフィルタがかかります。
$$ \frac{\sigma_i}{\sigma_i^2 + \lambda^2} $$
$\sigma_i \gg \lambda$ のとき: フィルタ値 $\approx 1/\sigma_i$（擬似逆行列とほぼ同じ） $\sigma_i \ll \lambda$ のとき: フィルタ値 $\approx \sigma_i / \lambda^2$（$\sigma_i$ に比例して小さくなる）
つまり、DLS法は大きな特異値の方向では通常の擬似逆行列と同等に動作し、小さな特異値の方向では $\delta \bm{q}$ を抑制します。これにより特異点近傍での発散が防がれます。
$\lambda$ の選び方
$\lambda$ の値は、精度と安定性のトレードオフを決定します。
- $\lambda$ が大きすぎる: 安定だが収束が遅く、最終的な位置精度が下がる
- $\lambda$ が小さすぎる: 精度は高いが、特異点近傍で発散しやすい
- $\lambda = 0$: 擬似逆行列法と一致（特異点で発散）
実用的には、$\lambda$ を固定値にするのではなく、特異点からの距離に応じて適応的に変化させる方法が効果的です。Nakamura-Hanafusa の方法では、ヤコビ行列の最小特異値 $\sigma_{\min}$ に基づいて $\lambda$ を調整します。
$$ \lambda^2 = \begin{cases} 0 & (\sigma_{\min} \geq \epsilon) \\ \lambda_{\max}^2 \left(1 – \left(\frac{\sigma_{\min}}{\epsilon}\right)^2\right) & (\sigma_{\min} < \epsilon) \end{cases} $$
ここで $\epsilon$ は閾値、$\lambda_{\max}$ は減衰係数の最大値です。特異点から離れた場所では $\lambda = 0$（擬似逆行列法と同じ高精度）、特異点近傍では $\lambda$ を大きくして安定性を確保します。
ここまでで逆運動学の主要な理論を一通りカバーしました。次のセクションでは、これらの手法をPythonで実装し、解析解と数値解を比較してみましょう。
Pythonでの実装と可視化
2リンク平面アームの解析的逆運動学
まず、2リンク平面マニピュレータの解析的逆運動学を実装し、肘上げ/肘下げの2解を可視化します。
import numpy as np
import matplotlib.pyplot as plt
def forward_kinematics_2link(q1, q2, l1, l2):
"""2リンク平面アームの順運動学"""
x1 = l1 * np.cos(q1)
y1 = l1 * np.sin(q1)
x2 = x1 + l2 * np.cos(q1 + q2)
y2 = y1 + l2 * np.sin(q1 + q2)
return (x1, y1), (x2, y2)
def inverse_kinematics_2link(xd, yd, l1, l2):
"""2リンク平面アームの解析的逆運動学（2解を返す）"""
r_sq = xd**2 + yd**2
cos_q2 = (r_sq - l1**2 - l2**2) / (2 * l1 * l2)
# 解の存在判定
if abs(cos_q2) > 1.0:
return None # 到達不能
sin_q2_pos = np.sqrt(1 - cos_q2**2)
sin_q2_neg = -sin_q2_pos
solutions = []
for sin_q2 in [sin_q2_pos, sin_q2_neg]:
q2 = np.arctan2(sin_q2, cos_q2)
alpha = np.arctan2(yd, xd)
beta = np.arctan2(l2 * sin_q2, l1 + l2 * cos_q2)
q1 = alpha - beta
solutions.append((q1, q2))
return solutions
次に、このコードを使って特定の目標位置に対する2つの解を可視化します。
import numpy as np
import matplotlib.pyplot as plt
def forward_kinematics_2link(q1, q2, l1, l2):
"""2リンク平面アームの順運動学"""
x1 = l1 * np.cos(q1)
y1 = l1 * np.sin(q1)
x2 = x1 + l2 * np.cos(q1 + q2)
y2 = y1 + l2 * np.sin(q1 + q2)
return (x1, y1), (x2, y2)
def inverse_kinematics_2link(xd, yd, l1, l2):
"""2リンク平面アームの解析的逆運動学（2解を返す）"""
r_sq = xd**2 + yd**2
cos_q2 = (r_sq - l1**2 - l2**2) / (2 * l1 * l2)
if abs(cos_q2) > 1.0:
return None
sin_q2_pos = np.sqrt(1 - cos_q2**2)
sin_q2_neg = -sin_q2_pos
solutions = []
for sin_q2 in [sin_q2_pos, sin_q2_neg]:
q2 = np.arctan2(sin_q2, cos_q2)
alpha = np.arctan2(yd, xd)
beta = np.arctan2(l2 * sin_q2, l1 + l2 * cos_q2)
q1 = alpha - beta
solutions.append((q1, q2))
return solutions
# パラメータ
l1, l2 = 1.0, 0.8
xd, yd = 1.2, 0.6
# 解析解を求める
solutions = inverse_kinematics_2link(xd, yd, l1, l2)
fig, axes = plt.subplots(1, 2, figsize=(12, 5))
titles = ["Elbow-up ($q_2 > 0$)", "Elbow-down ($q_2 < 0$)"]
for idx, (q1, q2) in enumerate(solutions):
ax = axes[idx]
(x1, y1), (x2, y2) = forward_kinematics_2link(q1, q2, l1, l2)
# アームの描画
ax.plot([0, x1, x2], [0, y1, y2], "o-", lw=3, markersize=8,
color="steelblue", label="Arm")
ax.plot(xd, yd, "rx", markersize=12, markeredgewidth=3,
label=f"Target ({xd}, {yd})")
# 作業空間の境界（参考）
theta = np.linspace(0, 2*np.pi, 100)
ax.plot((l1+l2)*np.cos(theta), (l1+l2)*np.sin(theta),
"k--", alpha=0.2, label="Workspace boundary")
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(fontsize=9)
ax.set_title(f"{titles[idx]}\n$q_1$={np.degrees(q1):.1f}°, "
f"$q_2$={np.degrees(q2):.1f}°")
plt.suptitle("Analytical Inverse Kinematics: Two Solutions", fontsize=14)
plt.tight_layout()
plt.show()
上のグラフから、同じ目標位置 $(1.2, 0.6)$ に対して2つの異なるアーム姿勢が得られることが確認できます。左側の肘上げ解では $q_2 > 0$ で肘が上方に曲がり、右側の肘下げ解では $q_2 < 0$ で肘が下方に曲がっています。いずれの場合も手先は正確に目標位置に到達しています。破線の円はアームの最大到達距離 $l_1 + l_2 = 1.8$ を示す作業空間境界です。
作業空間と解の存在範囲の可視化
次に、目標位置をxy平面上で変化させたとき、解が2つ存在する領域・1つの領域・存在しない領域を色分けして可視化します。
import numpy as np
import matplotlib.pyplot as plt
l1, l2 = 1.0, 0.8
# xy平面上のグリッド
x = np.linspace(-2.0, 2.0, 400)
y = np.linspace(-2.0, 2.0, 400)
X, Y = np.meshgrid(x, y)
R = np.sqrt(X**2 + Y**2)
# cos(q2) の計算
cos_q2 = (X**2 + Y**2 - l1**2 - l2**2) / (2 * l1 * l2)
# 解の数: |cos_q2| < 1 -> 2解, |cos_q2| = 1 -> 1解, |cos_q2| > 1 -> 0解
n_solutions = np.zeros_like(R)
n_solutions[np.abs(cos_q2) < 1.0 - 1e-10] = 2
n_solutions[np.abs(np.abs(cos_q2) - 1.0) < 1e-10] = 1
fig, ax = plt.subplots(figsize=(7, 7))
cmap = plt.cm.RdYlGn
im = ax.contourf(X, Y, n_solutions, levels=[-0.5, 0.5, 1.5, 2.5],
colors=["#f0f0f0", "#fdd49e", "#7fcdbb"], alpha=0.8)
# 境界円
theta = np.linspace(0, 2*np.pi, 200)
ax.plot((l1+l2)*np.cos(theta), (l1+l2)*np.sin(theta),
"k-", lw=1.5, label=f"$r = l_1 + l_2 = {l1+l2}$")
ax.plot(abs(l1-l2)*np.cos(theta), abs(l1-l2)*np.sin(theta),
"k--", lw=1.5, label=f"$r = |l_1 - l_2| = {abs(l1-l2)}$")
ax.set_xlim(-2.0, 2.0)
ax.set_ylim(-2.0, 2.0)
ax.set_aspect("equal")
ax.set_xlabel("$x$")
ax.set_ylabel("$y$")
ax.set_title("Number of IK Solutions in Workspace")
ax.legend(fontsize=10)
# カラーバーの代わりにテキスト
ax.text(1.3, 1.3, "2 solutions", fontsize=11, color="#2c7fb8",
fontweight="bold")
ax.text(0.02, 0.02, "0 solutions\n(inner void)", fontsize=10,
color="#d95f02", ha="center")
ax.text(1.8, -1.8, "0 solutions\n(out of reach)", fontsize=10,
color="#666666", ha="center")
plt.tight_layout()
plt.show()
このグラフは、2リンクアームの作業空間を鳥瞰したものです。緑色の環状領域（$|l_1 - l_2| < r < l_1 + l_2$、つまり $0.2 < r < 1.8$）が2つの解（肘上げ・肘下げ）が存在する領域です。外側の実線円（$r = 1.8$）の外は手が届かないため解なし、内側の破線円（$r = 0.2$）の内部も近すぎて手が届かないため解なしです。境界円上では $|c_2| = 1$ となり、解が1つに縮退する特異姿勢に対応します。
数値的逆運動学の実装 — 4つの手法を比較
ここでは、ニュートン・ラフソン法、ヤコビ転置法、擬似逆行列法、DLS法の4つを実装し、収束の様子を比較します。
まず、共通して使う順運動学とヤコビ行列の関数を定義します。
import numpy as np
import matplotlib.pyplot as plt
def fk_2link(q, l1=1.0, l2=0.8):
"""2リンク順運動学: 手先位置 (x, y) を返す"""
q1, q2 = q
x = l1 * np.cos(q1) + l2 * np.cos(q1 + q2)
y = l1 * np.sin(q1) + l2 * np.sin(q1 + q2)
return np.array([x, y])
def jacobian_2link(q, l1=1.0, l2=0.8):
"""2リンクアームのヤコビ行列 (2x2)"""
q1, q2 = q
J = np.array([
[-l1*np.sin(q1) - l2*np.sin(q1+q2), -l2*np.sin(q1+q2)],
[ l1*np.cos(q1) + l2*np.cos(q1+q2), l2*np.cos(q1+q2)]
])
return J
次に、4つの逆運動学ソルバーを実装します。
def ik_newton_raphson(xd, q0, l1=1.0, l2=0.8,
tol=1e-6, max_iter=100):
"""ニュートン・ラフソン法"""
q = q0.copy()
errors = []
for i in range(max_iter):
e = xd - fk_2link(q, l1, l2)
errors.append(np.linalg.norm(e))
if errors[-1] < tol:
break
J = jacobian_2link(q, l1, l2)
dq = np.linalg.solve(J, e)
q = q + dq
return q, errors
def ik_jacobian_transpose(xd, q0, l1=1.0, l2=0.8,
alpha=0.5, tol=1e-6, max_iter=500):
"""ヤコビ転置法"""
q = q0.copy()
errors = []
for i in range(max_iter):
e = xd - fk_2link(q, l1, l2)
errors.append(np.linalg.norm(e))
if errors[-1] < tol:
break
J = jacobian_2link(q, l1, l2)
dq = alpha * J.T @ e
q = q + dq
return q, errors
def ik_pseudoinverse(xd, q0, l1=1.0, l2=0.8,
tol=1e-6, max_iter=100):
"""擬似逆行列法"""
q = q0.copy()
errors = []
for i in range(max_iter):
e = xd - fk_2link(q, l1, l2)
errors.append(np.linalg.norm(e))
if errors[-1] < tol:
break
J = jacobian_2link(q, l1, l2)
J_pinv = np.linalg.pinv(J)
dq = J_pinv @ e
q = q + dq
return q, errors
def ik_dls(xd, q0, l1=1.0, l2=0.8,
lam=0.1, tol=1e-6, max_iter=100):
"""減衰最小二乗法 (DLS)"""
q = q0.copy()
errors = []
for i in range(max_iter):
e = xd - fk_2link(q, l1, l2)
errors.append(np.linalg.norm(e))
if errors[-1] < tol:
break
J = jacobian_2link(q, l1, l2)
JJT = J @ J.T
dq = J.T @ np.linalg.solve(JJT + lam**2 * np.eye(2), e)
q = q + dq
return q, errors
4つの手法を同じ条件で実行し、収束曲線を比較します。
import numpy as np
import matplotlib.pyplot as plt
# (fk_2link, jacobian_2link, 4つのソルバーは上で定義済み)
# 共通パラメータ
l1, l2 = 1.0, 0.8
xd = np.array([1.2, 0.6])
q0 = np.array([0.1, 0.1]) # 初期推定
# 各手法の実行
q_nr, err_nr = ik_newton_raphson(xd, q0, l1, l2)
q_jt, err_jt = ik_jacobian_transpose(xd, q0, l1, l2, alpha=0.5)
q_pi, err_pi = ik_pseudoinverse(xd, q0, l1, l2)
q_dls, err_dls = ik_dls(xd, q0, l1, l2, lam=0.1)
# 収束曲線のプロット
fig, ax = plt.subplots(figsize=(8, 5))
ax.semilogy(err_nr, "o-", label="Newton-Raphson", markersize=4)
ax.semilogy(err_jt, "s-", label="Jacobian Transpose ($\\alpha=0.5$)",
markersize=3, alpha=0.7)
ax.semilogy(err_pi, "^-", label="Pseudoinverse", markersize=4)
ax.semilogy(err_dls, "d-", label="DLS ($\\lambda=0.1$)", markersize=4)
ax.axhline(1e-6, color="gray", ls="--", alpha=0.5, label="Tolerance")
ax.set_xlabel("Iteration")
ax.set_ylabel("Position error $\\|\\mathbf{e}\\|$")
ax.set_title("Convergence Comparison of IK Methods")
ax.legend()
ax.grid(True, alpha=0.3)
plt.tight_layout()
plt.show()
# 結果の表示
print("=== 数値解の比較 ===")
for name, q, err in [("Newton-Raphson", q_nr, err_nr),
("Jacobian Transpose", q_jt, err_jt),
("Pseudoinverse", q_pi, err_pi),
("DLS", q_dls, err_dls)]:
x_result = fk_2link(q, l1, l2)
print(f"{name:22s}: q=({np.degrees(q[0]):7.2f}°, "
f"{np.degrees(q[1]):7.2f}°), "
f"pos=({x_result[0]:.6f}, {x_result[1]:.6f}), "
f"iter={len(err)}")
このグラフから、各手法の収束特性の違いが明確に読み取れます。
- ニュートン・ラフソン法は最も高速に収束します。2次収束の特性により、わずか数回の反復で機械精度レベルに到達します。ただし、これはヤコビ行列が正則であるという条件のもとです。
- 擬似逆行列法もニュートン・ラフソン法と同等の速さで収束します。2リンクアーム（$n = m = 2$）では両者は同じ更新式になるため、これは理論通りです。
- DLS法（$\lambda = 0.1$）はやや収束が遅くなりますが、安定に解に到達します。減衰項 $\lambda^2 \|\delta \bm{q}\|^2$ により更新量が抑制されるため、最終的な精度もわずかに低下する場合があります。
- ヤコビ転置法は最も収束が遅く、多くの反復を必要とします。最急降下法としての1次収束が原因です。
特異点近傍での比較
次に、目標位置をアームが完全に伸びきる方向（特異点近傍）に設定し、各手法の振る舞いを比較します。
import numpy as np
import matplotlib.pyplot as plt
# (fk_2link, jacobian_2link, 4つのソルバーは上で定義済み)
l1, l2 = 1.0, 0.8
# 特異点近傍の目標（ほぼ完全に伸びきった方向）
xd_singular = np.array([1.75, 0.0]) # r = 1.75, l1+l2 = 1.8
q0 = np.array([0.3, 0.3])
# 各手法の実行
_, err_nr_s = ik_newton_raphson(xd_singular, q0, l1, l2,
max_iter=50)
_, err_jt_s = ik_jacobian_transpose(xd_singular, q0, l1, l2,
alpha=0.3, max_iter=500)
_, err_pi_s = ik_pseudoinverse(xd_singular, q0, l1, l2,
max_iter=50)
_, err_dls_s = ik_dls(xd_singular, q0, l1, l2,
lam=0.05, max_iter=50)
fig, ax = plt.subplots(figsize=(8, 5))
ax.semilogy(err_nr_s, "o-", label="Newton-Raphson", markersize=4)
ax.semilogy(err_jt_s[:100], "s-", label="Jacobian Transpose",
markersize=3, alpha=0.7)
ax.semilogy(err_pi_s, "^-", label="Pseudoinverse", markersize=4)
ax.semilogy(err_dls_s, "d-", label="DLS ($\\lambda=0.05$)", markersize=4)
ax.axhline(1e-6, color="gray", ls="--", alpha=0.5)
ax.set_xlabel("Iteration")
ax.set_ylabel("Position error $\\|\\mathbf{e}\\|$")
ax.set_title("Convergence near Singularity "
f"(target $r = {np.linalg.norm(xd_singular):.2f}$, "
f"$l_1 + l_2 = {l1+l2}$)")
ax.legend()
ax.grid(True, alpha=0.3)
plt.tight_layout()
plt.show()
特異点近傍（$r = 1.75$, $l_1 + l_2 = 1.8$）での収束曲線を見ると、ニュートン・ラフソン法と擬似逆行列法は収束するものの、途中で大きな更新が入る振動的な挙動を見せることがあります。DLS法は減衰効果により安定に収束し、このような状況では最も信頼性の高い手法です。ヤコビ転置法も発散しませんが、収束に多くの反復を必要とします。実用上、特異点近傍を通過する軌道追従にはDLS法が最も適しています。
数値解と解析解の比較可視化
最後に、目標位置を円弧上で移動させ、解析解と数値解（DLS法）の結果を比較するアニメーション的な可視化を行います。
import numpy as np
import matplotlib.pyplot as plt
# (fk_2link, jacobian_2link, inverse_kinematics_2link, ik_dls は上で定義済み)
l1, l2 = 1.0, 0.8
# 半円弧状の目標軌道
n_points = 12
angles = np.linspace(np.pi/6, 5*np.pi/6, n_points)
radius = 1.2
targets = np.array([[radius*np.cos(a), radius*np.sin(a)]
for a in angles])
fig, axes = plt.subplots(2, 4, figsize=(16, 8))
axes = axes.flatten()
q_numerical = np.array([0.5, 0.5]) # 数値解の初期推定
for idx, (xd, yd) in enumerate(targets[:8]):
ax = axes[idx]
target = np.array([xd, yd])
# 解析解（肘上げ）
sol_analytical = inverse_kinematics_2link(xd, yd, l1, l2)
q_a1, q_a2 = sol_analytical[0]
(xa1, ya1), (xa2, ya2) = forward_kinematics_2link(
q_a1, q_a2, l1, l2)
# 数値解（DLS法、前回の解を初期値に）
q_numerical, _ = ik_dls(target, q_numerical, l1, l2,
lam=0.01, max_iter=200)
(xn1, yn1), (xn2, yn2) = forward_kinematics_2link(
q_numerical[0], q_numerical[1], l1, l2)
# 描画
ax.plot([0, xa1, xa2], [0, ya1, ya2], "o-", lw=2.5,
color="steelblue", markersize=6, label="Analytical")
ax.plot([0, xn1, xn2], [0, yn1, yn2], "s--", lw=2,
color="coral", markersize=5, alpha=0.8, label="DLS")
ax.plot(xd, yd, "kx", markersize=10, markeredgewidth=2)
ax.set_xlim(-2, 2)
ax.set_ylim(-0.5, 2)
ax.set_aspect("equal")
ax.grid(True, alpha=0.2)
ax.set_title(f"Target ({xd:.1f}, {yd:.1f})", fontsize=9)
if idx == 0:
ax.legend(fontsize=8)
plt.suptitle("Analytical vs. Numerical (DLS) IK along an Arc",
fontsize=14)
plt.tight_layout()
plt.show()
この比較から、DLS法の数値解が解析解とよく一致していることが確認できます。各パネルでは円弧上の異なる目標位置に対し、青の実線が解析解（肘上げ）、赤の破線がDLS法の数値解を示しています。数値解は前のフレームの解を初期値として使用しているため、解析解と同じ「肘上げ」解に追従しています。数値法の初期値選択が解の選択（肘上げ/肘下げ）に影響するという点は、実用上重要なポイントです。
ヤコビ行列の特異値と可操作性の可視化
逆運動学の理解を深めるため、関節空間の各点でヤコビ行列の最小特異値（特異点への近さの指標）を可視化します。
import numpy as np
import matplotlib.pyplot as plt
l1, l2 = 1.0, 0.8
# 関節空間のグリッド
q1_range = np.linspace(-np.pi, np.pi, 200)
q2_range = np.linspace(-np.pi, np.pi, 200)
Q1, Q2 = np.meshgrid(q1_range, q2_range)
# 各点での最小特異値を計算
sigma_min = np.zeros_like(Q1)
for i in range(Q1.shape[0]):
for j in range(Q1.shape[1]):
q = np.array([Q1[i, j], Q2[i, j]])
J = np.array([
[-l1*np.sin(q[0]) - l2*np.sin(q[0]+q[1]),
-l2*np.sin(q[0]+q[1])],
[ l1*np.cos(q[0]) + l2*np.cos(q[0]+q[1]),
l2*np.cos(q[0]+q[1])]
])
s = np.linalg.svd(J, compute_uv=False)
sigma_min[i, j] = s[-1]
fig, ax = plt.subplots(figsize=(8, 6))
c = ax.contourf(np.degrees(Q1), np.degrees(Q2), sigma_min,
levels=30, cmap="viridis")
plt.colorbar(c, ax=ax, label="$\\sigma_{\\min}$")
# 特異点の線（q2 = 0, ±π）
ax.axhline(0, color="red", ls="--", lw=1.5, alpha=0.7,
label="$q_2 = 0$ (singularity)")
ax.axhline(180, color="red", ls="--", lw=1.5, alpha=0.7)
ax.axhline(-180, color="red", ls="--", lw=1.5, alpha=0.7)
ax.set_xlabel("$q_1$ [deg]")
ax.set_ylabel("$q_2$ [deg]")
ax.set_title("Minimum Singular Value of Jacobian in Joint Space")
ax.legend(loc="upper right")
plt.tight_layout()
plt.show()
このヒートマップは、関節空間 $(q_1, q_2)$ の各点におけるヤコビ行列の最小特異値 $\sigma_{\min}$ を示しています。$\sigma_{\min}$ が大きい（黄色い）領域ではアームの可操作性が高く、逆運動学の数値解が安定に求まります。一方、$\sigma_{\min}$ がゼロに近い（暗い）領域は特異点近傍であり、$q_2 = 0$（赤い破線）に沿って帯状に広がっています。ここは数値解法が困難になる領域です。$q_2 = \pm 180°$ の線もまた特異点に対応しますが、実用上は $q_2 = 0$（完全伸展）の特異点がより頻繁に問題になります。
解析解と数値解の誤差比較（定量評価）
最後に、作業空間内の多数の目標点に対して解析解と数値解の一致度を定量的に評価します。
import numpy as np
import matplotlib.pyplot as plt
# (fk_2link, jacobian_2link, inverse_kinematics_2link, ik_dls は上で定義済み)
l1, l2 = 1.0, 0.8
rng = np.random.default_rng(42)
# 作業空間内のランダム目標点を生成
n_test = 500
errors_dls = []
errors_pos = []
q_init = np.array([0.5, 0.5])
for _ in range(n_test):
# 到達可能な範囲でランダムに目標を生成
r = rng.uniform(abs(l1 - l2) + 0.05, l1 + l2 - 0.05)
theta = rng.uniform(-np.pi, np.pi)
xd = r * np.cos(theta)
yd = r * np.sin(theta)
# 解析解（肘上げ）
sol = inverse_kinematics_2link(xd, yd, l1, l2)
if sol is None:
continue
q_analytical = np.array(sol[0])
# 数値解（DLS法）
q_numerical, _ = ik_dls(np.array([xd, yd]), q_init, l1, l2,
lam=0.01, max_iter=200, tol=1e-10)
# 位置の誤差
pos_err = np.linalg.norm(
fk_2link(q_numerical, l1, l2) - np.array([xd, yd]))
errors_pos.append(pos_err)
# 解析解との角度差（同じ解に収束した場合）
dq = q_numerical - q_analytical
dq = np.mod(dq + np.pi, 2*np.pi) - np.pi # [-π, π] に正規化
errors_dls.append(np.linalg.norm(dq))
fig, axes = plt.subplots(1, 2, figsize=(12, 4))
# 位置精度のヒストグラム
axes[0].hist(np.log10(np.array(errors_pos) + 1e-20), bins=50,
color="steelblue", edgecolor="white")
axes[0].set_xlabel("$\\log_{10}$ (position error)")
axes[0].set_ylabel("Count")
axes[0].set_title("DLS Position Accuracy (500 random targets)")
axes[0].axvline(np.log10(1e-6), color="red", ls="--",
label="$10^{-6}$")
axes[0].legend()
# 角度差のヒストグラム
axes[1].hist(np.degrees(errors_dls), bins=50,
color="coral", edgecolor="white")
axes[1].set_xlabel("Joint angle difference [deg]")
axes[1].set_ylabel("Count")
axes[1].set_title("DLS vs. Analytical: Joint Angle Difference")
plt.tight_layout()
plt.show()
print(f"位置誤差の中央値: {np.median(errors_pos):.2e}")
print(f"位置誤差の最大値: {np.max(errors_pos):.2e}")
左のヒストグラムは、DLS法の位置精度を示しています。ほとんどのテストケースで位置誤差が $10^{-6}$ 以下に収まっており、数値解法が十分な精度を達成していることがわかります。右のヒストグラムは、DLS法の解と解析解（肘上げ）の関節角度差を示しています。多くの場合、角度差は非常に小さく両者が一致していますが、初期値によっては肘下げ解に収束するケースもあり、角度差が大きくなることがあります。これは数値解法の初期値依存性を如実に示しています。
まとめ
本記事では、ロボットアームの逆運動学について、解析解と数値解の両面から解説しました。
- 逆運動学は順運動学の逆問題であり、手先の目標位置・姿勢から各関節角度を求める問題です。非線形性・多解性・解の不存在という3つの本質的な困難があります
- 解析的逆運動学は、2リンク平面アームのように構造がシンプルな場合に閉じた形の解を与えます。幾何学的方法（余弦定理）と代数的方法（連立方程式の行列操作）の2つのアプローチがあり、いずれも $\sin q_2 = \pm\sqrt{1-c_2^2}$ の符号に対応する肘上げ/肘下げの2解を生みます
- 数値的逆運動学は、ヤコビ行列を用いた反復法であり、任意のマニピュレータに適用できます。ニュートン・ラフソン法が最も高速に収束しますが、特異点で破綻します
- 減衰最小二乗法（DLS）は、$\|\bm{J}\delta\bm{q}-\bm{e}\|^2 + \lambda^2\|\delta\bm{q}\|^2$ を最小化することで、精度と安定性を両立します。特異点近傍でも発散せず、実用上最も信頼性の高い手法です
- 数値解法は初期値に依存して異なる解に収束するため、適切な初期値選択が重要です。軌道追従では前時刻の解を初期値に使うのが一般的です
逆運動学は「目標位置に対する関節角度」を静的に求める問題でした。しかし実際のロボット制御では、手先を一定の速度で動かしたい、すなわち速度レベルでの運動学が必要になります。次の記事では、ヤコビ行列をさらに深く掘り下げ、微分運動学（速度の運動学）と特異点解析について詳しく解説します。
次のステップとして、以下の記事も参考にしてください。
- ヤコビ行列と微分運動学 — 速度レベルの運動学とヤコビ行列の詳細解析