国際宇宙ステーション（ISS）の太陽電池パドルは、長さ約34メートル、質量300kg超の巨大構造物です。この大型パネルを設置するとき、ロボットアーム1本だけで作業する場面を想像してみてください。パネルの一端を掴んで持ち上げると、もう一端は自由にふらつき、ISSの構造体に衝突する危険があります。また、回転しながら漂流するデブリを捕獲する場面では、1本のアームで掴もうとした瞬間にデブリの回転モーメントがそのままアームとベース衛星に伝わり、衛星全体が制御不能な回転に陥りかねません。
こうした問題を解決するのがデュアルアーム（両腕）協調制御です。2本のアームが連携して物体を挟み込むように把持し、力とモーメントを分担することで、1本では不可能だった大型構造物の安定的な操作が実現できます。
デュアルアーム協調制御を理解すると、以下のような応用に取り組めるようになります。
- 軌道上組立: 大型構造物のモジュール結合（次世代宇宙ステーション、大型望遠鏡）
- デブリ除去: 回転するデブリの安定化と安全な脱軌道（Active Debris Removal）
- 衛星メンテナンス: 軌道上でのコンポーネント交換・燃料補給（On-Orbit Servicing）
- 月面・惑星基地建設: ロボットによる自律的な構造体組立
本記事の内容
- なぜデュアルアームが必要なのか — 単腕の限界
- デュアルアームの運動学 — 閉鎖運動連鎖
- マスター・スレーブ方式と対等協調方式の比較
- 力の分配問題 — 2本のアームに力をどう配分するか
- 内力の制御 — 物体を潰す/引っ張る力の管理
- 宇宙環境での特殊性 — ベースの反動と質量バランス
- 協調制御フレームワーク — 絶対座標系 + 相対座標系
- Pythonで2Dデュアルアームの協調動作シミュレーション
前提知識
この記事を読む前に、以下の記事を読んでおくと理解が深まります。
- 自由浮遊ロボットの動力学 — 宇宙空間でベースが固定されないロボットの動力学
- インピーダンス制御 — 力と位置を統合的に扱う制御手法
なぜデュアルアームが必要なのか
単腕ロボットの限界
地上の産業用ロボットの大半は1本のアームで動作しています。部品を掴んで運ぶ、溶接する、ねじを締める — これらの作業は1本のアームで十分こなせます。それはなぜでしょうか。地上では重力と固定された作業台が存在するからです。物体は重力で作業台に押し付けられ、安定した状態で加工を受けます。1本のアームは「動かす側」だけを担当すればよく、「固定する側」は環境が担ってくれるのです。
ところが宇宙空間では、この前提が崩れます。微小重力環境では物体を固定するものがありません。1本のアームで物体を押すと、物体は逃げていきます。物体を掴んだとしても、大型構造物の場合は1点把持では十分な拘束が得られず、回転自由度が残ります。
具体的に、単腕ロボットの限界を整理しましょう。
問題1: 大型構造物の安定把持
長さ $L$ の棒状構造物の一端を1本のアームで掴む場合を考えます。掴んだ点を原点とすると、構造物にはアームからの力 $\bm{f}$ とモーメント $\bm{\tau}$ が作用します。しかし、構造物が十分長い（$L$ が大きい）場合、先端の位置制御精度は把持点の角度誤差 $\delta\theta$ に対して $L \cdot \delta\theta$ のオーダーで劣化します。すなわち、アームの関節精度がどれだけ高くても、長い構造物の遠端は精密に制御できないのです。
問題2: 回転デブリの安定化
角速度 $\bm{\omega}$ で回転する質量 $m$、慣性モーメント $\bm{I}$ のデブリを考えます。1本のアームで掴んだ瞬間、デブリの角運動量 $\bm{L} = \bm{I}\bm{\omega}$ がアームを通じてベース衛星に伝達されます。ベース衛星が自由浮遊体であれば、この角運動量を受けて衛星自体が回転を始めます。1本のアームでは、デブリの回転を止めようとする反力がそのままベースに作用するため、ベースの姿勢維持と回転制動を同時に達成することが困難です。
問題3: 力の冗長性がない
1本のアームで物体を把持する場合、物体にかかる力はそのアームからの力のみです。これは「力の自由度 = アームの数」ということを意味します。アーム1本では力の配分に冗長性がなく、接触点での力を自由に調整できません。これが後述する「内力制御」の問題につながります。
デュアルアームの優位性
2本のアームを使うと、上記の全ての問題が解消または緩和されます。
- 安定把持: 2点で物体を挟むことで全自由度を拘束でき、遠端の位置誤差を劇的に低減
- 反動の分散: 2本のアームが逆方向の力を出すことで、ベースへの反動を相殺可能
- 力の冗長性: アーム2本の力の自由度から物体の運動に必要な力を引いた分が「内力」として自由に使える
ただし、デュアルアームは制御の複雑さが大幅に増加します。2本のアームが同じ物体を掴んだ瞬間に「閉鎖運動連鎖」が形成され、単純なアーム個別の制御では対応できません。この閉鎖運動連鎖の運動学から見ていきましょう。
デュアルアームの運動学 — 閉鎖運動連鎖
開運動連鎖と閉運動連鎖
ロボティクスで扱う運動連鎖（kinematic chain）には、大きく2種類あります。
開運動連鎖（open kinematic chain）は、基部から先端に向かって一本道でつながるリンク列です。通常の1本のロボットアームがこれに該当します。各関節は独立に動かせるため、$n$ 個の関節があれば $n$ 自由度が使えます。順運動学（関節角度 → 手先位置）も逆運動学も、開連鎖であれば標準的な手法で解くことができます。
一方、閉運動連鎖（closed kinematic chain）は、リンクがループを形成する構造です。デュアルアームが1つの物体を把持すると、「アーム1 → 物体 → アーム2 → ベース → アーム1」というループが閉じます。この瞬間、全ての関節を独立に動かすことはできなくなります。関節角度の間に拘束条件が生まれ、自由度が減少するのです。
日常で閉運動連鎖を体験するのは簡単です。両手でバスケットボールを持ってみてください。右手を動かすと左手も動かなければなりません。右手と左手の間にはボールを介した「拘束」が存在し、片手を自由に動かせる場合に比べて明らかに自由度が制限されています。これがまさに閉運動連鎖です。
拘束条件の数学的表現
デュアルアームが共通の剛体物体を把持している状況を定式化しましょう。
アーム $i$（$i = 1, 2$）の手先位置・姿勢を $\bm{x}_i \in \mathbb{R}^m$、物体の位置・姿勢を $\bm{x}_o \in \mathbb{R}^m$ とします。ここで $m$ は作業空間の次元（2Dなら $m = 3$：$x, y, \theta$、3Dなら $m = 6$：$x, y, z, \phi, \theta, \psi$）です。
アーム $i$ の把持点から物体の重心までの幾何学的関係を $\bm{g}_i$ とすると、閉鎖運動連鎖の拘束条件は次のように書けます。
$$ \begin{equation} \bm{x}_i = \bm{g}_i(\bm{x}_o) \quad (i = 1, 2) \end{equation} $$
これは「各アームの手先が、物体上の把持点に一致しなければならない」という幾何学的拘束です。
具体的に、物体の重心位置を $(x_o, y_o)$、姿勢角を $\theta_o$ とし、アーム $i$ の把持点が物体重心から $\bm{r}_i = (r_{ix}, r_{iy})$ の位置にあるとすると（物体座標系で表現）、拘束条件は次のようになります。
$$ \begin{equation} \bm{x}_i = \begin{pmatrix} x_o \\ y_o \\ \theta_o \end{pmatrix} + \begin{pmatrix} \cos\theta_o & -\sin\theta_o & 0 \\ \sin\theta_o & \cos\theta_o & 0 \\ 0 & 0 & 1 \end{pmatrix} \begin{pmatrix} r_{ix} \\ r_{iy} \\ 0 \end{pmatrix} \end{equation} $$
速度レベルの拘束
位置レベルの拘束を時間微分すると、速度レベルの拘束が得られます。これが制御で直接使われる形です。
各アームの手先速度を $\dot{\bm{x}}_i$、物体の速度を $\dot{\bm{x}}_o$ とすると、拘束条件の時間微分は次のようになります。
$$ \begin{equation} \dot{\bm{x}}_i = \bm{J}_{oi}(\bm{x}_o) \dot{\bm{x}}_o \quad (i = 1, 2) \end{equation} $$
ここで $\bm{J}_{oi} = \partial \bm{g}_i / \partial \bm{x}_o$ は物体の速度をアーム $i$ の手先速度に変換するヤコビ行列です。
一方、各アームの関節角度ベクトルを $\bm{q}_i$ とすると、通常のアームのヤコビアンにより次が成り立ちます。
$$ \begin{equation} \dot{\bm{x}}_i = \bm{J}_i(\bm{q}_i) \dot{\bm{q}}_i \quad (i = 1, 2) \end{equation} $$
これら2つの関係式を組み合わせることで、関節速度と物体速度の関係が得られます。
$$ \begin{equation} \bm{J}_i(\bm{q}_i) \dot{\bm{q}}_i = \bm{J}_{oi}(\bm{x}_o) \dot{\bm{x}}_o \quad (i = 1, 2) \end{equation} $$
この式こそが閉運動連鎖の核心です。2本のアームの関節速度 $\dot{\bm{q}}_1, \dot{\bm{q}}_2$ は、それぞれ独立に自由な値をとれるのではなく、共通の物体速度 $\dot{\bm{x}}_o$ を通じて結合されています。
自由度の分析
開運動連鎖の場合、2本のアームの関節が合計 $n = n_1 + n_2$ 個あれば $n$ 自由度です。しかし閉運動連鎖が形成されると、拘束条件の数だけ自由度が減少します。
拘束条件の数は、2つの把持点それぞれで $m$ 個（2Dなら3個、3Dなら6個）です。ただし物体自体が $m$ 自由度を持つので、独立な拘束の数は $2m – m = m$ です。
したがって、閉運動連鎖の有効自由度は次のようになります。
$$ \begin{equation} n_{\text{eff}} = n_1 + n_2 – m \end{equation} $$
2Dの場合（$m = 3$）で各アームが3関節なら $n_{\text{eff}} = 3 + 3 – 3 = 3$ です。6自由度の関節を持ちながら実効的には3自由度しか使えない — これが閉運動連鎖の「自由度の減少」です。
ここまでで、デュアルアームが物体を把持すると閉運動連鎖が形成され、運動学的な拘束が生まれることがわかりました。次に、この2本のアームをどのように協調させるかという制御戦略を見ていきます。
マスター・スレーブ方式 vs 対等協調方式
2つの基本アーキテクチャ
デュアルアームの制御アーキテクチャは、大きく2つに分類されます。
マスター・スレーブ方式は、文字通り「主従関係」をアーム間に設定する方法です。一方のアーム（マスター）が物体の運動を主導し、もう一方のアーム（スレーブ）はマスターの動きに追従します。
地上の例で言えば、2人で大きなテーブルを運ぶとき、1人が「せーの、右に回して」と指示を出し、もう1人がそれに合わせるイメージです。指示を出す人がマスター、合わせる人がスレーブです。
対等協調方式（symmetric cooperative control）は、2本のアームに主従関係を設けず、対等に物体の運動に寄与させる方法です。2人で荷物を運ぶとき、行き先だけ共有して「お互いが対等に力を出し合う」イメージです。
マスター・スレーブ方式の定式化
マスター・スレーブ方式では、マスターアーム（アーム1）が物体の位置制御を担当し、スレーブアーム（アーム2）はインピーダンス制御で追従します。
マスターアームの制御則は、通常の位置制御です。
$$ \begin{equation} \bm{\tau}_1 = \bm{J}_1^T \left[ \bm{K}_p (\bm{x}_{1d} – \bm{x}_1) + \bm{K}_d (\dot{\bm{x}}_{1d} – \dot{\bm{x}}_1) \right] + \bm{g}_1(\bm{q}_1) \end{equation} $$
ここで $\bm{\tau}_1$ は関節トルク、$\bm{K}_p, \bm{K}_d$ はそれぞれ位置・速度のゲイン行列、$\bm{x}_{1d}$ は目標手先位置、$\bm{g}_1(\bm{q}_1)$ は重力補償項です（宇宙空間では $\bm{g}_1 = \bm{0}$）。
スレーブアームは、インピーダンス制御によりマスターの動きに「柔らかく」追従します。
$$ \begin{equation} \bm{\tau}_2 = \bm{J}_2^T \left[ \bm{M}_d \ddot{\bm{e}}_2 + \bm{D}_d \dot{\bm{e}}_2 + \bm{K}_s \bm{e}_2 – \bm{f}_2 \right] \end{equation} $$
ここで $\bm{e}_2 = \bm{x}_{2d} – \bm{x}_2$ はスレーブの位置誤差、$\bm{M}_d, \bm{D}_d, \bm{K}_s$ はそれぞれ目標インピーダンスの慣性、ダンピング、剛性パラメータ、$\bm{f}_2$ はスレーブの手先に作用する外力です。
マスター・スレーブ方式の利点は、実装の簡潔さです。マスター側は通常の位置制御をそのまま使え、スレーブ側のインピーダンス制御も既存技術の組み合わせです。また、スレーブのインピーダンスパラメータを調整することで、閉運動連鎖における過拘束（overconstrained）の問題を自然に緩和できます。
一方、欠点は、マスターに負荷が集中することと、2本のアームの能力を対等に活かせないことです。マスターが故障した場合のフォールトトレランスにも課題があります。
対等協調方式の定式化
対等協調方式では、物体の目標運動 $\bm{x}_{od}$ を基準に、両アームが対等に力を分配します。各アームの目標手先位置は、閉運動連鎖の拘束条件から導出されます。
$$ \begin{equation} \bm{x}_{id} = \bm{g}_i(\bm{x}_{od}) \quad (i = 1, 2) \end{equation} $$
制御則は両アームとも同じ形式です。
$$ \begin{equation} \bm{\tau}_i = \bm{J}_i^T \left[ \alpha_i \bm{f}_{\text{motion}} + \bm{f}_{\text{internal},i} \right] + \bm{h}_i(\bm{q}_i, \dot{\bm{q}}_i) \end{equation} $$
ここで $\bm{f}_{\text{motion}}$ は物体の運動に必要な力、$\alpha_i$ は力の分配係数（$\alpha_1 + \alpha_2 = 1$）、$\bm{f}_{\text{internal},i}$ は内力成分、$\bm{h}_i$ は動力学補償項です。
対等協調方式の利点は、両アームの能力を最大限に活用できることと、力の分配を最適化できることです。欠点は、制御設計がマスター・スレーブ方式より複雑になることです。
方式選択の指針
| 基準 | マスター・スレーブ | 対等協調 |
|---|---|---|
| 実装の容易さ | 高い | 低い |
| 力の効率 | 低い（マスターに負荷集中） | 高い（均等分配可） |
| 精密作業 | やや不利 | 有利 |
| フォールトトレランス | 低い | 高い |
| 宇宙での適性 | 小型作業向け | 大型構造物・デブリ向け |
宇宙での大型構造物操作やデブリ除去では、力の効率とフォールトトレランスが重要になるため、対等協調方式が主流です。以降では対等協調方式を前提に議論を進めます。
2本のアームを対等に扱う方式を選んだとして、次に直面するのが「物体を動かすために必要な力を2本のアームにどう配分するか」という力の分配問題です。
力の分配問題
問題の定式化
いま、2本のアームが剛体物体を把持しており、物体に目標加速度 $\ddot{\bm{x}}_o$ を与えたいとします。物体の運動方程式は次のようになります。
$$ \begin{equation} \bm{M}_o \ddot{\bm{x}}_o = \bm{J}_{o1}^T \bm{f}_1 + \bm{J}_{o2}^T \bm{f}_2 + \bm{w}_{\text{ext}} \end{equation} $$
ここで $\bm{M}_o$ は物体の慣性行列、$\bm{f}_i$ はアーム $i$ が物体に加える力/モーメント（把持点でのレンチ）、$\bm{J}_{oi}^T$ はアーム $i$ の把持点での力を物体重心の力/モーメントに変換するヤコビ行列の転置、$\bm{w}_{\text{ext}}$ は外力（宇宙空間では通常ゼロ）です。
この式を簡潔に書くために、把持行列（grasp matrix）$\bm{G}$ を定義します。
$$ \begin{equation} \bm{G}^T = \begin{pmatrix} \bm{J}_{o1}^T & \bm{J}_{o2}^T \end{pmatrix} \end{equation} $$
そして結合力ベクトルを次のように定義します。
$$ \begin{equation} \bm{f} = \begin{pmatrix} \bm{f}_1 \\ \bm{f}_2 \end{pmatrix} \end{equation} $$
すると、物体の運動方程式は次のようにコンパクトに表せます。
$$ \begin{equation} \bm{M}_o \ddot{\bm{x}}_o = \bm{G}^T \bm{f} + \bm{w}_{\text{ext}} \end{equation} $$
不足決定系としての力分配
ここで重要なのは、$\bm{f}$ の次元と $\bm{G}^T$ の次元の関係です。$\bm{f} \in \mathbb{R}^{2m}$（2本のアームの力を連結）に対し、運動方程式は $m$ 個しかありません。2Dの場合を例にとると、$\bm{f} \in \mathbb{R}^6$（各アーム3成分 × 2本）に対して方程式は3つです。
つまり、力の分配問題は不足決定系（underdetermined system）です。同じ物体の運動を実現する $\bm{f}$ の組み合わせが無限に存在します。
これは直感的にも理解できます。2人で本を持ち上げるとき、お互いが同じ力で持ち上げることもできれば、一方が強めに持ち上げてもう一方が軽く添えるだけでもよいのです。さらに、2人が本を「挟む」力（お互いの手を近づける向きの力）は、本の上下方向の運動には影響しません。この「挟む力」が後述する内力です。
最小ノルム解による力分配
不足決定系の解を一意に決めるために、最小ノルム基準がよく使われます。物体に必要な力 $\bm{w}_{\text{req}} = \bm{M}_o \ddot{\bm{x}}_o – \bm{w}_{\text{ext}}$ が与えられたとき、方程式は次のようになります。
$$ \begin{equation} \bm{G}^T \bm{f} = \bm{w}_{\text{req}} \end{equation} $$
$\|\bm{f}\|^2$ を最小化する（力のノルムを最小にする）解は、疑似逆行列を使って次のように求まります。
$$ \begin{equation} \bm{f}^* = \bm{G}^{T\dagger} \bm{w}_{\text{req}} = \bm{G}(\bm{G}^T \bm{G})^{-1} \bm{w}_{\text{req}} \end{equation} $$
ここで $\bm{G}^{T\dagger}$ は $\bm{G}^T$ のMoore-Penrose疑似逆行列です。$\bm{G}^T$ は行数 $m$ < 列数 $2m$ の「横長」行列なので、右疑似逆行列 $\bm{G}(\bm{G}^T\bm{G})^{-1}$ が使われます。
重み付き最小ノルム解
実際には、2本のアームの能力（最大出力、関節の余裕度など）は異なることが多いです。その場合、重み行列 $\bm{W}$ を導入した重み付き最小ノルム解を使います。
$$ \begin{equation} \bm{f}^* = \bm{W}^{-1} \bm{G} (\bm{G}^T \bm{W}^{-1} \bm{G})^{-1} \bm{w}_{\text{req}} \end{equation} $$
$\bm{W} = \text{diag}(w_1 \bm{I}_m, w_2 \bm{I}_m)$ として、$w_1 > w_2$ とすればアーム1の負担が小さくなり、アーム2が多くの力を担当します。各アームのトルク余裕や関節角度の限界に応じて $w_i$ を動的に調整すれば、実時間で最適な力分配が実現できます。
力の分配で「物体を動かすために必要な力」を両アームに割り当てる方法がわかりました。しかし、不足決定系の解空間には、物体の運動に寄与しない力 — つまり内力が潜んでいます。この内力をどう管理するかが、デュアルアーム制御の最も繊細な問題です。
内力の制御
内力とは何か
2つの手のひらで本を挟む場面を思い浮かべてください。本を持ち上げるには上向きの力が必要ですが、本が滑り落ちないようにするには両側から「挟む力」も必要です。この挟む力を強くしても弱くしても、本の上下運動には影響しません。しかし、挟む力が弱すぎると本は滑り落ち、強すぎると本が変形します。
この「物体の運動に寄与しないが、把持の安定性に直結する力」が内力（internal force）です。
数学的に定義しましょう。力ベクトル $\bm{f}$ は、物体の運動に寄与する成分 $\bm{f}_{\text{motion}}$ と内力成分 $\bm{f}_{\text{internal}}$ に分解できます。
$$ \begin{equation} \bm{f} = \bm{f}_{\text{motion}} + \bm{f}_{\text{internal}} \end{equation} $$
内力の定義は、把持行列の零空間に属する力です。
$$ \begin{equation} \bm{G}^T \bm{f}_{\text{internal}} = \bm{0} \end{equation} $$
つまり、内力は物体の重心に正味の力やモーメントを与えません。2本のアームが互いに押し合う・引き合う力のうち、物体を介して打ち消し合う成分が内力です。
零空間の構造
$\bm{G}^T \in \mathbb{R}^{m \times 2m}$ の零空間 $\mathcal{N}(\bm{G}^T)$ の次元は、次のように計算できます。
$$ \begin{equation} \dim \mathcal{N}(\bm{G}^T) = 2m – \text{rank}(\bm{G}^T) \end{equation} $$
$\bm{G}^T$ がフルランク（rank $= m$）であれば、零空間の次元は $2m – m = m$ です。2Dの場合（$m = 3$）、内力は3次元の空間に存在します。これは、$x$ 方向の挟む力、$y$ 方向の挟む力、回転方向のねじり力の3成分に対応します。
零空間の基底を $\bm{N} \in \mathbb{R}^{2m \times m}$ とすると（$\bm{G}^T \bm{N} = \bm{0}$）、任意の内力は次のように表せます。
$$ \begin{equation} \bm{f}_{\text{internal}} = \bm{N} \bm{\lambda} \end{equation} $$
ここで $\bm{\lambda} \in \mathbb{R}^m$ は内力の大きさを決めるパラメータベクトルです。
内力の制御が重要な理由
内力は物体の運動に影響しませんが、以下の理由で適切に制御する必要があります。
理由1: 把持の安定性
摩擦把持の場合、各把持点での法線力が摩擦力を維持するのに十分でなければ、物体は滑って脱落します。摩擦係数を $\mu$ とすると、各把持点での接線力 $f_t$ と法線力 $f_n$ には次の制約があります。
$$ \begin{equation} |f_t| \leq \mu f_n \end{equation} $$
内力を適切に設定して $f_n$ を十分大きくすることで、この摩擦条件を満たします。
理由2: 物体の破損防止
宇宙構造物は軽量化のために薄肉構造が多く、過大な挟む力で破損する可能性があります。内力が大きすぎると物体に不要な応力が生じます。
理由3: エネルギー効率
内力は物体の運動には寄与しないのに、アクチュエータのトルクは消費します。宇宙ロボットでは電力が限られるため、内力を必要最小限に抑えることがエネルギー効率の点で重要です。
内力制御の設計
内力の目標値を $\bm{\lambda}_d$ とすると、全体の力指令は次のようになります。
$$ \begin{equation} \bm{f} = \bm{G}^{T\dagger} \bm{w}_{\text{req}} + \bm{N} \bm{\lambda}_d \end{equation} $$
第1項が物体の運動を実現する最小ノルム力、第2項が目標内力です。$\bm{\lambda}_d$ は、摩擦条件を満たしつつ物体を破損しない範囲で設定します。
実際の制御では、把持点での力センサのフィードバックを用いて、実内力 $\bm{\lambda}$ が目標値 $\bm{\lambda}_d$ に追従するよう制御します。
$$ \begin{equation} \bm{\lambda} = \bm{\lambda}_d + \bm{K}_\lambda (\bm{\lambda}_d – \hat{\bm{\lambda}}) \end{equation} $$
ここで $\hat{\bm{\lambda}}$ は力センサから推定された現在の内力、$\bm{K}_\lambda$ はフィードバックゲインです。
内力の概念を理解したところで、宇宙環境に特有の問題に目を移しましょう。地上のデュアルアームと決定的に異なるのは、ベース衛星が固定されていないという点です。
宇宙環境での特殊性
自由浮遊ベースの反動問題
地上のロボットは床にボルト止めされており、ベースは固定です。アームが物体を操作するとき、反力は全てベースを通じて地面に逃げます。しかし宇宙空間では、ベース衛星は自由浮遊体です。アームが物体に力を加えると、その反力がベース衛星を動かします。
これは自由浮遊ロボットの動力学で解説したとおりですが、デュアルアームでは反動がさらに深刻になります。なぜなら、2本のアームが同時に力を出すため、ベースへの反動が2倍になりうるからです。
自由浮遊系の運動量保存則を書きます。系全体（ベース + アーム1 + アーム2 + 物体）に外力が作用しない場合、運動量とと角運動量が保存されます。
$$ \begin{equation} \bm{p}_{\text{total}} = m_b \dot{\bm{x}}_b + \sum_{i=1}^{2} \sum_{j=1}^{n_i} m_{ij} \dot{\bm{x}}_{ij} + m_o \dot{\bm{x}}_o = \text{const} \end{equation} $$
$$ \begin{equation} \bm{L}_{\text{total}} = \bm{I}_b \bm{\omega}_b + \sum_{i=1}^{2} \sum_{j=1}^{n_i} \bm{I}_{ij} \bm{\omega}_{ij} + \bm{I}_o \bm{\omega}_o = \text{const} \end{equation} $$
ここで添字 $b$ はベース、$ij$ はアーム $i$ のリンク $j$、$o$ は把持物体を表します。初期状態で系全体が静止していれば、$\bm{p}_{\text{total}} = \bm{0}$、$\bm{L}_{\text{total}} = \bm{0}$ です。
反動相殺の戦略
デュアルアームの大きな利点は、2本のアームの動きを逆方向にすることで、ベースへの反動を相殺できることです。
運動量保存則 $\bm{p}_{\text{total}} = \bm{0}$ から、ベースの速度は次のように表せます。
$$ \begin{equation} \dot{\bm{x}}_b = -\frac{1}{m_b} \left( \sum_{i=1}^{2} \sum_{j=1}^{n_i} m_{ij} \dot{\bm{x}}_{ij} + m_o \dot{\bm{x}}_o \right) \end{equation} $$
もし2本のアームが対称的な構成で逆方向に動くなら、$\sum m_{1j}\dot{\bm{x}}_{1j} \approx -\sum m_{2j}\dot{\bm{x}}_{2j}$ となり、ベースへの並進方向の反動が相殺されます。これは物体を回転させる操作（2本のアームで物体を反対方向に押す）で特に有効です。
ただし、完全な相殺は一般に不可能です。2本のアームの質量配分や関節構成が完全に対称でない限り、残留反動が生じます。そこで実用的には、以下のような段階的アプローチをとります。
- 計画段階: アームの軌道を計画する際に、ベースへの反動を最小化する軌道を選ぶ
- 制御段階: 残留反動をリアクションホイール等の姿勢制御で吸収する
- 分配段階: 力の分配係数 $\alpha_i$ を調整し、ベースへの反動が最小になるよう最適化する
一般化ヤコビアン
自由浮遊系では、ベースが動くため、通常のヤコビアンに代わって一般化ヤコビアン（Generalized Jacobian Matrix, GJM）を使います。
固定ベースでは $\dot{\bm{x}}_i = \bm{J}_i \dot{\bm{q}}_i$ ですが、自由浮遊系ではベースの運動も含めて次のようになります。
$$ \begin{equation} \dot{\bm{x}}_i = \bm{J}_{bi} \dot{\bm{x}}_b + \bm{J}_{mi} \dot{\bm{q}}_i \end{equation} $$
ここで $\bm{J}_{bi}$ はベース速度から手先速度への写像、$\bm{J}_{mi}$ は関節速度から手先速度への写像です。運動量保存則を使ってベースの速度を消去すると、一般化ヤコビアン $\bm{J}_i^*$ が得られます。
$$ \begin{equation} \dot{\bm{x}}_i = \bm{J}_i^*(\bm{q}) \dot{\bm{q}}_i + \text{(もう一方のアームの寄与)} \end{equation} $$
一般化ヤコビアンは関節角度だけでなく系全体の質量配分に依存するため、固定ベースのヤコビアンよりも複雑です。しかし、これを正しく計算することがデュアルアームの精密制御の鍵となります。
質量バランスの考慮
宇宙でのデュアルアーム設計では、質量の対称配置が重要です。2本のアームがベース衛星の片側に偏って配置されると、アームの運動によるベースの反動が大きくなります。
理想的には、2本のアームはベースの重心に対して対称に配置します。これにより、同じ大きさで反対方向の動きが反動を自然に相殺し、ベースの姿勢乱れを最小化できます。ISSのカナダアーム2や技術試験衛星ETS-VIIのロボットアームの設計でも、この質量バランスの原則が反映されています。
宇宙特有の問題を理解したところで、次はデュアルアーム協調制御の理論的な統一フレームワークを紹介します。ここまでの個別の議論（運動学、力分配、内力、反動）を一つの制御構造にまとめ上げるのが、絶対・相対座標系による協調制御です。
協調制御フレームワーク — 絶対座標系と相対座標系
座標変換のアイデア
デュアルアーム制御の最も洗練されたフレームワークは、2本のアームの手先位置を「絶対座標」と「相対座標」に変換するものです。このアイデアは Uchiyama & Dauchez（1988）に遡り、現在でもデュアルアーム制御の標準的な枠組みとして使われています。
まず、なぜこの変換が有効なのかを直感的に理解しましょう。2人で長い棒を持って移動する場面を考えてください。この作業では、2つの異なる「目標」が同時に存在します。
- 棒全体を目的地に運ぶ — 棒の中心がどこにあるか（絶対位置）
- 棒を落とさないようにしっかり持つ — 2人の手の間隔と力（相対位置）
この2つの目標は互いに独立に制御できます。棒を右に動かしたいなら2人とも右に動けばよく（絶対位置の制御）、棒を回転させたいなら2人が逆方向に動けばよい（相対位置の制御）。この分離こそが、絶対・相対座標系の核心です。
絶対座標と相対座標の定義
2本のアームの手先位置 $\bm{x}_1, \bm{x}_2$ から、絶対座標 $\bm{x}_{\text{abs}}$ と相対座標 $\bm{x}_{\text{rel}}$ を定義します。
$$ \begin{equation} \bm{x}_{\text{abs}} = \frac{1}{2}(\bm{x}_1 + \bm{x}_2) \end{equation} $$
$$ \begin{equation} \bm{x}_{\text{rel}} = \bm{x}_1 – \bm{x}_2 \end{equation} $$
絶対座標は2つの手先の中点（物体の重心に近い位置に対応）、相対座標は2つの手先の差分（物体のサイズ・姿勢に対応）です。
この変換を行列形式で書くと、次のようになります。
$$ \begin{equation} \begin{pmatrix} \bm{x}_{\text{abs}} \\ \bm{x}_{\text{rel}} \end{pmatrix} = \begin{pmatrix} \frac{1}{2}\bm{I} & \frac{1}{2}\bm{I} \\ \bm{I} & -\bm{I} \end{pmatrix} \begin{pmatrix} \bm{x}_1 \\ \bm{x}_2 \end{pmatrix} = \bm{T} \begin{pmatrix} \bm{x}_1 \\ \bm{x}_2 \end{pmatrix} \end{equation} $$
変換行列 $\bm{T}$ は正則であるため、逆変換も可能です。
$$ \begin{equation} \begin{pmatrix} \bm{x}_1 \\ \bm{x}_2 \end{pmatrix} = \bm{T}^{-1} \begin{pmatrix} \bm{x}_{\text{abs}} \\ \bm{x}_{\text{rel}} \end{pmatrix} = \begin{pmatrix} \bm{I} & \frac{1}{2}\bm{I} \\ \bm{I} & -\frac{1}{2}\bm{I} \end{pmatrix} \begin{pmatrix} \bm{x}_{\text{abs}} \\ \bm{x}_{\text{rel}} \end{pmatrix} \end{equation} $$
$\bm{T}^{-1}$ を確認しましょう。第1行から $\bm{x}_1 = \bm{x}_{\text{abs}} + \frac{1}{2}\bm{x}_{\text{rel}}$ が得られ、これは定義式 $\bm{x}_{\text{abs}} = \frac{1}{2}(\bm{x}_1 + \bm{x}_2)$、$\bm{x}_{\text{rel}} = \bm{x}_1 – \bm{x}_2$ と整合します。
力空間の変換
位置の変換に対応して、力空間でも同様の変換を行います。仮想仕事の原理（$\delta W = \bm{f}^T \delta \bm{x}$）が変換の前後で保存されるためには、力の変換は位置変換の逆転置でなければなりません。
$$ \begin{equation} \begin{pmatrix} \bm{f}_{\text{abs}} \\ \bm{f}_{\text{rel}} \end{pmatrix} = (\bm{T}^{-1})^T \begin{pmatrix} \bm{f}_1 \\ \bm{f}_2 \end{pmatrix} = \begin{pmatrix} \bm{I} & \bm{I} \\ \frac{1}{2}\bm{I} & -\frac{1}{2}\bm{I} \end{pmatrix} \begin{pmatrix} \bm{f}_1 \\ \bm{f}_2 \end{pmatrix} \end{equation} $$
ここで仮想仕事の保存を確認します。元の座標系での仮想仕事は次のとおりです。
$$ \delta W = \bm{f}_1^T \delta\bm{x}_1 + \bm{f}_2^T \delta\bm{x}_2 $$
変換後の座標系での仮想仕事は次のとおりです。
$$ \delta W’ = \bm{f}_{\text{abs}}^T \delta\bm{x}_{\text{abs}} + \bm{f}_{\text{rel}}^T \delta\bm{x}_{\text{rel}} $$
位置変換 $\delta\bm{x} = \bm{T}^{-1} \delta\bm{x}’$ と力変換 $\bm{f}’ = (\bm{T}^{-1})^T \bm{f}$ を代入すると $\delta W’ = \bm{f}^T (\bm{T}^{-1})(\bm{T}^{-1})^{-1} \delta\bm{x}’ \cdot … $ となり、仮想仕事が保存されることが確認できます。
これらの定義から、力の物理的意味が明確になります。
- 絶対力 $\bm{f}_{\text{abs}} = \bm{f}_1 + \bm{f}_2$: 2本のアームが物体に加える合力。物体の並進・回転を生み出す
- 相対力 $\bm{f}_{\text{rel}} = \frac{1}{2}(\bm{f}_1 – \bm{f}_2)$: 2本のアームの力の差分。内力に対応する
協調制御の分離構造
絶対・相対座標系の最大の利点は、物体の運動制御と内力制御を分離できることです。
絶対座標系の制御（物体の運動）:
$$ \begin{equation} \bm{f}_{\text{abs}} = \bm{M}_d^{\text{abs}} (\ddot{\bm{x}}_{\text{abs},d} – \ddot{\bm{x}}_{\text{abs}}) + \bm{D}_d^{\text{abs}} (\dot{\bm{x}}_{\text{abs},d} – \dot{\bm{x}}_{\text{abs}}) + \bm{K}_d^{\text{abs}} (\bm{x}_{\text{abs},d} – \bm{x}_{\text{abs}}) \end{equation} $$
相対座標系の制御（内力）:
$$ \begin{equation} \bm{f}_{\text{rel}} = \bm{f}_{\text{rel},d} + \bm{D}_d^{\text{rel}} (\dot{\bm{x}}_{\text{rel},d} – \dot{\bm{x}}_{\text{rel}}) + \bm{K}_d^{\text{rel}} (\bm{x}_{\text{rel},d} – \bm{x}_{\text{rel}}) \end{equation} $$
絶対座標系の制御は通常のインピーダンス制御（あるいはPD制御）と同形式で、物体の位置・速度を目標値に追従させます。相対座標系の制御は、2つの手先の相対位置を維持しつつ、目標内力 $\bm{f}_{\text{rel},d}$ を実現します。
この分離により、物体の運動計画（絶対座標系）と把持力の管理（相対座標系）をそれぞれ独立に設計でき、制御系の見通しが格段によくなります。
アームへの力指令への変換
最終的に、各アームへの力指令は逆変換で求まります。
$$ \begin{equation} \bm{f}_1 = \frac{1}{2}\bm{f}_{\text{abs}} + \bm{f}_{\text{rel}} \end{equation} $$
$$ \begin{equation} \bm{f}_2 = \frac{1}{2}\bm{f}_{\text{abs}} – \bm{f}_{\text{rel}} \end{equation} $$
この式は先ほどの力の変換の逆変換から直接得られます。物体を右に動かしたいなら $\bm{f}_{\text{abs}}$ の $x$ 成分を正にし、挟む力を調整したいなら $\bm{f}_{\text{rel}}$ を変えるだけです。互いに干渉しないため、チューニングも容易になります。
ここまでで協調制御の理論的フレームワークが完成しました。いよいよ、これらの理論をPythonで実装し、2Dデュアルアームの協調動作をシミュレーションしてみましょう。
Pythonシミュレーション: 2Dデュアルアーム協調制御
シミュレーションの概要
ここでは、2D平面上でベースに固定された2本の2リンクアームが、剛体棒状物体を把持し、協調して目標位置・姿勢に移動させるシミュレーションを実装します。
シミュレーションで検証する内容は以下の3点です。
- 閉運動連鎖の拘束を満たしながら物体を移動できるか
- 絶対・相対座標系による制御が正しく機能するか
- 内力が目標値に制御されるか
まず、2リンクアームの順運動学とヤコビアンを定義します。
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import FancyArrowPatch
from matplotlib.collections import LineCollection
# 2リンクアームの順運動学
def forward_kinematics(q, L, base):
"""
2リンクアームの順運動学
q: [q1, q2] 関節角度
L: [L1, L2] リンク長
base: [bx, by] ベース位置
戻り値: 手先位置 [x, y], 各関節位置のリスト
"""
x0, y0 = base
x1 = x0 + L[0] * np.cos(q[0])
y1 = y0 + L[0] * np.sin(q[0])
x2 = x1 + L[1] * np.cos(q[0] + q[1])
y2 = y1 + L[1] * np.sin(q[0] + q[1])
joints = [(x0, y0), (x1, y1), (x2, y2)]
return np.array([x2, y2]), joints
# 2リンクアームのヤコビアン
def jacobian_2link(q, L):
"""
2リンクアームのヤコビアン (2x2)
"""
s1 = np.sin(q[0])
c1 = np.cos(q[0])
s12 = np.sin(q[0] + q[1])
c12 = np.cos(q[0] + q[1])
J = np.array([
[-L[0]*s1 - L[1]*s12, -L[1]*s12],
[ L[0]*c1 + L[1]*c12, L[1]*c12]
])
return J
# 逆運動学 (解析解)
def inverse_kinematics(target, L, base, elbow_sign=1):
"""
2リンクアームの逆運動学
elbow_sign: +1 (肘上), -1 (肘下)
"""
dx = target[0] - base[0]
dy = target[1] - base[1]
r2 = dx**2 + dy**2
cos_q2 = (r2 - L[0]**2 - L[1]**2) / (2 * L[0] * L[1])
cos_q2 = np.clip(cos_q2, -1, 1)
q2 = elbow_sign * np.arccos(cos_q2)
q1 = np.arctan2(dy, dx) - np.arctan2(L[1]*np.sin(q2), L[0] + L[1]*np.cos(q2))
return np.array([q1, q2])
上のコードでは、2リンクアームの基本要素を3つの関数で定義しています。forward_kinematics は関節角度から手先位置を計算し、可視化用に各関節の位置も返します。jacobian_2link は関節速度と手先速度の関係を表す $2 \times 2$ のヤコビ行列を計算します。inverse_kinematics は手先の目標位置から関節角度を解析的に求めます。elbow_sign パラメータで肘の向き（上/下）を指定でき、デュアルアームの左右で異なる肘の向きを選べるようにしています。
次に、デュアルアーム系の設定と協調制御の実装に進みます。
# デュアルアーム系のパラメータ
L1 = [1.0, 0.8] # アーム1のリンク長
L2 = [1.0, 0.8] # アーム2のリンク長
base1 = np.array([-1.5, 0.0]) # アーム1のベース位置
base2 = np.array([ 1.5, 0.0]) # アーム2のベース位置
# 物体のパラメータ
object_length = 1.2 # 物体の長さ（把持点間距離）
object_mass = 2.0
object_inertia = object_mass * object_length**2 / 12
# 初期状態: 物体を水平に把持
obj_pos_init = np.array([0.0, 1.5]) # 物体重心の初期位置
obj_angle_init = 0.0 # 物体の初期姿勢角
# 目標状態: 物体を右上に移動し少し回転
obj_pos_target = np.array([0.3, 1.8])
obj_angle_target = np.deg2rad(20)
# 把持点の計算（物体座標系での位置）
def grasp_points(obj_pos, obj_angle, half_len):
"""物体の重心位置と姿勢角から両端の把持点を計算"""
dx = half_len * np.cos(obj_angle)
dy = half_len * np.sin(obj_angle)
g1 = obj_pos + np.array([-dx, -dy]) # 左端（アーム1）
g2 = obj_pos + np.array([ dx, dy]) # 右端（アーム2）
return g1, g2
half_len = object_length / 2.0
g1_init, g2_init = grasp_points(obj_pos_init, obj_angle_init, half_len)
# 初期関節角度（逆運動学で計算）
q1 = inverse_kinematics(g1_init, L1, base1, elbow_sign=1)
q2 = inverse_kinematics(g2_init, L2, base2, elbow_sign=-1)
print("=== デュアルアームの初期設定 ===")
print(f"アーム1 ベース: {base1}, 関節角度: {np.rad2deg(q1)}")
print(f"アーム2 ベース: {base2}, 関節角度: {np.rad2deg(q2)}")
ee1, _ = forward_kinematics(q1, L1, base1)
ee2, _ = forward_kinematics(q2, L2, base2)
print(f"アーム1 手先: {ee1}")
print(f"アーム2 手先: {ee2}")
print(f"把持点1: {g1_init}, 把持点2: {g2_init}")
この初期設定では、2本のアームをベース位置 $(-1.5, 0)$ と $(1.5, 0)$ に配置し、長さ1.2の棒状物体を高さ1.5の位置で水平に把持する状況を作っています。アーム1は肘上（elbow_sign=1）、アーム2は肘下（elbow_sign=-1）の構えで、物体を挟み込む自然な姿勢をとります。
続いて、協調制御の核心である絶対・相対座標系の変換と制御則を実装します。
# 絶対・相対座標系の変換
def to_abs_rel(x1, x2):
"""アーム座標 → 絶対・相対座標"""
x_abs = 0.5 * (x1 + x2)
x_rel = x1 - x2
return x_abs, x_rel
def from_abs_rel(x_abs, x_rel):
"""絶対・相対座標 → アーム座標"""
x1 = x_abs + 0.5 * x_rel
x2 = x_abs - 0.5 * x_rel
return x1, x2
def force_to_abs_rel(f1, f2):
"""アーム力 → 絶対力・相対力"""
f_abs = f1 + f2
f_rel = 0.5 * (f1 - f2)
return f_abs, f_rel
def force_from_abs_rel(f_abs, f_rel):
"""絶対力・相対力 → アーム力"""
f1 = 0.5 * f_abs + f_rel
f2 = 0.5 * f_abs - f_rel
return f1, f2
変換関数は本記事の数式をそのまま実装したものです。to_abs_rel と from_abs_rel は位置の変換、force_to_abs_rel と force_from_abs_rel は力の変換を行います。仮想仕事の保存により、力の変換行列は位置変換行列の逆転置になっていることに注目してください。
次に、時間積分によるシミュレーションを実装します。
# シミュレーションパラメータ
dt = 0.005
T = 3.0
steps = int(T / dt)
# 制御ゲイン（絶対座標系）
Kp_abs = 50.0
Kd_abs = 20.0
# 制御ゲイン（相対座標系）
Kp_rel = 80.0
Kd_rel = 25.0
# 目標内力（把持方向の挟む力）
f_squeeze_target = 5.0
# 状態の初期化
obj_pos = obj_pos_init.copy()
obj_vel = np.zeros(2)
obj_angle = obj_angle_init
obj_omega = 0.0
# ログ用
log_obj_pos = []
log_obj_angle = []
log_abs_pos = []
log_rel_pos = []
log_f1 = []
log_f2 = []
log_f_abs = []
log_f_rel = []
log_time = []
for step in range(steps):
t = step * dt
# 現在の把持点
g1, g2 = grasp_points(obj_pos, obj_angle, half_len)
# 絶対・相対座標
x_abs, x_rel = to_abs_rel(g1, g2)
# 目標の把持点
g1_d, g2_d = grasp_points(obj_pos_target, obj_angle_target, half_len)
x_abs_d, x_rel_d = to_abs_rel(g1_d, g2_d)
# 速度（数値微分の代わりに状態から計算）
v_abs = obj_vel
v_rel = np.zeros(2) # 相対速度は物体が剛体なら回転成分のみ
# 絶対座標系の制御: 物体の運動
f_abs = Kp_abs * (x_abs_d - x_abs) + Kd_abs * (np.zeros(2) - v_abs)
# 相対座標系の制御: 把持の維持 + 内力
# 相対位置の目標値は初期値を維持（物体の形状を保つ）
f_rel_position = Kp_rel * (x_rel_d - x_rel) + Kd_rel * (np.zeros(2) - v_rel)
# 内力の追加（物体軸方向の挟む力）
squeeze_dir = np.array([np.cos(obj_angle), np.sin(obj_angle)])
f_rel = f_rel_position + f_squeeze_target * squeeze_dir
# 各アームの力に変換
f1, f2 = force_from_abs_rel(f_abs, f_rel)
# 物体の運動方程式（簡略化: 並進のみ）
f_total = f1 + f2 # 合力
torque_total = (np.cross(g1 - obj_pos, f1) +
np.cross(g2 - obj_pos, f2))
acc = f_total / object_mass
alpha = torque_total / object_inertia
# オイラー積分
obj_vel += acc * dt
obj_pos += obj_vel * dt
obj_omega += alpha * dt
obj_angle += obj_omega * dt
# ダンピング（数値安定性のため）
obj_vel *= 0.998
obj_omega *= 0.998
# ログ
log_obj_pos.append(obj_pos.copy())
log_obj_angle.append(obj_angle)
log_abs_pos.append(x_abs.copy())
log_rel_pos.append(x_rel.copy())
log_f1.append(f1.copy())
log_f2.append(f2.copy())
log_f_abs.append(f_abs.copy())
log_f_rel.append(f_rel.copy())
log_time.append(t)
# numpy配列に変換
log_obj_pos = np.array(log_obj_pos)
log_obj_angle = np.array(log_obj_angle)
log_abs_pos = np.array(log_abs_pos)
log_rel_pos = np.array(log_rel_pos)
log_f1 = np.array(log_f1)
log_f2 = np.array(log_f2)
log_f_abs = np.array(log_f_abs)
log_f_rel = np.array(log_f_rel)
log_time = np.array(log_time)
print("=== シミュレーション完了 ===")
print(f"最終位置: {log_obj_pos[-1]}")
print(f"最終角度: {np.rad2deg(log_obj_angle[-1]):.2f} deg")
print(f"目標位置: {obj_pos_target}")
print(f"目標角度: {np.rad2deg(obj_angle_target):.2f} deg")
シミュレーションの核心部分では、各タイムステップで以下の処理を行っています。まず現在の把持点から絶対・相対座標を計算し、次に絶対座標系のPD制御で物体を目標位置に向かわせる力（合力）を計算します。相対座標系では把持形状の維持と内力（挟む力）を制御します。これらを各アームの力に変換し、物体の運動方程式で位置を更新します。
絶対座標系のゲイン $K_p = 50, K_d = 20$ は、物体を滑らかに目標位置に移動させるための値です。相対座標系のゲイン $K_p = 80, K_d = 25$ はやや高めに設定し、把持の維持を確実にしています。内力の目標値 $f_{\text{squeeze}} = 5.0$ は、物体軸方向の挟む力として加えています。
では、シミュレーション結果を可視化しましょう。
fig, axes = plt.subplots(2, 2, figsize=(14, 10))
# (1) 物体の位置の時間変化
ax = axes[0, 0]
ax.plot(log_time, log_obj_pos[:, 0], label='x', linewidth=2)
ax.plot(log_time, log_obj_pos[:, 1], label='y', linewidth=2)
ax.axhline(obj_pos_target[0], color='C0', linestyle='--', alpha=0.5, label=f'x target = {obj_pos_target[0]}')
ax.axhline(obj_pos_target[1], color='C1', linestyle='--', alpha=0.5, label=f'y target = {obj_pos_target[1]}')
ax.set_xlabel('Time [s]')
ax.set_ylabel('Position [m]')
ax.set_title('Object Position')
ax.legend()
ax.grid(True, alpha=0.3)
# (2) 物体の角度の時間変化
ax = axes[0, 1]
ax.plot(log_time, np.rad2deg(log_obj_angle), linewidth=2, color='C2')
ax.axhline(np.rad2deg(obj_angle_target), color='C2', linestyle='--', alpha=0.5,
label=f'target = {np.rad2deg(obj_angle_target):.1f} deg')
ax.set_xlabel('Time [s]')
ax.set_ylabel('Angle [deg]')
ax.set_title('Object Orientation')
ax.legend()
ax.grid(True, alpha=0.3)
# (3) 各アームの力
ax = axes[1, 0]
ax.plot(log_time, np.linalg.norm(log_f1, axis=1), label='|f1| (Arm 1)', linewidth=2)
ax.plot(log_time, np.linalg.norm(log_f2, axis=1), label='|f2| (Arm 2)', linewidth=2)
ax.set_xlabel('Time [s]')
ax.set_ylabel('Force magnitude [N]')
ax.set_title('Arm Forces')
ax.legend()
ax.grid(True, alpha=0.3)
# (4) 絶対力と相対力
ax = axes[1, 1]
ax.plot(log_time, np.linalg.norm(log_f_abs, axis=1), label='|f_abs| (motion)', linewidth=2)
ax.plot(log_time, np.linalg.norm(log_f_rel, axis=1), label='|f_rel| (internal)', linewidth=2)
ax.set_xlabel('Time [s]')
ax.set_ylabel('Force magnitude [N]')
ax.set_title('Absolute vs Relative Force')
ax.legend()
ax.grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('dual_arm_time_response.png', dpi=150, bbox_inches='tight')
plt.show()
上のグラフから、制御の振る舞いが4つの側面から確認できます。
-
物体の位置（左上）: $x$ 座標は初期値0.0から目標値0.3へ、$y$ 座標は1.5から1.8へ、それぞれ約1秒で収束しています。オーバーシュートはダンピングにより抑えられ、滑らかな応答が得られています。
-
物体の角度（右上）: 姿勢角が0度から目標の20度へ回転しています。並進と回転が同時に、かつ干渉なく制御されていることがわかります。
-
各アームの力（左下）: 初期の過渡状態では両アームとも大きな力を出していますが、定常状態では内力成分（挟む力）のみが残り、均等な力になっています。
-
絶対力と相対力（右下）: 絶対力（物体の運動に使われる力）は過渡状態で大きく、定常状態でゼロに収束します。相対力（内力成分）は定常的に目標内力の値を維持しています。このグラフが、絶対・相対座標系による「運動制御と内力制御の分離」を最も直感的に示しています。
次に、アームと物体の動きをアニメーション的に可視化します。
fig, ax = plt.subplots(1, 1, figsize=(12, 8))
# 表示するタイムステップ（時間間隔を開けて）
display_steps = [0, 60, 120, 200, 300, 599]
colors = plt.cm.viridis(np.linspace(0, 1, len(display_steps)))
for idx, step_i in enumerate(display_steps):
t = log_time[step_i]
op = log_obj_pos[step_i]
oa = log_obj_angle[step_i]
g1, g2 = grasp_points(op, oa, half_len)
# アームの関節角度を逆運動学で計算
q1_vis = inverse_kinematics(g1, L1, base1, elbow_sign=1)
q2_vis = inverse_kinematics(g2, L2, base2, elbow_sign=-1)
_, joints1 = forward_kinematics(q1_vis, L1, base1)
_, joints2 = forward_kinematics(q2_vis, L2, base2)
alpha = 0.3 + 0.7 * (idx / (len(display_steps) - 1))
color = colors[idx]
# アーム1の描画
x_coords1 = [j[0] for j in joints1]
y_coords1 = [j[1] for j in joints1]
ax.plot(x_coords1, y_coords1, 'o-', color=color, linewidth=2.5,
markersize=6, alpha=alpha)
# アーム2の描画
x_coords2 = [j[0] for j in joints2]
y_coords2 = [j[1] for j in joints2]
ax.plot(x_coords2, y_coords2, 's-', color=color, linewidth=2.5,
markersize=6, alpha=alpha)
# 物体の描画
ax.plot([g1[0], g2[0]], [g1[1], g2[1]], '-', color=color,
linewidth=4, alpha=alpha)
# 時刻ラベル
ax.annotate(f't={t:.2f}s', xy=(op[0], op[1] + 0.12),
fontsize=8, ha='center', color=color, alpha=alpha)
# ベース位置のマーク
ax.plot(*base1, 'k^', markersize=12, label='Base 1')
ax.plot(*base2, 'k^', markersize=12, label='Base 2')
# 目標位置
g1_t, g2_t = grasp_points(obj_pos_target, obj_angle_target, half_len)
ax.plot([g1_t[0], g2_t[0]], [g1_t[1], g2_t[1]], 'r--',
linewidth=3, alpha=0.5, label='Target')
ax.set_xlabel('x [m]')
ax.set_ylabel('y [m]')
ax.set_title('Dual-Arm Cooperative Motion (Time Sequence)')
ax.set_aspect('equal')
ax.legend(loc='lower right')
ax.grid(True, alpha=0.3)
ax.set_xlim(-2.5, 2.5)
ax.set_ylim(-0.5, 2.8)
plt.tight_layout()
plt.savefig('dual_arm_motion_sequence.png', dpi=150, bbox_inches='tight')
plt.show()
このモーションシーケンス図から、デュアルアームの協調動作の全体像が把握できます。時刻が進むにつれて（色が暗い→明るいに変化）、物体（太い線分）が初期位置（水平・中央）から目標位置（右上・20度回転）に移動する様子が確認できます。2本のアームが同時に滑らかに動き、物体の両端を常に把持し続けている点が重要です。赤い破線が目標位置を示しており、最終時刻で目標に十分近づいていることがわかります。
最後に、力の分配と内力の関係を可視化します。
fig, axes = plt.subplots(1, 2, figsize=(14, 5))
# (1) 力ベクトルの時間変化
ax = axes[0]
ax.plot(log_time, log_f1[:, 0], label='f1_x', linewidth=1.5)
ax.plot(log_time, log_f1[:, 1], label='f1_y', linewidth=1.5)
ax.plot(log_time, log_f2[:, 0], label='f2_x', linewidth=1.5, linestyle='--')
ax.plot(log_time, log_f2[:, 1], label='f2_y', linewidth=1.5, linestyle='--')
ax.set_xlabel('Time [s]')
ax.set_ylabel('Force [N]')
ax.set_title('Force Components (Arm 1 vs Arm 2)')
ax.legend()
ax.grid(True, alpha=0.3)
# (2) 内力の分析
ax = axes[1]
# 物体軸方向の内力成分を計算
squeeze_force = []
for i in range(len(log_time)):
angle = log_obj_angle[i]
direction = np.array([np.cos(angle), np.sin(angle)])
# 相対力の物体軸方向成分 = 内力
f_rel_i = log_f_rel[i]
squeeze = np.dot(f_rel_i, direction)
squeeze_force.append(squeeze)
squeeze_force = np.array(squeeze_force)
ax.plot(log_time, squeeze_force, label='Squeeze force (actual)', linewidth=2)
ax.axhline(f_squeeze_target, color='r', linestyle='--', alpha=0.7,
label=f'Target = {f_squeeze_target} N')
ax.set_xlabel('Time [s]')
ax.set_ylabel('Internal Force [N]')
ax.set_title('Internal (Squeeze) Force Control')
ax.legend()
ax.grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('dual_arm_force_analysis.png', dpi=150, bbox_inches='tight')
plt.show()
この図は力の分配と内力制御の核心を示しています。
左のグラフでは、2本のアームの力成分を比較しています。過渡状態では $f_{1y}$ と $f_{2y}$ がともに正の値（上向き）をとり、物体を持ち上げる合力を生み出しています。一方で $f_{1x}$ と $f_{2x}$ は互いに逆符号の傾向を示し、これが物体の回転に使われています。定常状態では、$x$ 方向の力が残っていますが、これは物体を挟む内力（相対力）に対応しています。
右のグラフでは、物体軸方向の内力（挟む力）が目標値5.0 Nに追従する様子を示しています。過渡状態では物体の運動に伴う変動がありますが、定常状態では目標値にほぼ一致しています。これは、絶対・相対座標系による分離制御が正しく機能し、物体の運動制御と内力制御が互いに干渉していないことの証拠です。
力分配の最適性の検証
最後に、力の分配が最小ノルム基準に近いことを確認するコードを追加します。
# 力の効率分析
total_force_norm = np.linalg.norm(log_f1, axis=1) + np.linalg.norm(log_f2, axis=1)
net_force_norm = np.linalg.norm(log_f1 + log_f2, axis=1)
fig, ax = plt.subplots(figsize=(10, 5))
ax.plot(log_time, total_force_norm, label='Total (|f1| + |f2|)', linewidth=2)
ax.plot(log_time, net_force_norm, label='Net (|f1 + f2|)', linewidth=2)
ax.fill_between(log_time, net_force_norm, total_force_norm, alpha=0.2, color='C2',
label='Internal force contribution')
ax.set_xlabel('Time [s]')
ax.set_ylabel('Force [N]')
ax.set_title('Force Efficiency: Total vs Net Force')
ax.legend()
ax.grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('dual_arm_force_efficiency.png', dpi=150, bbox_inches='tight')
plt.show()
# 定常状態での効率
steady_start = int(2.0 / dt)
avg_total = np.mean(total_force_norm[steady_start:])
avg_net = np.mean(net_force_norm[steady_start:])
print(f"=== 定常状態の力の効率 ===")
print(f"平均合計力 (|f1| + |f2|): {avg_total:.3f} N")
print(f"平均正味力 (|f1 + f2|): {avg_net:.3f} N")
print(f"内力による超過: {avg_total - avg_net:.3f} N")
print(f"効率: {avg_net / avg_total * 100:.1f}%")
このグラフは、デュアルアームの「力の効率」を示します。青い線（Total）は2本のアームが出している力の絶対値の和、オレンジの線（Net）は合力の大きさです。両者の差分（緑の塗りつぶし）が内力に使われているエネルギーです。
過渡状態では合力が大きく、力の大半が物体の加速に使われています。しかし定常状態（$t > 2$ s）では合力はほぼゼロ（物体が静止）で、残っている力はすべて内力です。これは「物体を挟んで保持する」ために必要なコストであり、内力の目標値を下げればこのコストを削減できますが、把持の安定性とのトレードオフになります。宇宙ロボットの設計では、このバランスが重要な設計パラメータとなります。
まとめ
本記事では、宇宙での両腕ロボットの協調制御について、基礎理論からPythonシミュレーションまで解説しました。
- 閉運動連鎖: デュアルアームが物体を把持すると運動連鎖がループを形成し、関節間に拘束が生じる。有効自由度は $n_1 + n_2 – m$ に減少する
- 制御アーキテクチャ: マスター・スレーブ方式は実装が容易、対等協調方式は力の効率とフォールトトレランスに優れ、宇宙用途では後者が主流
- 力の分配: 不足決定系を疑似逆行列（最小ノルム基準）で解き、重み付きで各アームの能力に応じた配分が可能
- 内力制御: 把持行列の零空間に属する力（内力）を適切に制御することで、物体の安定把持と破損防止を両立
- 宇宙環境の特殊性: 自由浮遊ベースの反動を、2本のアームの対称動作で相殺可能。一般化ヤコビアンが必要
- 絶対・相対座標系: 物体の運動制御（絶対座標）と内力制御（相対座標）を分離する統一フレームワークが強力
デュアルアーム協調制御は、宇宙ロボティクスの中でも実用性が高く、研究が活発な分野です。本記事では剛体物体を扱いましたが、実際の宇宙構造物は大型の太陽電池パドルやアンテナのように柔軟性を持つことが多く、その場合は把持点での力が構造物の振動を励起する問題が生じます。
次のステップとして、以下の記事も参考にしてください。
- フレキシブルマニピュレータの制御 — 柔軟構造物を扱う宇宙マニピュレータの制御理論