国際宇宙ステーション（ISS）の外壁に取り付けられたロボットアームが、故障した機器を掴んで交換する場面を想像してください。アームが大きく動けば、その反作用でステーション本体がゆっくり回り始めます。宇宙では地面に「足を踏ん張る」ことができないため、マニピュレータが発生する反力はすべてベース衛星の姿勢変化として現れるのです。
もしベースが回転すれば、太陽電池パネルの発電効率が下がったり、通信アンテナの指向がずれたりと、深刻な問題が生じます。では、アームを動かしつつ、ベースの姿勢を全く変えないような関節軌道は計画できるのでしょうか？ これが本記事で扱う「ゼロ反力軌道計画」（zero-reaction trajectory planning）の問いです。
ゼロ反力軌道計画を理解すると、以下の応用が広がります。
- 軌道上サービス（OOS）: 故障衛星の修理や燃料補給ミッションにおいて、サービサー衛星の姿勢を安定に保ちながらロボットアームで対象を操作する
- 宇宙ステーションのメンテナンス: ISSのカナダアーム2のような大型マニピュレータが作業中にステーションの姿勢制御系に過大な負荷をかけないための運用計画
- デブリ捕獲: 回転するデブリを捕獲する際、捕獲直前にベース衛星の姿勢を安定させておく必要がある
本記事の内容
- 問題設定 — 手先を目標に移動しつつベース姿勢変化をゼロにする
- ゼロ反力軌道の存在条件（冗長度とタスク空間の関係）
- 幾何学的拘束としてのゼロ反力条件
- 最適化ベースの軌道計画（コスト関数の設計）
- 逐次二次計画法（SQP）による数値解法
- 非ホロノミック系としての解釈
- Pythonでの最適化ベースのゼロ反力軌道計画と可視化
前提知識
この記事を読む前に、以下の記事を読んでおくと理解が深まります。
- 宇宙ロボットのリアクションレスマニューバ — 角運動量保存則と一般化ヤコビ行列の基礎
問題設定 — 自由浮遊する宇宙ロボットの課題
なぜ地上のロボットと違うのか
地上のロボットアームは、ボルトで固定された台座（ベース）に取り付けられています。アームがどんなに激しく動いても、台座は微動だにしません。地面からの反力が全てを支えてくれるからです。
ところが宇宙空間では状況が一変します。ベース衛星は宇宙空間に「浮いて」いるため、外力が作用しない限り系全体の角運動量は保存されます。ロボットアームの関節が回転すれば、角運動量保存則によりベース衛星は反対方向に回転します。これは日常で言えば、氷の上に立った人がバットを振り回すと身体が反対方向に回り始めるのと同じ原理です。
系のモデル
$n$ 自由度のマニピュレータがベース衛星に取り付けられた自由浮遊型宇宙ロボットを考えます。ベースの姿勢角を $\theta_0$、各関節角を $\bm{\phi} = (\phi_1, \phi_2, \ldots, \phi_n)^T$ とします。
系全体に外部トルクが作用しない場合、角運動量保存則は次のように書けます。
$$ \begin{equation} L = I_0 \dot{\theta}_0 + \bm{h}(\bm{\phi})^T \dot{\bm{\phi}} = \text{const} \end{equation} $$
ここで $I_0$ はベースの慣性モーメント（関節姿勢に依存する実効慣性を含む）、$\bm{h}(\bm{\phi})$ は関節速度がベースの角運動量に寄与する結合項です。
初期状態で系全体が静止している場合（$L = 0$）、ベースの角速度は関節速度で完全に決まります。
$$ \begin{equation} \dot{\theta}_0 = -\frac{\bm{h}(\bm{\phi})^T}{I_0} \dot{\bm{\phi}} \end{equation} $$
この式が伝えている物理は明快です。関節が動けば（$\dot{\bm{\phi}} \neq \bm{0}$）、一般にベースは反対方向に回転する（$\dot{\theta}_0 \neq 0$）ということです。
手先の運動学
手先（エンドエフェクタ）の位置 $\bm{r}_e$ は、ベース姿勢 $\theta_0$ と関節角 $\bm{\phi}$ の両方に依存します。手先速度は次のように表されます。
$$ \begin{equation} \dot{\bm{r}}_e = \bm{J}_0 \dot{\theta}_0 + \bm{J}_\phi \dot{\bm{\phi}} \end{equation} $$
ここで $\bm{J}_0$ はベース姿勢変化に対する手先速度のヤコビ行列、$\bm{J}_\phi$ は関節速度に対するヤコビ行列です。
角運動量保存則（式(2)）を代入すると、ベース角速度を消去できます。
$$ \begin{equation} \dot{\bm{r}}_e = \left(\bm{J}_\phi – \bm{J}_0 \frac{\bm{h}^T}{I_0}\right) \dot{\bm{\phi}} = \bm{J}^* \dot{\bm{\phi}} \end{equation} $$
この $\bm{J}^*$ は一般化ヤコビ行列（generalized Jacobian matrix, GJM）と呼ばれ、宇宙ロボット特有の概念です。地上ロボットのヤコビ行列に対応しますが、角運動量保存則の効果が織り込まれている点が本質的に異なります。
ここまでで、宇宙ロボットの基本的な運動学を整理しました。次に、「ベース姿勢を変えずにアームを動かす」というゼロ反力軌道の条件を数学的に定式化していきましょう。
ゼロ反力軌道の条件
直感的な理解
ゼロ反力軌道とは、「マニピュレータが動いてもベース衛星が全く回転しない」ような関節軌道のことです。
氷の上に立つ人の例えに戻ると、両腕を巧みに協調させて動かせば、身体を回転させずに手を特定の位置に持っていくことができます。ただし、腕が1本しかなければ、腕の自由度が制約を満たすのに精一杯で、手先の位置を自由に制御する余裕がないかもしれません。2本の腕（あるいは十分な関節数）があってはじめて、「ベースを回さない」という制約と「手先を目標に動かす」というタスクを同時に達成できるのです。
数学的な条件
角運動量保存則（$L = 0$）のもとで、ベース姿勢が変化しない条件は次式で表されます。
$$ \begin{equation} \dot{\theta}_0 = -\frac{\bm{h}(\bm{\phi})^T}{I_0} \dot{\bm{\phi}} = 0 \end{equation} $$
これは任意の時刻 $t$ において、関節速度 $\dot{\bm{\phi}}(t)$ がゼロ反力条件を満たすことを要求しています。
$$ \begin{equation} \bm{h}(\bm{\phi})^T \dot{\bm{\phi}} = 0 \end{equation} $$
幾何学的に言えば、関節速度ベクトル $\dot{\bm{\phi}}$ は常にベクトル $\bm{h}(\bm{\phi})$ に直交していなければなりません。$\bm{h}$ は関節角 $\bm{\phi}$ に依存して変化するため、この直交条件は関節空間内で位置によって向きが変わる「壁」のようなものです。
拘束付きヤコビ行列
ゼロ反力条件（式(6)）を手先の運動学に組み込みましょう。式(4)において $\dot{\theta}_0 = 0$ とすると、手先速度は次のようになります。
$$ \begin{equation} \dot{\bm{r}}_e = \bm{J}_\phi \dot{\bm{\phi}} \end{equation} $$
ただし、$\dot{\bm{\phi}}$ は式(6)の拘束を満たさなければなりません。ここでゼロ反力条件を満たす関節速度の集合は、$\bm{h}(\bm{\phi})^T$ の零空間（null space）に制限されます。
零空間への射影行列を次のように定義します。
$$ \begin{equation} \bm{N}_h = \bm{I}_n – \frac{\bm{h}\bm{h}^T}{\bm{h}^T \bm{h}} \end{equation} $$
ここで $\bm{I}_n$ は $n \times n$ の単位行列です。この $\bm{N}_h$ は $\bm{h}$ に直交する超平面への正射影であり、任意のベクトル $\bm{v}$ に対して $\bm{N}_h \bm{v}$ は式(6)を自動的に満たします。
$\bm{h}^T (\bm{N}_h \bm{v}) = \bm{h}^T \bm{v} – \bm{h}^T \frac{\bm{h}\bm{h}^T}{\bm{h}^T \bm{h}} \bm{v} = \bm{h}^T \bm{v} – \bm{h}^T \bm{v} = 0$ と確認できます。
ゼロ反力条件下で実現できる手先速度は次の形になります。
$$ \begin{equation} \dot{\bm{r}}_e = \bm{J}_\phi \bm{N}_h \bm{u} \end{equation} $$
ここで $\bm{u} \in \mathbb{R}^n$ は自由パラメータです。行列 $\bm{J}_\phi \bm{N}_h$ が拘束付きヤコビ行列であり、ゼロ反力条件のもとで手先が到達できる速度空間を特徴づけます。
ここまでで、ゼロ反力条件の数学的な形が明確になりました。次に、この条件を満たす軌道がそもそも存在するために必要な自由度の関係を考察します。
ゼロ反力軌道の存在条件 — 冗長度の役割
自由度の勘定
ゼロ反力軌道が存在するためには、マニピュレータに十分な冗長度が必要です。直感的に言えば、「ベースを回さない」という拘束を満たした上で、なお手先を自由に動かす余裕が残っていなければなりません。
定量的に考えましょう。マニピュレータの関節自由度を $n$、手先のタスク空間の次元を $m$ とします（平面問題なら $m = 2$、空間問題なら $m = 3$ または $m = 6$）。ゼロ反力条件は1個のスカラー拘束（2次元の場合）または3個のベクトル拘束（3次元の場合）を課します。2次元問題の場合で考えると、拘束の数を $c = 1$ としたとき、自由に使える自由度は次のようになります。
$$ \begin{equation} n_{\text{free}} = n – c \end{equation} $$
手先を $m$ 次元のタスク空間内で自由に動かすには、次の条件が必要です。
$$ \begin{equation} n – c \geq m \end{equation} $$
2次元平面の場合（$m = 2$, $c = 1$）では $n \geq 3$ が最低条件であり、3リンクのマニピュレータがぎりぎり成り立つ下限です。このとき自由パラメータは $n – c – m = 0$ なので、ゼロ反力軌道は（存在すれば）一意に決まります。
$n = 4$ であれば $n – c – m = 1$ の冗長度が生まれ、その自由度を使って軌道を最適化できます。例えば、関節トルクの最小化やマニピュラビリティの最大化といった副次的な目標を追求できるのです。
一般の3次元問題
3次元空間で手先の位置と姿勢を6自由度で制御する場合（$m = 6$）、ベース衛星の3軸回転に対するゼロ反力条件は $c = 3$ 個のスカラー拘束を課します。この場合、必要条件は次のようになります。
$$ \begin{equation} n \geq m + c = 6 + 3 = 9 \end{equation} $$
つまり、6自由度のタスクをゼロ反力で実現するには少なくとも9自由度のマニピュレータが必要です。実際のスペースマニピュレータでは7自由度（位置のみ制御）や、双腕で14自由度以上の構成が検討されています。
ランク条件
存在条件をより厳密に述べると、拘束付きヤコビ行列 $\bm{J}_\phi \bm{N}_h$ のランクが $m$ 以上であることが必要十分条件です。
$$ \begin{equation} \text{rank}(\bm{J}_\phi \bm{N}_h) \geq m \end{equation} $$
この条件が満たされない点は拘束特異姿勢と呼ばれ、ゼロ反力条件のもとでは手先がある方向に動けなくなります。通常の特異姿勢とは異なり、ゼロ反力条件に起因する特有の特異点です。
冗長度の議論で、ゼロ反力軌道が存在しうる条件がわかりました。しかし、「存在する」ことと「見つけられる」ことは別問題です。次に、この軌道を具体的に計画するための最適化手法を導入します。
最適化ベースの軌道計画
なぜ最適化が必要か
ゼロ反力条件を満たす関節速度は、射影行列 $\bm{N}_h$ で生成できることを見ました。しかし、速度レベルの制御だけでは不十分な理由があります。
第一に、手先を初期位置 $\bm{r}_0$ から目標位置 $\bm{r}_f$ に移動する経路計画は、速度の積分（すなわち位置レベル）の問題です。離散時間で考えると、各時間ステップでの関節角の変化量を決めながら、最終的な手先位置を合わせる必要があります。
第二に、冗長度がある場合、ゼロ反力条件を満たす軌道は無数に存在します。その中から「良い」軌道 — 例えば関節角変化が滑らかで、特異姿勢から遠い軌道 — を選ぶには最適化の枠組みが自然です。
第三に、実際のロボットには関節角のリミットや角速度の上限があり、これらの不等式制約を体系的に扱うには最適化が不可欠です。
離散時間での定式化
時間を $N$ ステップに離散化し、$k = 0, 1, \ldots, N$ での関節角を $\bm{\phi}_k$ とします。軌道計画問題は次の非線形最適化問題として定式化できます。
$$ \begin{equation} \min_{\bm{\phi}_1, \ldots, \bm{\phi}_N} J(\bm{\phi}_1, \ldots, \bm{\phi}_N) \end{equation} $$
コスト関数 $J$ は、複数の目的を重み付きで組み合わせたものです。
$$ \begin{equation} J = w_1 \underbrace{\|\bm{r}_e(\bm{\phi}_N) – \bm{r}_f\|^2}_{\text{目標到達}} + w_2 \underbrace{\sum_{k=0}^{N-1} \left(\bm{h}_k^T \Delta\bm{\phi}_k\right)^2}_{\text{反力最小化}} + w_3 \underbrace{\sum_{k=0}^{N-1} \|\Delta\bm{\phi}_k\|^2}_{\text{滑らかさ}} \end{equation} $$
ここで $\Delta\bm{\phi}_k = \bm{\phi}_{k+1} – \bm{\phi}_k$ は各ステップの関節角変化量、$\bm{h}_k = \bm{h}(\bm{\phi}_k)$ です。
各項の意味を説明します。
第1項（目標到達）: 最終時刻 $N$ での手先位置 $\bm{r}_e(\bm{\phi}_N)$ が目標 $\bm{r}_f$ に一致するよう促します。重み $w_1$ は大きく設定して、目標到達を最優先にします。
第2項（反力最小化）: 各ステップで $\bm{h}_k^T \Delta\bm{\phi}_k \approx 0$ となるよう促します。これはゼロ反力条件 $\bm{h}^T \dot{\bm{\phi}} = 0$ の離散版です。理想的にはこの項がゼロになりますが、目標到達との両立が困難な場合にソフト拘束として機能します。
第3項（滑らかさ）: 関節角の急激な変化を抑え、滑らかな軌道を生成します。実機では角速度制限に対応する項です。
等式拘束としての扱い
反力最小化をコスト関数のペナルティ項ではなく、厳密な等式拘束として扱うこともできます。
$$ \begin{equation} \bm{h}(\bm{\phi}_k)^T (\bm{\phi}_{k+1} – \bm{\phi}_k) = 0 \quad (k = 0, 1, \ldots, N-1) \end{equation} $$
この定式化では $N$ 個の等式拘束が加わり、ゼロ反力条件が厳密に満たされます。ただし、等式拘束が厳しすぎて実行可能解が見つからない場合があるため、実用上はペナルティ法で大きな重みを与えるアプローチがしばしば採用されます。
不等式拘束
関節角リミットと角速度リミットを不等式拘束として追加できます。
$$ \begin{equation} \bm{\phi}_{\min} \leq \bm{\phi}_k \leq \bm{\phi}_{\max} \quad (k = 1, \ldots, N) \end{equation} $$
$$ \begin{equation} \|\Delta\bm{\phi}_k\| \leq \Delta\phi_{\max} \quad (k = 0, \ldots, N-1) \end{equation} $$
これらの制約により、物理的に実現可能な軌道が保証されます。
最適化問題の定式化ができました。次に、この非線形最適化を実際に解くためのアルゴリズムについて解説します。
逐次二次計画法（SQP）による数値解法
なぜSQPか
上で定式化した問題は、コスト関数と拘束がともに非線形です。手先の位置 $\bm{r}_e(\bm{\phi})$ は三角関数で記述される運動学の非線形関数であり、結合項 $\bm{h}(\bm{\phi})$ も関節角の非線形関数です。
このような非線形拘束付き最適化には、逐次二次計画法（Sequential Quadratic Programming, SQP）が強力な手法です。SQPの基本アイデアは、非線形問題を各反復で二次計画問題（QP）に近似して逐次的に解くことです。ニュートン法を拘束付き最適化に拡張したものと理解できます。
SQPの概要
設計変数を $\bm{x} = (\bm{\phi}_1^T, \bm{\phi}_2^T, \ldots, \bm{\phi}_N^T)^T \in \mathbb{R}^{nN}$ とまとめて、問題を次の標準形に書き直します。
$$ \begin{equation} \min_{\bm{x}} f(\bm{x}) \quad \text{subject to} \quad \bm{c}(\bm{x}) = \bm{0}, \quad \bm{x}_L \leq \bm{x} \leq \bm{x}_U \end{equation} $$
SQPの各反復 $i$ では、現在の点 $\bm{x}^{(i)}$ の周りで目的関数を2次近似し、拘束を1次近似したQP部分問題を解きます。
$$ \begin{equation} \min_{\bm{d}} \frac{1}{2} \bm{d}^T \bm{H}^{(i)} \bm{d} + \nabla f(\bm{x}^{(i)})^T \bm{d} \end{equation} $$
$$ \text{subject to} \quad \bm{c}(\bm{x}^{(i)}) + \nabla \bm{c}(\bm{x}^{(i)})^T \bm{d} = \bm{0} $$
ここで $\bm{d}$ は探索方向、$\bm{H}^{(i)}$ はラグランジアンのヘッセ行列（またはその近似）です。QP部分問題を解いて得た $\bm{d}^{(i)}$ を用いて次の点を更新します。
$$ \begin{equation} \bm{x}^{(i+1)} = \bm{x}^{(i)} + \alpha^{(i)} \bm{d}^{(i)} \end{equation} $$
$\alpha^{(i)}$ はライン探索で決定されるステップ幅です。
ラグランジアンとKKT条件
SQPが収束する先は、カルーシュ・クーン・タッカー（KKT）条件を満たす点です。ラグランジアンを次のように定義します。
$$ \begin{equation} \mathcal{L}(\bm{x}, \bm{\lambda}) = f(\bm{x}) + \bm{\lambda}^T \bm{c}(\bm{x}) \end{equation} $$
KKT条件（等式拘束の場合）は次の通りです。
$\nabla_{\bm{x}} \mathcal{L} = \bm{0}$（停留条件）を展開すると、
$$ \begin{equation} \nabla f(\bm{x}) + \nabla \bm{c}(\bm{x}) \bm{\lambda} = \bm{0} \end{equation} $$
$\bm{c}(\bm{x}) = \bm{0}$（実行可能性条件）を満たす点が最適解です。
ラグランジュ乗数 $\bm{\lambda}$ は、ゼロ反力拘束を維持するために「犠牲にしている」コスト — すなわち、拘束がなければさらに下げられるはずのコスト — の感度を表しています。乗数の大きさが拘束の「きつさ」の指標になるため、軌道の品質評価にも活用できます。
実装上の工夫
SQPの実装で重要なポイントを3つ挙げます。
ヘッセ行列の近似: 正確なヘッセ行列の計算は高コストなので、BFGS公式による準ニュートン近似がよく用いられます。SciPyの minimize 関数の method='SLSQP' はこの方式を内部で採用しています。
初期解の重要性: 非線形問題なので局所最適に陥る可能性があります。良い初期解を与えることが収束性を大きく左右します。典型的には、初期関節角 $\bm{\phi}_0$ と最終関節角 $\bm{\phi}_N$ を線形補間した直線軌道を初期解にします。
スケーリング: 目標到達項と反力項の単位が異なるため、重み $w_1, w_2, w_3$ を適切に設定する必要があります。反力項の重みは目標到達項の $10^2$〜$10^4$ 倍程度に設定することが多いです。
SQPの理論的枠組みを理解したところで、次にゼロ反力軌道計画が持つ深い数学的構造 — 非ホロノミック系としての側面 — を見ていきましょう。
非ホロノミック系としての解釈
ホロノミック拘束と非ホロノミック拘束
動力学の拘束にはホロノミック拘束と非ホロノミック拘束の2種類があります。
ホロノミック拘束は、位置変数だけの関数で表せる拘束です。例えば、「リンク長が一定」「2つの物体が接触している」といった幾何学的拘束がこれにあたります。
$$ \begin{equation} g(\bm{q}) = 0 \quad \text{（ホロノミック）} \end{equation} $$
一方、非ホロノミック拘束は速度変数を含み、位置だけでは表現できない拘束です。
$$ \begin{equation} \bm{a}(\bm{q})^T \dot{\bm{q}} = 0 \quad \text{（非ホロノミック）} \end{equation} $$
ゼロ反力条件 $\bm{h}(\bm{\phi})^T \dot{\bm{\phi}} = 0$ はまさにこの形をしています。
なぜ非ホロノミックなのか
ゼロ反力条件がホロノミックかどうかを判定するには、$\bm{h}(\bm{\phi})^T d\bm{\phi} = 0$ が完全微分（exact differential）になるかどうかを調べます。もし完全微分なら、ある関数 $g(\bm{\phi})$ が存在して $dg = \bm{h}^T d\bm{\phi}$ と書け、拘束は $g(\bm{\phi}) = \text{const}$ というホロノミック拘束に帰着します。
しかし、$n \geq 3$ の一般的なマニピュレータでは、$\bm{h}$ の成分が関節角の複雑な三角関数で構成されるため、フロベニウスの定理（Frobenius theorem）の可積分条件を満たしません。
フロベニウスの可積分条件を具体的に述べます。ベクトル場 $\bm{h}$ の成分 $h_i, h_j$ に対して、次の条件が全ての $i, j$ で成り立てばホロノミック（可積分）です。
$$ \begin{equation} \frac{\partial h_i}{\partial \phi_j} = \frac{\partial h_j}{\partial \phi_i} \quad \forall i, j \end{equation} $$
一般の宇宙ロボットでは、$\bm{h}$ の成分は各リンクの質量・長さ・慣性モーメントに依存する複雑な式となり、上の対称性条件は成り立ちません。したがって、ゼロ反力条件は典型的に非ホロノミック拘束です。
非ホロノミック性の意味
非ホロノミック性は、軌道計画にとって以下の深い意味を持ちます。
到達可能性: 非ホロノミック拘束は速度に制限を加えますが、位置空間での到達可能範囲を制限するとは限りません。自動車の平行駐車を思い出してください。車は横方向に直接移動できませんが（非ホロノミック拘束）、前進と後退を組み合わせれば任意の位置に駐車できます。同様に、宇宙ロボットもゼロ反力条件を守りながら、経路を巧みに選べば手先を任意の目標位置に移動できる可能性があります。
経路依存性: ホロノミック拘束と違い、非ホロノミック系では「どの経路を通ったか」が重要です。始点と終点の関節角が同じでも、途中の経路が異なれば途中のベース姿勢変化のパターンが変わります。これは、同じ出発地と目的地でも、走るルートによって車の向きが変わるのと同じです。
制御の困難さ: 非ホロノミック系の軌道計画は、ホロノミック系に比べて本質的に困難です。局所的な最適解が多数存在し、大域的最適解の探索は計算量が大きくなります。一方で、冗長度が高いほど解空間が広がり、良い解が見つかりやすくなります。
微分幾何的な視点
より数学的に言えば、ゼロ反力条件 $\bm{h}(\bm{\phi})^T d\bm{\phi} = 0$ は関節空間 $\mathbb{R}^n$ 上の1-形式（微分形式）$\omega = \sum_i h_i d\phi_i$ の零点集合として、$(n-1)$ 次元の分布（distribution, 微分幾何の意味）を定義します。
この分布が非可積分であることが非ホロノミック性に対応し、Chow-Rashevsky の定理によれば、分布がリー括弧演算で全空間を生成する（bracket-generating condition を満たす）なら、拘束を満たしつつ任意の2点を結ぶ経路が存在します。
この可制御性の結果は、十分な冗長度を持つ宇宙マニピュレータが、ゼロ反力条件を保ちながら手先を任意の目標に到達させうることを理論的に保証します。ただし、経路の「存在」と「効率的な計算」は別問題であり、だからこそ最適化ベースの軌道計画が実用上重要になるのです。
非ホロノミック系の理論的な背景を押さえたところで、いよいよPythonで具体的なゼロ反力軌道を計画し、可視化してみましょう。
Pythonでの実装 — 2次元3リンク宇宙ロボットのゼロ反力軌道計画
実装の概要
ここでは2次元平面上の3リンクマニピュレータが自由浮遊するベースに取り付けられたモデルを扱います。手先を初期位置から目標位置に移動しつつ、ベース姿勢変化を最小化する軌道を最適化で求めます。
2次元・3リンクの場合、手先タスクは2次元（$m = 2$）、ゼロ反力拘束は1次元（$c = 1$）なので、自由度の条件 $n \geq m + c = 3$ をちょうど満たします。冗長度はゼロですが、ゼロ反力軌道が存在する設定です。
まず、系の運動学とゼロ反力条件の計算関数を定義します。
import numpy as np
from scipy.optimize import minimize
# === 系のパラメータ ===
# 3リンク宇宙マニピュレータ（2次元平面）
n_links = 3
# リンクの長さ [m]
L = np.array([1.0, 0.8, 0.6])
# リンクの質量 [kg]
m_link = np.array([4.0, 3.0, 2.0])
# ベース衛星の質量 [kg]
m_base = 20.0
# ベース衛星の慣性モーメント [kg·m²]
I_base = 5.0
# 各リンクの慣性モーメント（一様な棒として I = mL²/12） [kg·m²]
I_link = m_link * L**2 / 12.0
# 系の全質量
M_total = m_base + np.sum(m_link)
def forward_kinematics(phi, theta0=0.0, base_pos=np.array([0.0, 0.0])):
"""
順運動学: 各リンク端の位置を計算する。
phi: 関節角 (n_links,) — ベースからの相対角
theta0: ベース姿勢角
base_pos: ベース中心の位置
戻り値: 各関節位置 (n_links+1, 2) — ベース端から手先まで
"""
positions = np.zeros((n_links + 1, 2))
positions[0] = base_pos
cumulative_angle = theta0
for i in range(n_links):
cumulative_angle += phi[i]
positions[i+1, 0] = positions[i, 0] + L[i] * np.cos(cumulative_angle)
positions[i+1, 1] = positions[i, 1] + L[i] * np.sin(cumulative_angle)
return positions
def end_effector_pos(phi, theta0=0.0, base_pos=np.array([0.0, 0.0])):
"""手先位置を返す"""
pos = forward_kinematics(phi, theta0, base_pos)
return pos[-1]
def jacobian_phi(phi, theta0=0.0):
"""
手先位置の関節角に対するヤコビ行列 J_phi (2 x n_links)
"""
J = np.zeros((2, n_links))
for j in range(n_links):
cumulative_angle = theta0
for k in range(j+1):
cumulative_angle += phi[k] if k <= j else 0
# j番目の関節角を変化させたときの手先位置変化
for i in range(j, n_links):
angle_i = theta0 + np.sum(phi[:i+1])
J[0, j] += -L[i] * np.sin(angle_i)
J[1, j] += L[i] * np.cos(angle_i)
return J
この関数群で、任意の関節角に対する手先位置とヤコビ行列を計算できます。順運動学では、ベース姿勢角 $\theta_0$ から始めて各関節角を累積しながらリンク端の座標を逐次的に求めています。
次に、角運動量保存則に関わる結合ベクトル $\bm{h}(\bm{\phi})$ を計算する関数を定義します。
def coupling_vector_h(phi, theta0=0.0):
"""
角運動量保存則の結合ベクトル h(phi) を計算する。
角運動量: L = I_base * dtheta0 + h(phi)^T * dphi = 0
h_j = 各リンクの角運動量への関節jの寄与
"""
h = np.zeros(n_links)
for j in range(n_links):
for i in range(j, n_links):
# リンクiの重心位置のj番目関節角に対する変化
angle_i = theta0 + np.sum(phi[:i+1])
# リンクiの重心までの距離（リンク中点）
r_cm_x_deriv = 0.0
r_cm_y_deriv = 0.0
# リンクi以前のリンク端の寄与
for k in range(j, i):
angle_k = theta0 + np.sum(phi[:k+1])
r_cm_x_deriv += -L[k] * np.sin(angle_k)
r_cm_y_deriv += L[k] * np.cos(angle_k)
# リンクiの重心（リンク中点）の寄与
r_cm_x_deriv += -0.5 * L[i] * np.sin(angle_i)
r_cm_y_deriv += 0.5 * L[i] * np.cos(angle_i)
# リンクiの重心の位置
r_cm_x = 0.0
r_cm_y = 0.0
for k in range(i):
angle_k = theta0 + np.sum(phi[:k+1])
r_cm_x += L[k] * np.cos(angle_k)
r_cm_y += L[k] * np.sin(angle_k)
r_cm_x += 0.5 * L[i] * np.cos(angle_i)
r_cm_y += 0.5 * L[i] * np.sin(angle_i)
# 角運動量への寄与: m_i * (r_cm x dr_cm/dphi_j) + I_i * d(angle_i)/dphi_j
cross = r_cm_x * r_cm_y_deriv - r_cm_y * r_cm_x_deriv
h[j] += m_link[i] * cross + I_link[i]
return h
def compute_base_rotation(phi_trajectory):
"""
関節軌道から各ステップのベース姿勢変化を計算する。
角運動量保存則 dtheta0 = -h^T * dphi / I_eff を離散積分。
"""
N_steps = len(phi_trajectory)
theta0 = np.zeros(N_steps)
for k in range(N_steps - 1):
h = coupling_vector_h(phi_trajectory[k], theta0[k])
dphi = phi_trajectory[k+1] - phi_trajectory[k]
# 実効慣性モーメント（簡略化のためI_baseを使用）
I_eff = I_base + np.dot(h, h) / (np.sum(m_link * L**2) + I_base)
dtheta0 = -np.dot(h, dphi) / I_eff
theta0[k+1] = theta0[k] + dtheta0
return theta0
結合ベクトル $\bm{h}$ の計算は少し複雑に見えますが、やっていることは各関節 $j$ の微小回転が系全体の角運動量にどれだけ寄与するかを、リンクの重心運動（$m_i (\bm{r}_{cm} \times \dot{\bm{r}}_{cm})$）とリンク自体の回転（$I_i \dot{\theta}_i$）の合計として計算しているだけです。
続いて、最適化問題を定式化して解きます。
# === 軌道計画の最適化 ===
# 初期・目標の設定
phi_init = np.array([0.3, 0.5, 0.4]) # 初期関節角 [rad]
r_target = np.array([1.8, 0.8]) # 手先の目標位置 [m]
N_steps = 20 # 離散化ステップ数
# 初期手先位置の確認
r_init = end_effector_pos(phi_init)
print(f"初期手先位置: ({r_init[0]:.3f}, {r_init[1]:.3f})")
print(f"目標手先位置: ({r_target[0]:.3f}, {r_target[1]:.3f})")
def cost_function(x):
"""
最適化のコスト関数。
x: 全ステップの関節角をフラット化したベクトル (N_steps * n_links,)
"""
phi_traj = x.reshape(N_steps, n_links)
# 重み設定
w_target = 1000.0 # 目標到達
w_reaction = 500.0 # 反力最小化
w_smooth = 1.0 # 滑らかさ
cost = 0.0
# 第1項: 目標到達コスト
r_final = end_effector_pos(phi_traj[-1])
cost += w_target * np.sum((r_final - r_target)**2)
# 第2項: ゼロ反力コスト + 第3項: 滑らかさコスト
theta0 = 0.0
for k in range(N_steps - 1):
dphi = phi_traj[k+1] - phi_traj[k]
h = coupling_vector_h(phi_traj[k], theta0)
# 反力項: (h^T dphi)^2
reaction = np.dot(h, dphi)
cost += w_reaction * reaction**2
# 滑らかさ項: ||dphi||^2
cost += w_smooth * np.sum(dphi**2)
# ベース姿勢の更新（反力評価のため）
I_eff = I_base
dtheta0 = -np.dot(h, dphi) / I_eff
theta0 += dtheta0
return cost
# 初期解: 線形補間（目標に近い関節角を推定）
# 逆運動学の近似解を目標関節角として使用
phi_target_guess = np.array([0.1, 0.8, 0.2]) # 手動で設定した目標付近の初期推定
phi_traj_init = np.zeros((N_steps, n_links))
for i in range(n_links):
phi_traj_init[:, i] = np.linspace(phi_init[i], phi_target_guess[i], N_steps)
x0 = phi_traj_init.flatten()
# 関節角の範囲制約
bounds = [(-np.pi, np.pi)] * (N_steps * n_links)
# 最適化の実行
print("最適化を実行中...")
result = minimize(
cost_function,
x0,
method='SLSQP',
bounds=bounds,
options={'maxiter': 500, 'ftol': 1e-10, 'disp': True}
)
print(f"最適化ステータス: {result.message}")
print(f"最終コスト: {result.fun:.6f}")
# 最適軌道の取得
phi_opt = result.x.reshape(N_steps, n_links)
ここでは scipy.optimize.minimize の SLSQP（Sequential Least Squares Programming）法を使用しています。SLSQPはSQPの一種であり、等式・不等式拘束と変数の上下界を同時に扱えます。初期解として線形補間軌道を与え、そこから最適化で改善しています。
次に、最適化結果を評価し、可視化します。
import matplotlib.pyplot as plt
import matplotlib.patches as patches
from matplotlib.collections import LineCollection
# === 結果の評価 ===
# ベース姿勢変化の計算
theta0_opt = compute_base_rotation(phi_opt)
# 比較用: 線形補間軌道のベース姿勢変化
theta0_linear = compute_base_rotation(phi_traj_init)
# 手先軌跡の計算
ee_traj_opt = np.array([end_effector_pos(phi_opt[k], theta0_opt[k])
for k in range(N_steps)])
ee_traj_linear = np.array([end_effector_pos(phi_traj_init[k], theta0_linear[k])
for k in range(N_steps)])
# 反力の計算（各ステップ）
reactions_opt = []
reactions_linear = []
for k in range(N_steps - 1):
h_opt = coupling_vector_h(phi_opt[k], theta0_opt[k])
dphi_opt = phi_opt[k+1] - phi_opt[k]
reactions_opt.append(np.dot(h_opt, dphi_opt))
h_lin = coupling_vector_h(phi_traj_init[k], theta0_linear[k])
dphi_lin = phi_traj_init[k+1] - phi_traj_init[k]
reactions_linear.append(np.dot(h_lin, dphi_lin))
reactions_opt = np.array(reactions_opt)
reactions_linear = np.array(reactions_linear)
# 最終手先位置
r_final_opt = end_effector_pos(phi_opt[-1], theta0_opt[-1])
r_final_lin = end_effector_pos(phi_traj_init[-1], theta0_linear[-1])
print(f"\n=== 結果の比較 ===")
print(f"目標位置: ({r_target[0]:.3f}, {r_target[1]:.3f})")
print(f"最適軌道 - 最終手先位置: ({r_final_opt[0]:.3f}, {r_final_opt[1]:.3f})")
print(f"線形補間 - 最終手先位置: ({r_final_lin[0]:.3f}, {r_final_lin[1]:.3f})")
print(f"最適軌道 - ベース最終姿勢変化: {np.degrees(theta0_opt[-1]):.4f} deg")
print(f"線形補間 - ベース最終姿勢変化: {np.degrees(theta0_linear[-1]):.4f} deg")
print(f"最適軌道 - 反力のRMS: {np.sqrt(np.mean(reactions_opt**2)):.6f}")
print(f"線形補間 - 反力のRMS: {np.sqrt(np.mean(reactions_linear**2)):.6f}")
この評価コードでは、最適化軌道と単純な線形補間軌道を比較しています。ベースの最終姿勢変化、手先の目標到達精度、反力のRMS値を定量的に比較することで、最適化の効果を確認できます。
最適化軌道は線形補間に比べてベース姿勢変化が大幅に低減されており、反力のRMSも著しく小さくなっていることが確認できるはずです。これは、最適化が $\bm{h}^T \Delta\bm{\phi} \approx 0$ を積極的に満たす経路を見つけていることを意味します。
続いて、結果を4つのサブプロットで可視化します。
fig, axes = plt.subplots(2, 2, figsize=(14, 12))
# --- (a) ベース姿勢変化の時間履歴 ---
ax = axes[0, 0]
steps = np.arange(N_steps)
ax.plot(steps, np.degrees(theta0_opt), 'b-o', markersize=4, label='Optimized trajectory')
ax.plot(steps, np.degrees(theta0_linear), 'r--s', markersize=4, label='Linear interpolation')
ax.axhline(y=0, color='gray', linestyle=':', alpha=0.5)
ax.set_xlabel('Step', fontsize=12)
ax.set_ylabel('Base rotation [deg]', fontsize=12)
ax.set_title('(a) Base attitude change', fontsize=13)
ax.legend(fontsize=10)
ax.grid(True, alpha=0.3)
# --- (b) 反力 h^T * dphi の時間履歴 ---
ax = axes[0, 1]
step_mid = np.arange(N_steps - 1)
ax.plot(step_mid, reactions_opt, 'b-o', markersize=4, label='Optimized')
ax.plot(step_mid, reactions_linear, 'r--s', markersize=4, label='Linear interp.')
ax.axhline(y=0, color='gray', linestyle=':', alpha=0.5)
ax.set_xlabel('Step', fontsize=12)
ax.set_ylabel(r'$\mathbf{h}^T \Delta\boldsymbol{\phi}$', fontsize=12)
ax.set_title('(b) Reaction torque per step', fontsize=13)
ax.legend(fontsize=10)
ax.grid(True, alpha=0.3)
# --- (c) 手先軌跡 ---
ax = axes[1, 0]
ax.plot(ee_traj_opt[:, 0], ee_traj_opt[:, 1], 'b-o', markersize=4,
label='Optimized', zorder=3)
ax.plot(ee_traj_linear[:, 0], ee_traj_linear[:, 1], 'r--s', markersize=4,
label='Linear interp.', zorder=2)
ax.plot(r_init[0], r_init[1], 'go', markersize=12, label='Start', zorder=5)
ax.plot(r_target[0], r_target[1], 'r*', markersize=15, label='Target', zorder=5)
ax.set_xlabel('x [m]', fontsize=12)
ax.set_ylabel('y [m]', fontsize=12)
ax.set_title('(c) End-effector trajectory', fontsize=13)
ax.legend(fontsize=10)
ax.set_aspect('equal')
ax.grid(True, alpha=0.3)
# --- (d) ロボットの初期姿勢と最終姿勢 ---
ax = axes[1, 1]
# 初期姿勢
pos_init = forward_kinematics(phi_init, 0.0)
ax.plot(pos_init[:, 0], pos_init[:, 1], 'g-o', linewidth=3, markersize=8,
label='Initial config.', zorder=3)
# 最適化後の最終姿勢
pos_final_opt = forward_kinematics(phi_opt[-1], theta0_opt[-1])
ax.plot(pos_final_opt[:, 0], pos_final_opt[:, 1], 'b-o', linewidth=3, markersize=8,
label='Optimized final', zorder=3)
# 線形補間の最終姿勢
pos_final_lin = forward_kinematics(phi_traj_init[-1], theta0_linear[-1])
ax.plot(pos_final_lin[:, 0], pos_final_lin[:, 1], 'r--o', linewidth=3, markersize=8,
label='Linear interp. final', zorder=2)
# ベース衛星の描画（四角形）
base_size = 0.3
rect = patches.Rectangle((-base_size/2, -base_size/2), base_size, base_size,
linewidth=2, edgecolor='gray', facecolor='lightgray',
zorder=1, label='Base satellite')
ax.add_patch(rect)
ax.plot(r_target[0], r_target[1], 'r*', markersize=15, label='Target', zorder=5)
ax.set_xlabel('x [m]', fontsize=12)
ax.set_ylabel('y [m]', fontsize=12)
ax.set_title('(d) Robot configurations', fontsize=13)
ax.legend(fontsize=9, loc='upper left')
ax.set_aspect('equal')
ax.grid(True, alpha=0.3)
ax.set_xlim(-1.5, 3.0)
ax.set_ylim(-1.0, 2.5)
plt.tight_layout()
plt.savefig('zero_reaction_trajectory.png', dpi=150, bbox_inches='tight')
plt.show()
4つのサブプロットの結果を読み解きましょう。
図(a) ベース姿勢変化: 最適化軌道（青線）は線形補間軌道（赤破線）に比べてベース姿勢変化が大幅に抑えられていることがわかります。線形補間では各ステップで蓄積する反力がベースを連続的に回転させますが、最適化軌道はベースの回転がほぼゼロ近傍に留まっています。
図(b) 反力の時間履歴: 各ステップでの $\bm{h}^T \Delta\bm{\phi}$ の値は、最適化軌道ではゼロ付近に張り付いているのに対し、線形補間では大きな値を取ります。これはゼロ反力条件が各ステップでほぼ満たされていることの直接的な証拠です。
図(c) 手先軌跡: 最適化軌道と線形補間軌道では手先が辿る経路が異なります。最適化軌道は必ずしも直線的ではなく、ゼロ反力条件を満たすために迂回した経路を取ることがあります。これは非ホロノミック系の特徴 — 拘束を満たすために経路選択が重要 — を反映しています。
図(d) ロボット姿勢: 初期姿勢（緑）、最適化後の最終姿勢（青）、線形補間の最終姿勢（赤）を比較しています。最適化後はベースがほぼ回転せずに手先が目標位置に到達していることが視覚的に確認できます。
感度分析 — 反力の重みを変えた比較
最適化の重み $w_{\text{reaction}}$ を変化させたときの挙動を調べ、目標到達精度とベース姿勢保持のトレードオフを可視化してみましょう。
# === 反力重みの感度分析 ===
w_reaction_values = [0, 10, 100, 500, 2000, 10000]
results_sensitivity = []
for w_r in w_reaction_values:
def cost_func_wr(x, w_r=w_r):
phi_traj = x.reshape(N_steps, n_links)
w_target = 1000.0
w_smooth = 1.0
cost = 0.0
r_final = end_effector_pos(phi_traj[-1])
cost += w_target * np.sum((r_final - r_target)**2)
theta0 = 0.0
for k in range(N_steps - 1):
dphi = phi_traj[k+1] - phi_traj[k]
h = coupling_vector_h(phi_traj[k], theta0)
reaction = np.dot(h, dphi)
cost += w_r * reaction**2
cost += w_smooth * np.sum(dphi**2)
dtheta0 = -np.dot(h, dphi) / I_base
theta0 += dtheta0
return cost
res = minimize(cost_func_wr, x0, method='SLSQP', bounds=bounds,
options={'maxiter': 500, 'ftol': 1e-10})
phi_res = res.x.reshape(N_steps, n_links)
theta0_res = compute_base_rotation(phi_res)
r_final_res = end_effector_pos(phi_res[-1], theta0_res[-1])
results_sensitivity.append({
'w_reaction': w_r,
'base_rotation_deg': np.degrees(theta0_res[-1]),
'target_error': np.linalg.norm(r_final_res - r_target),
'theta0_trajectory': theta0_res
})
print(f"w_reaction={w_r:>6}: base rot = {np.degrees(theta0_res[-1]):>8.4f} deg, "
f"target err = {np.linalg.norm(r_final_res - r_target):.6f} m")
# 感度分析の可視化
fig, axes = plt.subplots(1, 2, figsize=(14, 5))
# (a) ベース姿勢変化 vs 反力重み
ax = axes[0]
w_vals = [r['w_reaction'] for r in results_sensitivity]
base_rots = [abs(r['base_rotation_deg']) for r in results_sensitivity]
ax.semilogx([max(w, 0.1) for w in w_vals], base_rots, 'bo-', markersize=8, linewidth=2)
ax.set_xlabel(r'$w_{\mathrm{reaction}}$', fontsize=13)
ax.set_ylabel('|Base rotation| [deg]', fontsize=12)
ax.set_title('(a) Base rotation vs reaction weight', fontsize=13)
ax.grid(True, alpha=0.3)
# (b) 各重みでのベース姿勢変化の時間履歴
ax = axes[1]
colors = plt.cm.viridis(np.linspace(0, 1, len(w_reaction_values)))
for i, r in enumerate(results_sensitivity):
label = f"$w_r$={r['w_reaction']}"
ax.plot(np.arange(N_steps), np.degrees(r['theta0_trajectory']),
color=colors[i], linewidth=2, label=label)
ax.axhline(y=0, color='gray', linestyle=':', alpha=0.5)
ax.set_xlabel('Step', fontsize=12)
ax.set_ylabel('Base rotation [deg]', fontsize=12)
ax.set_title('(b) Base attitude history for different weights', fontsize=13)
ax.legend(fontsize=9, loc='best')
ax.grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('sensitivity_analysis.png', dpi=150, bbox_inches='tight')
plt.show()
感度分析の結果から、反力重みとベース姿勢変化のトレードオフが明確に見て取れます。
図(a) は、反力重み $w_{\text{reaction}}$ を対数スケールで大きくするにつれ、ベース姿勢変化の絶対値が単調に減少する様子を示しています。$w_{\text{reaction}} = 0$（反力を全く気にしない場合）ではベースが数度回転するのに対し、$w_{\text{reaction}} = 10000$ ではほぼゼロに抑えられています。
図(b) の時間履歴を見ると、低い重みでは各ステップで蓄積する反力がベースを一方向に押し続けるのに対し、高い重みでは反力が相殺され、ベース姿勢がほぼ一定に保たれていることがわかります。ただし、重みを極端に大きくすると手先の目標到達精度が犠牲になる場合があることに注意が必要です。実用上は、ミッション要求に応じてこのトレードオフのバランスを調整します。
アニメーション用のスナップショット
最後に、最適化軌道に沿ったロボットのスナップショットを複数時刻で重ね描きし、動作の全体像を一枚の図で把握できるようにします。
fig, ax = plt.subplots(1, 1, figsize=(12, 8))
# スナップショットの時刻インデックス
snapshot_indices = np.linspace(0, N_steps - 1, 8, dtype=int)
colors_snap = plt.cm.Blues(np.linspace(0.3, 1.0, len(snapshot_indices)))
for idx, k in enumerate(snapshot_indices):
pos = forward_kinematics(phi_opt[k], theta0_opt[k])
alpha = 0.3 + 0.7 * idx / (len(snapshot_indices) - 1)
# リンクの描画
ax.plot(pos[:, 0], pos[:, 1], '-o', color=colors_snap[idx],
linewidth=2.5, markersize=6, alpha=alpha,
label=f'Step {k}' if idx in [0, len(snapshot_indices)//2, len(snapshot_indices)-1] else None)
# ベース衛星の描画
base_size = 0.25
rect = patches.Rectangle(
(pos[0, 0] - base_size/2, pos[0, 1] - base_size/2),
base_size, base_size,
angle=np.degrees(theta0_opt[k]),
linewidth=1.5, edgecolor=colors_snap[idx],
facecolor=colors_snap[idx], alpha=alpha * 0.3,
zorder=1
)
ax.add_patch(rect)
# 手先軌跡
ax.plot(ee_traj_opt[:, 0], ee_traj_opt[:, 1], 'b--', linewidth=1, alpha=0.5,
label='EE trajectory')
# 初期位置と目標位置
ax.plot(r_init[0], r_init[1], 'go', markersize=14, zorder=5, label='Start')
ax.plot(r_target[0], r_target[1], 'r*', markersize=18, zorder=5, label='Target')
ax.set_xlabel('x [m]', fontsize=13)
ax.set_ylabel('y [m]', fontsize=13)
ax.set_title('Zero-Reaction Trajectory — Snapshots', fontsize=14)
ax.legend(fontsize=10, loc='upper left')
ax.set_aspect('equal')
ax.grid(True, alpha=0.3)
ax.set_xlim(-1.5, 3.5)
ax.set_ylim(-1.5, 2.5)
plt.tight_layout()
plt.savefig('zero_reaction_snapshots.png', dpi=150, bbox_inches='tight')
plt.show()
この重ね描き図では、薄い色が初期時刻、濃い色が最終時刻に対応しています。時間の経過とともにリンクの姿勢が滑らかに変化し、手先が目標に向かって移動する様子が一目で把握できます。
注目すべきは、ベース衛星（各スナップショットの四角形）の位置と向きがほぼ変化していないことです。これがゼロ反力軌道の本質 — マニピュレータが大きく姿勢を変えても、その反力がベースに伝わらないよう関節運動が巧みに協調している — を視覚的に示しています。各スナップショットでリンクの「折れ曲がり方」が複雑に変化しているのは、非ホロノミック拘束を満たすために直線的ではない経路を辿っていることの表れです。
実用上の考慮事項と発展
実際の宇宙ミッションへの適用
ここまでの議論を実際のミッションに適用する際には、いくつかの追加考慮が必要です。
計算速度: 軌道上のオンボードコンピュータはリソースが限られています。SQPのような反復解法は地上で事前計算する方法と、軌道上でリアルタイムに計算する方法があります。事前計算では様々なタスク目標に対する軌道をルックアップテーブルとして保存し、実行時に補間して使います。リアルタイム計算では、より単純な射影法（$\dot{\bm{\phi}} = \bm{N}_h \bm{J}_\phi^{+} \dot{\bm{r}}_e^d$ のような逆運動学の射影）を用いることが多いです。
モデル誤差: 実際のマニピュレータでは、リンクの質量特性に不確かさがあります。結合ベクトル $\bm{h}$ の計算にモデル誤差が含まれると、「ゼロ反力」のつもりでも実際にはわずかな反力が残ります。この残留反力を姿勢制御系（リアクションホイール等）で補償するハイブリッド戦略が実用的です。
柔軟性の影響: 大型のスペースマニピュレータ（ISS のカナダアーム2 のように全長 17m 以上）では、リンクの柔軟性が無視できません。柔軟リンクの振動は追加的な反力を生じさせ、剛体モデルに基づくゼロ反力軌道が厳密には成り立たなくなります。柔軟マルチボディダイナミクスに基づく拡張が研究されています。
双腕マニピュレータへの拡張
ゼロ反力軌道計画は、双腕マニピュレータで特に威力を発揮します。1台のマニピュレータでは冗長度が不足する場合でも、2台の腕を協調させれば十分な自由度が確保できます。
例えば、一方の腕が対象物を操作している間、もう一方の腕が「カウンターウェイト」のように動いて反力を相殺するという運用が考えられます。このとき、系全体の結合ベクトル $\bm{h}$ は両腕の全関節角に依存し、最適化の設計変数は倍増しますが、解空間が広がるため良い解が見つかりやすくなります。
他の最適化手法
本記事ではSQP（SLSQP）を用いましたが、他の有力な手法もあります。
内点法（Interior Point Method）: 不等式拘束を対数バリア関数で処理する手法で、大規模問題に強い特性があります。
直接コロケーション法（Direct Collocation）: 状態変数と制御入力を同時に離散化し、大規模だが構造化されたNLP（非線形計画問題）として解きます。ロボティクスの軌道最適化では広く用いられています。
微分動的計画法（Differential Dynamic Programming, DDP）: 動的計画法のアイデアをベルマン方程式の2次近似で実現する手法です。制御系の時間構造を活用するため、時間離散化された軌道最適化に適しています。
まとめ
本記事では、宇宙ロボットのゼロ反力軌道計画について、問題設定から数学的定式化、最適化による数値解法、非ホロノミック系としての理論的理解、そしてPythonでの実装まで一貫して解説しました。
- ゼロ反力条件 $\bm{h}(\bm{\phi})^T \dot{\bm{\phi}} = 0$ は、関節速度が結合ベクトル $\bm{h}$ に直交することを要求する。これによりベース衛星の姿勢変化がゼロになる
- 存在条件: 手先タスクの次元 $m$ とゼロ反力拘束の数 $c$ に対して、関節自由度 $n \geq m + c$ が必要。冗長度 $n – m – c$ が大きいほど軌道の最適化余地が広がる
- 最適化ベースの軌道計画: 目標到達、反力最小化、滑らかさを統合したコスト関数をSQPで最小化する。ペナルティ法により等式拘束を柔軟に扱える
- 非ホロノミック性: ゼロ反力条件はフロベニウスの可積分条件を満たさない非ホロノミック拘束であり、経路依存性や非自明な到達可能性といった特徴を持つ
- Python実装: 2次元3リンクモデルで、SLSQPによる最適化がベース姿勢変化を大幅に低減できることを確認した
ゼロ反力軌道計画は、宇宙ロボティクスにおける姿勢外乱抑制の基盤技術です。次のステップとして、微小重力環境での接触ダイナミクスの理解へ進むことで、実際の作業（把持、組立、修理）を含むより現実的な宇宙ロボットの運用計画が可能になります。
次のステップとして、以下の記事も参考にしてください。
- 宇宙ロボットのリアクションレスマニューバ — ゼロ反力条件の基礎となる角運動量保存と一般化ヤコビ行列
- 微小重力環境の接触ダイナミクス — ゼロ反力軌道の先にある、物体操作の力学