ロボットアームに「手先をこの位置に動かせ」と指令を出すとき、6自由度のアームなら解はほぼ一意に決まります。しかし、7自由度以上のアームでは事情が異なります。同じ手先位置を実現する関節角の組み合わせが無限に存在するのです。人間の腕で試してみましょう。テーブルの上のコップに手を伸ばすとき、肘を上げても下げても手はコップに届きます。これが「冗長自由度」の直感的な意味です。
宇宙ロボットの世界では、この冗長性が決定的に重要です。国際宇宙ステーション（ISS）のCanadarm2は7自由度を持ち、宇宙飛行士の船外活動を支援したり、補給船をキャッチしたりします。宇宙空間では障害物（太陽電池パドル、アンテナ、実験モジュール）が密集しており、手先を目標に届けるだけでなく、腕全体が障害物を避ける姿勢をとる必要があります。6自由度では手先位置を指定した時点で姿勢が一意に決まってしまいますが、7自由度なら「余った1自由度」で障害物回避や特異姿勢の回避が可能になります。
冗長自由度マニピュレータの制御を理解すると、以下のような応用が開けます。
- 宇宙ステーションでの多目的作業: Canadarm2のように、障害物を避けながら大型モジュールを運搬する
- 軌道上サービス: 故障衛星の修理ミッションで、太陽電池パネルやアンテナを避けつつ修理箇所にアクセスする
- 手術ロボット: da Vinciのような冗長アームで、臓器を避けながら手術器具を精密に配置する
- 産業用ロボット: KUKA iiwaなどの7軸アームで、狭い作業空間内での柔軟な作業を実現する
本記事の内容
- 冗長自由度の定義と直感的理解
- 擬似逆行列（Moore-Penrose）による最小ノルム解の導出
- ヌル空間射影の数学的基礎
- 副目的の設計（関節リミット回避、特異姿勢回避、障害物回避）
- 重み付き擬似逆行列
- タスク優先度制御（多層タスク）
- Pythonで7DOFマニピュレータのヌル空間運動を可視化
前提知識
この記事を読む前に、以下の記事を読んでおくと理解が深まります。
特に、ヤコビ行列 $\bm{J}$ がタスク空間速度 $\dot{\bm{x}}$ と関節空間速度 $\dot{\bm{q}}$ を結ぶ関係式
$$ \dot{\bm{x}} = \bm{J}(\bm{q}) \, \dot{\bm{q}} $$
を前提知識として使います。ヤコビ行列の構成方法や意味については前提記事を参照してください。
冗長性とは何か — 直感的理解
自由度の余り
マニピュレータの冗長性（redundancy）とは、「タスクを遂行するために必要な自由度よりも、実際の関節数が多い」状態を指します。
日常的な例で考えてみましょう。あなたがホワイトボードにペンで点を打つとき、ペン先の位置は3次元空間中の1点（$x, y, z$）なので3自由度のタスクです。ところが、あなたの腕は肩（3自由度）、肘（1自由度）、手首（3自由度）の合計7自由度を持っています。つまり、3自由度のタスクに対して7自由度のアームを使っている — 4自由度分の余裕があるのです。この余裕のおかげで、同じ点にペンを当てたまま肘の位置を自由に変えられます。
この「余った自由度」が冗長自由度であり、その数を冗長度（degree of redundancy）と呼びます。
数学的定義
タスク空間の次元を $m$、関節空間の次元（関節数）を $n$ とすると、ヤコビ行列 $\bm{J}$ は $m \times n$ の行列です。冗長度 $r$ は次のように定義されます。
$$ r = n – m $$
$r > 0$ のとき、マニピュレータは運動学的に冗長（kinematically redundant）であると言います。
| ロボット | 関節数 $n$ | タスク次元 $m$ | 冗長度 $r$ |
|---|---|---|---|
| 平面2リンク | 2 | 2（位置） | 0 |
| PUMA 560 | 6 | 6（位置+姿勢） | 0 |
| Canadarm2 | 7 | 6 | 1 |
| JEMRMS（きぼうアーム） | 6 | 6 | 0 |
| 双腕ロボット（片腕7DOF） | 14 | 12 | 2 |
Canadarm2の冗長度はわずか1ですが、この「たった1自由度の余裕」が宇宙空間での作業を劇的に改善します。6自由度のアームでは手先位置・姿勢を指定すると関節角がほぼ決まってしまいますが、7自由度なら「肘の角度」に相当する自由度が残り、障害物回避に使えるのです。
逆運動学の観点から見た冗長性
前提記事で学んだように、微分運動学の基本方程式は
$$ \dot{\bm{x}} = \bm{J}(\bm{q}) \, \dot{\bm{q}} $$
です。ここで $\dot{\bm{x}} \in \mathbb{R}^m$、$\dot{\bm{q}} \in \mathbb{R}^n$、$\bm{J} \in \mathbb{R}^{m \times n}$ です。
$n > m$ のとき、この方程式は劣決定系（underdetermined system）になります。未知数 $n$ 個に対して方程式が $m$ 個しかないため、解が一意に定まりません。逆に言えば、解の自由度が $n – m$ 次元分だけ残るのです。
「一意に定まらない」と聞くと困った問題に思えるかもしれませんが、ロボティクスではこれを利点として活用します。主タスク（手先を所望の速度で動かす）を満たしつつ、残った自由度で副タスク（障害物回避、特異姿勢回避など）を達成するのです。
では、劣決定系の解をどのように求めるのでしょうか。ここで登場するのが擬似逆行列です。
擬似逆行列（Moore-Penrose）と最小ノルム解
なぜ通常の逆行列が使えないのか
$n = m$（正方で正則なヤコビ行列）のとき、逆運動学は単純に
$$ \dot{\bm{q}} = \bm{J}^{-1} \dot{\bm{x}} $$
で解けます。しかし、冗長マニピュレータでは $\bm{J}$ は $m \times n$（$m < n$）の横長行列であり、正方行列ではないので通常の逆行列は存在しません。
ではどうするか。「方程式 $\dot{\bm{x}} = \bm{J} \dot{\bm{q}}$ を満たす $\dot{\bm{q}}$ の中で、何らかの意味で最良のものを選ぶ」というアプローチをとります。
最小ノルム解の動機
劣決定系 $\dot{\bm{x}} = \bm{J} \dot{\bm{q}}$ の解は無限に存在しますが、物理的な観点から「関節速度のノルム $\|\dot{\bm{q}}\|$ が最小の解」を選ぶのが自然です。理由は明快で、関節速度が小さいほどアクチュエータへの負荷が小さく、消費エネルギーも少なくて済むからです。
このような「拘束条件を満たしつつノルムを最小にする」問題は、ラグランジュの未定乗数法で解けます。
ラグランジュ未定乗数法による導出
最小化問題を定式化します。
$$ \min_{\dot{\bm{q}}} \frac{1}{2} \dot{\bm{q}}^T \dot{\bm{q}} \quad \text{subject to} \quad \bm{J} \dot{\bm{q}} = \dot{\bm{x}} $$
目的関数は関節速度ベクトルのノルムの2乗（$1/2$ は微分の便宜上つけた係数）、制約条件は微分運動学の方程式そのものです。
ラグランジュ関数を構成します。未定乗数ベクトルを $\bm{\lambda} \in \mathbb{R}^m$ として
$$ \mathcal{L}(\dot{\bm{q}}, \bm{\lambda}) = \frac{1}{2} \dot{\bm{q}}^T \dot{\bm{q}} + \bm{\lambda}^T (\dot{\bm{x}} – \bm{J} \dot{\bm{q}}) $$
最適性の必要条件（停留条件）として、$\dot{\bm{q}}$ と $\bm{\lambda}$ に関する偏微分をゼロとおきます。
まず $\dot{\bm{q}}$ について偏微分すると
$$ \frac{\partial \mathcal{L}}{\partial \dot{\bm{q}}} = \dot{\bm{q}} – \bm{J}^T \bm{\lambda} = \bm{0} $$
これより
$$ \dot{\bm{q}} = \bm{J}^T \bm{\lambda} $$
が得られます。次に $\bm{\lambda}$ について偏微分すると（これは制約条件そのものです）
$$ \frac{\partial \mathcal{L}}{\partial \bm{\lambda}} = \dot{\bm{x}} – \bm{J} \dot{\bm{q}} = \bm{0} $$
$\dot{\bm{q}} = \bm{J}^T \bm{\lambda}$ を制約条件 $\bm{J} \dot{\bm{q}} = \dot{\bm{x}}$ に代入すると
$$ \bm{J} \bm{J}^T \bm{\lambda} = \dot{\bm{x}} $$
ここで $\bm{J}$ がフルランク（$\text{rank}(\bm{J}) = m$）のとき、$\bm{J}\bm{J}^T$ は $m \times m$ の正定値対称行列となるので逆行列が存在します。両辺に左から $(\bm{J}\bm{J}^T)^{-1}$ をかけると
$$ \bm{\lambda} = (\bm{J}\bm{J}^T)^{-1} \dot{\bm{x}} $$
これを $\dot{\bm{q}} = \bm{J}^T \bm{\lambda}$ に代入して
$$ \dot{\bm{q}} = \bm{J}^T (\bm{J}\bm{J}^T)^{-1} \dot{\bm{x}} $$
右辺に現れた $\bm{J}^T (\bm{J}\bm{J}^T)^{-1}$ こそが、$\bm{J}$ の右擬似逆行列（Moore-Penrose pseudoinverse）です。
Moore-Penrose擬似逆行列の定義
$m \times n$ 行列 $\bm{J}$（$m \leq n$、フルランク）の右擬似逆行列（右一般化逆行列）を
$$ \bm{J}^{\dagger} = \bm{J}^T (\bm{J}\bm{J}^T)^{-1} $$
と定義します。$\bm{J}^{\dagger}$ は $n \times m$ の行列です。
この擬似逆行列は以下の性質を満たします。
性質1（右逆行列条件）: $\bm{J}\bm{J}^{\dagger} = \bm{I}_m$ — つまり $\dot{\bm{q}} = \bm{J}^{\dagger} \dot{\bm{x}}$ は確かに制約 $\bm{J}\dot{\bm{q}} = \dot{\bm{x}}$ を満たします。
確認してみましょう。
$$ \bm{J}\bm{J}^{\dagger} = \bm{J} \cdot \bm{J}^T (\bm{J}\bm{J}^T)^{-1} = (\bm{J}\bm{J}^T)(\bm{J}\bm{J}^T)^{-1} = \bm{I}_m $$
性質2（最小ノルム性）: $\bm{J}\dot{\bm{q}} = \dot{\bm{x}}$ を満たすすべての $\dot{\bm{q}}$ の中で、$\dot{\bm{q}} = \bm{J}^{\dagger} \dot{\bm{x}}$ は $\|\dot{\bm{q}}\|$ が最小です。
性質3（Moore-Penroseの4条件）: $\bm{J}^{\dagger}$ は以下の4条件を全て満たす唯一の行列です。
$$ \bm{J}\bm{J}^{\dagger}\bm{J} = \bm{J}, \quad \bm{J}^{\dagger}\bm{J}\bm{J}^{\dagger} = \bm{J}^{\dagger}, \quad (\bm{J}\bm{J}^{\dagger})^T = \bm{J}\bm{J}^{\dagger}, \quad (\bm{J}^{\dagger}\bm{J})^T = \bm{J}^{\dagger}\bm{J} $$
なお、$\bm{J}$ がフルランクでない場合（特異姿勢の近傍など）は $\bm{J}\bm{J}^T$ が正則でなくなり、上記の公式がそのままでは使えません。この場合は特異値分解（SVD）を用いた一般的な擬似逆行列の計算が必要になります。NumPyの numpy.linalg.pinv はSVDベースで実装されており、ランク落ちの場合でも安定に計算できます。
ここまでで、擬似逆行列を使えば「タスクを満たす最小ノルムの関節速度」が求まることがわかりました。しかし、最小ノルム解はあくまでも「主タスクだけ」を考えた解です。冗長自由度の真の強みは、主タスクに加えて副タスクを同時に達成できることにあります。その鍵となるのがヌル空間射影です。
ヌル空間射影の数学
ヌル空間とは何か
ヤコビ行列 $\bm{J}$ のヌル空間（null space, 零空間）$\mathcal{N}(\bm{J})$ とは
$$ \mathcal{N}(\bm{J}) = \{ \dot{\bm{q}} \in \mathbb{R}^n \mid \bm{J} \dot{\bm{q}} = \bm{0} \} $$
で定義される部分空間です。直感的に言うと、手先を全く動かさずに関節を動かせる方向の集合です。
人間の腕で言えば、手をテーブルの上に固定したまま肘だけを上下に動かす動き — これがヌル空間内の運動です。手先の位置は変わりませんが、腕の姿勢（肘の位置）は変化します。
冗長マニピュレータでは $\text{dim}(\mathcal{N}(\bm{J})) = n – \text{rank}(\bm{J})$ です。$\bm{J}$ がフルランク（$\text{rank}(\bm{J}) = m$）のとき、ヌル空間の次元は $n – m = r$（冗長度）となります。つまり、冗長度の分だけヌル空間に「遊び」があるのです。
ヌル空間射影行列
ヌル空間への射影行列は次のように定義されます。
$$ \bm{N} = \bm{I}_n – \bm{J}^{\dagger} \bm{J} $$
ここで $\bm{I}_n$ は $n \times n$ の単位行列です。
この行列が「ヌル空間への射影」であることを確認しましょう。任意のベクトル $\bm{z} \in \mathbb{R}^n$ に対して $\bm{N}\bm{z}$ がヌル空間に属すること、すなわち $\bm{J}(\bm{N}\bm{z}) = \bm{0}$ を示します。
$$ \bm{J}\bm{N}\bm{z} = \bm{J}(\bm{I}_n – \bm{J}^{\dagger}\bm{J})\bm{z} = \bm{J}\bm{z} – \bm{J}\bm{J}^{\dagger}\bm{J}\bm{z} $$
ここで性質1（Moore-Penroseの第1条件 $\bm{J}\bm{J}^{\dagger}\bm{J} = \bm{J}$）を使うと
$$ = \bm{J}\bm{z} – \bm{J}\bm{z} = \bm{0} $$
確かに $\bm{N}\bm{z}$ はヌル空間に属します。つまり、$\bm{N}$ をかけたベクトルは、手先の運動に一切影響を与えない関節速度を表しています。
さらに、$\bm{N}$ は冪等性（$\bm{N}^2 = \bm{N}$）を持ちます。これは射影行列の基本的な性質です。
$$ \bm{N}^2 = (\bm{I}_n – \bm{J}^{\dagger}\bm{J})(\bm{I}_n – \bm{J}^{\dagger}\bm{J}) $$
展開すると
$$ = \bm{I}_n – \bm{J}^{\dagger}\bm{J} – \bm{J}^{\dagger}\bm{J} + \bm{J}^{\dagger}\bm{J}\bm{J}^{\dagger}\bm{J} $$
Moore-Penroseの第2条件 $\bm{J}^{\dagger}\bm{J}\bm{J}^{\dagger} = \bm{J}^{\dagger}$ を使うと $\bm{J}^{\dagger}\bm{J}\bm{J}^{\dagger}\bm{J} = \bm{J}^{\dagger}\bm{J}$ となるので
$$ = \bm{I}_n – \bm{J}^{\dagger}\bm{J} – \bm{J}^{\dagger}\bm{J} + \bm{J}^{\dagger}\bm{J} = \bm{I}_n – \bm{J}^{\dagger}\bm{J} = \bm{N} $$
冗長マニピュレータの一般解
以上の準備を踏まえて、劣決定系 $\dot{\bm{x}} = \bm{J}\dot{\bm{q}}$ の一般解を書き下しましょう。
$$ \dot{\bm{q}} = \underbrace{\bm{J}^{\dagger} \dot{\bm{x}}}_{\text{主タスク（最小ノルム解）}} + \underbrace{(\bm{I}_n – \bm{J}^{\dagger}\bm{J}) \bm{z}}_{\text{ヌル空間成分}} $$
ここで $\bm{z} \in \mathbb{R}^n$ は任意のベクトルです。
この式は2つの成分からなっています。
第1項 $\bm{J}^{\dagger}\dot{\bm{x}}$: 主タスクを達成するための関節速度。手先を所望の速度 $\dot{\bm{x}}$ で動かすための最小ノルム解です。
第2項 $(\bm{I}_n – \bm{J}^{\dagger}\bm{J})\bm{z}$: ヌル空間成分。手先速度に影響を与えずに関節を動かす成分です。$\bm{z}$ を適切に選ぶことで、主タスクを乱さずに副目的を達成できます。
この構造が冗長マニピュレータ制御の核心です。第2項がなければ（$\bm{z} = \bm{0}$）単なる最小ノルム解になりますが、$\bm{z}$ を上手に設計することで、関節リミット回避、特異姿勢回避、障害物回避といった副目的を同時に満たせるのです。
では、具体的にどのような $\bm{z}$ を選べばよいのでしょうか。次のセクションで、代表的な副目的の設計法を見ていきます。
副目的の設計 — 勾配射影法
勾配射影法の考え方
副目的を達成するための最も基本的な方法が勾配射影法（gradient projection method）です。副目的を表すスカラー関数 $w(\bm{q})$ を定義し、その勾配方向にヌル空間内で移動するというアイデアです。
具体的には、$\bm{z}$ をスカラー関数 $w(\bm{q})$ の勾配に比例させます。
$$ \bm{z} = k_0 \nabla_{\bm{q}} w(\bm{q}) = k_0 \left(\frac{\partial w}{\partial q_1}, \frac{\partial w}{\partial q_2}, \ldots, \frac{\partial w}{\partial q_n}\right)^T $$
ここで $k_0 > 0$ はゲインパラメータです。$w(\bm{q})$ を最大化したい場合は正の勾配を、最小化したい場合は負の勾配を使います（$k_0$ の符号で調整）。
一般解に代入すると
$$ \dot{\bm{q}} = \bm{J}^{\dagger} \dot{\bm{x}} + k_0 (\bm{I}_n – \bm{J}^{\dagger}\bm{J}) \nabla_{\bm{q}} w(\bm{q}) $$
この式の美しさは、ヌル空間射影 $(\bm{I}_n – \bm{J}^{\dagger}\bm{J})$ が自動的に勾配ベクトルのうち主タスクに影響する成分を除去してくれることです。$\nabla_{\bm{q}} w$ がどんなベクトルであっても、射影後の成分は手先運動に干渉しません。
副目的1: 関節リミット回避
宇宙ロボットの各関節には可動範囲（関節リミット）があります。関節角 $q_i$ が上限 $q_{i,\max}$ や下限 $q_{i,\min}$ に近づくと、それ以上動けなくなりタスクの遂行が不可能になります。
関節リミットからなるべく離れた「関節空間の中央」に留まるための評価関数として、以下が広く使われます。
$$ w_{\text{JL}}(\bm{q}) = -\frac{1}{2n} \sum_{i=1}^{n} \left(\frac{q_i – \bar{q}_i}{q_{i,\max} – q_{i,\min}}\right)^2 $$
ここで $\bar{q}_i = (q_{i,\max} + q_{i,\min})/2$ は各関節の可動範囲の中央値です。
この関数は、関節角が可動範囲の中央にあるとき最大（$0$）をとり、リミットに近づくほど小さくなります。したがって、$w_{\text{JL}}$ を最大化する方向にヌル空間内で動けば、関節角を中央に引き戻す効果が得られます。
勾配を計算すると
$$ \frac{\partial w_{\text{JL}}}{\partial q_i} = -\frac{1}{n} \cdot \frac{q_i – \bar{q}_i}{(q_{i,\max} – q_{i,\min})^2} $$
符号に注意しましょう。$q_i > \bar{q}_i$（中央より上にある）のとき勾配は負、$q_i < \bar{q}_i$（中央より下にある）のとき勾配は正です。つまり $k_0 > 0$ で勾配に従って動くと、関節角は常に中央に向かいます。最大化したいので $\bm{z} = +k_0 \nabla_{\bm{q}} w_{\text{JL}}$ とします（$w_{\text{JL}}$ は常に非正の値をとるため、$\nabla_{\bm{q}} w_{\text{JL}}$ 方向に動くと値が大きくなる=中央に近づく）。
副目的2: 特異姿勢回避
マニピュレータが特異姿勢（singular configuration）に近づくと、ヤコビ行列のランクが落ち、一部の方向に手先が動かせなくなります。特異姿勢の近傍では擬似逆行列の要素が発散し、関節速度が非常に大きくなって危険です。
特異姿勢からの距離を表す指標として可操作度（manipulability）があります。
$$ w_{\text{man}}(\bm{q}) = \sqrt{\det(\bm{J}\bm{J}^T)} $$
可操作度は吉川（1985）が提案した指標で、手先がどれだけ「自由に」動けるかを1つのスカラー値で表現します。幾何学的には、ヤコビ行列の特異値を半径とする楕円体（可操作度楕円体）の体積に比例します。特異姿勢では $\det(\bm{J}\bm{J}^T) = 0$ となり、可操作度はゼロになります。
可操作度を最大化するようにヌル空間内で動けば、特異姿勢から遠ざかることができます。
$$ \bm{z} = k_0 \nabla_{\bm{q}} w_{\text{man}}(\bm{q}) $$
可操作度の勾配は数値微分で計算できます。
$$ \frac{\partial w_{\text{man}}}{\partial q_i} \approx \frac{w_{\text{man}}(\bm{q} + \epsilon \bm{e}_i) – w_{\text{man}}(\bm{q} – \epsilon \bm{e}_i)}{2\epsilon} $$
ここで $\bm{e}_i$ は $i$ 番目の要素が1、他が0の単位ベクトル、$\epsilon$ は微小量（例えば $10^{-6}$ rad）です。
副目的3: 障害物回避
宇宙ステーション上での作業では、マニピュレータの腕全体が太陽電池パドルやアンテナなどの障害物に接触しないようにする必要があります。手先だけでなくリンク全体と障害物の距離を考慮しなければなりません。
典型的なアプローチは、各リンク上の代表点と障害物の最近接点の距離 $d_i$ を用いた評価関数です。
$$ w_{\text{obs}}(\bm{q}) = \min_i d_i(\bm{q}) $$
あるいは、反発ポテンシャル関数を使う方法もあります。
$$ w_{\text{obs}}(\bm{q}) = -\sum_i \frac{1}{d_i(\bm{q})^2} $$
距離が近いほどポテンシャルが急激に大きくなり、強い反発力が生まれます。
宇宙ロボットでは、障害物回避と特異姿勢回避を同時に考慮する必要がある場面が多くあります。しかし、ヌル空間は有限次元（冗長度 $r$ 次元）しかないため、複数の副目的が競合する可能性があります。このような場合に有効なのが、副目的にも優先順位をつけるタスク優先度制御です。しかし、その前にまず、副目的の重みを関節ごとに調整できる重み付き擬似逆行列を見ておきましょう。
重み付き擬似逆行列
動機 — なぜ重みが必要か
標準的な擬似逆行列 $\bm{J}^{\dagger} = \bm{J}^T(\bm{J}\bm{J}^T)^{-1}$ は、全ての関節速度を等しく扱い、$\|\dot{\bm{q}}\|^2 = \sum_i \dot{q}_i^2$ を最小化します。しかし、実際のロボットでは関節ごとに事情が異なります。
- 大きなアクチュエータ（肩関節など）は大きなトルクを出せるが、小さなアクチュエータ（手首関節など）は出せるトルクが限られる
- 関節リミットに近い関節はなるべく動かしたくない
- 宇宙ロボットでは、母船に近い関節を動かすと母船全体の姿勢が乱れるため、なるべく先端側の関節を使いたい
このような場合、関節ごとに「動かしやすさ」の重みを設定する重み付き擬似逆行列が有効です。
重み付き擬似逆行列の定義
関節空間の重み行列を $\bm{W} \in \mathbb{R}^{n \times n}$（正定値対称行列）とします。重み付きノルム
$$ \|\dot{\bm{q}}\|_{\bm{W}}^2 = \dot{\bm{q}}^T \bm{W} \dot{\bm{q}} $$
を最小化する最適化問題を解くと、重み付き擬似逆行列が得られます。
$$ \min_{\dot{\bm{q}}} \frac{1}{2} \dot{\bm{q}}^T \bm{W} \dot{\bm{q}} \quad \text{subject to} \quad \bm{J} \dot{\bm{q}} = \dot{\bm{x}} $$
ラグランジュ関数を構成して先ほどと同様に解くと
$$ \mathcal{L} = \frac{1}{2}\dot{\bm{q}}^T \bm{W} \dot{\bm{q}} + \bm{\lambda}^T(\dot{\bm{x}} – \bm{J}\dot{\bm{q}}) $$
$\dot{\bm{q}}$ に関する停留条件より
$$ \bm{W}\dot{\bm{q}} – \bm{J}^T\bm{\lambda} = \bm{0} \quad \Rightarrow \quad \dot{\bm{q}} = \bm{W}^{-1}\bm{J}^T\bm{\lambda} $$
これを制約条件に代入すると
$$ \bm{J}\bm{W}^{-1}\bm{J}^T\bm{\lambda} = \dot{\bm{x}} \quad \Rightarrow \quad \bm{\lambda} = (\bm{J}\bm{W}^{-1}\bm{J}^T)^{-1}\dot{\bm{x}} $$
したがって
$$ \dot{\bm{q}} = \bm{W}^{-1}\bm{J}^T(\bm{J}\bm{W}^{-1}\bm{J}^T)^{-1}\dot{\bm{x}} $$
重み付き擬似逆行列は
$$ \bm{J}_W^{\dagger} = \bm{W}^{-1}\bm{J}^T(\bm{J}\bm{W}^{-1}\bm{J}^T)^{-1} $$
です。$\bm{W} = \bm{I}_n$ のとき、標準的な擬似逆行列 $\bm{J}^{\dagger}$ に一致することが確認できます。
重み行列の設計
$\bm{W}$ を対角行列 $\bm{W} = \text{diag}(w_1, w_2, \ldots, w_n)$ とするのが最も簡単です。$w_i$ が大きいほど関節 $i$ は「動かすコストが高い」ことを意味し、結果として関節 $i$ の速度が抑制されます。
例えば、$i$ 番目の関節がリミットに近い場合、$w_i$ を大きくして動きにくくするダイナミックな重み設定が有効です。
$$ w_i = 1 + \alpha \left|\frac{q_i – \bar{q}_i}{q_{i,\max} – q_{i,\min}}\right|^{\beta} $$
$\alpha, \beta > 0$ はパラメータで、関節リミットに近づくほど $w_i$ が急激に大きくなるように $\beta = 2$ や $\beta = 4$ が使われます。
重み付き擬似逆行列を使ったヌル空間射影行列は
$$ \bm{N}_W = \bm{I}_n – \bm{J}_W^{\dagger}\bm{J} $$
となり、一般解は
$$ \dot{\bm{q}} = \bm{J}_W^{\dagger}\dot{\bm{x}} + (\bm{I}_n – \bm{J}_W^{\dagger}\bm{J})\bm{z} $$
です。重み付きの場合も、ヌル空間成分が主タスクに影響しないことは $\bm{J}(\bm{I}_n – \bm{J}_W^{\dagger}\bm{J})\bm{z} = \bm{0}$ から確認できます。
ここまでで、1つの主タスクと1つの副目的を組み合わせる方法を見てきました。しかし、実際のミッションでは「手先を目標に動かす（主タスク）」「特異姿勢を回避する（副目的1）」「関節リミットを回避する（副目的2）」のように、複数のタスクに優先順位をつけて同時に実行したい場面があります。これを実現するのが次に紹介するタスク優先度制御です。
タスク優先度制御
多層タスクの必要性
宇宙ロボットのミッションでは、複数のタスクを同時に遂行する必要があります。しかし、全てのタスクを同等に扱うと、互いに矛盾するタスクが競合したときにどちらも中途半端にしか達成できません。
例えば、Canadarm2がモジュールを運搬するシナリオを考えましょう。
| 優先度 | タスク | 次元 |
|---|---|---|
| 最高 | 手先の位置・姿勢制御 | 6 |
| 高 | 障害物回避 | 1（最近接距離） |
| 中 | 特異姿勢回避 | 1 |
| 低 | 関節リミット回避 | 1 |
高優先度のタスクが低優先度のタスクに邪魔されてはなりません。タスク優先度制御（task priority control）は、Nakamura & Hanafusa（1987）やSiciliano & Slotine（1991）が提案した枠組みで、各タスクに明確な優先順位を設定し、高優先度タスクのヌル空間内でのみ低優先度タスクを遂行します。
2階層の定式化
最も基本的なケースとして、タスク1（高優先度、ヤコビ行列 $\bm{J}_1 \in \mathbb{R}^{m_1 \times n}$）とタスク2（低優先度、ヤコビ行列 $\bm{J}_2 \in \mathbb{R}^{m_2 \times n}$）の2つを考えます。
タスク1: $\dot{\bm{x}}_1 = \bm{J}_1 \dot{\bm{q}}$（最優先で達成）
タスク2: $\dot{\bm{x}}_2 = \bm{J}_2 \dot{\bm{q}}$（タスク1を乱さない範囲で達成）
まず、タスク1だけを考えた一般解は
$$ \dot{\bm{q}} = \bm{J}_1^{\dagger} \dot{\bm{x}}_1 + (\bm{I}_n – \bm{J}_1^{\dagger}\bm{J}_1)\bm{z} $$
ここで、$\bm{z}$ をタスク2を達成するように選びます。タスク2の方程式 $\dot{\bm{x}}_2 = \bm{J}_2 \dot{\bm{q}}$ に上の一般解を代入すると
$$ \dot{\bm{x}}_2 = \bm{J}_2 \bm{J}_1^{\dagger} \dot{\bm{x}}_1 + \bm{J}_2 (\bm{I}_n – \bm{J}_1^{\dagger}\bm{J}_1)\bm{z} $$
$\bm{z}$ について解くために、$\bm{J}_2(\bm{I}_n – \bm{J}_1^{\dagger}\bm{J}_1)$ を $\hat{\bm{J}}_2$ とおきます。これは「タスク1のヌル空間に射影されたタスク2のヤコビ行列」です。
$$ \hat{\bm{J}}_2 = \bm{J}_2 (\bm{I}_n – \bm{J}_1^{\dagger}\bm{J}_1) $$
$\hat{\bm{J}}_2$ の擬似逆行列を使って $\bm{z}$ を求めると
$$ \bm{z} = \hat{\bm{J}}_2^{\dagger} (\dot{\bm{x}}_2 – \bm{J}_2 \bm{J}_1^{\dagger} \dot{\bm{x}}_1) $$
これを一般解に代入して整理すると、2階層タスク優先度制御の解が得られます。
$$ \dot{\bm{q}} = \bm{J}_1^{\dagger} \dot{\bm{x}}_1 + (\bm{I}_n – \bm{J}_1^{\dagger}\bm{J}_1) \hat{\bm{J}}_2^{\dagger} (\dot{\bm{x}}_2 – \bm{J}_2 \bm{J}_1^{\dagger} \dot{\bm{x}}_1) $$
この式を読み解きましょう。
第1項 $\bm{J}_1^{\dagger}\dot{\bm{x}}_1$: タスク1を達成する最小ノルム解。
第2項 $(\bm{I}_n – \bm{J}_1^{\dagger}\bm{J}_1)\hat{\bm{J}}_2^{\dagger}(\dot{\bm{x}}_2 – \bm{J}_2\bm{J}_1^{\dagger}\dot{\bm{x}}_1)$: タスク1のヌル空間内でタスク2をできるだけ達成する成分。$(\dot{\bm{x}}_2 – \bm{J}_2\bm{J}_1^{\dagger}\dot{\bm{x}}_1)$ は「タスク2の所望速度のうち、タスク1の最小ノルム解だけでは達成できない残り」を表します。
重要なポイントは、タスク1は常に完全に達成されることです。第2項はタスク1のヌル空間に属するため、タスク1の達成を一切妨げません。一方、タスク2はヌル空間の次元が許す範囲でしか達成できないため、完全には達成されない場合があります。
多階層への拡張
2階層の考え方は、3つ以上のタスクに再帰的に拡張できます。$k$ 番目のタスク（ヤコビ行列 $\bm{J}_k$、所望速度 $\dot{\bm{x}}_k$）に対して
$$ \dot{\bm{q}}_k = \dot{\bm{q}}_{k-1} + (\bm{J}_k \bm{N}_{k-1})^{\dagger}(\dot{\bm{x}}_k – \bm{J}_k \dot{\bm{q}}_{k-1}) $$
ここで $\bm{N}_{k-1}$ は第1〜第$(k-1)$タスクの結合ヌル空間射影行列、$\dot{\bm{q}}_{k-1}$ は第$(k-1)$タスクまでの解です。初期値は $\dot{\bm{q}}_0 = \bm{0}$、$\bm{N}_0 = \bm{I}_n$ です。
各階層で利用可能な自由度は、上位タスクに使われた分だけ減っていきます。$n$ 個の関節で合計 $n$ 次元分のタスクしか達成できないため、タスクの次元の総和が $n$ を超えると、下位タスクは大幅に妥協されます。
タスク優先度制御の理論的基盤が整ったところで、いよいよこれらの概念をPythonで実装し、7自由度マニピュレータのヌル空間運動を可視化してみましょう。
Pythonでの実装と可視化
平面冗長マニピュレータのシミュレーション
理論を直感的に理解するために、まず平面4リンク冗長マニピュレータ（$n = 4$、タスク空間は手先位置 $m = 2$、冗長度 $r = 2$）を実装します。4リンクにすることでヌル空間が2次元になり、冗長性の効果がはっきり観察できます。
まず、順運動学とヤコビ行列を実装します。
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Circle
def forward_kinematics(q, link_lengths):
"""平面nリンクマニピュレータの順運動学
Parameters
----------
q : array-like, shape (n,)
関節角 [rad]
link_lengths : array-like, shape (n,)
各リンクの長さ
Returns
-------
positions : ndarray, shape (n+1, 2)
ベース + 各関節 + 手先の位置
"""
n = len(q)
positions = np.zeros((n + 1, 2))
angle = 0.0
for i in range(n):
angle += q[i]
positions[i + 1, 0] = positions[i, 0] + link_lengths[i] * np.cos(angle)
positions[i + 1, 1] = positions[i, 1] + link_lengths[i] * np.sin(angle)
return positions
def compute_jacobian(q, link_lengths):
"""平面nリンクマニピュレータのヤコビ行列（位置のみ、2×n）
Parameters
----------
q : array-like, shape (n,)
関節角 [rad]
link_lengths : array-like, shape (n,)
各リンクの長さ
Returns
-------
J : ndarray, shape (2, n)
ヤコビ行列
"""
n = len(q)
J = np.zeros((2, n))
for i in range(n):
angle = np.sum(q[:i + 1])
for j in range(i, n):
# j >= i のリンクの回転に関節iが寄与
pass
# 正しい計算: 関節iが手先位置に与える影響
J = np.zeros((2, n))
for i in range(n):
cum_angle = np.sum(q[:i + 1])
# 関節iから手先までの寄与
for k in range(i, n):
ak = np.sum(q[:k + 1])
J[0, i] -= link_lengths[k] * np.sin(ak)
J[1, i] += link_lengths[k] * np.cos(ak)
return J
次に、擬似逆行列とヌル空間射影を使った逆運動学を実装します。
def pseudo_inverse(J):
"""右擬似逆行列 J^† = J^T (J J^T)^{-1}"""
return J.T @ np.linalg.inv(J @ J.T)
def null_space_projector(J):
"""ヌル空間射影行列 N = I - J^† J"""
n = J.shape[1]
J_pinv = pseudo_inverse(J)
return np.eye(n) - J_pinv @ J
def manipulability(q, link_lengths):
"""可操作度 w = sqrt(det(J J^T))"""
J = compute_jacobian(q, link_lengths)
return np.sqrt(max(np.linalg.det(J @ J.T), 0.0))
def grad_manipulability(q, link_lengths, eps=1e-6):
"""可操作度の数値勾配"""
n = len(q)
grad = np.zeros(n)
for i in range(n):
q_plus = q.copy()
q_minus = q.copy()
q_plus[i] += eps
q_minus[i] -= eps
grad[i] = (manipulability(q_plus, link_lengths)
- manipulability(q_minus, link_lengths)) / (2 * eps)
return grad
def grad_joint_limit_avoidance(q, q_min, q_max):
"""関節リミット回避の勾配（可動範囲の中央に向かう）"""
n = len(q)
q_mid = (q_max + q_min) / 2.0
q_range = q_max - q_min
grad = -(q - q_mid) / (n * q_range**2)
return grad
上の2つのコードブロックで、平面冗長マニピュレータの基本的な計算要素が揃いました。forward_kinematics は各関節位置を累積角度で計算し、compute_jacobian は各関節が手先位置に与える微小変位を列ベクトルとして構成しています。pseudo_inverse は $\bm{J}^T(\bm{J}\bm{J}^T)^{-1}$ をそのまま実装し、null_space_projector はヌル空間射影行列 $\bm{I} – \bm{J}^{\dagger}\bm{J}$ を返します。
ヌル空間運動の可視化
まず、手先を固定したまま関節姿勢だけが変化するヌル空間運動を可視化します。これが冗長性の最も直感的なデモンストレーションです。
import numpy as np
import matplotlib.pyplot as plt
def forward_kinematics(q, link_lengths):
"""平面nリンクマニピュレータの順運動学"""
n = len(q)
positions = np.zeros((n + 1, 2))
angle = 0.0
for i in range(n):
angle += q[i]
positions[i + 1, 0] = positions[i, 0] + link_lengths[i] * np.cos(angle)
positions[i + 1, 1] = positions[i, 1] + link_lengths[i] * np.sin(angle)
return positions
def compute_jacobian(q, link_lengths):
"""平面nリンクマニピュレータのヤコビ行列（2×n）"""
n = len(q)
J = np.zeros((2, n))
for i in range(n):
for k in range(i, n):
ak = np.sum(q[:k + 1])
J[0, i] -= link_lengths[k] * np.sin(ak)
J[1, i] += link_lengths[k] * np.cos(ak)
return J
# --- パラメータ設定 ---
n_links = 4
link_lengths = np.array([1.0, 0.8, 0.6, 0.4])
q = np.array([0.5, 0.3, -0.2, 0.4]) # 初期関節角 [rad]
# 手先を固定し、ヌル空間内でランダム方向に動かす
J = compute_jacobian(q, link_lengths)
J_pinv = np.linalg.pinv(J)
N = np.eye(n_links) - J_pinv @ J
# 初期手先位置
pos_init = forward_kinematics(q, link_lengths)
end_effector = pos_init[-1].copy()
fig, axes = plt.subplots(1, 3, figsize=(18, 6))
# ヌル空間の基底ベクトルを取得（SVDで）
U, s, Vt = np.linalg.svd(J)
null_basis = Vt[2:, :] # m=2 なので、第3,4行がヌル空間基底
for idx, (alpha, title) in enumerate([
(0.0, "Null-space motion: $\\alpha = 0$"),
(1.5, "Null-space motion: $\\alpha = 1.5$"),
(-1.5, "Null-space motion: $\\alpha = -1.5$")
]):
ax = axes[idx]
q_current = q.copy()
# ヌル空間の第1基底方向に動かす
dt = 0.02
n_steps = 80
trajectories = []
for step in range(n_steps):
J_curr = compute_jacobian(q_current, link_lengths)
N_curr = np.eye(n_links) - np.linalg.pinv(J_curr) @ J_curr
# ヌル空間方向のベクトル
z = np.array([1.0, -0.5, 0.3, -0.8]) # 任意ベクトル
dq_null = alpha * N_curr @ z
# 手先位置のずれを補正（数値誤差蓄積対策）
pos_curr = forward_kinematics(q_current, link_lengths)
x_error = end_effector - pos_curr[-1]
dq_correction = np.linalg.pinv(J_curr) @ x_error * 5.0
q_current = q_current + (dq_null + dq_correction) * dt
trajectories.append(q_current.copy())
# 描画: 初期姿勢（薄い色）と数フレーム
for i, q_frame in enumerate(trajectories[::10]):
alpha_val = 0.2 + 0.8 * (i / (len(trajectories[::10]) - 1))
pos = forward_kinematics(q_frame, link_lengths)
ax.plot(pos[:, 0], pos[:, 1], 'o-', color=plt.cm.viridis(alpha_val),
linewidth=2, markersize=4, alpha=alpha_val)
# 手先位置を赤で強調
ax.plot(end_effector[0], end_effector[1], 'r*', markersize=15, zorder=10)
ax.set_xlim(-1.5, 3.5)
ax.set_ylim(-2.0, 3.0)
ax.set_aspect('equal')
ax.grid(True, alpha=0.3)
ax.set_title(title, fontsize=12)
ax.set_xlabel('x')
ax.set_ylabel('y')
plt.tight_layout()
plt.savefig('null_space_motion.png', dpi=150, bbox_inches='tight')
plt.show()
上のグラフには3つのパネルが表示されます。左パネル（$\alpha = 0$）は初期姿勢のまま静止しており、ヌル空間入力がないので関節は動きません。中央パネル（$\alpha = 1.5$）と右パネル（$\alpha = -1.5$）では、手先位置（赤い星印）を完全に固定したまま、アームの姿勢が大きく変化している様子が観察できます。色が薄い姿勢から濃い姿勢へと時間が進んでおり、4本のリンクが波打つように動いています。これがヌル空間運動の本質 — 手先を動かさずに関節構成だけを変える運動です。
副目的付きの逆運動学シミュレーション
次に、手先を直線的に移動させながら、同時に副目的（関節リミット回避）をヌル空間で追求するシミュレーションを実装します。
import numpy as np
import matplotlib.pyplot as plt
def forward_kinematics(q, link_lengths):
"""平面nリンクマニピュレータの順運動学"""
n = len(q)
positions = np.zeros((n + 1, 2))
angle = 0.0
for i in range(n):
angle += q[i]
positions[i + 1, 0] = positions[i, 0] + link_lengths[i] * np.cos(angle)
positions[i + 1, 1] = positions[i, 1] + link_lengths[i] * np.sin(angle)
return positions
def compute_jacobian(q, link_lengths):
"""平面nリンクマニピュレータのヤコビ行列（2×n）"""
n = len(q)
J = np.zeros((2, n))
for i in range(n):
for k in range(i, n):
ak = np.sum(q[:k + 1])
J[0, i] -= link_lengths[k] * np.sin(ak)
J[1, i] += link_lengths[k] * np.cos(ak)
return J
def manipulability(q, link_lengths):
"""可操作度"""
J = compute_jacobian(q, link_lengths)
return np.sqrt(max(np.linalg.det(J @ J.T), 0.0))
# --- パラメータ ---
n_links = 4
link_lengths = np.array([1.0, 0.8, 0.6, 0.4])
q_min = np.array([-np.pi, -np.pi, -np.pi, -np.pi])
q_max = np.array([np.pi, np.pi, np.pi, np.pi])
# 初期関節角
q_init = np.array([0.8, 0.6, -0.3, 0.2])
# 手先の目標軌道（直線）
pos_init = forward_kinematics(q_init, link_lengths)[-1]
pos_goal = pos_init + np.array([0.0, -1.5]) # 下方向に移動
n_steps = 200
dt = 0.01
# --- 3つのケースを比較 ---
cases = {
"Pseudo-inverse only (no null-space)": {"k0": 0.0, "use_jl": False},
"Null-space: joint limit avoidance": {"k0": 5.0, "use_jl": True},
"Null-space: manipulability max": {"k0": 2.0, "use_jl": False},
}
fig, axes = plt.subplots(1, 3, figsize=(18, 6))
results = {}
for ax_idx, (case_name, params) in enumerate(cases.items()):
ax = axes[ax_idx]
q = q_init.copy()
joint_history = [q.copy()]
manip_history = [manipulability(q, link_lengths)]
ee_history = [forward_kinematics(q, link_lengths)[-1].copy()]
for step in range(n_steps):
t = (step + 1) / n_steps
x_desired = pos_init + t * (pos_goal - pos_init)
pos_current = forward_kinematics(q, link_lengths)[-1]
x_error = x_desired - pos_current
J = compute_jacobian(q, link_lengths)
J_pinv = np.linalg.pinv(J)
# 主タスク: 手先位置の追従
dq_main = J_pinv @ (x_error / dt)
# 副タスク
if params["use_jl"]:
q_mid = (q_max + q_min) / 2.0
q_range = q_max - q_min
grad_w = -(q - q_mid) / (n_links * q_range**2)
dq_null = params["k0"] * (np.eye(n_links) - J_pinv @ J) @ grad_w
elif params["k0"] > 0:
# 可操作度最大化
eps = 1e-6
grad_m = np.zeros(n_links)
w0 = manipulability(q, link_lengths)
for i in range(n_links):
q_p = q.copy(); q_p[i] += eps
grad_m[i] = (manipulability(q_p, link_lengths) - w0) / eps
dq_null = params["k0"] * (np.eye(n_links) - J_pinv @ J) @ grad_m
else:
dq_null = np.zeros(n_links)
dq = dq_main + dq_null
# 関節速度の制限
max_dq = 2.0
if np.max(np.abs(dq)) > max_dq:
dq = dq * max_dq / np.max(np.abs(dq))
q = q + dq * dt
q = np.clip(q, q_min, q_max)
joint_history.append(q.copy())
manip_history.append(manipulability(q, link_lengths))
ee_history.append(forward_kinematics(q, link_lengths)[-1].copy())
results[case_name] = {
'joint_history': joint_history,
'manip_history': manip_history,
'ee_history': ee_history
}
# アーム姿勢を描画（数フレーム）
frames = np.linspace(0, n_steps, 8, dtype=int)
for i, f in enumerate(frames):
alpha_val = 0.2 + 0.8 * i / (len(frames) - 1)
pos = forward_kinematics(joint_history[f], link_lengths)
ax.plot(pos[:, 0], pos[:, 1], 'o-',
color=plt.cm.plasma(i / (len(frames) - 1)),
linewidth=2, markersize=4, alpha=alpha_val)
# 手先軌道
ee_arr = np.array(ee_history)
ax.plot(ee_arr[:, 0], ee_arr[:, 1], 'r--', linewidth=1.5, label='End-effector path')
ax.plot(pos_goal[0], pos_goal[1], 'r*', markersize=12)
ax.set_xlim(-2.0, 3.5)
ax.set_ylim(-2.5, 3.0)
ax.set_aspect('equal')
ax.grid(True, alpha=0.3)
ax.set_title(case_name, fontsize=10)
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.legend(fontsize=8)
plt.tight_layout()
plt.savefig('redundancy_comparison.png', dpi=150, bbox_inches='tight')
plt.show()
3つのパネルを比較すると、冗長自由度の活用効果が明確にわかります。
左パネル（擬似逆行列のみ）: 副目的なしの最小ノルム解です。手先は目標点（赤い星印）に到達していますが、アームの姿勢は「なりゆき」に任されています。関節角がリミット近くに偏ったり、不自然に折れ曲がった姿勢が現れる可能性があります。
中央パネル（関節リミット回避）: ヌル空間で関節角を可動範囲の中央に引き戻す力が働いています。手先の軌道は左パネルとほぼ同じ（赤い破線）ですが、アームの姿勢がより「自然」に見えるはずです。関節角が極端な値をとらず、バランスの良い姿勢を保っています。
右パネル（可操作度最大化）: ヌル空間で可操作度を最大化しています。アームが特異姿勢（リンクが一直線に伸びきる）に近づかないよう、「肘を曲げた」姿勢を積極的に維持しています。
可操作度と関節角の時系列比較
副目的の効果をより定量的に確認するため、可操作度と関節角の時間変化を比較します。
import numpy as np
import matplotlib.pyplot as plt
fig, axes = plt.subplots(2, 1, figsize=(12, 8))
# 可操作度の比較
ax = axes[0]
colors = ['tab:blue', 'tab:orange', 'tab:green']
for idx, (case_name, data) in enumerate(results.items()):
ax.plot(data['manip_history'], color=colors[idx], linewidth=2,
label=case_name)
ax.set_xlabel('Step', fontsize=12)
ax.set_ylabel('Manipulability', fontsize=12)
ax.set_title('Manipulability comparison', fontsize=14)
ax.legend(fontsize=9)
ax.grid(True, alpha=0.3)
# 関節角の偏差（可動範囲中央からの距離）の比較
ax = axes[1]
for idx, (case_name, data) in enumerate(results.items()):
q_mid = np.zeros(n_links) # 中央は0
deviations = [np.sqrt(np.mean((q_step - q_mid)**2))
for q_step in data['joint_history']]
ax.plot(deviations, color=colors[idx], linewidth=2, label=case_name)
ax.set_xlabel('Step', fontsize=12)
ax.set_ylabel('RMS deviation from center', fontsize=12)
ax.set_title('Joint angle deviation from center', fontsize=14)
ax.legend(fontsize=9)
ax.grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('metrics_comparison.png', dpi=150, bbox_inches='tight')
plt.show()
上段の可操作度グラフを見ると、3つのケースで明確な差が現れています。緑の線（可操作度最大化）は常に最も高い可操作度を維持しており、特異姿勢から積極的に離れていることがわかります。青の線（擬似逆行列のみ）は可操作度が大きく低下する区間があり、特異姿勢に接近するリスクが高いことを示しています。オレンジの線（関節リミット回避）はその中間に位置しますが、可操作度の維持を明示的に目的としていないため、状況によっては青線と近い挙動を示します。
下段の関節角偏差グラフでは、オレンジの線（関節リミット回避）のRMS偏差が最も小さく、関節角が可動範囲の中央付近に保たれていることがわかります。これに対して、青線と緑線は関節角の偏りが大きくなる傾向にあります。特に緑線は可操作度を優先するため、関節角が中央から離れることを許容しています。
このトレードオフは重要です。1つの副目的を最適化すると、他の指標は必ずしも良くならないということです。可操作度最大化を選べば特異姿勢は回避できますが、関節リミットに接近する可能性があります。逆に関節リミット回避を選べば関節は安全ですが、特異姿勢に陥る可能性があります。これが、前のセクションで述べたタスク優先度制御が必要になる理由です。
タスク優先度制御の実装
最後に、タスク優先度制御を実装して、主タスク（手先追従）と副タスク（障害物回避）を階層的に同時達成する例を示します。平面4リンクアームの近くに障害物（円柱）を配置し、手先を目標点に動かしながらリンクが障害物を避ける動きを実現します。
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Circle
def forward_kinematics(q, link_lengths):
"""平面nリンクマニピュレータの順運動学"""
n = len(q)
positions = np.zeros((n + 1, 2))
angle = 0.0
for i in range(n):
angle += q[i]
positions[i + 1, 0] = positions[i, 0] + link_lengths[i] * np.cos(angle)
positions[i + 1, 1] = positions[i, 1] + link_lengths[i] * np.sin(angle)
return positions
def compute_jacobian(q, link_lengths):
"""平面nリンクマニピュレータのヤコビ行列（2×n）"""
n = len(q)
J = np.zeros((2, n))
for i in range(n):
for k in range(i, n):
ak = np.sum(q[:k + 1])
J[0, i] -= link_lengths[k] * np.sin(ak)
J[1, i] += link_lengths[k] * np.cos(ak)
return J
def compute_link_point_jacobian(q, link_lengths, link_idx, s=0.5):
"""リンク上の点のヤコビ行列（2×n）
link_idx番目のリンクの中点（s=0.5）の位置に対するヤコビ行列
"""
n = len(q)
J = np.zeros((2, n))
for i in range(n):
if i <= link_idx:
# 関節iはlink_idx上の点に影響する
angle_sum = np.sum(q[:i + 1])
# i番目からlink_idx-1番目までのリンクの寄与
for k in range(i, link_idx):
ak = np.sum(q[:k + 1])
J[0, i] -= link_lengths[k] * np.sin(ak)
J[1, i] += link_lengths[k] * np.cos(ak)
# link_idx番目のリンクの部分的寄与（s倍）
ak = np.sum(q[:link_idx + 1])
J[0, i] -= s * link_lengths[link_idx] * np.sin(ak)
J[1, i] += s * link_lengths[link_idx] * np.cos(ak)
return J
def closest_point_on_link(p1, p2, obs_center):
"""線分p1-p2上の障害物中心への最近接点"""
d = p2 - p1
t = np.dot(obs_center - p1, d) / (np.dot(d, d) + 1e-10)
t = np.clip(t, 0, 1)
return p1 + t * d, t
# --- パラメータ ---
n_links = 4
link_lengths = np.array([1.0, 0.8, 0.6, 0.4])
q_init = np.array([0.6, 0.8, -0.4, 0.3])
# 障害物
obs_center = np.array([1.2, 0.8])
obs_radius = 0.3
# 手先の目標
pos_init_fk = forward_kinematics(q_init, link_lengths)[-1]
pos_goal = np.array([1.5, -0.5])
n_steps = 300
dt = 0.01
k_obs = 3.0 # 障害物回避ゲイン
d_threshold = 0.5 # この距離以下で障害物回避が起動
# --- シミュレーション（タスク優先度制御 vs 擬似逆行列のみ）---
fig, axes = plt.subplots(1, 2, figsize=(14, 7))
titles = ["Pseudo-inverse only", "Task priority: obstacle avoidance"]
for case_idx in range(2):
ax = axes[case_idx]
q = q_init.copy()
ee_traj = []
for step in range(n_steps):
t = (step + 1) / n_steps
x_desired = pos_init_fk + t * (pos_goal - pos_init_fk)
positions = forward_kinematics(q, link_lengths)
pos_ee = positions[-1]
x_error = x_desired - pos_ee
J1 = compute_jacobian(q, link_lengths)
J1_pinv = np.linalg.pinv(J1)
# 主タスク
dx1 = x_error / dt * 0.5
dq = J1_pinv @ dx1
if case_idx == 1:
# 障害物回避（タスク優先度制御）
N1 = np.eye(n_links) - J1_pinv @ J1
# 各リンクと障害物の距離を計算
for link_i in range(n_links):
p1 = positions[link_i]
p2 = positions[link_i + 1]
cp, s_param = closest_point_on_link(p1, p2, obs_center)
dist = np.linalg.norm(cp - obs_center) - obs_radius
if dist < d_threshold and dist > 0.01:
# 反発方向
repel_dir = (cp - obs_center)
repel_dir = repel_dir / (np.linalg.norm(repel_dir) + 1e-10)
# リンク上の点のヤコビ行列
J_obs = compute_link_point_jacobian(q, link_lengths,
link_i, s_param)
# タスク優先度: ヌル空間内で障害物から離れる
J_obs_null = J_obs @ N1
J_obs_null_pinv = np.linalg.pinv(J_obs_null)
repel_vel = k_obs * repel_dir / (dist**2)
dq += N1 @ J_obs_null_pinv @ repel_vel
# 速度制限
max_dq = 3.0
if np.max(np.abs(dq)) > max_dq:
dq = dq * max_dq / np.max(np.abs(dq))
q = q + dq * dt
ee_traj.append(forward_kinematics(q, link_lengths)[-1].copy())
# --- 描画 ---
# 障害物
circle = Circle(obs_center, obs_radius, color='red', alpha=0.4, zorder=5)
ax.add_patch(circle)
circle_border = Circle(obs_center, d_threshold, color='red',
fill=False, linestyle='--', alpha=0.3)
ax.add_patch(circle_border)
# アーム姿勢（複数フレーム）
frames = np.linspace(0, n_steps - 1, 8, dtype=int)
q_replay = q_init.copy()
all_q = [q_init.copy()]
q_replay = q_init.copy()
for step in range(n_steps):
t = (step + 1) / n_steps
x_desired = pos_init_fk + t * (pos_goal - pos_init_fk)
positions = forward_kinematics(q_replay, link_lengths)
pos_ee = positions[-1]
x_error = x_desired - pos_ee
J1 = compute_jacobian(q_replay, link_lengths)
J1_pinv = np.linalg.pinv(J1)
dx1 = x_error / dt * 0.5
dq = J1_pinv @ dx1
if case_idx == 1:
N1 = np.eye(n_links) - J1_pinv @ J1
for link_i in range(n_links):
p1 = positions[link_i]
p2 = positions[link_i + 1]
cp, s_param = closest_point_on_link(p1, p2, obs_center)
dist = np.linalg.norm(cp - obs_center) - obs_radius
if dist < d_threshold and dist > 0.01:
repel_dir = (cp - obs_center)
repel_dir = repel_dir / (np.linalg.norm(repel_dir) + 1e-10)
J_obs = compute_link_point_jacobian(q_replay, link_lengths,
link_i, s_param)
J_obs_null = J_obs @ N1
J_obs_null_pinv = np.linalg.pinv(J_obs_null)
repel_vel = k_obs * repel_dir / (dist**2)
dq += N1 @ J_obs_null_pinv @ repel_vel
max_dq = 3.0
if np.max(np.abs(dq)) > max_dq:
dq = dq * max_dq / np.max(np.abs(dq))
q_replay = q_replay + dq * dt
all_q.append(q_replay.copy())
for i, f in enumerate(frames):
alpha_val = 0.15 + 0.85 * i / (len(frames) - 1)
pos = forward_kinematics(all_q[f], link_lengths)
ax.plot(pos[:, 0], pos[:, 1], 'o-',
color=plt.cm.coolwarm(i / (len(frames) - 1)),
linewidth=2.5, markersize=5, alpha=alpha_val)
# 手先軌道
ee_arr = np.array(ee_traj)
ax.plot(ee_arr[:, 0], ee_arr[:, 1], 'g--', linewidth=1.5,
label='End-effector path')
ax.plot(pos_goal[0], pos_goal[1], 'g*', markersize=15, zorder=10)
ax.set_xlim(-1.5, 3.5)
ax.set_ylim(-2.0, 2.5)
ax.set_aspect('equal')
ax.grid(True, alpha=0.3)
ax.set_title(titles[case_idx], fontsize=13)
ax.set_xlabel('x', fontsize=12)
ax.set_ylabel('y', fontsize=12)
ax.legend(fontsize=9)
plt.tight_layout()
plt.savefig('task_priority_obstacle.png', dpi=150, bbox_inches='tight')
plt.show()
この可視化の結果は非常に印象的です。
左パネル（擬似逆行列のみ）: 手先は目標点（緑の星印）に到達していますが、途中でリンクが障害物（赤い円）を貫通している可能性があります。擬似逆行列は最小ノルム解を返すだけで、障害物の存在を一切考慮しません。
右パネル（タスク優先度制御）: 手先は同じ目標点に到達しますが、アームの姿勢が大きく異なります。障害物の近くを通過するリンクが存在すると、ヌル空間内で障害物から離れる方向に関節が自動的に調整されます。赤い破線の円（閾値距離 $d_{\text{threshold}}$）内にリンクが入ると反発力が発生し、アームが障害物を「よける」動きをとります。手先の軌道（緑の破線）自体はほぼ同じですが、腕全体の挙動は劇的に異なります。
これこそが冗長マニピュレータとタスク優先度制御の真価です。主タスク（手先追従）は完全に達成しつつ、余った自由度で安全性（障害物回避）を確保する — Canadarm2が宇宙ステーション上で日々行っていることの本質がここにあります。
数値的な注意点
実装にあたって、いくつかの数値的な注意点を補足しておきます。
特異点近傍の安定性: $\bm{J}\bm{J}^T$ が特異に近いとき、$\bm{J}^{\dagger}$ の要素が発散して数値的に不安定になります。これを防ぐためにダンピング付き擬似逆行列（Damped Least Squares, DLS）が広く使われます。
$$ \bm{J}_{\text{DLS}}^{\dagger} = \bm{J}^T(\bm{J}\bm{J}^T + \lambda^2 \bm{I}_m)^{-1} $$
ここで $\lambda > 0$ はダンピング係数です。$\lambda$ が大きいほど安定ですが、タスクの追従精度が低下するトレードオフがあります。実用上は、可操作度に応じて $\lambda$ を適応的に調整する手法（Nakamura & Hanafusa, 1986）が使われます。
import numpy as np
def damped_pseudo_inverse(J, damping=0.01):
"""ダンピング付き擬似逆行列 (DLS法)
J^†_DLS = J^T (J J^T + λ² I)^{-1}
"""
m = J.shape[0]
return J.T @ np.linalg.inv(J @ J.T + damping**2 * np.eye(m))
このダンピング付き擬似逆行列は、特異値が小さい方向の速度を抑制する効果があります。$\lambda = 0$ のとき通常の擬似逆行列に一致し、$\lambda \to \infty$ のとき $\bm{J}_{\text{DLS}}^{\dagger} \to \bm{0}$ となります。宇宙ロボットの実運用では $\lambda = 0.01 \sim 0.1$ 程度の値が典型的です。
NumPyの pinv 関数: 本記事では教育目的で $\bm{J}^T(\bm{J}\bm{J}^T)^{-1}$ を直接計算しましたが、実用上は numpy.linalg.pinv を使うことを推奨します。pinv はSVDに基づく実装であり、ランク落ちの場合でも数値的に安定です。
宇宙ロボティクスにおける冗長性の実例
ここまでの理論を踏まえて、実際の宇宙ロボットにおける冗長性の活用事例を紹介します。
Canadarm2（SSRMS）
ISSのCanadarm2は長さ17.6m、7自由度のロボットアームです。7つの関節は肩（3DOF）、肘（1DOF）、手首（3DOF）で構成され、最大116,000kgのペイロードを操作できます。
Canadarm2の特徴的な機能はインチワーム運動（inchworm locomotion）です。両端にエンドエフェクタを持ち、尺取虫のようにISSの表面を「歩いて」移動できます。この移動中も7自由度の冗長性を活かして、太陽電池パドルやラジエータなどの障害物を回避しています。
運用時の典型的なタスク優先度は以下の通りです。
- 最高優先度: ペイロードの安全（衝突回避）
- 高優先度: 手先の位置・姿勢制御
- 中優先度: 特異姿勢回避
- 低優先度: 関節リミット回避・消費電力最小化
ETS-VII（おりひめ・ひこぼし）
JAXAが1997年に打ち上げた技術試験衛星ETS-VIIは、世界初の軌道上ロボット実験を行いました。6自由度のロボットアームを搭載し、非冗長な構成でしたが、この実験の知見が後のJEMRMS（きぼうアーム）やHTV把持技術に活かされています。
ETS-VIIの経験から、宇宙ロボットには冗長性が不可欠であることが再認識されました。6自由度では手先の位置・姿勢を指定すると解がほぼ一意に決まり、障害物回避や特異姿勢回避の余地がなかったためです。
次世代宇宙ロボット
現在開発中の軌道上サービス・製造（OSAM）ミッションでは、さらに高い冗長性を持つロボットが計画されています。
- 双腕ロボット: 2本の7DOFアームを協調制御（合計14DOF）
- テンタクル型アーム: 超冗長（10DOF以上）の蛇型ロボットで、狭い空間への進入
- 自由飛行ロボット: アーム+母船の6DOFで合計13DOF以上の冗長システム
これらの高冗長ロボットでは、本記事で学んだヌル空間射影やタスク優先度制御がさらに重要な役割を果たします。
理論と実装の両面から冗長マニピュレータの制御を見てきました。最後に、学んだ内容を整理しましょう。
まとめ
本記事では、冗長自由度マニピュレータの制御手法を擬似逆行列とヌル空間射影を中心に解説しました。
- 冗長性の定義: 関節数 $n$ がタスク空間次元 $m$ より大きいとき、冗長度 $r = n – m$ の自由度が余る。この余った自由度で副目的を達成できる
- 擬似逆行列 $\bm{J}^{\dagger} = \bm{J}^T(\bm{J}\bm{J}^T)^{-1}$: 劣決定系の最小ノルム解を与える。ラグランジュ未定乗数法から導出できる
- ヌル空間射影: $(\bm{I} – \bm{J}^{\dagger}\bm{J})\bm{z}$ は手先運動に影響を与えない関節速度。$\bm{z}$ に副目的の勾配を設定する勾配射影法で、関節リミット回避・特異姿勢回避・障害物回避を達成
- 重み付き擬似逆行列: 関節ごとに動かしやすさの重みを設定。$\bm{J}_W^{\dagger} = \bm{W}^{-1}\bm{J}^T(\bm{J}\bm{W}^{-1}\bm{J}^T)^{-1}$
- タスク優先度制御: 複数タスクに階層的な優先順位を設定。高優先度タスクのヌル空間内でのみ低優先度タスクを遂行
- 宇宙ロボットでの実践: Canadarm2（7DOF）はこれらの手法を駆使して、障害物が密集するISS上で安全に作業を行っている
冗長マニピュレータの制御は、逆運動学の「解の不定性」を弱点ではなく強みに変える発想の転換です。宇宙空間のような厳しい環境では、この余分な自由度が安全性と柔軟性の決定的な差を生みます。
次の記事では、マニピュレータの作業空間解析と到達可能性について解説します。冗長性がどれだけ作業空間を拡大するか、到達可能領域の計算方法とその可視化を学びます。
参考文献
- Siciliano, B., & Slotine, J. J. E. (1991). A general framework for managing multiple tasks in highly redundant robotic systems. ICAR, 1211-1216.
- Nakamura, Y., & Hanafusa, H. (1987). Optimal redundancy control of robot manipulators. The International Journal of Robotics Research, 5(1), 32-42.
- 吉川恒夫 (1985). マニピュレータの可操作度. 日本ロボット学会誌, 3(5), 396-403.
- Maciejewski, A. A., & Klein, C. A. (1985). Obstacle avoidance for kinematically redundant manipulators in dynamically varying environments. The International Journal of Robotics Research, 4(3), 109-117.