国際宇宙ステーション（ISS）に搭載されたロボットアーム「カナダアーム2」が、接近するデブリを回避しようとした瞬間、特定の姿勢でアームがまったく動かなくなる — そんな事態が起きたら取り返しのつかない大事故につながります。地上の産業用ロボットであれば一瞬停止するだけで済むかもしれませんが、宇宙空間ではロボットアームの動作不能は即座にミッション失敗、最悪の場合は宇宙飛行士の生命に関わる問題です。
なぜロボットアームは特定の姿勢で「動けなくなる」のでしょうか？ そして、ある姿勢でどの方向にどれだけスムーズに動けるのかを、事前に定量的に評価する方法はないのでしょうか？
この2つの問いに答えるのが、特異姿勢（singularity）と可操作度楕円体（manipulability ellipsoid）です。特異姿勢とは、ヤコビ行列のランクが落ちてロボットの運動自由度が失われる姿勢のことです。一方、可操作度楕円体は、ある姿勢におけるロボットの運動能力を楕円体の形状として幾何学的に可視化する手法です。
これらの概念を理解すると、次のような応用が広がります。
- 宇宙ロボットアームの経路計画: 特異姿勢を回避する軌道を設計し、デブリ捕獲やドッキング操作の安全性を確保する
- 産業用ロボットの最適配置: 可操作度が高い姿勢で作業できるようにロボットの設置位置を決定する
- 冗長マニピュレータの制御: 余分な自由度を活用して特異姿勢を避けつつ作業を遂行する
- 手術支援ロボットの設計: 手先が繊細に動ける姿勢領域を把握し、安全な手術空間を設計する
本記事の内容
- 特異姿勢の直感的理解と数学的定義
- 2リンクマニピュレータにおける特異姿勢の幾何学的解釈
- 特異姿勢の分類（境界特異・内部特異）
- 可操作度の定義と吉川の可操作度指標
- 速度楕円体と力楕円体の関係
- Pythonによる可操作度楕円体の可視化
前提知識
この記事を読む前に、以下の記事を読んでおくと理解が深まります。
- 剛体の姿勢表現と同次変換行列 — 座標系の変換と同次変換行列の基礎
- DHパラメータと順運動学 — DHパラメータを使った順運動学の体系的な計算方法
- ヤコビ行列と微分運動学 — 関節速度と手先速度を結ぶヤコビ行列の導出
特に、ヤコビ行列 $\bm{J}(\bm{q})$ が関節速度ベクトル $\dot{\bm{q}}$ と手先速度ベクトル $\dot{\bm{x}}$ を
$$ \dot{\bm{x}} = \bm{J}(\bm{q}) \dot{\bm{q}} $$
と結びつけることは、本記事全体を通して繰り返し使う最も重要な関係式です。
ここまでの準備が整ったところで、まずは特異姿勢とは何かを直感的に掴むところから始めましょう。
特異姿勢とは — 直感的理解
腕を伸ばし切ると動けなくなる
まず、身近な例で特異姿勢の直感を掴みましょう。自分の腕を真っ直ぐ前に伸ばし切った状態を想像してください。肘が完全に伸びた状態で、指先を「もっと前に」押し出そうとしても、腕の長さ以上には届きません。肘と手首の関節をどう回しても、指先は前方にはまったく動けないのです。
これが特異姿勢の本質です。関節は自由に動かせるのに、手先が特定の方向に動けなくなる — つまり、関節空間の運動が作業空間の運動に変換されなくなるのです。
情報が失われるボトルネック
もう一つの見方として、ヤコビ行列を「変換器」として捉えてみましょう。ヤコビ行列は関節速度を手先速度に変換する装置です。正常な姿勢では、関節速度のすべての情報が手先速度に伝わります。しかし特異姿勢では、この変換器にボトルネックが生じ、一部の情報が失われてしまいます。
具体的には、ある関節速度の組み合わせを入力しても、手先速度がゼロになってしまう方向が存在します。これは、ヤコビ行列の零空間（null space）が非自明になることを意味します。つまり、$\bm{J} \dot{\bm{q}} = \bm{0}$ を満たす $\dot{\bm{q}} \neq \bm{0}$ が存在してしまうのです。
宇宙での深刻さ
地上の産業用ロボットでは、特異姿勢に近づくと速度制限をかけたり、手動で復帰させたりすることができます。しかし宇宙ロボットアームでは事情が異なります。
- リアルタイム性: デブリ捕獲やドッキングでは、手先を素早く正確に動かす必要がある
- 遠隔操作の遅延: 地上からの制御指令には通信遅延があるため、特異姿勢付近での微妙な操作は困難
- リカバリーの困難さ: 宇宙空間では人が手動で姿勢を変えることが容易ではない
このため、宇宙ロボティクスでは特異姿勢を「避けるべき危険領域」として事前に把握し、経路計画の段階で回避策を組み込むことが不可欠です。
ここまでで特異姿勢の直感的なイメージを掴みました。では、この「動けなくなる」現象を数学的にはどう定義するのでしょうか？ 次のセクションで、ヤコビ行列のランク落ちという観点から厳密な定義を与えます。
特異姿勢の数学的定義
ヤコビ行列のランクと特異姿勢
$n$ 自由度のマニピュレータが $m$ 次元の作業空間で動くとき、ヤコビ行列 $\bm{J}(\bm{q})$ は $m \times n$ の行列です。ヤコビ行列のランク（rank）は、手先が独立に動ける方向の数を表しています。
通常の姿勢（非特異姿勢、regular configuration）では、$\bm{J}(\bm{q})$ のランクは $\min(m, n)$ に等しく、手先は作業空間のすべての方向に動けます。しかし、特定の姿勢では、ランクが $\min(m, n)$ よりも小さくなることがあります。
特異姿勢（singular configuration）とは、ヤコビ行列のランクが最大ランクより低下する関節角度 $\bm{q}$ の集合です。数式で表すと、姿勢 $\bm{q}^*$ が特異姿勢であるとは
$$ \begin{equation} \text{rank}(\bm{J}(\bm{q}^*)) < \min(m, n) \end{equation} $$
が成り立つことを意味します。
正方ヤコビ行列の場合
自由度の数と作業空間の次元が等しい場合（$n = m$）、ヤコビ行列は正方行列になります。このとき、特異姿勢の条件はより分かりやすい形で表現できます。
正方行列のランクが最大でないことは、行列式がゼロであることと同値です。したがって、
$$ \begin{equation} \det(\bm{J}(\bm{q}^*)) = 0 \end{equation} $$
が特異姿勢の必要十分条件になります。行列式がゼロになると、$\bm{J}$ の逆行列 $\bm{J}^{-1}$ が存在しなくなります。これは、手先速度 $\dot{\bm{x}}$ から関節速度 $\dot{\bm{q}}$ を
$$ \dot{\bm{q}} = \bm{J}^{-1} \dot{\bm{x}} $$
のように一意に求められなくなることを意味します。つまり、特異姿勢では「手先をこの方向に動かしたい」という要求に対して、関節をどう動かせばよいかが定まらなくなるのです。
特異姿勢で何が起こるか
特異姿勢の近傍で起こる現象を3つ整理しましょう。
手先速度の制約: 特異姿勢では、手先が動けない方向が生じます。$m$ 次元の作業空間で $\text{rank}(\bm{J}) = r < m$ であれば、手先は $r$ 次元の部分空間にしか速度を持てません。つまり $m - r$ 個の方向への運動が失われます。
関節速度の発散: 特異姿勢の近傍（完全には特異でないが、近い状態）では、手先をわずかに動かすために巨大な関節速度が必要になります。逆運動学の解 $\dot{\bm{q}} = \bm{J}^{-1} \dot{\bm{x}}$ において、$\bm{J}$ が特異に近づくと $\bm{J}^{-1}$ の要素が発散するためです。
力の伝達の喪失: 速度の問題と双対的に、力の伝達にも問題が生じます。特異姿勢では、ある方向の外力に対して関節トルクで抵抗できなくなります。これは仮想仕事の原理から導かれる静力学の関係
$$ \bm{\tau} = \bm{J}^T \bm{F} $$
において、$\bm{J}^T$ のランクが落ちることに対応しています。
これらの3つの現象は、いずれもヤコビ行列のランク落ちという一つの数学的構造から派生しています。
ここまでで特異姿勢の数学的な定義と、それがもたらす影響を理解しました。次に、最も単純な2リンクマニピュレータを題材にして、特異姿勢を幾何学的に「見て」理解しましょう。
2リンクマニピュレータの特異姿勢
モデルの設定
2リンク平面マニピュレータは、特異姿勢を理解するための最良の教材です。2つの回転関節（関節角度 $q_1, q_2$）と2本のリンク（長さ $l_1, l_2$）からなるシンプルな構造ですが、特異姿勢の本質をすべて含んでいます。
DHパラメータと順運動学で導出したように、手先位置 $(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} $$
で与えられます。
ヤコビ行列の導出
ヤコビ行列と微分運動学で学んだ方法で、手先位置を関節角度で偏微分してヤコビ行列を求めます。
$x$ を $q_1$ で偏微分すると
$$ \frac{\partial x}{\partial q_1} = -l_1 \sin q_1 – l_2 \sin(q_1 + q_2) $$
$x$ を $q_2$ で偏微分すると
$$ \frac{\partial x}{\partial q_2} = -l_2 \sin(q_1 + q_2) $$
同様に $y$ についても偏微分を行うと
$$ \frac{\partial y}{\partial q_1} = l_1 \cos q_1 + l_2 \cos(q_1 + q_2) $$
$$ \frac{\partial y}{\partial q_2} = l_2 \cos(q_1 + q_2) $$
これらをまとめると、$2 \times 2$ のヤコビ行列が得られます。
$$ \begin{equation} \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} \end{equation} $$
行列式の計算
$2 \times 2$ 行列の行列式を計算しましょう。$\bm{J}$ の $(1,1)$ 成分を $J_{11}$, $(1,2)$ 成分を $J_{12}$ のように書くと、$\det(\bm{J}) = J_{11} J_{22} – J_{12} J_{21}$ です。
第1項 $J_{11} J_{22}$ を展開すると
$$ (-l_1 \sin q_1 – l_2 \sin(q_1 + q_2)) \cdot l_2 \cos(q_1 + q_2) = -l_1 l_2 \sin q_1 \cos(q_1 + q_2) – l_2^2 \sin(q_1 + q_2) \cos(q_1 + q_2) $$
第2項 $-J_{12} J_{21}$ を展開すると
$$ -(-l_2 \sin(q_1 + q_2)) \cdot (l_1 \cos q_1 + l_2 \cos(q_1 + q_2)) = l_1 l_2 \sin(q_1 + q_2) \cos q_1 + l_2^2 \sin(q_1 + q_2) \cos(q_1 + q_2) $$
これらを足し合わせると、$l_2^2$ の項が相殺されて
$$ \det(\bm{J}) = l_1 l_2 \left[ \sin(q_1 + q_2) \cos q_1 – \sin q_1 \cos(q_1 + q_2) \right] $$
ここで三角関数の加法定理 $\sin(\alpha – \beta) = \sin \alpha \cos \beta – \cos \alpha \sin \beta$ を適用します。括弧内は $\alpha = q_1 + q_2$, $\beta = q_1$ として $\sin((q_1 + q_2) – q_1) = \sin q_2$ に他なりません。したがって、
$$ \begin{equation} \det(\bm{J}) = l_1 l_2 \sin q_2 \end{equation} $$
この結果は驚くほどシンプルです。行列式は $\sin q_2$ にのみ依存し、$q_1$ にはまったく依存しません。
特異姿勢の条件
$l_1 > 0, l_2 > 0$ なので、$\det(\bm{J}) = 0$ となる条件は
$$ \sin q_2 = 0 \quad \Longleftrightarrow \quad q_2 = 0 \text{ または } q_2 = \pi $$
の2つです。それぞれの幾何学的意味を見てみましょう。
$q_2 = 0$ の場合（腕を伸ばし切った状態）: 第2リンクが第1リンクと完全に同じ方向を向きます。腕全体が1本の棒のようになり、手先は関節1を中心とする半径 $l_1 + l_2$ の円周上にしか動けません。手先を「腕の延長方向」に動かすことはできません。これは作業空間の境界に対応するため、境界特異姿勢と呼ばれます。
$q_2 = \pi$ の場合（腕を折り畳んだ状態）: 第2リンクが第1リンクと正反対の方向を向きます。手先は関節1を中心とする半径 $|l_1 – l_2|$ の円周上にしか動けません。$l_1 = l_2$ の場合、手先は原点に重なり、完全に身動きが取れなくなります。これは作業空間の内部で起こりうるため、内部特異姿勢と呼ばれます。
幾何学的な解釈
特異姿勢の幾何学的な意味をもう少し掘り下げます。$q_2 = 0$ のとき、2本のリンクは一直線上に並びます。このとき、関節1を回しても関節2を回しても、手先は同じ「円弧の接線方向」にしか動けません。つまり、2つの関節が冗長になり、実質的に1自由度しかない状態です。
数学的には、ヤコビ行列の2つの列ベクトルが平行になることに対応します。$q_2 = 0$ のとき、
$$ \bm{J} = \begin{pmatrix} -(l_1 + l_2) \sin q_1 & -l_2 \sin q_1 \\ (l_1 + l_2) \cos q_1 & l_2 \cos q_1 \end{pmatrix} $$
第1列は第2列の $(l_1 + l_2)/l_2$ 倍であり、確かに線形従属です。2つの列ベクトルが張る空間は2次元ではなく1次元に潰れてしまい、手先は1方向にしか動けなくなるのです。
ここまでで2リンクマニピュレータの特異姿勢を具体的に理解しました。$\sin q_2 = 0$ という単純な条件から、腕の伸び切りと折り畳みという直感的に分かりやすい2つの特異姿勢が導かれました。次に、この分類をより一般的なマニピュレータに拡張します。
特異姿勢の分類
境界特異と内部特異
一般のマニピュレータの特異姿勢は、発生する場所によって大きく2種類に分類されます。
境界特異姿勢（boundary singularity, workspace boundary singularity）は、手先が作業空間の境界に達したときに発生します。腕を最大限に伸ばし切った状態が典型例です。
直感的には、「これ以上遠くに手を伸ばせない」という物理的な限界に対応しています。2リンクマニピュレータでは $q_2 = 0$、つまり手先が原点から距離 $l_1 + l_2$ の位置にある場合です。
境界特異姿勢の特徴は、手先が動けない方向がリンクの並びの方向（放射方向）と一致する点です。腕が真っ直ぐに伸びているので、その延長方向にはもう動けません。
内部特異姿勢（internal singularity）は、作業空間の境界ではなく内部で発生します。2リンクマニピュレータでは $q_2 = \pi$ のケースがこれに当たります。
内部特異姿勢は境界特異姿勢よりも直感的に理解しにくいことが多く、特に6自由度以上のマニピュレータでは複雑な幾何学的条件で発生します。たとえば、3つの回転軸が一点で交わるような配置（手首特異）が典型的です。
手首特異と肩特異
6自由度の産業用ロボット（6R型）では、特異姿勢をさらに具体的に分類できます。
手首特異姿勢（wrist singularity）は、手首の3つの回転軸（関節4, 5, 6）のうち2つが同一直線上に並ぶ配置で発生します。関節5の回転角が $0$ または $\pi$ のときに起こり、関節4と関節6の回転が同じ効果を持ってしまいます。これは「ジンバルロック」と同じ原理であり、剛体の姿勢表現と同次変換行列で学んだオイラー角の特異性とも深く関連しています。
肩特異姿勢（shoulder singularity）は、手首の中心点が関節1の回転軸上に来たときに発生します。この配置では、関節1を回しても手首中心は動かないため、自由度が1つ失われます。
肘特異姿勢（elbow singularity）は、関節2と関節3のリンクが一直線上に並んだときに発生します。これは2リンクマニピュレータの境界特異姿勢の3次元版です。
特異姿勢の回避
実際のロボット制御では、特異姿勢を回避するために様々な手法が用いられます。
減衰最小二乗法（Damped Least Squares, DLS）は、逆運動学で $\bm{J}^{-1}$ の代わりに
$$ \dot{\bm{q}} = \bm{J}^T (\bm{J} \bm{J}^T + \lambda^2 \bm{I})^{-1} \dot{\bm{x}} $$
を用いる方法です。ダンピング係数 $\lambda$ により、特異姿勢付近での関節速度の発散を防ぎます。$\lambda$ が大きいほど安定になりますが、手先の位置精度は犠牲になります。
冗長自由度の活用: 自由度が作業空間の次元より多い冗長マニピュレータでは、零空間の自由度を使って特異姿勢を回避する関節運動を生成できます。これは次の記事で詳しく扱います。
経路計画による回避: 作業空間で目標経路を設定する段階で、特異姿勢を通過しない経路を選ぶ方法です。これには、可操作度を「コスト」として経路最適化に組み込む手法が有効です。
ここまでで特異姿勢の分類と回避策の概要を学びました。しかし、特異姿勢から離れていれば本当に安全なのでしょうか？ 特異姿勢のごく近傍でも関節速度が非常に大きくなるなど、実用上の問題があります。そこで必要になるのが、「現在の姿勢でどれだけ自由に動けるか」を連続的に評価する指標 — すなわち可操作度です。
可操作度の定義
特異姿勢からの距離を測りたい
特異姿勢が問題であることは理解できました。しかし、実際のロボット制御で重要なのは「特異かどうか」という二値的な判定ではなく、「特異姿勢にどれだけ近いか」「どの方向にどれだけ動きやすいか」という連続的な評価です。
たとえば、ある姿勢で $\det(\bm{J}) = 0.01$ と $\det(\bm{J}) = 10.0$ では、前者の方が特異姿勢に近く、手先の運動性能が大幅に低下しています。この「運動のしやすさ」を定量化する指標が可操作度（manipulability）です。
吉川の可操作度
1985年に吉川恒夫によって提案された可操作度指標は、ロボティクスで最も広く使われている運動性能指標です。吉川の可操作度の背景にあるアイデアは、「関節速度を一定のノルムに制限したとき、手先がどれだけ広い範囲に速度を出せるか」を一つの数値で表すことです。
$m \times n$ のヤコビ行列 $\bm{J}(\bm{q})$ に対して、可操作度 $w$ は
$$ \begin{equation} w(\bm{q}) = \sqrt{\det(\bm{J}(\bm{q}) \bm{J}(\bm{q})^T)} \end{equation} $$
で定義されます。
この式が何を表現しているか、直感的に理解しましょう。$\bm{J} \bm{J}^T$ は $m \times m$ の対称半正定値行列です（非特異姿勢では正定値）。この行列の固有値 $\sigma_1^2, \sigma_2^2, \dots, \sigma_m^2$ は、手先が各固有方向にどれだけ速く動けるかの二乗に比例します（$\sigma_i$ はヤコビ行列の特異値です）。
行列式は固有値の積に等しいので、
$$ \det(\bm{J} \bm{J}^T) = \sigma_1^2 \sigma_2^2 \cdots \sigma_m^2 $$
したがって、
$$ w = \sigma_1 \sigma_2 \cdots \sigma_m $$
となります。可操作度は全特異値の積、つまり全方向の運動性能の「総合力」を表しています。
正方ヤコビ行列の場合の簡略化
$n = m$（自由度と作業空間の次元が等しい）の場合、$\bm{J}$ は正方行列です。このとき、行列式の積の公式を適用できます。
$\bm{J} \bm{J}^T$ の行列式は $\det(\bm{J} \bm{J}^T) = \det(\bm{J}) \cdot \det(\bm{J}^T)$ と因数分解でき、$\det(\bm{J}^T) = \det(\bm{J})$ より
$$ \det(\bm{J} \bm{J}^T) = \det(\bm{J})^2 $$
が成り立ちます。したがって、
$$ w = |\det(\bm{J})| $$
と簡潔に書けます。2リンクマニピュレータの場合、先ほど導出した結果から
$$ w = |l_1 l_2 \sin q_2| $$
です。$q_2 = 0$ や $q_2 = \pi$（特異姿勢）で $w = 0$、$q_2 = \pm \pi/2$（第2リンクが第1リンクに垂直）で $w$ は最大値 $l_1 l_2$ になります。これは直感にもよく合います。肘が90度に曲がっているとき、手先は最も自由に動けるはずです。
可操作度の性質
可操作度 $w$ は以下の性質を持ちます。
- 非負性: $w \geq 0$。常にゼロ以上です
- 特異姿勢での消失: $w = 0$ であることと、$\bm{q}$ が特異姿勢であることは同値です
- 連続性: $w$ は関節角度 $\bm{q}$ の連続関数です。特異姿勢に近づくにつれて滑らかに減少します
- スケール依存性: $w$ はリンクの長さに依存するため、異なるロボット間の比較には正規化が必要です
条件数との比較
可操作度と並んでよく使われる指標に、条件数（condition number）があります。条件数は
$$ \kappa(\bm{J}) = \frac{\sigma_{\max}}{\sigma_{\min}} $$
で定義されます。$\sigma_{\max}$ と $\sigma_{\min}$ はそれぞれヤコビ行列の最大特異値と最小特異値です。
可操作度と条件数の違いを理解するため、具体例を考えましょう。特異値が $(\sigma_1, \sigma_2) = (10, 10)$ の姿勢Aと $(\sigma_1, \sigma_2) = (100, 1)$ の姿勢Bがあるとします。
| 指標 | 姿勢A $(10, 10)$ | 姿勢B $(100, 1)$ |
|---|---|---|
| 可操作度 $w$ | $100$ | $100$ |
| 条件数 $\kappa$ | $1$ | $100$ |
可操作度は同じ値ですが、条件数は姿勢Bの方がはるかに大きくなっています。姿勢Bでは、一方向には非常に速く動けますが、別の方向にはほとんど動けません。つまり、可操作度は運動能力の「総量」を、条件数は運動能力の「均一性」を測る指標です。
等方的（isotropic）な運動が求められる作業では条件数が重要であり、$\kappa = 1$ の姿勢を等方姿勢（isotropic configuration）と呼びます。一方、ある特定方向に素早く動ければよい作業では、可操作度の方が適切な評価指標になることもあります。
両方の指標を組み合わせて「条件数が一定値以下で、かつ可操作度が最大」となる姿勢を探す、という最適化問題を解くアプローチも実用的です。
ここまでで、可操作度というスカラー値の指標を学びました。しかし、スカラー値では「どの方向に動きやすいか」という方向の情報が失われます。方向の情報も含めて可視化するために、次のセクションで可操作度楕円体を導入します。
可操作度楕円体 — 速度と力の楕円体
単位球から楕円体への写像
可操作度楕円体の導出は、非常にエレガントな幾何学的構造に基づいています。出発点は、関節速度空間における単位球です。
関節速度 $\dot{\bm{q}}$ がノルム1以下、すなわち
$$ \|\dot{\bm{q}}\| \leq 1 \quad \Longleftrightarrow \quad \dot{\bm{q}}^T \dot{\bm{q}} \leq 1 $$
を満たす領域を考えます。これは関節速度空間における $n$ 次元の単位球です。物理的には、「各関節の速度の二乗和が1以下」という制約を意味しています。
この単位球を、ヤコビ行列 $\bm{J}$ を通じて作業空間に写像したらどのような形になるでしょうか？ 関節速度と手先速度の関係 $\dot{\bm{x}} = \bm{J} \dot{\bm{q}}$ を使い、$n = m$（正方ヤコビ行列）の場合で導出を進めます。
$\dot{\bm{q}} = \bm{J}^{-1} \dot{\bm{x}}$ を単位球の不等式に代入すると
$$ (\bm{J}^{-1} \dot{\bm{x}})^T (\bm{J}^{-1} \dot{\bm{x}}) \leq 1 $$
転置の性質 $(\bm{A}\bm{b})^T = \bm{b}^T \bm{A}^T$ を使って整理すると
$$ \dot{\bm{x}}^T (\bm{J}^{-1})^T \bm{J}^{-1} \dot{\bm{x}} \leq 1 $$
ここで $(\bm{J}^{-1})^T = (\bm{J}^T)^{-1}$ であることを用い、さらに $(\bm{J}^T)^{-1} \bm{J}^{-1} = (\bm{J} \bm{J}^T)^{-1}$ を使うと
$$ \begin{equation} \dot{\bm{x}}^T (\bm{J} \bm{J}^T)^{-1} \dot{\bm{x}} \leq 1 \end{equation} $$
が得られます。これは作業空間における楕円体の方程式に他なりません。行列 $\bm{J} \bm{J}^T$ によって楕円体の形状が決まります。
速度楕円体
上で導出した楕円体を速度楕円体（velocity ellipsoid）または可操作度楕円体（manipulability ellipsoid）と呼びます。
$$ \mathcal{E}_v = \{ \dot{\bm{x}} \in \mathbb{R}^m \mid \dot{\bm{x}}^T (\bm{J} \bm{J}^T)^{-1} \dot{\bm{x}} \leq 1 \} $$
この楕円体の主軸と半径は、$\bm{J} \bm{J}^T$ の固有値分解（または $\bm{J}$ の特異値分解）から得られます。
$\bm{J}$ の特異値分解（SVD）を $\bm{J} = \bm{U} \bm{\Sigma} \bm{V}^T$ とすると、$\bm{J} \bm{J}^T = \bm{U} \bm{\Sigma} \bm{V}^T \bm{V} \bm{\Sigma}^T \bm{U}^T = \bm{U} \bm{\Sigma}^2 \bm{U}^T$ です。ここで $\bm{\Sigma} = \text{diag}(\sigma_1, \sigma_2, \dots, \sigma_m)$ は特異値を対角に並べた行列、$\bm{U}$ は左特異ベクトルからなる直交行列です。
したがって、楕円体の主軸方向は $\bm{U}$ の列ベクトル $\bm{u}_1, \bm{u}_2, \dots, \bm{u}_m$ で与えられ、各主軸の半径は対応する特異値 $\sigma_1, \sigma_2, \dots, \sigma_m$ です。
直感的に解釈すると、速度楕円体は「関節速度を全力（ノルム1）で出したとき、手先が到達可能な速度の範囲」を表しています。楕円体が大きい方向ほど、手先はその方向に速く動けます。楕円体が潰れて細長くなっている場合、短軸方向には手先がほとんど動けません。そして、特異姿勢では楕円体の1つ以上の軸が長さゼロに潰れ、楕円体が退化します。
力楕円体
速度と双対的に、力の楕円体も定義できます。マニピュレータの静力学の関係式
$$ \bm{\tau} = \bm{J}^T \bm{F} $$
から出発します。ここで $\bm{\tau}$ は関節トルク、$\bm{F}$ は手先に作用する力です。
関節トルク $\bm{\tau}$ がノルム1以下の制約 $\bm{\tau}^T \bm{\tau} \leq 1$ を考えます。$\bm{\tau} = \bm{J}^T \bm{F}$ を代入すると
$$ (\bm{J}^T \bm{F})^T (\bm{J}^T \bm{F}) \leq 1 $$
転置を展開して $(\bm{J}^T \bm{F})^T = \bm{F}^T \bm{J}$ を使うと
$$ \bm{F}^T \bm{J} \bm{J}^T \bm{F} \leq 1 $$
よって、力楕円体は
$$ \begin{equation} \mathcal{E}_f = \{ \bm{F} \in \mathbb{R}^m \mid \bm{F}^T \bm{J} \bm{J}^T \bm{F} \leq 1 \} \end{equation} $$
で定義されます。
速度楕円体と力楕円体の双対関係
ここで重要な事実に気づきます。速度楕円体の定義行列は $(\bm{J} \bm{J}^T)^{-1}$ であり、力楕円体の定義行列は $\bm{J} \bm{J}^T$ です。これらは逆行列の関係にあります。
逆行列の固有値は元の行列の固有値の逆数になります。したがって、速度楕円体で長軸方向（特異値 $\sigma_i$ が大きい方向）は、力楕円体では短軸方向になります。逆に、速度楕円体で短軸方向は、力楕円体では長軸方向になります。
これは物理的に自然な結果です。手先が素早く動ける方向では、関節の小さな変位で手先が大きく動くため、同じ関節トルクで生じる手先の力は小さくなります。逆に、手先が動きにくい方向では、関節の大きな変位で手先がわずかしか動かないため、てこの原理により大きな力を発揮できます。
数式で確認しましょう。速度楕円体の主軸の半径は $\sigma_i$（大きいほど速く動ける）であり、力楕円体の主軸の半径は $1/\sigma_i$（$\sigma_i$ が大きいほど力は小さい）です。
この双対関係を表にまとめます。
| 速度楕円体 | 力楕円体 | |
|---|---|---|
| 定義行列 | $(\bm{J}\bm{J}^T)^{-1}$ | $\bm{J}\bm{J}^T$ |
| 主軸半径 | $\sigma_i$ | $1/\sigma_i$ |
| 長軸方向 | 速く動ける方向 | 力が弱い方向 |
| 短軸方向 | 動きにくい方向 | 力が強い方向 |
| 体積 | $\propto w$ | $\propto 1/w$ |
可操作度と楕円体の体積
速度楕円体の体積は、各主軸半径の積に比例します。$m$ 次元の楕円体の体積は
$$ V = C_m \prod_{i=1}^{m} \sigma_i = C_m \cdot w $$
です（$C_m$ は $m$ 次元の単位球の体積で、2次元の場合は $C_2 = \pi$、3次元の場合は $C_3 = 4\pi/3$ です）。つまり、可操作度 $w$ は速度楕円体の体積に比例しています。可操作度が大きいほど、手先が到達できる速度の範囲が広い — これが可操作度の幾何学的意味です。
一方、楕円体の体積が同じでも形状は異なりうることに注意が必要です。非常に細長い楕円体と丸い楕円体で体積が同じ場合、可操作度は同じ値になります。楕円体の「丸さ」を評価するのが先ほどの条件数であり、$\kappa = 1$ のとき楕円体は球に退化します。
ここまでで、可操作度楕円体の理論的基盤が整いました。速度楕円体と力楕円体が双対の関係にあること、可操作度が楕円体の体積に対応すること、条件数が楕円体の形状（等方性）を評価することを理解しました。それでは、これらをPythonで実際に計算し、可視化してみましょう。
Pythonでの実装と可視化
2リンクマニピュレータの基本実装
まず、2リンクマニピュレータの順運動学とヤコビ行列を計算する関数を実装します。ここで定義する関数群は、以降のすべての可視化コードの基盤となります。
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Ellipse
# 2リンクマニピュレータのパラメータ
L1 = 1.0 # リンク1の長さ
L2 = 0.8 # リンク2の長さ
def forward_kinematics(q1, q2, l1=L1, l2=L2):
"""順運動学: 各関節位置と手先位置を返す"""
x0, y0 = 0.0, 0.0
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 (x0, y0), (x1, y1), (x2, y2)
def jacobian(q1, q2, l1=L1, l2=L2):
"""ヤコビ行列を計算"""
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
def manipulability(J):
"""可操作度 w = sqrt(det(J J^T)) を計算"""
return np.sqrt(max(np.linalg.det(J @ J.T), 0.0))
# 動作確認
q1_test, q2_test = np.pi / 4, np.pi / 3
J_test = jacobian(q1_test, q2_test)
w_test = manipulability(J_test)
print(f"q1 = pi/4, q2 = pi/3")
print(f"Jacobian:\n{J_test}")
print(f"Manipulability: {w_test:.4f}")
print(f"Analytical: {L1 * L2 * abs(np.sin(q2_test)):.4f}")
forward_kinematics 関数は各関節と手先のxy座標を返し、jacobian 関数は先ほど導出した $2 \times 2$ のヤコビ行列を返します。manipulability 関数は $w = \sqrt{\det(\bm{J}\bm{J}^T)}$ を計算しますが、数値誤差でわずかに負になった場合に備えて max(..., 0.0) で安全策を入れています。動作確認として、解析解 $w = l_1 l_2 |\sin q_2|$ との一致を確認しています。
特異姿勢と非特異姿勢の比較
次に、特異姿勢（$q_2 = 0$, 腕を伸ばし切った状態）、特異に近い姿勢、非特異姿勢の3つを並べて、速度楕円体の変化を可視化します。
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Ellipse
L1 = 1.0
L2 = 0.8
def forward_kinematics(q1, q2, l1=L1, l2=L2):
"""順運動学"""
x0, y0 = 0.0, 0.0
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 (x0, y0), (x1, y1), (x2, y2)
def jacobian(q1, q2, l1=L1, l2=L2):
"""ヤコビ行列"""
return 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)]
])
def draw_manipulator(ax, q1, q2, title):
"""マニピュレータとその速度楕円体を描画"""
p0, p1, p2 = forward_kinematics(q1, q2)
# リンクを描画
ax.plot([p0[0], p1[0]], [p0[1], p1[1]], 'o-',
color='#2196F3', linewidth=3, markersize=8, zorder=5)
ax.plot([p1[0], p2[0]], [p1[1], p2[1]], 'o-',
color='#FF9800', linewidth=3, markersize=8, zorder=5)
ax.plot(*p2, 'o', color='#F44336', markersize=10, zorder=6)
# ヤコビ行列と可操作度の計算
J = jacobian(q1, q2)
w = abs(np.linalg.det(J))
# SVDで速度楕円体のパラメータを取得
U, S, Vt = np.linalg.svd(J)
angle = np.degrees(np.arctan2(U[1, 0], U[0, 0]))
# 楕円体（特異値が十分大きい場合のみ描画）
if S.min() > 1e-6:
scale = 0.3
ellipse = Ellipse(
xy=p2, width=2 * S[0] * scale,
height=2 * S[1] * scale, angle=angle,
fill=True, alpha=0.3, color='#4CAF50',
linewidth=2, edgecolor='#2E7D32')
ax.add_patch(ellipse)
ax.set_title(f'{title}\n$w$ = {w:.4f}', fontsize=12)
ax.set_xlim(-2.5, 2.5)
ax.set_ylim(-2.0, 2.5)
ax.set_aspect('equal')
ax.grid(True, alpha=0.3)
ax.axhline(y=0, color='gray', linewidth=0.5)
ax.axvline(x=0, color='gray', linewidth=0.5)
fig, axes = plt.subplots(1, 3, figsize=(16, 5))
# ケース1: 非特異姿勢（q2 = pi/3）
draw_manipulator(axes[0], np.pi / 4, np.pi / 3,
r'Non-singular ($q_2 = \pi/3$)')
# ケース2: 特異に近い姿勢（q2 = pi/12）
draw_manipulator(axes[1], np.pi / 4, np.pi / 12,
r'Near-singular ($q_2 = \pi/12$)')
# ケース3: 特異姿勢（q2 = 0）
draw_manipulator(axes[2], np.pi / 4, 0.0,
r'Singular ($q_2 = 0$)')
plt.tight_layout()
plt.savefig('singularity_comparison.png', dpi=150,
bbox_inches='tight')
plt.show()
このグラフは、$q_1 = \pi/4$ を固定し、$q_2$ を変えた3つの姿勢を並べています。左の非特異姿勢（$q_2 = \pi/3$）では、手先に描かれた緑の楕円体がほぼ円に近く、手先はどの方向にもバランスよく動けます。可操作度 $w$ も $l_1 l_2 \sin(\pi/3) \approx 0.693$ と大きな値を示します。
中央の特異に近い姿勢（$q_2 = \pi/12$）では、楕円体が細長く潰れ始めています。腕の延長方向に沿った短軸が急速に短くなり、その方向に手先が動きにくくなっていることが視覚的に分かります。可操作度も $w \approx 0.207$ と低下しています。
右の特異姿勢（$q_2 = 0$）では、腕が完全に伸び切っており、可操作度は $w = 0$ です。楕円体は完全に退化して線分になるため、描画をスキップしています。この姿勢では手先は腕に垂直な方向（接線方向）にしか動けません。
可操作度の q2 依存性
$q_2$ を連続的に変化させたときの可操作度の変化をプロットして、理論式 $w = l_1 l_2 |\sin q_2|$ を数値計算で確認しましょう。
import numpy as np
import matplotlib.pyplot as plt
L1 = 1.0
L2 = 0.8
def jacobian(q1, q2, l1=L1, l2=L2):
"""ヤコビ行列"""
return 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)]
])
q1_fixed = np.pi / 4
q2_range = np.linspace(-np.pi, np.pi, 500)
# 数値計算による可操作度
w_numerical = np.array([
abs(np.linalg.det(jacobian(q1_fixed, q2)))
for q2 in q2_range
])
# 解析解
w_analytical = L1 * L2 * np.abs(np.sin(q2_range))
fig, ax = plt.subplots(figsize=(10, 5))
ax.plot(np.degrees(q2_range), w_numerical, 'b-',
linewidth=2, label='Numerical: $|\\det(J)|$')
ax.plot(np.degrees(q2_range), w_analytical, 'r--',
linewidth=2, label=r'Analytical: $l_1 l_2 |\sin q_2|$')
# 特異姿勢の位置にマーカー
for q2_sing in [0, 180, -180]:
ax.axvline(x=q2_sing, color='gray', linestyle=':', alpha=0.5)
ax.text(q2_sing + 3, L1 * L2 * 0.9, 'Singular',
fontsize=9, color='gray')
ax.set_xlabel(r'$q_2$ [deg]', fontsize=12)
ax.set_ylabel('Manipulability $w$', fontsize=12)
ax.set_title('Manipulability vs Joint Angle $q_2$', fontsize=14)
ax.legend(fontsize=11)
ax.grid(True, alpha=0.3)
ax.set_xlim(-180, 180)
ax.set_ylim(0, L1 * L2 * 1.1)
plt.tight_layout()
plt.savefig('manipulability_vs_q2.png', dpi=150,
bbox_inches='tight')
plt.show()
青の実線（数値計算）と赤の破線（解析解 $l_1 l_2 |\sin q_2|$）が完全に重なっており、導出の正しさが数値的にも確認できます。$q_2 = 0^\circ$ と $q_2 = \pm 180^\circ$ で可操作度がゼロになり、これが特異姿勢です。$q_2 = \pm 90^\circ$ で可操作度は最大値 $l_1 l_2 = 0.8$ に達します。グラフの形状は $|\sin q_2|$ そのものであり、$q_1$ の値にはまったく依存しないことも理論通りです。
姿勢変化に伴う速度楕円体の変化
$q_2$ を変化させながら速度楕円体がどのように変化するかを、複数の姿勢を重ねて描画します。これにより、特異姿勢に近づくにつれて楕円体がどのように潰れていくかを一目で把握できます。
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Ellipse
from matplotlib.colors import Normalize
from matplotlib.cm import ScalarMappable
L1 = 1.0
L2 = 0.8
def forward_kinematics(q1, q2, l1=L1, l2=L2):
"""順運動学"""
x0, y0 = 0.0, 0.0
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 (x0, y0), (x1, y1), (x2, y2)
def jacobian(q1, q2, l1=L1, l2=L2):
"""ヤコビ行列"""
return 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)]
])
fig, ax = plt.subplots(figsize=(10, 10))
q1 = np.pi / 6 # 関節1は固定
q2_values = np.linspace(np.pi * 0.05, np.pi * 0.95, 10)
cmap = plt.cm.viridis
norm = Normalize(vmin=q2_values.min(), vmax=q2_values.max())
for q2 in q2_values:
p0, p1, p2 = forward_kinematics(q1, q2)
color = cmap(norm(q2))
# リンクを薄く描画
ax.plot([p0[0], p1[0]], [p0[1], p1[1]], '-',
color=color, linewidth=1.5, alpha=0.5)
ax.plot([p1[0], p2[0]], [p1[1], p2[1]], '-',
color=color, linewidth=1.5, alpha=0.5)
# SVDで速度楕円体を計算
J = jacobian(q1, q2)
U, S, Vt = np.linalg.svd(J)
angle = np.degrees(np.arctan2(U[1, 0], U[0, 0]))
scale = 0.2
ellipse = Ellipse(
xy=p2, width=2 * S[0] * scale,
height=2 * S[1] * scale, angle=angle,
fill=False, linewidth=2, edgecolor=color, alpha=0.8)
ax.add_patch(ellipse)
ax.plot(*p2, 'o', color=color, markersize=5)
# 作業空間の境界
theta_circle = np.linspace(0, 2 * np.pi, 200)
ax.plot((L1 + L2) * np.cos(theta_circle),
(L1 + L2) * np.sin(theta_circle),
'k--', alpha=0.2, linewidth=1,
label='Workspace boundary')
ax.plot(abs(L1 - L2) * np.cos(theta_circle),
abs(L1 - L2) * np.sin(theta_circle),
'k:', alpha=0.2, linewidth=1,
label='Inner boundary')
# カラーバー
sm = ScalarMappable(cmap=cmap, norm=norm)
sm.set_array([])
cbar = plt.colorbar(sm, ax=ax, shrink=0.6,
label=r'$q_2$ [rad]')
ax.set_xlabel('x [m]', fontsize=12)
ax.set_ylabel('y [m]', fontsize=12)
ax.set_title('Velocity Ellipsoids at Different Configurations',
fontsize=14)
ax.set_aspect('equal')
ax.grid(True, alpha=0.2)
ax.legend(loc='lower left', fontsize=10)
ax.set_xlim(-2.5, 2.5)
ax.set_ylim(-1.5, 2.5)
plt.tight_layout()
plt.savefig('velocity_ellipsoids_sweep.png', dpi=150,
bbox_inches='tight')
plt.show()
この図は、$q_1 = \pi/6$ を固定し、$q_2$ を小さな値（ほぼ伸び切り、紫色）から大きな値（深く曲げた状態、黄色）まで変化させたときの、手先位置と速度楕円体の変化を示しています。
$q_2$ が小さい（紫色、腕が伸び切りに近い）姿勢では、楕円体が非常に細長くなり、作業空間の境界（黒の破線円）付近に位置しています。これは境界特異姿勢に近い状態で、腕の延長方向（放射方向）にほとんど動けないことを反映しています。
$q_2$ が $\pi/2$ 付近（緑色）になると、楕円体がほぼ円形に近づき、どの方向にも均等に動けることが分かります。これが可操作度が最も高い姿勢領域です。
$q_2$ がさらに大きくなる（黄色）と、再び楕円体が変形し始め、今度は内部特異姿勢（$q_2 = \pi$）に近づく方向の変化が見られます。楕円体がリング状の軌跡を描く手先位置に沿って変化する様子は、作業空間における運動性能の空間的な分布を直感的に示しています。
速度楕円体と力楕円体の比較
速度楕円体と力楕円体の双対関係をコードで確認します。同一姿勢で両方の楕円体を描画し、「速度が出やすい方向は力が弱い」という理論的関係を可視化しましょう。
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Ellipse
L1 = 1.0
L2 = 0.8
def forward_kinematics(q1, q2, l1=L1, l2=L2):
"""順運動学"""
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 (0.0, 0.0), (x1, y1), (x2, y2)
def jacobian(q1, q2, l1=L1, l2=L2):
"""ヤコビ行列"""
return 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)]
])
q1, q2 = np.pi / 4, np.pi / 5 # 非対称な姿勢を選択
p0, p1, p2 = forward_kinematics(q1, q2)
J = jacobian(q1, q2)
U, S, Vt = np.linalg.svd(J)
angle = np.degrees(np.arctan2(U[1, 0], U[0, 0]))
fig, axes = plt.subplots(1, 2, figsize=(14, 6))
labels = ['Velocity Ellipsoid', 'Force Ellipsoid']
colors = ['#4CAF50', '#E91E63']
for idx, (ax, etype) in enumerate(
zip(axes, ['velocity', 'force'])):
# リンクの描画
ax.plot([p0[0], p1[0]], [p0[1], p1[1]], 'o-',
color='#2196F3', linewidth=3,
markersize=8, zorder=5)
ax.plot([p1[0], p2[0]], [p1[1], p2[1]], 'o-',
color='#FF9800', linewidth=3,
markersize=8, zorder=5)
ax.plot(*p2, 'o', color='#F44336',
markersize=10, zorder=6)
scale = 0.3
if etype == 'velocity':
# 速度楕円体: 半径 = sigma_i
w_ell = 2 * S[0] * scale
h_ell = 2 * S[1] * scale
title = (f'Velocity Ellipsoid\n'
f'$\\sigma_1={S[0]:.3f},'
f'\\ \\sigma_2={S[1]:.3f}$')
else:
# 力楕円体: 半径 = 1/sigma_i
w_ell = 2 * (1.0 / S[0]) * scale
h_ell = 2 * (1.0 / S[1]) * scale
title = (f'Force Ellipsoid\n'
f'$1/\\sigma_1={1/S[0]:.3f},'
f'\\ 1/\\sigma_2={1/S[1]:.3f}$')
ellipse = Ellipse(
xy=p2, width=w_ell, height=h_ell,
angle=angle, fill=True, alpha=0.3,
color=colors[idx], linewidth=2,
edgecolor=colors[idx])
ax.add_patch(ellipse)
# 主軸方向の矢印
for i in range(2):
if etype == 'velocity':
length = S[i] * scale
else:
length = (1.0 / S[i]) * scale
dx = U[0, i] * length
dy = U[1, i] * length
ax.annotate(
'', xy=(p2[0] + dx, p2[1] + dy),
xytext=p2,
arrowprops=dict(arrowstyle='->',
color='black', lw=1.5))
ax.set_title(title, fontsize=12)
ax.set_xlim(-1.0, 2.5)
ax.set_ylim(-0.5, 2.5)
ax.set_aspect('equal')
ax.grid(True, alpha=0.3)
ax.set_xlabel('x [m]', fontsize=11)
ax.set_ylabel('y [m]', fontsize=11)
plt.tight_layout()
plt.savefig('velocity_vs_force_ellipsoid.png', dpi=150,
bbox_inches='tight')
plt.show()
左の速度楕円体（緑）と右の力楕円体（ピンク）を比較すると、両者の主軸方向は同じですが、長軸と短軸の関係が逆転していることが明確に見て取れます。速度楕円体で長軸だった方向（$\sigma_1$ が大きい方向、手先が速く動ける方向）は、力楕円体では短軸になっており（$1/\sigma_1$ が小さい、力が小さい）、逆もまた然りです。
この双対関係は、ロボットの作業設計において実用的な意味を持ちます。たとえば、ドリルで穴を開ける作業のようにドリル方向に大きな力が必要な場合は、その方向が力楕円体の長軸になるような姿勢を選ぶべきです。一方、ペインティングのように滑らかに素早く腕を動かす作業では、速度楕円体を基準に姿勢を最適化します。
作業空間全体の可操作度マップ
最後に、作業空間の各点における可操作度と条件数をカラーマップで可視化します。これにより、ロボットの「動きやすさの地図」を作ることができます。
import numpy as np
import matplotlib.pyplot as plt
L1 = 1.0
L2 = 0.8
def jacobian(q1, q2, l1=L1, l2=L2):
"""ヤコビ行列"""
return 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)]
])
def inverse_kinematics(x, y, l1=L1, l2=L2,
elbow_up=True):
"""逆運動学: (x, y) から (q1, q2) を計算"""
r2 = x**2 + y**2
cos_q2 = (r2 - l1**2 - l2**2) / (2 * l1 * l2)
if cos_q2 < -1.0 or cos_q2 > 1.0:
return None, None
if elbow_up:
sin_q2 = np.sqrt(1 - cos_q2**2)
else:
sin_q2 = -np.sqrt(1 - cos_q2**2)
q2 = np.arctan2(sin_q2, cos_q2)
q1 = np.arctan2(y, x) - np.arctan2(
l2 * np.sin(q2), l1 + l2 * np.cos(q2))
return q1, q2
# 作業空間をグリッドで走査
resolution = 300
x_range = np.linspace(-(L1 + L2 + 0.1),
L1 + L2 + 0.1, resolution)
y_range = np.linspace(-(L1 + L2 + 0.1),
L1 + L2 + 0.1, resolution)
X, Y = np.meshgrid(x_range, y_range)
W = np.full_like(X, np.nan)
K = np.full_like(X, np.nan)
for i in range(resolution):
for j in range(resolution):
q1, q2 = inverse_kinematics(X[i, j], Y[i, j])
if q1 is not None:
J = jacobian(q1, q2)
svd_vals = np.linalg.svd(J, compute_uv=False)
W[i, j] = svd_vals[0] * svd_vals[1]
if svd_vals[1] > 1e-10:
K[i, j] = svd_vals[0] / svd_vals[1]
fig, axes = plt.subplots(1, 2, figsize=(16, 7))
# 可操作度マップ
im1 = axes[0].pcolormesh(X, Y, W, cmap='inferno',
shading='auto')
plt.colorbar(im1, ax=axes[0],
label='Manipulability $w$', shrink=0.8)
axes[0].set_title('Manipulability Map', fontsize=14)
axes[0].set_xlabel('x [m]')
axes[0].set_ylabel('y [m]')
axes[0].set_aspect('equal')
# 条件数マップ（対数スケール）
im2 = axes[1].pcolormesh(X, Y, np.log10(K),
cmap='RdYlGn_r',
shading='auto',
vmin=0, vmax=2.5)
plt.colorbar(im2, ax=axes[1],
label=r'$\log_{10}(\kappa)$', shrink=0.8)
axes[1].set_title('Condition Number Map', fontsize=14)
axes[1].set_xlabel('x [m]')
axes[1].set_ylabel('y [m]')
axes[1].set_aspect('equal')
# 両方に作業空間の境界を描画
theta = np.linspace(0, 2 * np.pi, 200)
for ax in axes:
ax.plot((L1 + L2) * np.cos(theta),
(L1 + L2) * np.sin(theta),
'w--', linewidth=1.5, alpha=0.7)
ax.plot(abs(L1 - L2) * np.cos(theta),
abs(L1 - L2) * np.sin(theta),
'w:', linewidth=1.5, alpha=0.7)
plt.tight_layout()
plt.savefig('manipulability_vs_condition.png', dpi=150,
bbox_inches='tight')
plt.show()
左の可操作度マップと右の条件数マップ（対数スケール）を見比べると、両者が異なる情報を提供していることが分かります。
可操作度マップ（左）では、外側の境界（白い破線）と内側の境界（白い点線）の付近が暗く（$w \approx 0$）、中間のリング状領域が明るくなっています。可操作度が最大になる領域は外側と内側の境界の中間にあり、ここでは $q_2 \approx \pm \pi/2$ となって肘が適度に曲がった状態です。
条件数マップ（右）では、赤色の領域が条件数が大きい（運動が不均一な）場所、緑色の領域が条件数が小さい（運動が均一な）場所です。作業空間の境界付近では条件数が発散しています（特異姿勢で $\sigma_{\min} \to 0$ のため）。注目すべきは、可操作度が高い領域でも条件数が必ずしも小さくないケースがある点です。可操作度は全方向の運動能力の「積」なので、一方向だけ極端に良い場合でも大きな値を取りえます。一方、条件数は最大と最小の「比」なので、運動の均一性をダイレクトに反映します。
実用的なロボットの経路計画では、可操作度を閾値以上に保ちつつ条件数も一定以下に抑えるという複合的な基準を用いることが多いです。
まとめ
本記事では、ロボットアームの特異姿勢と可操作度楕円体について、直感的な理解から数学的定義、そしてPythonによる可視化まで一貫して解説しました。
- 特異姿勢は、ヤコビ行列のランクが低下する関節角度の集合であり、手先が特定方向に動けなくなる、関節速度が発散する、力の伝達が失われるという3つの問題を引き起こす
- 2リンクマニピュレータでは特異姿勢の条件が $\sin q_2 = 0$ という明快な式になり、腕の伸び切り（$q_2 = 0$, 境界特異）と折り畳み（$q_2 = \pi$, 内部特異）の2種類が存在する
- 吉川の可操作度 $w = \sqrt{\det(\bm{J}\bm{J}^T)}$ は全特異値の積であり、速度楕円体の体積に比例する。特異姿勢でゼロになり、特異姿勢からの「距離」を連続的に評価できる
- 速度楕円体と力楕円体は双対の関係にあり、速度が出やすい方向は力が弱く、力が強い方向は速度が出にくい。これは「てこの原理」の一般化である
- 条件数は可操作度とは異なり、運動能力の均一性（等方性）を評価する指標であり、作業の性質に応じて可操作度と使い分ける
これらの概念は、宇宙ロボットアームの安全な経路計画、産業用ロボットの最適な設置設計、手術支援ロボットの動作空間設計など、幅広い応用で不可欠な基盤知識です。
しかし、特異姿勢の問題には根本的な解決策があります。それは冗長マニピュレータ — 作業空間の次元よりも多くの自由度を持つロボットを使うことです。冗長マニピュレータでは、零空間の自由度を活用して、手先の運動を変えずに特異姿勢を回避する関節運動を生成できます。ISSのカナダアーム2が7自由度を持つのも、まさにこの理由からです。
次の記事では、冗長マニピュレータの逆運動学と擬似逆行列による制御について詳しく解説します。
- 冗長マニピュレータと擬似逆行列 — 零空間を活用した冗長マニピュレータの制御