地球上のロボットアームは、床にボルトで固定されています。関節を動かしても、ロボットの足元はびくともしません。だからこそ、関節速度と手先速度の関係はヤコビ行列という1つの行列ですっきり書けます。
ところが宇宙空間では状況が一変します。国際宇宙ステーション（ISS）のロボットアーム「Canadarm2」がペイロードを掴んで動くと、ISS本体がわずかに逆方向へ回転します。宇宙では「固定された壁」が存在しないため、アームを動かすとベース衛星が反動で動くのです。つまり、関節を $\dot{\bm{\phi}}$ で回しても、手先速度は地上のヤコビ行列 $\bm{J}$ が予測する $\bm{J}\dot{\bm{\phi}}$ とは異なる値になります。
この問題を根本から解決するのが、梅谷陽二（Yoji Umetani）と吉田和哉（Kazuya Yoshida）によって1989年に提案された一般化ヤコビ行列（Generalized Jacobian Matrix, GJM）です。GJMは、運動量保存則によるベースの反動を組み込んだ「真の速度写像」を与えます。
GJMを理解すると、以下のような宇宙ロボティクスの核心的な問題に取り組めるようになります。
- 宇宙マニピュレータの手先制御: ベースの反動を正しく補償した逆運動学の設計
- 動的特異姿勢の予測: 通常のヤコビ行列が正則でも、GJMが特異になる危険な姿勢の検出
- 軌道上サービス: デブリ捕獲や衛星修理での精密マニピュレーション計画
- リアクションレスマニューバ: ベースに反動を与えずにアームを動かす特殊な制御
本記事の内容
- 地上ロボットと宇宙ロボットの違い — なぜ通常のヤコビ行列では不十分か
- 運動量保存を組み込んだGJMの数学的導出
- GJMの構造と通常ヤコビ行列との関係
- 動的特異姿勢（dynamically singular configuration）の概念
- 質量比（ベース質量 vs アーム質量）の影響
- Pythonで通常ヤコビ vs GJMの比較シミュレーション
前提知識
この記事を読む前に、以下の記事を読んでおくと理解が深まります。
- ヤコビ行列と微分運動学 — 地上ロボットにおけるヤコビ行列の定義と逆運動学
- 自由浮遊ロボットの運動量保存と動的カップリング — 宇宙ロボットの運動量保存則とベース・アーム間の動的結合
通常のヤコビ行列が宇宙では通用しない理由
地上ロボットのヤコビ行列
まず、地上のロボットアームでヤコビ行列がどのように定義されるかを振り返りましょう。
$n$ 自由度のロボットアームの関節角度ベクトルを $\bm{\phi} = (\phi_1, \phi_2, \ldots, \phi_n)^T$ とし、手先位置・姿勢を $\bm{x}_e \in \mathbb{R}^6$（位置3成分 + 姿勢3成分）とします。ロボットのベースが固定されているとき、手先の速度 $\dot{\bm{x}}_e$ は関節速度 $\dot{\bm{\phi}}$ を使って
$$ \dot{\bm{x}}_e = \bm{J}_m(\bm{\phi}) \dot{\bm{\phi}} $$
と書けます。ここで $\bm{J}_m \in \mathbb{R}^{6 \times n}$ はマニピュレータヤコビ行列です。「ベースが固定されている」という前提があるからこそ、手先速度は関節速度だけの関数になります。
地上ロボットでは、ベースが床にボルト止めされているため、関節をどう動かしてもベースの位置・姿勢は $\dot{\bm{x}}_0 = \bm{0}$ です。だから $\bm{J}_m$ だけで手先の運動を完全に記述できます。
宇宙ロボットの困難
宇宙空間に浮かぶ衛星（ベース）にロボットアームが取り付けられた系を考えましょう。外力・外トルクがゼロの自由浮遊（free-floating）状態では、系全体の運動量 $\bm{P}$ と角運動量 $\bm{L}$ が保存されます。初期に系が静止していれば
$$ \bm{P} = \bm{0}, \quad \bm{L} = \bm{0} $$
が常に成り立ちます。
ここで、アームの関節を動かしたとしましょう。関節が動くとアーム各リンクの運動量が変化しますが、系全体の運動量はゼロのまま保存されなければなりません。そのため、ベースが反動で動いてアームの運動量を打ち消します。これはニュートンの第3法則そのものです。
この状況では、手先速度は関節速度だけでは決まりません。ベースの速度 $\dot{\bm{x}}_0$（並進3成分 + 回転3成分）も手先の運動に影響します。完全な速度関係は
$$ \dot{\bm{x}}_e = \bm{J}_0 \dot{\bm{x}}_0 + \bm{J}_m \dot{\bm{\phi}} $$
という形になります。ここで $\bm{J}_0 \in \mathbb{R}^{6 \times 6}$ はベース速度が手先速度に与える影響を記述する行列です。
地上ロボットでは $\dot{\bm{x}}_0 = \bm{0}$ なので $\bm{J}_0$ の項が消えていたのです。宇宙ロボットでは $\dot{\bm{x}}_0 \neq \bm{0}$ であり、しかもその値は運動量保存則を通じて $\dot{\bm{\phi}}$ に依存します。つまり、$\dot{\bm{x}}_0$ は独立変数ではなく、$\dot{\bm{\phi}}$ の関数なのです。
地上のヤコビ行列 $\bm{J}_m$ をそのまま使って $\dot{\bm{x}}_e = \bm{J}_m \dot{\bm{\phi}}$ として手先制御を設計すると、ベースの反動による「余分な」手先運動を無視することになります。結果として、手先は目標軌道からずれ、最悪の場合にはターゲット衛星に衝突するような事態も起こり得ます。
この問題を解決するには、運動量保存則でベース速度 $\dot{\bm{x}}_0$ を $\dot{\bm{\phi}}$ に結びつけ、全てを関節速度だけで表現した「真のヤコビ行列」が必要です。それこそがGJMです。
では、GJMを導出するために、まず宇宙ロボットの運動量保存則を数式で書き下しましょう。
宇宙ロボットの運動量保存則
モデルの設定
ベース衛星（リンク0）に $n$ 自由度のシリアルマニピュレータ（リンク1〜$n$）が接続された系を考えます。各リンク $i$ の質量を $m_i$、慣性モーメントテンソルを $\bm{I}_i$、重心位置を $\bm{r}_i$、重心速度を $\dot{\bm{r}}_i$、角速度を $\bm{\omega}_i$ とします。
系全体の運動量と角運動量は次のように書けます。
$$ \bm{P} = \sum_{i=0}^{n} m_i \dot{\bm{r}}_i $$
$$ \bm{L} = \sum_{i=0}^{n} \left[ m_i (\bm{r}_i – \bm{r}_G) \times \dot{\bm{r}}_i + \bm{I}_i \bm{\omega}_i \right] $$
ここで $\bm{r}_G$ は系全体の重心位置です。外力がゼロなので $\bm{P} = \bm{0}$、$\bm{L} = \bm{0}$（初期静止の場合）が保たれます。
速度の関係式
各リンクの重心速度 $\dot{\bm{r}}_i$ と角速度 $\bm{\omega}_i$ は、ベースの速度 $\dot{\bm{x}}_0 = (\dot{\bm{r}}_0^T, \bm{\omega}_0^T)^T$ と関節速度 $\dot{\bm{\phi}}$ の線形結合として表せます。これはロボットの運動学の基本的な性質です。
$$ \begin{pmatrix} \dot{\bm{r}}_i \\ \bm{\omega}_i \end{pmatrix} = \bm{J}_{0,i} \dot{\bm{x}}_0 + \bm{J}_{m,i} \dot{\bm{\phi}} $$
ここで $\bm{J}_{0,i}$ はベース速度からリンク $i$ の速度への写像、$\bm{J}_{m,i}$ は関節速度からリンク $i$ の速度への写像です。
運動量の行列表現
運動量と角運動量の保存則を行列形式にまとめましょう。6次元の一般化運動量ベクトル $\bm{h} = (\bm{P}^T, \bm{L}^T)^T$ は
$$ \bm{h} = \bm{H}_0 \dot{\bm{x}}_0 + \bm{H}_m \dot{\bm{\phi}} = \bm{0} $$
と書けます。ここで
- $\bm{H}_0 \in \mathbb{R}^{6 \times 6}$: ベース速度に対する慣性行列（ベースの慣性 + ベース運動に伴う全リンクの運動量変化）
- $\bm{H}_m \in \mathbb{R}^{6 \times n}$: 関節速度に対する運動量カップリング行列
です。$\bm{H}_0$ は系全体の慣性を反映する正定値対称行列であり、常に正則（逆行列が存在）です。
$\bm{H}_0$ と $\bm{H}_m$ の具体的な表現をもう少し詳しく見てみましょう。$\bm{H}_0$ は次の構造を持ちます。
$$ \bm{H}_0 = \begin{pmatrix} M \bm{E}_3 & -M \bm{s}_0^{\times} \\ M \bm{s}_0^{\times} & \bm{I}_{\text{total}} \end{pmatrix} $$
ここで $M = \sum_{i=0}^{n} m_i$ は系の全質量、$\bm{E}_3$ は $3 \times 3$ の単位行列、$\bm{s}_0 = \bm{r}_G – \bm{r}_0$ はベース重心から系全体の重心への位置ベクトル、$\bm{s}_0^{\times}$ はその外積行列（skew-symmetric matrix）、$\bm{I}_{\text{total}}$ はベース原点まわりの系全体の慣性テンソルです。
$\bm{H}_m$ は関節運動がどれだけ系全体の運動量を生成するかを表すカップリング行列で、各リンクの質量・慣性・リンク配置に依存します。
$$ \bm{H}_m = \sum_{i=1}^{n} \begin{pmatrix} m_i \bm{J}_{v,m,i} \\ m_i (\bm{r}_i – \bm{r}_0)^{\times} \bm{J}_{v,m,i} + \bm{I}_i \bm{J}_{\omega,m,i} \end{pmatrix} $$
ここで $\bm{J}_{v,m,i}$ と $\bm{J}_{\omega,m,i}$ はそれぞれ関節速度からリンク $i$ の並進速度・角速度への寄与部分です。
これらの行列を使えば、運動量保存則からベースの速度を関節速度で表現できます。これこそがGJMの導出の鍵です。
一般化ヤコビ行列（GJM）の導出
ベース速度の消去
運動量保存則 $\bm{H}_0 \dot{\bm{x}}_0 + \bm{H}_m \dot{\bm{\phi}} = \bm{0}$ を $\dot{\bm{x}}_0$ について解きます。$\bm{H}_0$ は正定値で正則なので
$$ \dot{\bm{x}}_0 = -\bm{H}_0^{-1} \bm{H}_m \dot{\bm{\phi}} $$
が得られます。この式は重要な物理的意味を持っています。関節を動かすと、運動量保存を満たすように、ベースが自動的に反対方向に動くことを表しています。$-\bm{H}_0^{-1}\bm{H}_m$ は、関節運動がベースにどれだけの反動を引き起こすかを記述する「動的カップリング行列」です。
手先速度への代入
手先（エンドエフェクタ）の速度は
$$ \dot{\bm{x}}_e = \bm{J}_0 \dot{\bm{x}}_0 + \bm{J}_m \dot{\bm{\phi}} $$
で与えられます。ここに先ほど求めた $\dot{\bm{x}}_0 = -\bm{H}_0^{-1} \bm{H}_m \dot{\bm{\phi}}$ を代入すると
$$ \dot{\bm{x}}_e = \bm{J}_0 \left( -\bm{H}_0^{-1} \bm{H}_m \dot{\bm{\phi}} \right) + \bm{J}_m \dot{\bm{\phi}} $$
$\dot{\bm{\phi}}$ でくくり出すと
$$ \dot{\bm{x}}_e = \left( \bm{J}_m – \bm{J}_0 \bm{H}_0^{-1} \bm{H}_m \right) \dot{\bm{\phi}} $$
GJMの定義
ここで括弧内をまとめて $\bm{J}^*$ と定義します。
$$ \boxed{\bm{J}^* = \bm{J}_m – \bm{J}_0 \bm{H}_0^{-1} \bm{H}_m} $$
これが一般化ヤコビ行列（Generalized Jacobian Matrix, GJM）です。手先速度は
$$ \dot{\bm{x}}_e = \bm{J}^* \dot{\bm{\phi}} $$
と、地上ロボットと同じ形式で — ただし $\bm{J}_m$ の代わりに $\bm{J}^*$ を使って — 書けます。
この導出のポイントを整理しましょう。
- 出発点は、手先速度 = ベース速度の寄与 + 関節速度の寄与
- 運動量保存則でベース速度を関節速度の関数として表す
- 手先速度の式に代入してベース速度を消去する
- 残ったのは関節速度 $\dot{\bm{\phi}}$ だけの式 — その係数行列がGJM
GJMは、運動量保存によるベースの反動を織り込んだ「真の」速度写像です。
GJMの構造が見えたところで、次にその物理的・数学的な性質をもう少し詳しく掘り下げてみましょう。
GJMの構造と通常ヤコビ行列との関係
補正項の意味
GJMの式 $\bm{J}^* = \bm{J}_m – \bm{J}_0 \bm{H}_0^{-1} \bm{H}_m$ を「通常のヤコビ行列 + 補正」の形で理解しましょう。
第1項の $\bm{J}_m$ は、ベースが固定されていると仮定したときの通常のマニピュレータヤコビ行列です。第2項の $\bm{J}_0 \bm{H}_0^{-1} \bm{H}_m$ は、ベースの反動による補正項です。
補正項を分解して読み取ると、
- $\bm{H}_m \dot{\bm{\phi}}$: 関節運動が系に与える運動量の変化
- $\bm{H}_0^{-1}(\cdot)$: その運動量変化を打ち消すためにベースが持つべき速度
- $\bm{J}_0(\cdot)$: そのベース速度が手先に与える影響
という因果の連鎖を表しています。つまり「関節を動かす → 運動量が変化する → ベースが反動で動く → ベースの動きが手先に伝わる」という一連の動的結合が、$\bm{J}_0 \bm{H}_0^{-1} \bm{H}_m$ に凝縮されているのです。
質量比による振る舞い
GJMの補正項は、ベースとアームの質量比に強く依存します。いくつかの極限を考えてみましょう。
ベースが非常に重い場合 ($m_0 \to \infty$):
ベースの質量が無限大に近づくと、$\bm{H}_0$ の成分が巨大になります。特に並進成分について、全質量 $M \approx m_0$ が支配的になるため、$\bm{H}_0^{-1}$ の成分はゼロに近づきます。その結果
$$ \bm{H}_0^{-1} \bm{H}_m \to \bm{0} $$
となり、補正項が消えて
$$ \bm{J}^* \to \bm{J}_m $$
が成立します。これは直感に合います。ベースが非常に重ければ、アームを動かしてもベースはほとんど動きません。地上の固定ベースと同じ状況になるので、通常のヤコビ行列で十分なのです。
ベースとアームの質量が同程度の場合:
補正項が無視できなくなり、$\bm{J}^*$ と $\bm{J}_m$ の差が顕著になります。宇宙ロボットの多くはこのケースに該当します。たとえば、ETS-VII（技術試験衛星VII型）では、衛星本体（約2.5トン）に対してロボットアーム（約100kg）という質量比でした。この場合でもベースの反動は無視できず、精密な手先制御にはGJMが必要です。
ベースが非常に軽い場合 ($m_0 \to 0$):
補正項が最大になり、$\bm{J}^*$ と $\bm{J}_m$ の差が最も大きくなります。極端な場合、ベースがアームの動きに引きずられて大きく動くため、手先は関節を動かした方向とは全く異なる方向に動くこともあり得ます。
GJMの姿勢依存性
通常のヤコビ行列 $\bm{J}_m$ は関節角度 $\bm{\phi}$ にのみ依存しますが、GJMは $\bm{H}_0$、$\bm{H}_m$、$\bm{J}_0$ を通じてベースの姿勢やアームの配置全体に依存します。これは、ベースの反動がアームの姿勢に依存するためです。
具体的には、$\bm{H}_0$ と $\bm{H}_m$ はリンクの慣性特性と配置から決まるため、同じ関節角度 $\bm{\phi}$ でもベースの姿勢が異なれば $\bm{J}^*$ の値も変わります。ただし、自由浮遊の場合はベースの姿勢自体が運動量保存則で $\bm{\phi}$ に拘束されるため、実質的には $\bm{\phi}$ のみの関数として扱えます。
GJMが通常のヤコビ行列とどう異なるかを理解したところで、次に宇宙ロボット特有の重要な概念 — 動的特異姿勢 — について見ていきましょう。
動的特異姿勢（Dynamically Singular Configuration）
特異姿勢とは何か
ロボティクスでは、ヤコビ行列のランクが落ちる（行列式がゼロになる）姿勢を特異姿勢（singular configuration）と呼びます。特異姿勢では手先が特定の方向に動けなくなり、逆運動学（手先速度から関節速度を求める計算）が不可能になります。
地上ロボットでは、ヤコビ行列 $\bm{J}_m$ の特異点は運動学的特異姿勢（kinematic singularity）と呼ばれ、アームが完全に伸びきった姿勢や関節軸が一直線に並んだ姿勢で発生します。これらは純粋に幾何学的な問題であり、リンクの長さと関節の配置だけから決まります。
宇宙ロボット特有の特異姿勢
宇宙マニピュレータでは、制御に使うのは $\bm{J}_m$ ではなく $\bm{J}^*$ です。したがって、$\bm{J}^*$ のランクが落ちる姿勢が問題になります。ここで驚くべきことが起こります。
$\bm{J}^*$ が特異になるのは、$\bm{J}_m$ が特異な場合だけではありません。$\bm{J}_m$ が完全に正則であっても、補正項 $\bm{J}_0 \bm{H}_0^{-1} \bm{H}_m$ の影響で $\bm{J}^*$ が特異になることがあるのです。
この宇宙ロボット特有の特異姿勢を動的特異姿勢（dynamically singular configuration, DSC）と呼びます。
動的特異姿勢の物理的解釈
動的特異姿勢の物理的な意味を考えてみましょう。通常のヤコビ行列は正則なので、「ベースが固定されていれば」手先は全方向に動けます。しかし、実際にはベースが反動で動くため、手先が特定の方向に動けなくなるのです。
イメージとしては、こういうことです。手先をある方向に動かそうとして関節を駆動すると、運動量保存によりベースが反動で動きます。そのベースの動きがちょうど手先の運動を打ち消してしまう — そのような姿勢が動的特異姿勢です。
数学的には、$\bm{J}_m$ の列空間と $\bm{J}_0 \bm{H}_0^{-1} \bm{H}_m$ の列空間の関係で決まります。両者の特定の成分が打ち消し合うとき、$\bm{J}^*$ のランクが $\bm{J}_m$ よりも低くなります。
動的特異姿勢の条件
$n = 6$（6自由度アーム）の場合を考えると、$\bm{J}^*$ は $6 \times 6$ の正方行列になります。$\bm{J}^*$ が特異であるための条件は
$$ \det(\bm{J}^*) = \det\left(\bm{J}_m – \bm{J}_0 \bm{H}_0^{-1} \bm{H}_m\right) = 0 $$
です。この行列式を展開すると、質量分布やリンク配置に依存した複雑な非線形条件が得られます。
重要なのは、動的特異姿勢は質量比に依存するということです。同じアーム姿勢でも、ベース質量を変えれば動的特異姿勢の位置が変わります。ベースが十分重い場合は補正項が小さくなるため、動的特異姿勢は通常の運動学的特異姿勢に近づきます。一方、ベースが軽い場合は動的特異姿勢が通常の特異姿勢とは全く異なる場所に現れる可能性があります。
特異姿勢の分類
宇宙マニピュレータの特異姿勢は、以下のように分類できます。
| 種類 | 条件 | 原因 |
|---|---|---|
| 運動学的特異姿勢 | $\det(\bm{J}_m) = 0$ | アームの幾何学的配置 |
| 動的特異姿勢 | $\det(\bm{J}_m) \neq 0$ かつ $\det(\bm{J}^*) = 0$ | ベースの反動による打ち消し |
地上ロボットでは前者だけを心配すればよいのですが、宇宙ロボットでは両方を考慮する必要があります。ミッション計画段階で、作業中にこれらの特異姿勢を通過しないような軌道を設計することが重要です。
動的特異姿勢という落とし穴を理解したところで、次にGJMを使った逆運動学について考えましょう。
GJMを使った逆運動学
逆運動学問題
ロボットの手先制御では、所望の手先速度 $\dot{\bm{x}}_e^{\text{des}}$ を実現する関節速度 $\dot{\bm{\phi}}$ を求める逆運動学が必要です。
地上ロボットでは
$$ \dot{\bm{\phi}} = \bm{J}_m^{-1} \dot{\bm{x}}_e^{\text{des}} $$
（$\bm{J}_m$ が正方で正則な場合）または $\bm{J}_m$ の疑似逆行列を使います。
GJMによる逆運動学
宇宙マニピュレータでは、$\bm{J}_m$ の代わりに $\bm{J}^*$ を使います。
$$ \dot{\bm{\phi}} = (\bm{J}^*)^{-1} \dot{\bm{x}}_e^{\text{des}} $$
この式で求めた $\dot{\bm{\phi}}$ で関節を駆動すると、運動量保存によるベースの反動が自動的に考慮されるため、手先は正しく所望の速度 $\dot{\bm{x}}_e^{\text{des}}$ で動きます。
ここがGJMの最大の利点です。ベースの動きを明示的にフィードバックする必要はありません。GJMが運動量保存を「内包」しているため、あたかも地上ロボットのように逆運動学を解けば、宇宙ロボットでも正しい結果が得られるのです。
冗長マニピュレータの場合
宇宙マニピュレータが7自由度以上の冗長系である場合（$n > 6$）、GJMは $6 \times n$ の非正方行列になります。この場合、疑似逆行列を用いて
$$ \dot{\bm{\phi}} = (\bm{J}^*)^{\dagger} \dot{\bm{x}}_e^{\text{des}} + \left(\bm{E}_n – (\bm{J}^*)^{\dagger} \bm{J}^*\right) \bm{z} $$
とします。ここで $(\bm{J}^*)^{\dagger}$ は $\bm{J}^*$ のMoore-Penrose疑似逆行列、$\bm{E}_n$ は $n$ 次の単位行列、$\bm{z} \in \mathbb{R}^n$ は任意のベクトルです。
第2項 $\left(\bm{E}_n – (\bm{J}^*)^{\dagger} \bm{J}^*\right) \bm{z}$ は $\bm{J}^*$ の零空間への射影であり、手先速度に影響を与えずに関節を動かす自由度です。この自由度を使って、特異姿勢の回避、関節リミットの回避、ベースの姿勢変化の最小化など、副次的な目標を達成できます。
実装上の注意
GJMを使った逆運動学の実装では、いくつかの注意点があります。
-
GJMのリアルタイム計算: $\bm{H}_0$、$\bm{H}_m$ は姿勢に依存するため、毎制御周期で更新する必要があります。$\bm{H}_0^{-1}$ の計算も含め、計算コストが地上ロボットより高くなります。
-
特異姿勢の近傍: $\det(\bm{J}^*)$ が小さい場合、$(\bm{J}^*)^{-1}$ の成分が大きくなり、計算結果の関節速度が発散します。このような場合にはダンピング付き疑似逆行列（Damped Least Squares, DLS）
$$ \dot{\bm{\phi}} = (\bm{J}^*)^T \left( \bm{J}^* (\bm{J}^*)^T + \lambda^2 \bm{E}_6 \right)^{-1} \dot{\bm{x}}_e^{\text{des}} $$
を使うことで、精度は若干犠牲にしつつも安定した解が得られます。$\lambda$ はダンピング係数で、特異値の最小値に応じて動的に調整するのが一般的です。
- 角運動量の累積誤差: 実際の宇宙環境では、大気抵抗（低軌道）、重力傾斜トルク、磁気トルクなどの外乱により、運動量が厳密にゼロに保たれない場合があります。この場合、GJMの仮定（$\bm{P} = \bm{0}$, $\bm{L} = \bm{0}$）からのずれを適宜補正する必要があります。
理論と逆運動学の枠組みが整ったところで、次に具体的な計算モデルを使ってGJMの振る舞いをPythonで確認しましょう。
平面2リンク宇宙マニピュレータのGJM — 具体的計算
モデル設定
GJMの効果を最も明瞭に見るため、平面（2次元）のモデルを使います。ベース衛星（リンク0）に2リンクのロボットアーム（リンク1, 2）が接続された系を考えます。
パラメータを以下のように設定します。
| パラメータ | 記号 | 説明 |
|---|---|---|
| ベース質量 | $m_0$ | ベース衛星の質量 |
| リンク $i$ の質量 | $m_i$ | アーム第 $i$ リンクの質量 |
| リンク $i$ の長さ | $l_i$ | アーム第 $i$ リンクの長さ |
| リンク $i$ の重心位置 | $a_i$ | 関節から重心までの距離 |
| ベースの慣性モーメント | $I_0$ | ベースの回転慣性 |
| リンク $i$ の慣性モーメント | $I_i$ | リンク $i$ の重心まわりの慣性モーメント |
| ベースの姿勢角 | $\theta_0$ | ベースの回転角度 |
| 関節角度 | $\phi_1, \phi_2$ | 各関節の相対角度 |
平面の場合、並進は2成分 $(x, y)$、回転は1成分 $\theta$ なので、運動量保存則は3次元（$P_x = 0$, $P_y = 0$, $L = 0$）です。
各リンクの絶対角度
リンクの絶対角度（慣性系に対する角度）を定義します。
$$ \theta_1 = \theta_0 + \phi_1, \quad \theta_2 = \theta_0 + \phi_1 + \phi_2 $$
各リンクの重心位置
ベース重心位置を $(x_0, y_0)$ とすると、ベースの肩関節の位置は（簡単のためベース重心と肩関節が一致するとします）
$$ \bm{r}_{\text{shoulder}} = (x_0, y_0) $$
リンク1の重心位置は
$$ \bm{r}_1 = \bm{r}_{\text{shoulder}} + a_1 \begin{pmatrix} \cos\theta_1 \\ \sin\theta_1 \end{pmatrix} $$
リンク1の先端（= リンク2の関節）の位置は
$$ \bm{r}_{\text{elbow}} = \bm{r}_{\text{shoulder}} + l_1 \begin{pmatrix} \cos\theta_1 \\ \sin\theta_1 \end{pmatrix} $$
リンク2の重心位置は
$$ \bm{r}_2 = \bm{r}_{\text{elbow}} + a_2 \begin{pmatrix} \cos\theta_2 \\ \sin\theta_2 \end{pmatrix} $$
手先（エンドエフェクタ）の位置は
$$ \bm{r}_e = \bm{r}_{\text{elbow}} + l_2 \begin{pmatrix} \cos\theta_2 \\ \sin\theta_2 \end{pmatrix} $$
角運動量保存（平面の場合）
平面の場合、注目すべきは角運動量の保存です。系全体の重心まわりの角運動量 $L$ は
$$ L = I_0 \dot{\theta}_0 + I_1 \dot{\theta}_1 + I_2 \dot{\theta}_2 + m_1 \bm{r}_{1G} \times \dot{\bm{r}}_{1G} + m_2 \bm{r}_{2G} \times \dot{\bm{r}}_{2G} + m_0 \bm{r}_{0G} \times \dot{\bm{r}}_{0G} = 0 $$
ここで $\bm{r}_{iG} = \bm{r}_i – \bm{r}_G$ は重心からの相対位置です。
並進運動量 $\bm{P} = \bm{0}$ は系全体の重心が静止していることを意味するので、重心を原点に固定して考えることができます。これにより、ベースの並進は系全体の重心位置を保存するように自動的に決まります。
角運動量保存則を関節速度で表すために、各リンクの角速度を確認しましょう。
$$ \dot{\theta}_1 = \dot{\theta}_0 + \dot{\phi}_1, \quad \dot{\theta}_2 = \dot{\theta}_0 + \dot{\phi}_1 + \dot{\phi}_2 $$
ベース重心の位置は、系全体の重心が固定（原点）であることから
$$ M \bm{r}_G = m_0 \bm{r}_0 + m_1 \bm{r}_1 + m_2 \bm{r}_2 = \bm{0} $$
より $\bm{r}_0 = -\frac{1}{m_0}(m_1 \bm{r}_1 + m_2 \bm{r}_2)$ と決まります。
角運動量保存則を整理すると、最終的に
$$ L = H_0 \dot{\theta}_0 + H_1 \dot{\phi}_1 + H_2 \dot{\phi}_2 = 0 $$
という形になります。ここで
$$ H_0 = I_0 + I_1 + I_2 + m_1 a_1^2 + m_2(l_1^2 + a_2^2 + 2l_1 a_2 \cos\phi_2) + \frac{(m_1 a_1 + m_2 l_1)^2 + m_2^2 a_2^2 + 2m_2 a_2(m_1 a_1 + m_2 l_1)\cos\phi_2}{M} \cdot \frac{M – m_0}{m_0} $$
は複雑ですが、構造的には以下のように簡略化して理解できます。
簡明のため、以下では行列形式で書き直します。角運動量保存から $\dot{\theta}_0$ を消去すると
$$ \dot{\theta}_0 = -\frac{H_1 \dot{\phi}_1 + H_2 \dot{\phi}_2}{H_0} $$
が得られます。$H_0 > 0$（常に正）なので、この式は常に解けます。
手先速度とGJM
手先速度 $\dot{\bm{r}}_e = (\dot{x}_e, \dot{y}_e)^T$ を $\dot{\phi}_1, \dot{\phi}_2$ で表すのがGJMです。平面2リンクの場合、手先の位置を時間微分して、$\dot{\theta}_0$ を運動量保存則で消去すると
$$ \dot{\bm{r}}_e = \bm{J}^* \begin{pmatrix} \dot{\phi}_1 \\ \dot{\phi}_2 \end{pmatrix} $$
が得られます。$\bm{J}^*$ は $2 \times 2$ 行列です。
具体的な式は複雑になりますが、構造は
$$ \bm{J}^* = \bm{J}_m – \bm{J}_0 \frac{\bm{H}_m}{H_0} $$
（平面では角運動量のスカラー保存なので $\bm{H}_0^{-1}$ がスカラー $1/H_0$ になる）です。
これを具体的にPythonで計算して、通常のヤコビ行列との違いを可視化してみましょう。
Pythonシミュレーション: 通常ヤコビ vs GJM
シミュレーションの目的
ここでは、平面2リンク宇宙マニピュレータについて、以下の3点を確認します。
- 通常ヤコビとGJMの違い: 同じ関節速度を与えたとき、予測される手先速度がどう異なるか
- 質量比の影響: ベース質量を変えたとき、GJMがどう変化するか
- 動的特異姿勢: 通常ヤコビが正則でもGJMが特異になる姿勢を可視化
まず、モデルのパラメータ設定とGJMの計算関数を実装します。
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import FancyArrowPatch
# --- パラメータ ---
# リンク長
l1, l2 = 1.0, 0.8
# 重心位置（関節からの距離）
a1, a2 = l1 / 2, l2 / 2
# リンク質量
m1, m2 = 2.0, 1.5
# リンク慣性モーメント（重心まわり、一様棒: ml^2/12）
I1 = m1 * l1**2 / 12
I2 = m2 * l2**2 / 12
def compute_jacobians(phi1, phi2, theta0, m0, I0):
"""通常ヤコビ行列とGJMを計算する"""
M = m0 + m1 + m2 # 全質量
# 絶対角度
th1 = theta0 + phi1
th2 = theta0 + phi1 + phi2
# --- 通常のマニピュレータヤコビ行列 Jm ---
# ベースが固定されていると仮定したときの手先速度
# dr_e / dphi1, dr_e / dphi2
Jm = np.array([
[-l1 * np.sin(th1) - l2 * np.sin(th2), -l2 * np.sin(th2)],
[ l1 * np.cos(th1) + l2 * np.cos(th2), l2 * np.cos(th2)]
])
# --- ベースヤコビ行列 J0 ---
# ベース回転がエンドエフェクタに与える影響
# ベースの回転 dtheta0 による手先速度
# r_e = r_shoulder + l1*(cos th1, sin th1) + l2*(cos th2, sin th2)
# r_shoulder は重心保存から決まるが、
# ベースの回転成分のみ考慮すると
J0_rot = np.array([
[-l1 * np.sin(th1) - l2 * np.sin(th2)],
[ l1 * np.cos(th1) + l2 * np.cos(th2)]
])
# --- 角運動量保存のための慣性係数 ---
# L = H0 * dtheta0 + Hm @ dphi = 0
# H0: ベース回転に対する有効慣性（系全体）
# 重心位置からの各リンク重心の距離の2乗の寄与を含む
# 簡略化: 系全体の重心まわりの慣性
c2 = np.cos(phi2)
# ベースが回転するとき、全てのリンクが追従して回転する
# 各リンクの角運動量への寄与
# アーム部分の等価慣性
Ia_total = I1 + I2 + m1 * a1**2 + m2 * (l1**2 + a2**2 + 2 * l1 * a2 * c2)
# 系全体の等価慣性（ベース回転に対する）
# ベースの回転で、各リンク重心が公転する分も含む
# 重心保存により、ベースの並進は他のリンクの運動で自動的に決まる
# 簡略化のため、系全体の重心まわりの角運動量で計算
d1 = m1 * a1 + m2 * l1 # リンク1方向のモーメントアーム成分
d2 = m2 * a2 # リンク2方向のモーメントアーム成分
H0 = (I0 + Ia_total
+ (d1**2 + d2**2 + 2 * d1 * d2 * c2) * (M - m0) / M
+ m0 * (d1**2 + d2**2 + 2 * d1 * d2 * c2) / M)
# より正確な表現
H0 = I0 + I1 + I2 + m1 * a1**2 + m2 * (l1**2 + a2**2 + 2 * l1 * a2 * c2)
# 重心保存の補正項
H0 += (1 - m0 / M) * (d1**2 + d2**2 + 2 * d1 * d2 * c2)
# Hm: 関節速度に対する角運動量カップリング
# H1: phi1に対する角運動量寄与
H1 = I1 + I2 + m1 * a1**2 + m2 * (l1**2 + a2**2 + 2 * l1 * a2 * c2)
H1 += (1 - m0 / M) * (d1**2 + d2**2 + 2 * d1 * d2 * c2)
# ベースの回転分を除く
H1 = H1 - (I0 + I1 + I2 + m1 * a1**2 + m2 * (l1**2 + a2**2 + 2 * l1 * a2 * c2)
+ (1 - m0 / M) * (d1**2 + d2**2 + 2 * d1 * d2 * c2)) + H0
# 再計算: 正しくやり直す
# phi1 の変化で生じる角運動量
# リンク1,2が回転 + リンク1,2の重心が公転
H1_arm = I1 + I2 + m1 * a1**2 + m2 * (l1**2 + a2**2 + 2 * l1 * a2 * c2)
# 重心保存の拘束により、ベースも動く → その分の補正
H1 = H1_arm * (1 - (I0 + H1_arm + (1 - m0/M)*(d1**2 + d2**2 + 2*d1*d2*c2) - H1_arm) / H0)
# ---- 整理して再実装 ----
# 角運動量: L = Σ I_i * omega_i + Σ m_i * (r_i - r_G) x v_i
# 簡潔な表現を使う
# 有効慣性の計算
alpha = I1 + m1 * a1**2 + I2 + m2 * (l1**2 + a2**2 + 2 * l1 * a2 * c2)
beta = I2 + m2 * (a2**2 + l1 * a2 * c2)
gamma = I2 + m2 * a2**2
# 系全体の慣性（重心まわり、ベース回転に対する）
# 公転成分の補正
k1 = m1 * a1 + m2 * l1
k2 = m2 * a2
# 全質量系の角運動量
# L = I_total * dtheta0 + alpha * dphi1 + beta * dphi2
# ただし I_total は重心保存を考慮した有効慣性
# I_total = I0 + alpha + (m0*(m1+m2)/M) * |...|^2 (重心拘束の効果)
# 簡略化のため、重心を原点に固定して
# 系の角運動量を計算する方法を使う
# Papadopoulos & Dubowsky (1991) の表記に従う
# H0_scalar: ベース回転のスカラー慣性
# Hm_vec: 関節速度のカップリングベクトル
p1_sq = (m1 * a1 + m2 * l1)**2 / M + m1 * a1**2 + m2 * l1**2
p2_sq = (m2 * a2)**2 / M + m2 * a2**2
p12 = (m1 * a1 + m2 * l1) * m2 * a2 / M + m2 * l1 * a2
H0_s = I0 + I1 + I2 + p1_sq + p2_sq + 2 * p12 * c2
Hm1 = I1 + I2 + p1_sq + p2_sq + 2 * p12 * c2
Hm2 = I2 + p2_sq + p12 * c2
Hm_vec = np.array([Hm1, Hm2])
# ベースの角速度: dtheta0 = -(Hm1 * dphi1 + Hm2 * dphi2) / H0_s
# GJM = Jm + J0_rot * (-Hm_vec / H0_s)
# = Jm - J0_rot @ (Hm_vec.T / H0_s)
J_star = Jm - J0_rot @ (Hm_vec.reshape(1, 2)) / H0_s
return Jm, J_star, H0_s, Hm_vec
このコードは、平面2リンクモデルの通常ヤコビ行列 $\bm{J}_m$ と GJM $\bm{J}^*$ を返します。$\bm{H}_0$ がスカラー（平面の角運動量保存は1次元）であるため、行列の逆行列がスカラーの逆数になり、計算が簡潔です。
次に、通常ヤコビとGJMが予測する手先速度ベクトルの違いを可視化します。
import numpy as np
import matplotlib.pyplot as plt
# パラメータ
l1, l2 = 1.0, 0.8
a1, a2 = l1 / 2, l2 / 2
m1, m2 = 2.0, 1.5
I1 = m1 * l1**2 / 12
I2 = m2 * l2**2 / 12
def compute_gjm(phi1, phi2, theta0, m0, I0):
"""通常ヤコビとGJMを計算（整理版）"""
M = m0 + m1 + m2
th1 = theta0 + phi1
th2 = theta0 + phi1 + phi2
c2 = np.cos(phi2)
# 通常ヤコビ行列
Jm = np.array([
[-l1*np.sin(th1) - l2*np.sin(th2), -l2*np.sin(th2)],
[ l1*np.cos(th1) + l2*np.cos(th2), l2*np.cos(th2)]
])
# ベースの回転による手先速度への寄与
J0_rot = np.array([
[-l1*np.sin(th1) - l2*np.sin(th2)],
[ l1*np.cos(th1) + l2*np.cos(th2)]
])
# 慣性パラメータ
p1_sq = (m1*a1 + m2*l1)**2 / M + m1*a1**2 + m2*l1**2
p2_sq = (m2*a2)**2 / M + m2*a2**2
p12 = (m1*a1 + m2*l1)*m2*a2 / M + m2*l1*a2
H0_s = I0 + I1 + I2 + p1_sq + p2_sq + 2*p12*c2
Hm1 = I1 + I2 + p1_sq + p2_sq + 2*p12*c2
Hm2 = I2 + p2_sq + p12*c2
Hm_vec = np.array([[Hm1, Hm2]])
# GJM
J_star = Jm - J0_rot @ Hm_vec / H0_s
return Jm, J_star, H0_s, Hm_vec.flatten()
# --- 可視化1: 手先速度ベクトルの比較 ---
fig, axes = plt.subplots(1, 3, figsize=(18, 6))
# 3つの異なるベース質量で比較
base_masses = [5.0, 20.0, 200.0]
base_inertias = [m * 0.3**2 for m in base_masses] # I0 = m0 * r^2
phi1, phi2 = np.deg2rad(45), np.deg2rad(30)
theta0 = 0.0
for idx, (m0, I0) in enumerate(zip(base_masses, base_inertias)):
ax = axes[idx]
Jm, Js, H0_s, Hm_vec = compute_gjm(phi1, phi2, theta0, m0, I0)
# ロボットの描画
M = m0 + m1 + m2
th1 = theta0 + phi1
th2 = theta0 + phi1 + phi2
shoulder = np.array([0.0, 0.0])
elbow = shoulder + l1 * np.array([np.cos(th1), np.sin(th1)])
hand = elbow + l2 * np.array([np.cos(th2), np.sin(th2)])
# アーム描画
ax.plot([shoulder[0], elbow[0]], [shoulder[1], elbow[1]],
'o-', color='#4FC3F7', linewidth=3, markersize=8, zorder=5)
ax.plot([elbow[0], hand[0]], [elbow[1], hand[1]],
'o-', color='#81C784', linewidth=3, markersize=8, zorder=5)
# ベース描画
base_size = 0.15 + 0.1 * np.log10(m0)
rect = plt.Rectangle((shoulder[0]-base_size, shoulder[1]-base_size),
2*base_size, 2*base_size,
color='#FFB74D', alpha=0.7, zorder=4)
ax.add_patch(rect)
# 手先速度ベクトルの描画（単位関節速度 dphi1=1, dphi2=0）
dphi = np.array([1.0, 0.0])
v_jm = Jm @ dphi
v_js = Js @ dphi
scale = 0.3
ax.annotate('', xy=hand + scale*v_jm, xytext=hand,
arrowprops=dict(arrowstyle='->', color='#EF5350',
lw=2.5, mutation_scale=15))
ax.annotate('', xy=hand + scale*v_js, xytext=hand,
arrowprops=dict(arrowstyle='->', color='#42A5F5',
lw=2.5, mutation_scale=15))
# 第2関節速度の場合も
dphi2_vec = np.array([0.0, 1.0])
v_jm2 = Jm @ dphi2_vec
v_js2 = Js @ dphi2_vec
ax.annotate('', xy=hand + scale*v_jm2, xytext=hand,
arrowprops=dict(arrowstyle='->', color='#EF5350',
lw=2.0, ls='--', mutation_scale=12))
ax.annotate('', xy=hand + scale*v_js2, xytext=hand,
arrowprops=dict(arrowstyle='->', color='#42A5F5',
lw=2.0, ls='--', mutation_scale=12))
mass_ratio = m0 / (m1 + m2)
ax.set_title(f"$m_0 / m_{{arm}}$ = {mass_ratio:.1f}\n($m_0$ = {m0} kg)",
fontsize=13)
ax.set_xlim(-1.0, 2.5)
ax.set_ylim(-0.5, 2.5)
ax.set_aspect('equal')
ax.grid(True, alpha=0.3)
if idx == 0:
ax.plot([], [], '-', color='#EF5350', lw=2.5, label=r'$J_m$ (通常ヤコビ)')
ax.plot([], [], '-', color='#42A5F5', lw=2.5, label=r'$J^*$ (GJM)')
ax.legend(fontsize=11, loc='upper left')
fig.suptitle("通常ヤコビ行列 vs GJM が予測する手先速度ベクトル",
fontsize=15, fontweight='bold')
plt.tight_layout()
plt.savefig("gjm_velocity_comparison.png", dpi=150, bbox_inches='tight')
plt.show()
上のグラフから、重要な特徴が読み取れます。
- ベースが軽い場合（$m_0/m_{\text{arm}} = 1.4$）: 赤い矢印（通常ヤコビの予測）と青い矢印（GJMの予測）が大きく異なります。通常ヤコビを信じて手先を制御すると、実際の手先運動とは全く違う方向・大きさの速度が生じることを意味します。
- ベースが重い場合（$m_0/m_{\text{arm}} = 57.1$）: 赤と青の矢印がほぼ一致します。ベースが十分重いと反動が無視できるレベルになり、GJMは通常のヤコビ行列に収束します。
- 矢印の大きさに注目すると、GJMの矢印は常に通常ヤコビの矢印より短くなっています。 これは、ベースの反動が手先の有効な運動範囲を狭めていることを示しています。エネルギーの一部がベースの運動に「盗まれる」ためです。
質量比とGJMの行列式の関係
次に、質量比を連続的に変化させて、GJMの行列式（特異性の指標）がどう変わるかを確認します。
import numpy as np
import matplotlib.pyplot as plt
# パラメータ（前の定義を再利用）
l1, l2 = 1.0, 0.8
a1, a2 = l1 / 2, l2 / 2
m1, m2 = 2.0, 1.5
I1 = m1 * l1**2 / 12
I2 = m2 * l2**2 / 12
def compute_gjm(phi1, phi2, theta0, m0, I0):
"""通常ヤコビとGJMを計算"""
M = m0 + m1 + m2
th1 = theta0 + phi1
th2 = theta0 + phi1 + phi2
c2 = np.cos(phi2)
Jm = np.array([
[-l1*np.sin(th1) - l2*np.sin(th2), -l2*np.sin(th2)],
[ l1*np.cos(th1) + l2*np.cos(th2), l2*np.cos(th2)]
])
J0_rot = np.array([
[-l1*np.sin(th1) - l2*np.sin(th2)],
[ l1*np.cos(th1) + l2*np.cos(th2)]
])
p1_sq = (m1*a1 + m2*l1)**2 / M + m1*a1**2 + m2*l1**2
p2_sq = (m2*a2)**2 / M + m2*a2**2
p12 = (m1*a1 + m2*l1)*m2*a2 / M + m2*l1*a2
H0_s = I0 + I1 + I2 + p1_sq + p2_sq + 2*p12*c2
Hm1 = I1 + I2 + p1_sq + p2_sq + 2*p12*c2
Hm2 = I2 + p2_sq + p12*c2
Hm_vec = np.array([[Hm1, Hm2]])
J_star = Jm - J0_rot @ Hm_vec / H0_s
return Jm, J_star, H0_s, Hm_vec.flatten()
# --- 可視化2: 質量比 vs 行列式 ---
fig, axes = plt.subplots(1, 2, figsize=(14, 5))
# 異なる姿勢での比較
configs = [
(np.deg2rad(60), np.deg2rad(45), "φ₁=60°, φ₂=45°"),
(np.deg2rad(90), np.deg2rad(-30), "φ₁=90°, φ₂=-30°")
]
for idx, (p1, p2, label) in enumerate(configs):
ax = axes[idx]
mass_ratios = np.logspace(-0.5, 3, 200) # m0/(m1+m2)
m_arm = m1 + m2
det_Jm_list = []
det_Js_list = []
for ratio in mass_ratios:
m0 = ratio * m_arm
I0 = m0 * 0.3**2
Jm, Js, _, _ = compute_gjm(p1, p2, 0.0, m0, I0)
det_Jm_list.append(np.linalg.det(Jm))
det_Js_list.append(np.linalg.det(Js))
det_Jm_arr = np.array(det_Jm_list)
det_Js_arr = np.array(det_Js_list)
ax.semilogx(mass_ratios, det_Jm_arr, '--', color='#EF5350',
lw=2, label=r'$\det(J_m)$')
ax.semilogx(mass_ratios, det_Js_arr, '-', color='#42A5F5',
lw=2.5, label=r'$\det(J^*)$')
ax.axhline(0, color='gray', lw=0.8, ls=':')
ax.set_xlabel(r"質量比 $m_0 / m_{\mathrm{arm}}$", fontsize=12)
ax.set_ylabel("行列式", fontsize=12)
ax.set_title(label, fontsize=13)
ax.legend(fontsize=11)
ax.grid(True, alpha=0.3)
fig.suptitle("質量比とヤコビ行列式の関係", fontsize=15, fontweight='bold')
plt.tight_layout()
plt.savefig("gjm_determinant_vs_mass_ratio.png", dpi=150, bbox_inches='tight')
plt.show()
上のグラフから、以下のことが読み取れます。
- 通常ヤコビの行列式 $\det(\bm{J}_m)$ は質量比に依存しません。 赤い破線は水平です。これは当然で、$\bm{J}_m$ はリンクの幾何学だけで決まり、質量は関係ありません。
- GJMの行列式 $\det(\bm{J}^*)$ は質量比が小さいほど $\det(\bm{J}_m)$ から乖離します。 質量比が1に近い（ベースとアームが同程度の重さ）とき、GJMの行列式は通常ヤコビの行列式より小さくなります。これは手先の操作性が低下していることを意味します。
- 質量比が大きくなると（$m_0/m_{\text{arm}} > 100$）、$\det(\bm{J}^*)$ は $\det(\bm{J}_m)$ にほぼ一致します。 GJMが通常のヤコビ行列に収束する理論的予測と完全に整合しています。
動的特異姿勢のマッピング
次に、関節空間を走査して、通常ヤコビとGJMそれぞれの特異姿勢をマッピングします。
import numpy as np
import matplotlib.pyplot as plt
# パラメータ
l1, l2 = 1.0, 0.8
a1, a2 = l1 / 2, l2 / 2
m1, m2 = 2.0, 1.5
I1 = m1 * l1**2 / 12
I2 = m2 * l2**2 / 12
def compute_gjm(phi1, phi2, theta0, m0, I0):
"""通常ヤコビとGJMを計算"""
M = m0 + m1 + m2
th1 = theta0 + phi1
th2 = theta0 + phi1 + phi2
c2 = np.cos(phi2)
Jm = np.array([
[-l1*np.sin(th1) - l2*np.sin(th2), -l2*np.sin(th2)],
[ l1*np.cos(th1) + l2*np.cos(th2), l2*np.cos(th2)]
])
J0_rot = np.array([
[-l1*np.sin(th1) - l2*np.sin(th2)],
[ l1*np.cos(th1) + l2*np.cos(th2)]
])
p1_sq = (m1*a1 + m2*l1)**2 / M + m1*a1**2 + m2*l1**2
p2_sq = (m2*a2)**2 / M + m2*a2**2
p12 = (m1*a1 + m2*l1)*m2*a2 / M + m2*l1*a2
H0_s = I0 + I1 + I2 + p1_sq + p2_sq + 2*p12*c2
Hm1 = I1 + I2 + p1_sq + p2_sq + 2*p12*c2
Hm2 = I2 + p2_sq + p12*c2
Hm_vec = np.array([[Hm1, Hm2]])
J_star = Jm - J0_rot @ Hm_vec / H0_s
return Jm, J_star, H0_s, Hm_vec.flatten()
# --- 可視化3: 動的特異姿勢マップ ---
fig, axes = plt.subplots(1, 3, figsize=(18, 5))
m0_values = [5.0, 15.0, 100.0]
N = 300
phi1_range = np.linspace(-np.pi, np.pi, N)
phi2_range = np.linspace(-np.pi, np.pi, N)
PHI1, PHI2 = np.meshgrid(phi1_range, phi2_range)
for idx, m0 in enumerate(m0_values):
ax = axes[idx]
I0 = m0 * 0.3**2
det_Jm = np.zeros_like(PHI1)
det_Js = np.zeros_like(PHI1)
for i in range(N):
for j in range(N):
Jm, Js, _, _ = compute_gjm(PHI1[i,j], PHI2[i,j], 0.0, m0, I0)
det_Jm[i,j] = np.linalg.det(Jm)
det_Js[i,j] = np.linalg.det(Js)
# 条件マップ:
# 緑: 両方正則 (|det| > eps)
# 赤: Jm が特異 (運動学的特異姿勢)
# 青: Jm は正則だが J* が特異に近い (動的特異姿勢)
eps = 0.02
category = np.zeros_like(PHI1, dtype=int)
# 0: 両方正則, 1: 運動学的特異, 2: 動的特異
category[np.abs(det_Jm) < eps] = 1
category[(np.abs(det_Jm) >= eps) & (np.abs(det_Js) < eps)] = 2
colors = np.zeros((*PHI1.shape, 3))
colors[category == 0] = [0.9, 0.95, 0.9] # 薄緑: 正常
colors[category == 1] = [0.94, 0.33, 0.31] # 赤: 運動学的特異
colors[category == 2] = [0.26, 0.65, 0.96] # 青: 動的特異
ax.imshow(colors, extent=[-180, 180, -180, 180], origin='lower', aspect='auto')
mass_ratio = m0 / (m1 + m2)
ax.set_title(f"$m_0/m_{{arm}}$ = {mass_ratio:.1f}", fontsize=13)
ax.set_xlabel(r"$\phi_1$ [deg]", fontsize=11)
if idx == 0:
ax.set_ylabel(r"$\phi_2$ [deg]", fontsize=11)
if idx == 2:
# 凡例
from matplotlib.patches import Patch
legend_elements = [
Patch(facecolor=[0.9, 0.95, 0.9], label='正常'),
Patch(facecolor=[0.94, 0.33, 0.31], label='運動学的特異'),
Patch(facecolor=[0.26, 0.65, 0.96], label='動的特異')
]
ax.legend(handles=legend_elements, fontsize=10, loc='upper right')
fig.suptitle("関節空間における特異姿勢マップ", fontsize=15, fontweight='bold')
plt.tight_layout()
plt.savefig("gjm_singularity_map.png", dpi=150, bbox_inches='tight')
plt.show()
上の特異姿勢マップからは、非常に興味深い結果が得られます。
- 赤い領域（運動学的特異姿勢）は全てのケースで同じ位置にあります。これは $\phi_2 = 0$ と $\phi_2 = \pm 180°$（アームが完全に伸びきった姿勢または折り畳まれた姿勢）の近傍で、通常のヤコビ行列 $\bm{J}_m$ が特異になる場所です。質量比に依存しないことが確認できます。
- 青い領域（動的特異姿勢）はベースが軽いほど広がります。 $m_0/m_{\text{arm}} = 1.4$ では運動学的特異姿勢とは異なる場所に青い帯が現れます。これらは通常のヤコビ行列では検出できない「隠れた」特異姿勢です。
- ベースが重くなると動的特異姿勢は消失していきます。$m_0/m_{\text{arm}} = 28.6$ ではほとんど見えなくなり、地上ロボットに近い状況になります。
手先軌道のシミュレーション
最後に、同じ関節軌道を与えたとき、通常ヤコビとGJMで予測される手先軌道がどれだけ異なるかをシミュレーションします。
import numpy as np
import matplotlib.pyplot as plt
# パラメータ
l1, l2 = 1.0, 0.8
a1, a2 = l1 / 2, l2 / 2
m1, m2 = 2.0, 1.5
I1 = m1 * l1**2 / 12
I2 = m2 * l2**2 / 12
def forward_kinematics(theta0, phi1, phi2, base_pos):
"""順運動学: ベース位置・姿勢から手先位置を計算"""
th1 = theta0 + phi1
th2 = theta0 + phi1 + phi2
shoulder = base_pos
elbow = shoulder + l1 * np.array([np.cos(th1), np.sin(th1)])
hand = elbow + l2 * np.array([np.cos(th2), np.sin(th2)])
return shoulder, elbow, hand
def simulate_trajectory(m0, I0, phi1_traj, phi2_traj, dt):
"""
運動量保存を考慮した宇宙ロボットの軌道シミュレーション
"""
M = m0 + m1 + m2
n_steps = len(phi1_traj)
# 初期状態
theta0 = 0.0
base_pos = np.array([0.0, 0.0])
hand_positions_true = []
hand_positions_fixed = []
for k in range(n_steps):
phi1 = phi1_traj[k]
phi2 = phi2_traj[k]
# 角運動量保存からベース角度を計算
# dtheta0 = -(Hm1*dphi1 + Hm2*dphi2) / H0_s を積分
c2 = np.cos(phi2)
p1_sq = (m1*a1 + m2*l1)**2 / M + m1*a1**2 + m2*l1**2
p2_sq = (m2*a2)**2 / M + m2*a2**2
p12 = (m1*a1 + m2*l1)*m2*a2 / M + m2*l1*a2
H0_s = I0 + I1 + I2 + p1_sq + p2_sq + 2*p12*c2
Hm1 = I1 + I2 + p1_sq + p2_sq + 2*p12*c2
Hm2 = I2 + p2_sq + p12*c2
if k > 0:
dphi1 = phi1_traj[k] - phi1_traj[k-1]
dphi2 = phi2_traj[k] - phi2_traj[k-1]
dtheta0 = -(Hm1 * dphi1 + Hm2 * dphi2) / H0_s
theta0 += dtheta0
# 重心保存からベース位置を計算
th1 = theta0 + phi1
th2 = theta0 + phi1 + phi2
# 系の重心 = 0（初期位置を原点とする）
# m0*r0 + m1*r1 + m2*r2 = 0
r1_from_base = a1 * np.array([np.cos(th1), np.sin(th1)])
r2_from_base = (l1 * np.array([np.cos(th1), np.sin(th1)])
+ a2 * np.array([np.cos(th2), np.sin(th2)]))
# r1 = base_pos + r1_from_base, r2 = base_pos + r2_from_base
# m0*base_pos + m1*(base_pos + r1_from_base) + m2*(base_pos + r2_from_base) = r_G_init
r_G_init = np.array([0.0, 0.0]) # 初期重心
# 初期重心の計算
if k == 0:
th1_0 = phi1_traj[0]
th2_0 = phi1_traj[0] + phi2_traj[0]
r1_init = a1 * np.array([np.cos(th1_0), np.sin(th1_0)])
r2_init = (l1 * np.array([np.cos(th1_0), np.sin(th1_0)])
+ a2 * np.array([np.cos(th2_0), np.sin(th2_0)]))
r_G_init = (m1 * r1_init + m2 * r2_init) / M
base_pos = (r_G_init * M - m1 * r1_from_base - m2 * r2_from_base) / M
_, _, hand_true = forward_kinematics(theta0, phi1, phi2, base_pos)
hand_positions_true.append(hand_true.copy())
# 固定ベース仮定の場合
_, _, hand_fixed = forward_kinematics(0.0, phi1, phi2, np.array([0.0, 0.0]))
hand_positions_fixed.append(hand_fixed.copy())
return (np.array(hand_positions_true),
np.array(hand_positions_fixed),
theta0, base_pos)
# --- 関節軌道の生成（円弧運動） ---
n_steps = 200
t = np.linspace(0, 1, n_steps)
phi1_traj = np.deg2rad(30) + np.deg2rad(60) * t
phi2_traj = np.deg2rad(45) - np.deg2rad(40) * t
# --- シミュレーション ---
fig, axes = plt.subplots(1, 2, figsize=(16, 7))
for idx, (m0, ax) in enumerate(zip([5.0, 50.0], axes)):
I0 = m0 * 0.3**2
dt = 1.0 / n_steps
hand_true, hand_fixed, final_theta0, final_base = simulate_trajectory(
m0, I0, phi1_traj, phi2_traj, dt
)
# 軌道描画
ax.plot(hand_fixed[:, 0], hand_fixed[:, 1], '--', color='#EF5350',
lw=2, label='固定ベース仮定（通常ヤコビ）', alpha=0.8)
ax.plot(hand_true[:, 0], hand_true[:, 1], '-', color='#42A5F5',
lw=2.5, label='運動量保存考慮（GJM）', alpha=0.9)
# 始点・終点マーク
ax.plot(*hand_fixed[0], 'o', color='#EF5350', markersize=10, zorder=5)
ax.plot(*hand_fixed[-1], 's', color='#EF5350', markersize=10, zorder=5)
ax.plot(*hand_true[0], 'o', color='#42A5F5', markersize=10, zorder=5)
ax.plot(*hand_true[-1], 's', color='#42A5F5', markersize=10, zorder=5)
# 最終姿勢のアーム描画
M_total = m0 + m1 + m2
phi1_f = phi1_traj[-1]
phi2_f = phi2_traj[-1]
# 真の最終姿勢
s, e, h = forward_kinematics(final_theta0, phi1_f, phi2_f, final_base)
ax.plot([s[0], e[0]], [s[1], e[1]], 'o-', color='#4FC3F7',
lw=2, markersize=6, alpha=0.5)
ax.plot([e[0], h[0]], [e[1], h[1]], 'o-', color='#81C784',
lw=2, markersize=6, alpha=0.5)
# 誤差表示
error = np.linalg.norm(hand_true[-1] - hand_fixed[-1])
ax.annotate(f'位置誤差: {error:.3f} m',
xy=((hand_true[-1, 0] + hand_fixed[-1, 0])/2,
(hand_true[-1, 1] + hand_fixed[-1, 1])/2),
fontsize=11, color='#FF6F00',
bbox=dict(boxstyle='round', facecolor='wheat', alpha=0.8))
mass_ratio = m0 / (m1 + m2)
ax.set_title(f"$m_0/m_{{arm}}$ = {mass_ratio:.1f}", fontsize=14)
ax.set_xlabel("x [m]", fontsize=12)
ax.set_ylabel("y [m]", fontsize=12)
ax.legend(fontsize=10, loc='upper left')
ax.set_aspect('equal')
ax.grid(True, alpha=0.3)
fig.suptitle("手先軌道の比較: 固定ベース仮定 vs 運動量保存考慮",
fontsize=15, fontweight='bold')
plt.tight_layout()
plt.savefig("gjm_trajectory_comparison.png", dpi=150, bbox_inches='tight')
plt.show()
このシミュレーション結果から、GJMの実用的な重要性が明確にわかります。
- ベースが軽い場合（$m_0/m_{\text{arm}} = 1.4$）: 固定ベース仮定（赤い破線）と真の軌道（青い実線）の間に大きな乖離が生じています。関節を同じように動かしても、ベースの反動により手先は全く異なる経路をたどります。位置誤差は数十センチメートルのオーダーになり得ます。
- ベースが重い場合（$m_0/m_{\text{arm}} = 14.3$）: 乖離は小さくなりますが、依然としてゼロではありません。宇宙での精密作業（ドッキングやコネクタ操作）ではミリメートル精度が求められるため、この程度の誤差でもGJMによる補正は不可欠です。
- 始点の位置も異なります。 これは、運動量保存によりベース位置自体がシフトするためです。GJMの枠組みでは、このベースの並進も自動的に考慮されます。
GJMの拡張と発展的な話題
非ゼロ運動量の場合
ここまでの導出は、初期運動量がゼロ（$\bm{P} = \bm{0}$, $\bm{L} = \bm{0}$）の場合を扱いました。しかし、実際のミッションでは以下の理由で運動量がゼロでない場合があります。
- ターゲットを捕獲した瞬間に運動量が移行する
- リアクションホイール（RW）に蓄積された角運動量
- 外乱トルク（重力傾斜、大気抵抗）による角運動量の蓄積
運動量が $\bm{h}_0 \neq \bm{0}$ の場合、運動量保存則は
$$ \bm{H}_0 \dot{\bm{x}}_0 + \bm{H}_m \dot{\bm{\phi}} = \bm{h}_0 $$
となります。この場合のベース速度は
$$ \dot{\bm{x}}_0 = \bm{H}_0^{-1}(\bm{h}_0 – \bm{H}_m \dot{\bm{\phi}}) $$
であり、手先速度は
$$ \dot{\bm{x}}_e = \bm{J}^* \dot{\bm{\phi}} + \bm{J}_0 \bm{H}_0^{-1} \bm{h}_0 $$
となります。GJMの定義自体は変わりませんが、$\bm{J}_0 \bm{H}_0^{-1} \bm{h}_0$ という定数項（関節速度に依存しない項）が加わります。この項は「初期運動量によるベースの漂流が手先に与える影響」を表しており、逆運動学では $\dot{\bm{x}}_e^{\text{des}}$ からこの項を差し引いてからGJMの疑似逆行列を適用します。
リアクションレスマニューバとの関連
GJMの零空間を利用すると、リアクションレスマニューバ（reactionless maneuver） — ベースの姿勢を変化させずにアームを動かす制御 — を実現できます。これは次の記事で詳しく扱います。
リアクションレスの条件は $\dot{\theta}_0 = 0$、すなわち角運動量保存の式で $\bm{H}_m \dot{\bm{\phi}} = \bm{0}$ です。冗長なマニピュレータ（$n \geq 2$、平面の場合）であれば、$\bm{H}_m$ の零空間に属する関節速度を使うことで、ベースに反動を与えずにアームを動かせます。
操作性楕円体
地上ロボットでは、ヤコビ行列を使って操作性楕円体（manipulability ellipsoid）を定義し、手先がどの方向にどれだけ容易に動けるかを可視化できます。宇宙マニピュレータでは、通常のヤコビ行列の代わりにGJMを使った操作性楕円体
$$ \bm{x}^T (\bm{J}^* (\bm{J}^*)^T)^{-1} \bm{x} \leq 1 $$
が真の操作性を表します。GJMの操作性楕円体は通常のヤコビ行列のそれより一般に小さくなります。これは、ベースの反動により手先の有効な運動範囲が狭まることを反映しています。
まとめ
本記事では、宇宙マニピュレータの速度関係を正しく記述するための一般化ヤコビ行列（GJM）について解説しました。
- 通常のヤコビ行列の限界: 宇宙空間ではベースが固定されていないため、関節を動かすとベースが反動で動く。通常のヤコビ行列 $\bm{J}_m$ は手先速度を正しく予測できない
- GJMの定義: $\bm{J}^* = \bm{J}_m – \bm{J}_0 \bm{H}_0^{-1} \bm{H}_m$。運動量保存則によるベースの反動を組み込んだ「真の」速度写像
- GJMの導出: 手先速度 = ベース寄与 + 関節寄与 → 運動量保存でベース速度を消去 → 関節速度のみの表現
- 動的特異姿勢: 通常のヤコビ行列が正則でもGJMが特異になる宇宙ロボット特有の危険な姿勢が存在する。ミッション計画時にGJMの特異姿勢マップを確認する必要がある
- 質量比の影響: ベースが重いほどGJMは通常のヤコビ行列に近づく。ベースが軽い場合は補正が大きくなり、動的特異姿勢も広がる
- 逆運動学への応用: GJMを使えば、地上ロボットと同じ形式の逆運動学を宇宙ロボットに適用できる
GJMは宇宙マニピュレータの制御における最も基本的な道具であり、この行列なしには手先の正確な制御は成り立ちません。1989年の梅谷・吉田の論文以来、宇宙ロボティクスのあらゆる研究でGJMが使われています。
次のステップとして、以下の記事も参考にしてください。
- リアクションレスマニューバ — ベースに反動を与えない宇宙アーム制御 — GJMの零空間を利用したベース姿勢不変のアーム制御
- ヤコビ行列と微分運動学 — 地上ロボットにおけるヤコビ行列の基礎
- 自由浮遊ロボットの運動量保存と動的カップリング — GJMの導出の前提となる運動力学