宇宙空間でロボットアームがデブリを捕獲する場面を想像してください。デブリは予測不能な回転をしており、把持点の位置と姿勢が刻一刻と変わります。地上の工場ロボットのように「事前に決めた経路をなぞる」だけでは、把持点を逸してしまうか、最悪の場合デブリと衝突してロボット本体を損傷するかもしれません。こうした状況では、実行中にリアルタイムで軌道を修正する能力が不可欠です。
この「オンライン軌道生成とリアルタイム再計画」の技術は、宇宙ロボティクスに限らず幅広い分野で活きます。
- 自動運転: 歩行者の飛び出しや前方車両の急ブレーキに瞬時に対応する経路変更
- ドローンの自律飛行: 突風や他機との接近に応じた飛行経路の即時修正
- 手術支援ロボット: 患者の呼吸による臓器の動きに追従するリアルタイム制御
- 軌道上サービス: 回転デブリや協力的でないターゲット衛星へのランデブー・ドッキング
本記事では、オンライン軌道生成の代表的な3手法 — ポテンシャルフィールド法、DMP（Dynamic Movement Primitives）、MPC（Model Predictive Control） — を数式の導出からPython実装まで一気通貫で解説します。Ch.3「衝突回避と経路計画」の締めくくりとして、前回のオフライン経路計画（RRT/PRM）では対応しきれない動的環境への適応手法を身につけましょう。
本記事の内容
- オフライン計画とオンライン計画の本質的な違い
- ポテンシャルフィールド法の原理と局所最適問題の対策
- DMP（Dynamic Movement Primitives）による軌道変形
- MPC（Model Predictive Control）による最適化ベースの再計画
- 計算時間とリアルタイム性のトレードオフ
- Pythonによるポテンシャルフィールド法とDMPの実装・可視化
前提知識
この記事を読む前に、以下の記事を読んでおくと理解が深まります。
- 衝突回避の経路計画（RRT/PRM） — オフラインの経路計画アルゴリズム
- 計算トルク法による軌道追従制御 — 軌道追従の基本制御則
オフライン計画 vs オンライン計画
オフライン計画の限界
前回の記事で扱ったRRT（Rapidly-exploring Random Tree）やPRM（Probabilistic Roadmap）は、オフライン計画の代表例です。これらは環境の障害物配置が既知で、ロボットが動き出す前にすべての計画を完了させる手法でした。
オフライン計画は以下のような前提に立っています。
- 環境が静的である — 障害物の位置や形状は変化しない
- モデルが正確である — ロボットのキネマティクスやダイナミクスが十分に精密に分かっている
- 計算時間に制約がない — 動作開始前に必要なだけ時間をかけて最適解を探索できる
しかし宇宙環境では、これらの前提がことごとく崩れます。回転するデブリは数秒で姿勢が大きく変わり、微小重力下では予想外のドリフトが起こります。また、宇宙ロボットは自身の反力で母船が回転してしまう「フリーフライング系」であるため、モデルの不確かさも大きくなります。さらに、ミッション中の状況変化に対して「一旦停止して再計画」する余裕がないことも多いのです。
オンライン計画とは
オンライン計画（オンライン軌道生成） は、ロボットが動作を実行している最中に、センサ情報のフィードバックを受けて軌道を修正する手法です。計画と実行が時間的にオーバーラップする点が、オフライン計画との決定的な違いです。
両者の違いを表にまとめます。
| 特性 | オフライン計画 | オンライン計画 |
|---|---|---|
| 計画タイミング | 動作開始前に完了 | 動作中にリアルタイムで実行 |
| 環境の仮定 | 静的（既知の障害物） | 動的（障害物が移動・変形する） |
| センサフィードバック | 使用しない | 常時利用する |
| 計算時間の制約 | 比較的緩い | 制御周期内（数ms〜数十ms）で完了必須 |
| 解の最適性 | 大域的最適に近い解が得られやすい | 局所的な最適解にとどまることが多い |
| 代表的手法 | RRT, PRM, A* | ポテンシャルフィールド, DMP, MPC |
オンライン計画では、制御周期ごとに「今のセンサ情報に基づいて、次の瞬間にどこへ向かうべきか」を高速に判断する必要があります。そのため、計算の高速性と解の品質の間に本質的なトレードオフが存在します。
この表からわかるように、オフライン計画は「時間をかけて良い解を見つける」アプローチであり、オンライン計画は「短い時間で実用的な解を見つける」アプローチです。実際のシステムでは、両者を組み合わせることが多く、オフライン計画で大まかな経路を決めておき、オンライン計画でリアルタイムの微調整を行うハイブリッド方式が広く用いられています。
それでは、オンライン軌道生成の中でも最も直感的な手法であるポテンシャルフィールド法から見ていきましょう。
ポテンシャルフィールド法
ポテンシャルフィールドの直感
ポテンシャルフィールド法は、重力場のアナロジーで理解できます。ボールを斜面に置くと、ボールは低い方へ転がります。同じように、ロボットを「仮想的なポテンシャルの坂」の上に乗せて、ポテンシャルが下がる方向に動かすのがこの手法の本質です。
具体的には、空間内に2種類の仮想的な力場を設定します。
- 引力場（Attractive field）: 目標地点がロボットを引き寄せる。目標に近づくほどポテンシャルが低くなる
- 斥力場（Repulsive field）: 障害物がロボットを押し返す。障害物に近づくほどポテンシャルが高くなる
ロボットは、引力場と斥力場を合成した合成ポテンシャル場の勾配方向に沿って移動します。目標に向かいながらも障害物を避ける — この自然な挙動が、非常にシンプルな数式だけで実現できるのがポテンシャルフィールド法の魅力です。
引力ポテンシャルの定義
引力ポテンシャルは、ロボットの現在位置 $\bm{q}$ と目標位置 $\bm{q}_{\text{goal}}$ の距離の2乗に比例する形が最もシンプルです。これは「目標から離れるほどポテンシャルが急激に高くなる」という性質を表します。
$$ U_{\text{att}}(\bm{q}) = \frac{1}{2} k_{\text{att}} \| \bm{q} – \bm{q}_{\text{goal}} \|^2 $$
ここで $k_{\text{att}} > 0$ は引力ゲインです。このポテンシャルの負の勾配が引力を与えます。
勾配を計算すると、
$$ \bm{F}_{\text{att}}(\bm{q}) = -\nabla U_{\text{att}}(\bm{q}) = -k_{\text{att}} (\bm{q} – \bm{q}_{\text{goal}}) $$
となります。これは目標に向かう方向のベクトルであり、目標から遠いほど大きな力が生じます。フックの法則（バネの復元力）と同じ形であることに注目してください。ロボットは仮想的なバネで目標地点に結ばれているようなものです。
斥力ポテンシャルの定義
障害物による斥力ポテンシャルは、障害物に近づきすぎた場合にだけ作用するように設計します。ロボットと障害物 $i$ の距離を $d_i(\bm{q}) = \| \bm{q} – \bm{q}_{\text{obs},i} \|$ とし、影響範囲を $d_0$ とすると、斥力ポテンシャルは次のように定義されます。
$$ U_{\text{rep},i}(\bm{q}) = \begin{cases} \displaystyle \frac{1}{2} k_{\text{rep}} \left( \frac{1}{d_i(\bm{q})} – \frac{1}{d_0} \right)^2 & (d_i(\bm{q}) \le d_0) \\[6pt] 0 & (d_i(\bm{q}) > d_0) \end{cases} $$
ここで $k_{\text{rep}} > 0$ は斥力ゲインです。この式の形を直感的に理解しましょう。$1/d_i$ という項は、障害物に近づくほど急激に増大します。一方、$d_i > d_0$ のとき（十分離れているとき）はポテンシャルがゼロになるため、遠くの障害物は無視されます。
斥力ポテンシャルの勾配を計算します。$d_i(\bm{q}) \le d_0$ の場合、まず $1/d_i$ の勾配を求める必要があります。
$d_i(\bm{q}) = \| \bm{q} – \bm{q}_{\text{obs},i} \|$ であるから、
$$ \nabla d_i(\bm{q}) = \frac{\bm{q} – \bm{q}_{\text{obs},i}}{\| \bm{q} – \bm{q}_{\text{obs},i} \|} $$
次に、合成関数の微分（連鎖律）を適用すると、
$$ \nabla \left( \frac{1}{d_i} \right) = -\frac{1}{d_i^2} \nabla d_i = -\frac{\bm{q} – \bm{q}_{\text{obs},i}}{d_i^3} $$
これを用いて $U_{\text{rep},i}$ の勾配を求めると、
$$ \nabla U_{\text{rep},i}(\bm{q}) = k_{\text{rep}} \left( \frac{1}{d_i} – \frac{1}{d_0} \right) \cdot \left( -\frac{1}{d_i^2} \right) \cdot \nabla d_i $$
整理すると、斥力ベクトルは次のようになります。
$$ \bm{F}_{\text{rep},i}(\bm{q}) = -\nabla U_{\text{rep},i}(\bm{q}) = k_{\text{rep}} \left( \frac{1}{d_i} – \frac{1}{d_0} \right) \frac{1}{d_i^2} \cdot \frac{\bm{q} – \bm{q}_{\text{obs},i}}{d_i} $$
この力は障害物から遠ざかる方向を向いており、障害物に近いほど急激に大きくなります。
合成ポテンシャルと運動則
全体のポテンシャルは、引力ポテンシャルと全障害物からの斥力ポテンシャルの和で表されます。
$$ U(\bm{q}) = U_{\text{att}}(\bm{q}) + \sum_{i=1}^{N_{\text{obs}}} U_{\text{rep},i}(\bm{q}) $$
ロボットに作用する合成力は、
$$ \bm{F}(\bm{q}) = \bm{F}_{\text{att}}(\bm{q}) + \sum_{i=1}^{N_{\text{obs}}} \bm{F}_{\text{rep},i}(\bm{q}) $$
となり、ロボットの速度指令を次のように決定します。
$$ \dot{\bm{q}} = \alpha \bm{F}(\bm{q}) $$
ここで $\alpha > 0$ はステップサイズ（速度ゲイン）です。この運動則は勾配降下法と本質的に同じであり、ロボットはポテンシャルが最も急速に減少する方向へ移動します。
ポテンシャルフィールド法の最大の利点は、この計算の軽さです。現在位置と目標・障害物の位置関係だけから力を求めるので、制御周期（1ms程度）でも余裕を持って計算できます。これがリアルタイム再計画に適している理由です。
しかし、こうした単純な手法には見落とせない落とし穴があります。次に、ポテンシャルフィールド法が陥りやすい局所最適問題を見ていきましょう。
ポテンシャルフィールド法の局所最適問題と対策
局所最適に陥るメカニズム
ポテンシャルフィールド法の最も深刻な弱点は、局所最小値（local minimum） に捕まってしまう問題です。これは、引力と斥力がちょうど釣り合ってしまい、ロボットが目標に到達できないまま停止してしまう現象です。
典型的な例を考えましょう。目標がロボットの正面にあり、その直線上に障害物が立ちはだかっているとします。障害物の斥力がロボットを後ろに押し返し、目標の引力が前に引っ張る — この2力がバランスすると、ロボットは身動きが取れなくなります。
物理的な谷底に例えるとわかりやすいでしょう。ボールが谷底にはまると、どの方向に動いてもポテンシャルが上がるため、ボールはそこから抜け出せません。目標地点はもっと低いところにあるかもしれませんが、目の前の「谷」を越えられないのです。
また、障害物に沿って進むべき場面で、障害物の端が目標の反対方向にある場合にも問題が生じます。ロボットは一度目標から離れる方向に移動しなければなりませんが、引力がそれを許しません。
対策1: ランダム摂動
最もシンプルな対策は、ロボットが停止（あるいは振動）したことを検出したら、ランダムな力を加えて局所最小値から脱出させる方法です。
$$ \bm{F}_{\text{total}}(\bm{q}) = \bm{F}(\bm{q}) + \bm{F}_{\text{noise}} $$
ここで $\bm{F}_{\text{noise}}$ はランダムなベクトルです。焼きなまし法（Simulated Annealing）と同様の発想で、確率的にポテンシャルの山を乗り越えます。
この方法は実装が極めて簡単ですが、脱出の保証がなく、効率が悪い場合もあります。
対策2: 仮想壁面追従（Wall Following）
障害物の表面に沿って移動する「壁面追従」を組み込む方法です。局所最小値を検出したら、斥力の方向を $90°$ 回転させた方向に力を生成します。
2次元の場合、斥力ベクトル $\bm{F}_{\text{rep}} = (F_x, F_y)$ に対して、回転行列を適用します。
$$ \bm{F}_{\text{wall}} = \begin{pmatrix} 0 & -1 \\ 1 & 0 \end{pmatrix} \bm{F}_{\text{rep}} = \begin{pmatrix} -F_y \\ F_x \end{pmatrix} $$
これにより、障害物を「なぞる」ようにして回り込むことができます。回転方向（時計回りか反時計回りか）は、目標方向に近い方を選択します。
対策3: ナビゲーション関数
より理論的に洗練された対策として、ナビゲーション関数（Navigation Function） があります。これは、目標地点のみが唯一の極小値となるようにポテンシャル関数を設計する手法です。Rimon と Koditschek（1992）が提唱しました。
ナビゲーション関数 $\varphi(\bm{q})$ は、以下の性質を満たすように構成されます。
- 目標地点 $\bm{q}_{\text{goal}}$ でのみ $\varphi = 0$（唯一の極小値）
- ワークスペースの境界と障害物の表面で $\varphi = 1$（最大値）
- $\varphi$ は滑らかで、目標点以外に極値を持たない
具体的な構成として、球体ワールドにおけるナビゲーション関数は次の形をとります。
$$ \varphi(\bm{q}) = \frac{\| \bm{q} – \bm{q}_{\text{goal}} \|^{2k}}{\left( \| \bm{q} – \bm{q}_{\text{goal}} \|^{2k} + \beta(\bm{q}) \right)^{1/k}} $$
ここで $\beta(\bm{q})$ は障害物と境界からの距離を統合した関数、$k$ は十分に大きな正整数です。$k$ を大きくすると局所最小値が消えることが証明されています。
ナビゲーション関数は理論的には完璧ですが、実用上は環境モデルが正確に必要であり、複雑な形状の障害物への拡張が難しいという課題があります。
実際のシステムでは、上記の対策を組み合わせて使うことが多く、特に「まずポテンシャルフィールドで高速に動かし、停滞を検出したら壁面追従に切り替える」というハイブリッド戦略が実用的です。
ポテンシャルフィールド法は「力」を使って軌道を生成しましたが、あらかじめ用意した参照軌道を「変形」するアプローチも存在します。次に紹介するDMPは、まさにそのような発想に基づく手法です。
DMP（Dynamic Movement Primitives）— 力項で軌道を変形する
DMPの直感
DMP（Dynamic Movement Primitives）は、お手本の動作を覚えて再生しつつ、必要に応じて変形する手法です。日常のアナロジーで説明しましょう。
書道の達人が「永」の字を書く動作を考えてください。達人は基本の筆運びを身体で覚えていますが、紙のサイズが変わっても、傾いた台の上でも、風で紙が動いても、「永」の字を書けます。つまり、基本動作のパターンを保持しつつ、目標位置や外部環境に応じて柔軟に変形できるのです。
DMPはこれを数式で実現します。減衰バネ系（安定なダイナミクス）をベースとして、そこに学習した力項（forcing term）を加えることで、お手本の軌道形状を再現します。そして、目標位置を変えたり外力項を追加したりすることで、軌道をリアルタイムに変形できます。
DMPの数学的定式化
DMPは変換系（transformation system）と正準系（canonical system）の2つのサブシステムから構成されます。
正準系（Canonical System）
正準系は、動作の「時間的進行」を表す単純な1階の微分方程式です。
$$ \tau \dot{s} = -\alpha_s s $$
ここで $s$ は位相変数（phase variable）、$\tau$ は時間スケーリング定数、$\alpha_s > 0$ は減衰定数です。初期値 $s(0) = 1$ から指数的に $s \to 0$ に減衰していきます。
$$ s(t) = \exp\left( -\frac{\alpha_s}{\tau} t \right) $$
なぜ時間 $t$ を直接使わず、位相変数 $s$ を導入するのでしょうか。理由は2つあります。第一に、$\tau$ を変えるだけで動作の速度を変えられます（時間スケーリング）。第二に、$s$ は単調減少なので、動作の「進捗率」として自然に解釈でき、後述する力項の基底関数を $s$ の関数として定義できます。
変換系（Transformation System）
変換系は、減衰バネ系に力項 $f(s)$ を加えた2階の微分方程式です。1自由度の場合を示します。
$$ \tau \dot{v} = \alpha_v (\beta_v (g – y) – v) + f(s) $$
$$ \tau \dot{y} = v $$
ここで、
- $y$: 位置（出力）
- $v$: スケーリングされた速度
- $g$: 目標位置
- $\alpha_v, \beta_v > 0$: ダイナミクスのパラメータ
- $f(s)$: 力項（forcing term）
$f(s) = 0$ のとき、この系は単なる臨界減衰バネ系です。$\alpha_v = 4\beta_v$ と設定すると臨界減衰となり、$y$ は振動なく $g$ に収束します。バネ定数が大きいほど速く収束し、減衰が効いているのでオーバーシュートしません。
力項 $f(s)$ は、この安定な収束挙動を歪ませる役割を果たします。力項があることで、単調な収束ではなく、お手本のような複雑な軌道形状を再現できるのです。
力項（Forcing Term）
力項は、正規化されたガウス基底関数の重み付き線形結合で表現されます。
$$ f(s) = \frac{\sum_{i=1}^{N} w_i \psi_i(s) \cdot s}{\sum_{i=1}^{N} \psi_i(s)} \cdot (g – y_0) $$
ここで、
- $w_i$: 学習で決定される重みパラメータ
- $\psi_i(s) = \exp\left( -h_i (s – c_i)^2 \right)$: ガウス基底関数
- $c_i$: 基底関数の中心（$s$ 軸上に等間隔に配置）
- $h_i$: 基底関数の幅を決めるパラメータ
- $y_0$: 初期位置
- $(g – y_0)$: スケーリング項（目標位置が変わっても形状を保つ）
重要な点が2つあります。第一に、力項に $s$ が掛かっていることで、$s \to 0$（動作終盤）では力項が自動的にゼロに減衰します。これにより、動作の終端では必ず目標位置 $g$ に到達する安定性が保証されます。第二に、$(g – y_0)$ でスケーリングしているため、目標位置を変えても軌道の「形」が保存されます。
重みの学習
お手本の軌道 $y_{\text{demo}}(t)$ が与えられたとき、重み $w_i$ を求める手順は以下の通りです。
まず、お手本軌道から目標とする力項 $f_{\text{target}}(s)$ を逆算します。変換系の式を $f(s)$ について解くと、
$$ f_{\text{target}}(s) = \tau \dot{v}_{\text{demo}} – \alpha_v (\beta_v (g – y_{\text{demo}}) – v_{\text{demo}}) $$
右辺の $\dot{v}_{\text{demo}}$, $v_{\text{demo}}$, $y_{\text{demo}}$ はお手本軌道の数値微分から得られます。
次に、各時刻 $t_j$ での $f_{\text{target}}(s_j)$ を目標値として、重み付き最小二乗法で $w_i$ を求めます。各基底関数 $\psi_i$ について、
$$ w_i = \frac{\sum_j \psi_i(s_j) \cdot s_j \cdot f_{\text{target}}(s_j)}{\sum_j \psi_i(s_j) \cdot s_j^2} $$
この学習は、各 $w_i$ について独立に解ける（閉形式の解がある）ため、非常に高速です。反復最適化が不要である点も実用上の大きなメリットです。
DMPによるオンライン再計画
DMPがオンライン再計画に適しているのは、以下の修正がリアルタイムで可能だからです。
1. 目標位置の変更: $g$ を新しい値に差し替えるだけで、軌道が自動的に新目標へ向かいます。スケーリング項 $(g – y_0)$ のおかげで、軌道の形状は保存されます。
2. 障害物回避: 変換系に追加の外力項 $\bm{C}$ を加えます。
$$ \tau \dot{v} = \alpha_v (\beta_v (g – y) – v) + f(s) + C $$
例えば、ポテンシャルフィールド法の斥力をこの $C$ として使うことで、DMPの滑らかな軌道生成と障害物回避を両立できます。
3. 時間スケーリング: $\tau$ を変更するだけで、軌道形状を保ったまま動作速度を変更できます。急ぐ場面では $\tau$ を小さく、慎重に動く場面では大きくします。
このように、DMPは「力場による反応的手法」と「最適化ベースの計画的手法」の中間に位置する、柔軟な軌道生成フレームワークです。しかし、DMPはあくまで参照軌道の変形であり、制約条件（トルクリミット、関節角度制限など）を体系的に扱うことは得意ではありません。こうした制約を明示的に考慮した軌道再計画が必要な場面では、MPC（Model Predictive Control）が威力を発揮します。
MPC（Model Predictive Control）による軌道再計画
MPCの基本思想
MPC（Model Predictive Control、モデル予測制御）は、「先を予測して、今の行動を決める」制御手法です。チェスや囲碁の先読みに例えるとわかりやすいでしょう。プレイヤーは数手先まで展開を読み、その中で最も有利な最初の一手を打ちます。相手の手が見えたら、改めて数手先を読み直して次の一手を決める — この繰り返しがMPCの本質です。
具体的には、MPCは各制御周期で以下の最適化問題を解きます。
MPCの最適化問題
ロボットの状態を $\bm{x}$、制御入力を $\bm{u}$、離散時間のシステムモデルを $\bm{x}_{k+1} = f(\bm{x}_k, \bm{u}_k)$ とします。MPCは、現在時刻 $k$ から予測ホライズン $N$ ステップ先までの最適制御問題を解きます。
$$ \min_{\bm{u}_k, \bm{u}_{k+1}, \dots, \bm{u}_{k+N-1}} \left[ \sum_{j=0}^{N-1} \ell(\bm{x}_{k+j}, \bm{u}_{k+j}) + V_f(\bm{x}_{k+N}) \right] $$
ここで、
- $\ell(\bm{x}, \bm{u})$: ステージコスト（各時刻のペナルティ）
- $V_f(\bm{x}_{k+N})$: 終端コスト（予測ホライズン末端でのペナルティ）
この最適化は以下の制約の下で行われます。
$$ \bm{x}_{k+j+1} = f(\bm{x}_{k+j}, \bm{u}_{k+j}), \quad j = 0, 1, \dots, N-1 $$
$$ \bm{x}_{k+j} \in \mathcal{X}, \quad \bm{u}_{k+j} \in \mathcal{U} $$
$\mathcal{X}$ は状態の許容範囲（関節角度リミットなど）、$\mathcal{U}$ は入力の許容範囲（トルクリミットなど）を表します。
最適化を解いて得られる入力列 $\bm{u}_k^*, \bm{u}_{k+1}^*, \dots, \bm{u}_{k+N-1}^*$ のうち、最初の入力 $\bm{u}_k^*$ のみを実際に適用し、次のステップで再び最適化を解きます。このプロセスを後退ホライズン（Receding Horizon）と呼びます。
なぜ最初の一手だけを使うのか
直感的には、「せっかく $N$ ステップ分の最適入力を計算したのに、1ステップ分しか使わないのはもったいない」と感じるかもしれません。しかし、これこそがMPCの強みです。
1ステップ実行した後、センサから最新の状態 $\bm{x}_{k+1}$ を観測し、最新情報に基づいて再び最適化を解きます。これにより、モデルの誤差や外乱の影響を自然に補正できるのです。もし $N$ ステップ分をすべて使い切ってしまうと（オープンループ制御）、その間に生じた状態のずれが蓄積してしまいます。
ステージコストの設計例
宇宙ロボットの軌道再計画では、ステージコストは以下のように設計されることが多いです。
$$ \ell(\bm{x}, \bm{u}) = (\bm{x} – \bm{x}_{\text{ref}})^T \bm{Q} (\bm{x} – \bm{x}_{\text{ref}}) + \bm{u}^T \bm{R} \bm{u} $$
第1項は参照軌道 $\bm{x}_{\text{ref}}$ からのずれに対するペナルティ（重み行列 $\bm{Q} \succeq 0$）、第2項は制御入力の大きさに対するペナルティ（重み行列 $\bm{R} \succ 0$）です。
$\bm{Q}$ と $\bm{R}$ のバランスが性能を大きく左右します。$\bm{Q}$ を大きくすると参照軌道への追従が優先され、$\bm{R}$ を大きくすると制御入力の節約（燃料節約）が優先されます。宇宙ミッションでは燃料は貴重な資源であるため、$\bm{R}$ の設計は特に重要です。
障害物回避の制約
MPCの強みは、障害物回避を制約条件として明示的に扱える点です。ポテンシャルフィールド法のように反応的に力を加えるのではなく、予測ホライズン全体で「この領域には入らない」ことを保証できます。
障害物 $i$ の中心 $\bm{p}_{\text{obs},i}$、安全半径 $r_i$ に対して、
$$ \| \bm{p}_{k+j} – \bm{p}_{\text{obs},i} \| \ge r_i, \quad j = 0, 1, \dots, N $$
ここで $\bm{p}_{k+j}$ はロボットの手先位置（状態 $\bm{x}_{k+j}$ から順運動学で計算）です。この非線形不等式制約により、予測ホライズン内で障害物との衝突が確実に回避されます。
ただし、この制約は非凸であるため、一般的な二次計画法（QP）では直接扱えません。実用上は以下のような手法で対処します。
- 逐次凸近似（Sequential Convex Programming, SCP）: 非凸制約を線形化して凸問題に近似し、反復的に解く
- ペナルティ法: 制約違反をコスト関数にペナルティとして追加する
- CBF（Control Barrier Function）: 安全制約を状態依存の不等式として定式化する
MPCの計算コスト
MPCの最大の課題は計算コストです。各制御周期で最適化問題を解く必要があるため、問題のサイズと許容される計算時間のバランスが重要です。
線形システム・二次コストの場合（線形MPC）は、二次計画問題（QP）に帰着し、数ms以内で解けます。一方、非線形モデルや非凸制約を含む場合（非線形MPC）は、数十ms〜数百msが必要になることもあります。
計算時間を短縮するための工夫として、以下が研究されています。
- ウォームスタート: 前ステップの解を初期値として再利用する
- 予測ホライズンの短縮: $N$ を小さくして問題サイズを減らす（ただし性能は低下）
- 事前計算: 典型的なシナリオの解をオフラインで計算しておき、オンラインでは補間する
- GPUによる並列化: 複数のシューティング区間を並列に処理する
MPCは最も体系的で制約を厳密に扱える手法ですが、計算コストの面でリアルタイム性との折り合いが必要です。次のセクションでは、この3手法の特性を比較し、計算時間とリアルタイム性のトレードオフについて整理します。
計算時間とリアルタイム性のトレードオフ
3手法の特性比較
ここまで紹介した3手法を、リアルタイム再計画の観点から比較します。
| 特性 | ポテンシャルフィールド | DMP | MPC |
|---|---|---|---|
| 計算時間（1ステップ） | $O(N_{\text{obs}})$ 〜数$\mu$s | $O(N_{\text{basis}})$ 〜数$\mu$s | $O(N^3)$ 〜数ms以上 |
| 事前学習 | 不要 | 必要（デモ軌道） | 不要（モデルは必要） |
| 制約の取り扱い | 暗黙的（力ベース） | 追加の力項で対応 | 明示的（制約条件） |
| 局所最適問題 | あり | なし（収束保証） | 非凸なら可能性あり |
| 軌道の滑らかさ | パラメータ依存 | 基底関数で保証 | コスト設計で制御 |
| 目標変更への対応 | 即座に反映 | 即座に反映 | 次の最適化で反映 |
| 動的障害物への対応 | 得意 | 追加力項で可能 | 制約更新で可能 |
リアルタイム性の定義
「リアルタイム」とは、単に「速い」という意味ではありません。制御工学における厳密な定義は、決められた時間内に必ず計算が完了すること（時間的決定性） です。
宇宙ロボットの場合、制御周期は典型的に以下の値をとります。
- 内部制御ループ（関節サーボ）: 1 kHz（1ms周期）
- 外部制御ループ（軌道計画）: 10〜100 Hz（10〜100ms周期）
- ミッション計画レベル: 1 Hz以下（数秒〜数分周期）
オンライン軌道生成は通常、外部制御ループに位置するため、10〜100msの計算時間が許容されます。この時間内に収まるかどうかが、各手法のリアルタイム適用可能性を決定します。
階層的アーキテクチャ
実用的なシステムでは、上記の3手法を階層的に組み合わせることが多いです。
- ミッション計画層（オフライン / 低頻度更新）: RRTやPRMで大まかな経路を計画
- 軌道生成層（数十ms周期）: DMPやMPCで参照軌道を生成・更新
- 反応層（数ms周期）: ポテンシャルフィールド法で即時的な衝突回避
この階層構造の利点は、各層が適切な時間スケールで処理を行えることです。大域的な経路は計算時間をかけて最適化し、局所的な障害物回避は高速な反応的手法で処理します。異なる抽象度と時間スケールを持つ手法を組み合わせることで、計算コストと解の品質を両立しています。
宇宙ステーションのロボットアーム「JEMRMS」や「ERA」では、実際にこのような階層的アーキテクチャが採用されており、ミッション計画レベルでは地上局からのコマンドに基づきオフラインで経路を決定し、実行時には搭載コンピュータがリアルタイムで微調整を行います。
理論と設計方針を理解したところで、実際にPythonで実装して動作を確認しましょう。まずはポテンシャルフィールド法から実装します。
Pythonでポテンシャルフィールド法を実装する
2次元のポテンシャルフィールドと経路生成
まず、2次元平面でポテンシャルフィールド法を実装し、ロボットが障害物を避けながら目標に到達する様子を可視化します。
import numpy as np
import matplotlib.pyplot as plt
# --- パラメータ設定 ---
k_att = 1.0 # 引力ゲイン
k_rep = 100.0 # 斥力ゲイン
d0 = 3.0 # 斥力の影響範囲
alpha = 0.1 # ステップサイズ
max_iter = 500 # 最大反復回数
goal_tol = 0.2 # 目標到達判定の閾値
# 開始位置・目標位置
q_start = np.array([0.0, 0.0])
q_goal = np.array([10.0, 10.0])
# 障害物の位置（中心座標のリスト）
obstacles = np.array([
[3.0, 3.0],
[5.0, 5.5],
[7.0, 4.0],
[6.0, 8.0],
[8.5, 7.0],
])
def attractive_force(q, q_goal, k_att):
"""引力ベクトルを計算"""
return -k_att * (q - q_goal)
def repulsive_force(q, obstacles, k_rep, d0):
"""全障害物からの斥力ベクトルの合計を計算"""
f_rep = np.zeros(2)
for obs in obstacles:
d = np.linalg.norm(q - obs)
if d < 1e-6:
d = 1e-6 # ゼロ除算回避
if d <= d0:
# 斥力の大きさと方向
magnitude = k_rep * (1.0/d - 1.0/d0) / (d**2)
direction = (q - obs) / d
f_rep += magnitude * direction
return f_rep
# --- 経路の生成 ---
path = [q_start.copy()]
q = q_start.copy()
for i in range(max_iter):
f_att = attractive_force(q, q_goal, k_att)
f_rep = repulsive_force(q, obstacles, k_rep, d0)
f_total = f_att + f_rep
# 力の大きさを制限（安定性のため）
f_norm = np.linalg.norm(f_total)
if f_norm > 5.0:
f_total = f_total / f_norm * 5.0
q = q + alpha * f_total
path.append(q.copy())
# 目標到達判定
if np.linalg.norm(q - q_goal) < goal_tol:
print(f"目標に到達しました（{i+1}ステップ）")
break
path = np.array(path)
このコードでは、各ステップで引力と斥力を計算し、合成力の方向にロボットを移動させています。力の大きさに上限を設けている点に注意してください。これは、障害物の近くで斥力が極端に大きくなることを防ぐための実用的な工夫です。
次に、ポテンシャル場と経路を可視化します。
import numpy as np
import matplotlib.pyplot as plt
# ポテンシャル場の計算（可視化用）
x_range = np.linspace(-1, 12, 200)
y_range = np.linspace(-1, 12, 200)
X, Y = np.meshgrid(x_range, y_range)
U = np.zeros_like(X)
for i in range(X.shape[0]):
for j in range(X.shape[1]):
q_ij = np.array([X[i, j], Y[i, j]])
# 引力ポテンシャル
U[i, j] = 0.5 * k_att * np.linalg.norm(q_ij - q_goal)**2
# 斥力ポテンシャル
for obs in obstacles:
d = np.linalg.norm(q_ij - obs)
if d < 0.3:
d = 0.3 # 可視化時のクリッピング
if d <= d0:
U[i, j] += 0.5 * k_rep * (1.0/d - 1.0/d0)**2
# ポテンシャルを上限でクリッピング（可視化のため）
U = np.clip(U, 0, 150)
fig, axes = plt.subplots(1, 2, figsize=(16, 7))
# 左: ポテンシャル場の等高線と経路
ax = axes[0]
contour = ax.contourf(X, Y, U, levels=50, cmap='viridis')
fig.colorbar(contour, ax=ax, label='Potential U(q)')
ax.plot(path[:, 0], path[:, 1], 'w-', linewidth=2, label='Path')
ax.plot(q_start[0], q_start[1], 'go', markersize=12, label='Start')
ax.plot(q_goal[0], q_goal[1], 'r*', markersize=15, label='Goal')
for obs in obstacles:
circle = plt.Circle(obs, 0.5, color='red', alpha=0.7)
ax.add_patch(circle)
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_title('Potential Field and Path')
ax.legend(loc='upper left')
ax.set_xlim(-1, 12)
ax.set_ylim(-1, 12)
ax.set_aspect('equal')
# 右: 経路の拡大図
ax = axes[1]
ax.plot(path[:, 0], path[:, 1], 'b-o', markersize=2, linewidth=1.5, label='Path')
ax.plot(q_start[0], q_start[1], 'go', markersize=12, label='Start')
ax.plot(q_goal[0], q_goal[1], 'r*', markersize=15, label='Goal')
for obs in obstacles:
circle_fill = plt.Circle(obs, 0.5, color='red', alpha=0.5)
circle_influence = plt.Circle(obs, d0, color='red', fill=False,
linestyle='--', alpha=0.3)
ax.add_patch(circle_fill)
ax.add_patch(circle_influence)
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_title('Path with Obstacle Influence Zones')
ax.legend(loc='upper left')
ax.set_xlim(-1, 12)
ax.set_ylim(-1, 12)
ax.set_aspect('equal')
ax.grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('potential_field_path.png', dpi=150, bbox_inches='tight')
plt.show()
上のグラフから、以下の特徴が読み取れます。
- 左図のポテンシャル場: 目標地点（赤い星印）に向かってポテンシャルが低下する「谷」が形成されている一方、各障害物の周囲にはポテンシャルの「山」が見えます。ロボットの経路（白い線）は、この合成ポテンシャルの谷間を辿るように滑らかに目標へ向かっています。
- 右図の影響範囲: 赤い破線円が各障害物の斥力影響範囲 $d_0$ を示しています。経路はこの影響範囲の外側を巧みに通過しつつ、全体としては目標方向に進んでいることがわかります。障害物同士の影響範囲が重なっている箇所では、経路が大きく迂回しています。
局所最適に陥る例と壁面追従による脱出
次に、局所最適に陥る典型的な配置と、壁面追従による脱出を実装します。
import numpy as np
import matplotlib.pyplot as plt
# 局所最適に陥りやすい配置
q_start_trap = np.array([0.0, 5.0])
q_goal_trap = np.array([10.0, 5.0])
obstacles_trap = np.array([
[5.0, 5.0], # 目標との直線上に障害物
[5.0, 3.5], # 上下を塞ぐ
[5.0, 6.5],
])
k_att_t = 1.0
k_rep_t = 80.0
d0_t = 3.0
alpha_t = 0.05
max_iter_t = 1000
def run_potential_field(q_start, q_goal, obstacles, k_att, k_rep, d0,
alpha, max_iter, use_wall_following=False):
"""ポテンシャルフィールド法で経路生成（壁面追従オプション付き）"""
path = [q_start.copy()]
q = q_start.copy()
stuck_count = 0
wall_following = False
for i in range(max_iter):
f_att = attractive_force(q, q_goal, k_att)
f_rep = repulsive_force(q, obstacles, k_rep, d0)
f_total = f_att + f_rep
# 壁面追従モード
if use_wall_following:
f_norm = np.linalg.norm(f_total)
if f_norm < 0.3:
stuck_count += 1
else:
stuck_count = max(0, stuck_count - 1)
if stuck_count > 10:
# 斥力を90度回転させた方向に進む
f_rep_norm = np.linalg.norm(f_rep)
if f_rep_norm > 1e-6:
f_wall = np.array([-f_rep[1], f_rep[0]])
f_total = f_att + f_rep + 2.0 * f_wall
wall_following = True
elif wall_following and np.linalg.norm(f_total) > 1.0:
wall_following = False
stuck_count = 0
# 力の制限
f_norm = np.linalg.norm(f_total)
if f_norm > 5.0:
f_total = f_total / f_norm * 5.0
q = q + alpha * f_total
path.append(q.copy())
if np.linalg.norm(q - q_goal) < 0.3:
break
return np.array(path)
# 通常のポテンシャルフィールド法
path_normal = run_potential_field(
q_start_trap, q_goal_trap, obstacles_trap,
k_att_t, k_rep_t, d0_t, alpha_t, max_iter_t,
use_wall_following=False
)
# 壁面追従付き
path_wf = run_potential_field(
q_start_trap, q_goal_trap, obstacles_trap,
k_att_t, k_rep_t, d0_t, alpha_t, max_iter_t,
use_wall_following=True
)
fig, axes = plt.subplots(1, 2, figsize=(16, 7))
for ax, p, title in zip(axes, [path_normal, path_wf],
['Standard (stuck)', 'Wall Following']):
ax.plot(p[:, 0], p[:, 1], 'b-o', markersize=1, linewidth=1.5, label='Path')
ax.plot(q_start_trap[0], q_start_trap[1], 'go', markersize=12, label='Start')
ax.plot(q_goal_trap[0], q_goal_trap[1], 'r*', markersize=15, label='Goal')
for obs in obstacles_trap:
circle = plt.Circle(obs, 0.4, color='red', alpha=0.6)
circle_d0 = plt.Circle(obs, d0_t, color='red', fill=False,
linestyle='--', alpha=0.2)
ax.add_patch(circle)
ax.add_patch(circle_d0)
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_title(title)
ax.legend(loc='upper left')
ax.set_xlim(-1, 12)
ax.set_ylim(0, 10)
ax.set_aspect('equal')
ax.grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('local_minimum_comparison.png', dpi=150, bbox_inches='tight')
plt.show()
上のグラフから、局所最適問題とその対策の効果が明確に見えます。
- 左図（Standard）: ロボットが障害物の手前で停滞しています。3つの障害物が目標方向を塞いでおり、引力と斥力が釣り合って合成力がほぼゼロになっているのです。経路の末端が目標に到達していないことに注目してください。これが典型的な局所最小値の罠です。
- 右図（Wall Following）: 壁面追従を導入することで、ロボットは障害物の壁に沿って回り込み、局所最小値から脱出しています。斥力の90度回転という単純な操作だけで、大幅に到達性能が改善されることがわかります。
ポテンシャルフィールド法の実装と限界を確認できたところで、次はDMPを実装し、お手本軌道の学習と目標位置変更による軌道変形を体験しましょう。
PythonでDMPを実装する
DMPクラスの実装
1自由度のDMPをクラスとして実装します。正準系、変換系、力項の学習を一つのクラスにまとめます。
import numpy as np
import matplotlib.pyplot as plt
class DMP:
"""1自由度のDynamic Movement Primitives"""
def __init__(self, n_basis=25, alpha_v=25.0, beta_v=6.25, alpha_s=1.0):
"""
Parameters
----------
n_basis : int
ガウス基底関数の数
alpha_v : float
変換系の減衰係数
beta_v : float
変換系のバネ係数（alpha_v = 4*beta_v で臨界減衰）
alpha_s : float
正準系の減衰係数
"""
self.n_basis = n_basis
self.alpha_v = alpha_v
self.beta_v = beta_v
self.alpha_s = alpha_s
# 基底関数のパラメータを設定
self.c = np.exp(-alpha_s * np.linspace(0, 1, n_basis))
self.h = 1.0 / (np.diff(self.c)**2)
self.h = np.append(self.h, self.h[-1])
self.w = np.zeros(n_basis) # 重みパラメータ
self.tau = 1.0
self.y0 = 0.0
self.g = 1.0
def canonical_system(self, t):
"""正準系: s(t) = exp(-alpha_s / tau * t)"""
return np.exp(-self.alpha_s / self.tau * t)
def basis_functions(self, s):
"""ガウス基底関数を評価"""
return np.exp(-self.h * (s - self.c)**2)
def forcing_term(self, s):
"""力項 f(s) を計算"""
psi = self.basis_functions(s)
# 正規化された重み付き和
f = np.dot(self.w, psi) * s / (np.sum(psi) + 1e-10)
f *= (self.g - self.y0) # スケーリング
return f
def learn_from_demo(self, y_demo, dy_demo, ddy_demo, t_demo):
"""デモ軌道から重みを学習"""
self.tau = t_demo[-1]
self.y0 = y_demo[0]
self.g = y_demo[-1]
# デモ軌道に対応する位相変数
s_demo = self.canonical_system(t_demo)
# 目標力項を逆算
# f_target = tau * ddy - alpha_v * (beta_v * (g - y) - tau * dy)
f_target = (self.tau**2 * ddy_demo
- self.alpha_v * (self.beta_v * (self.g - y_demo)
- self.tau * dy_demo))
# スケーリングを除去
if abs(self.g - self.y0) > 1e-6:
f_target /= (self.g - self.y0)
# 重み付き最小二乗法で各基底関数の重みを学習
for i in range(self.n_basis):
psi = self.basis_functions(s_demo)[:, i] if s_demo.ndim > 0 else self.basis_functions(s_demo)[i]
# 各時刻での基底関数値を計算
psi_values = np.array([self.basis_functions(s)[i] for s in s_demo])
numerator = np.sum(psi_values * s_demo * f_target)
denominator = np.sum(psi_values * s_demo**2) + 1e-10
self.w[i] = numerator / denominator
def generate(self, y0, g, tau, dt=0.001, coupling_term=None):
"""軌道を生成"""
n_steps = int(tau / dt)
y = np.zeros(n_steps)
dy = np.zeros(n_steps)
t = np.zeros(n_steps)
y[0] = y0
dy[0] = 0.0
v = 0.0 # スケーリングされた速度
original_g = self.g
original_y0 = self.y0
self.g = g
self.y0 = y0
for i in range(1, n_steps):
t[i] = i * dt
s = self.canonical_system(t[i])
f = self.forcing_term(s)
# 外力（障害物回避など）
c_term = 0.0
if coupling_term is not None:
c_term = coupling_term(y[i-1], t[i])
# 変換系の更新（オイラー法）
dv = (self.alpha_v * (self.beta_v * (g - y[i-1]) - v)
+ f + c_term) / tau
v += dv * dt
dy[i] = v / tau
y[i] = y[i-1] + dy[i] * dt
self.g = original_g
self.y0 = original_y0
return t, y, dy
デモ軌道の学習と再生
お手本として正弦波を含む軌道を生成し、DMPに学習させます。その後、目標位置を変更して軌道がどう変形するかを確認します。
import numpy as np
import matplotlib.pyplot as plt
# --- デモ軌道の生成（正弦波を含む滑らかな軌道）---
tau_demo = 2.0
dt = 0.001
t_demo = np.arange(0, tau_demo, dt)
# 0から1へ向かいつつ、途中で波打つ軌道
y_demo = t_demo / tau_demo + 0.3 * np.sin(2 * np.pi * t_demo / tau_demo)
dy_demo = np.gradient(y_demo, dt)
ddy_demo = np.gradient(dy_demo, dt)
# DMPの学習
dmp = DMP(n_basis=30, alpha_v=25.0, beta_v=6.25, alpha_s=1.0)
dmp.learn_from_demo(y_demo, dy_demo, ddy_demo, t_demo)
# 元の目標で再生
t1, y1, dy1 = dmp.generate(y0=0.0, g=y_demo[-1], tau=tau_demo, dt=dt)
# 目標位置を変更して再生
t2, y2, dy2 = dmp.generate(y0=0.0, g=2.0, tau=tau_demo, dt=dt)
# 目標位置をさらに変更（負の方向）
t3, y3, dy3 = dmp.generate(y0=0.0, g=-0.5, tau=tau_demo, dt=dt)
# 時間スケーリング（速く実行）
t4, y4, dy4 = dmp.generate(y0=0.0, g=y_demo[-1], tau=tau_demo * 0.5, dt=dt)
fig, axes = plt.subplots(2, 2, figsize=(14, 10))
# 左上: デモ軌道 vs DMP再生
ax = axes[0, 0]
ax.plot(t_demo, y_demo, 'k--', linewidth=2, label='Demo trajectory')
ax.plot(t1, y1, 'b-', linewidth=1.5, label='DMP reproduction')
ax.set_xlabel('Time [s]')
ax.set_ylabel('Position y')
ax.set_title('Demo vs DMP Reproduction')
ax.legend()
ax.grid(True, alpha=0.3)
# 右上: 目標位置変更
ax = axes[0, 1]
ax.plot(t1, y1, 'b-', linewidth=1.5, label=f'g = {y_demo[-1]:.2f} (original)')
ax.plot(t2, y2, 'r-', linewidth=1.5, label='g = 2.0')
ax.plot(t3, y3, 'g-', linewidth=1.5, label='g = -0.5')
ax.set_xlabel('Time [s]')
ax.set_ylabel('Position y')
ax.set_title('Goal Position Modification')
ax.legend()
ax.grid(True, alpha=0.3)
# 左下: 時間スケーリング
ax = axes[1, 0]
ax.plot(t1, y1, 'b-', linewidth=1.5, label=f'tau = {tau_demo:.1f}s (original)')
ax.plot(t4, y4, 'm-', linewidth=1.5, label=f'tau = {tau_demo*0.5:.1f}s (fast)')
ax.set_xlabel('Time [s]')
ax.set_ylabel('Position y')
ax.set_title('Time Scaling')
ax.legend()
ax.grid(True, alpha=0.3)
# 右下: 基底関数の重み
ax = axes[1, 1]
ax.bar(range(dmp.n_basis), dmp.w, color='steelblue', alpha=0.7)
ax.set_xlabel('Basis function index')
ax.set_ylabel('Weight $w_i$')
ax.set_title('Learned Weights')
ax.grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('dmp_demo.png', dpi=150, bbox_inches='tight')
plt.show()
上の4つのグラフから、DMPの重要な特性が確認できます。
- 左上（再生精度）: DMPの再生軌道（青実線）がデモ軌道（黒破線）をよく近似しています。30個の基底関数で、正弦波を含む複雑な軌道形状を忠実に再現できていることがわかります。
- 右上（目標変更）: 目標位置を $g = 2.0$ や $g = -0.5$ に変更しても、軌道の「うねり」の形状が保存されたまま、スケーリングされています。これは力項のスケーリング項 $(g – y_0)$ の効果です。目標が逆方向になると軌道も反転しますが、形状のパターンは維持されています。
- 左下（時間スケーリング）: $\tau$ を半分にすることで、軌道形状はそのままに実行時間が半分になっています。正準系の位相変数 $s$ を介して時間を管理しているため、この操作は非常に自然です。
- 右下（重みパラメータ）: 学習された重み $w_i$ の分布を見ると、動作の前半（基底関数のインデックスが小さい方）で大きな重みがあり、これは正弦波の振幅が大きい区間に対応しています。
障害物回避を組み込んだDMP
DMPの変換系にポテンシャルフィールドの斥力を追加し、障害物を回避する軌道を生成します。ここでは2次元に拡張して可視化します。
import numpy as np
import matplotlib.pyplot as plt
class DMP2D:
"""2自由度のDMP（x, y各軸に独立なDMPを持つ）"""
def __init__(self, n_basis=25, alpha_v=25.0, beta_v=6.25, alpha_s=1.0):
self.dmp_x = DMP(n_basis, alpha_v, beta_v, alpha_s)
self.dmp_y = DMP(n_basis, alpha_v, beta_v, alpha_s)
def learn_from_demo(self, xy_demo, t_demo):
dt = t_demo[1] - t_demo[0]
dx = np.gradient(xy_demo[:, 0], dt)
ddx = np.gradient(dx, dt)
dy = np.gradient(xy_demo[:, 1], dt)
ddy = np.gradient(dy, dt)
self.dmp_x.learn_from_demo(xy_demo[:, 0], dx, ddx, t_demo)
self.dmp_y.learn_from_demo(xy_demo[:, 1], dy, ddy, t_demo)
def generate(self, start, goal, tau, dt=0.001, obstacles=None, k_rep=50.0, d0=1.5):
n_steps = int(tau / dt)
# 障害物回避の結合項を定義
def coupling_x(y_val, t, y_other):
if obstacles is None:
return 0.0
pos = np.array([y_val, y_other])
f = 0.0
for obs in obstacles:
d = np.linalg.norm(pos - obs)
if d < d0 and d > 1e-6:
f += k_rep * (1.0/d - 1.0/d0) / d**2 * (y_val - obs[0]) / d
return f
def coupling_y(y_val, t, y_other):
if obstacles is None:
return 0.0
pos = np.array([y_other, y_val])
f = 0.0
for obs in obstacles:
d = np.linalg.norm(pos - obs)
if d < d0 and d > 1e-6:
f += k_rep * (1.0/d - 1.0/d0) / d**2 * (y_val - obs[1]) / d
return f
# 両軸を同時に更新
x = np.zeros(n_steps)
y = np.zeros(n_steps)
t = np.zeros(n_steps)
x[0] = start[0]
y[0] = start[1]
vx, vy = 0.0, 0.0
self.dmp_x.y0 = start[0]
self.dmp_x.g = goal[0]
self.dmp_y.y0 = start[1]
self.dmp_y.g = goal[1]
for i in range(1, n_steps):
t[i] = i * dt
s = self.dmp_x.canonical_system(t[i])
fx = self.dmp_x.forcing_term(s)
fy = self.dmp_y.forcing_term(s)
cx = coupling_x(x[i-1], t[i], y[i-1])
cy = coupling_y(y[i-1], t[i], x[i-1])
dvx = (self.dmp_x.alpha_v * (self.dmp_x.beta_v * (goal[0] - x[i-1]) - vx)
+ fx + cx) / tau
dvy = (self.dmp_y.alpha_v * (self.dmp_y.beta_v * (goal[1] - y[i-1]) - vy)
+ fy + cy) / tau
vx += dvx * dt
vy += dvy * dt
x[i] = x[i-1] + vx / tau * dt
y[i] = y[i-1] + vy / tau * dt
return t, np.column_stack([x, y])
# デモ軌道: 直線（始点→終点）
tau_2d = 2.0
dt_2d = 0.001
t_2d = np.arange(0, tau_2d, dt_2d)
xy_demo = np.column_stack([
t_2d / tau_2d * 10.0, # x: 0 -> 10
5.0 + 0.0 * t_2d # y: 一定（直線）
])
dmp2d = DMP2D(n_basis=25)
dmp2d.learn_from_demo(xy_demo, t_2d)
# 障害物なしの再生
_, traj_no_obs = dmp2d.generate(
start=np.array([0.0, 5.0]),
goal=np.array([10.0, 5.0]),
tau=tau_2d, dt=dt_2d
)
# 障害物ありの再生
obs_dmp = np.array([[5.0, 5.0], [7.0, 5.5]])
_, traj_with_obs = dmp2d.generate(
start=np.array([0.0, 5.0]),
goal=np.array([10.0, 5.0]),
tau=tau_2d, dt=dt_2d,
obstacles=obs_dmp, k_rep=80.0, d0=2.0
)
# 目標位置を変更しつつ障害物回避
_, traj_new_goal = dmp2d.generate(
start=np.array([0.0, 5.0]),
goal=np.array([10.0, 8.0]),
tau=tau_2d, dt=dt_2d,
obstacles=obs_dmp, k_rep=80.0, d0=2.0
)
fig, ax = plt.subplots(1, 1, figsize=(12, 6))
ax.plot(traj_no_obs[:, 0], traj_no_obs[:, 1], 'b-', linewidth=2,
label='No obstacles (g=[10,5])')
ax.plot(traj_with_obs[:, 0], traj_with_obs[:, 1], 'r-', linewidth=2,
label='With obstacles (g=[10,5])')
ax.plot(traj_new_goal[:, 0], traj_new_goal[:, 1], 'g-', linewidth=2,
label='With obstacles (g=[10,8])')
# 障害物の描画
for obs in obs_dmp:
circle = plt.Circle(obs, 0.4, color='gray', alpha=0.7, zorder=5)
circle_d0 = plt.Circle(obs, 2.0, color='gray', fill=False,
linestyle='--', alpha=0.3)
ax.add_patch(circle)
ax.add_patch(circle_d0)
ax.plot(0, 5, 'ko', markersize=10, label='Start')
ax.plot(10, 5, 'b*', markersize=12)
ax.plot(10, 8, 'g*', markersize=12)
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_title('DMP with Obstacle Avoidance and Goal Modification')
ax.legend(loc='upper left')
ax.set_xlim(-1, 12)
ax.set_ylim(2, 10)
ax.set_aspect('equal')
ax.grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('dmp_2d_obstacle.png', dpi=150, bbox_inches='tight')
plt.show()
この結果から、DMPの柔軟性が明確にわかります。
- 青線（障害物なし）: デモ軌道の直線を忠実に再現しています。始点から終点へ滑らかに収束する挙動は、臨界減衰バネ系のベースダイナミクスによるものです。
- 赤線（障害物あり、元の目標）: 障害物の近くで軌道が上方または下方に迂回しています。この変形はポテンシャルフィールドの斥力を結合項 $C$ として加えることで実現されています。注目すべきは、障害物から離れた後に軌道が元の目標方向に自然に戻る点です。これはDMPのベースダイナミクスの安定性（目標への収束性）の恩恵です。
- 緑線（障害物あり、新目標）: 目標位置を $[10, 8]$ に変更すると、全体の軌道が上方にシフトしています。障害物回避と目標変更の両方が同時に機能しており、DMPの「重ね合わせ」の性質が見て取れます。
宇宙ロボティクスへの応用と実例
宇宙環境特有の課題
ここまで一般的な枠組みで解説してきたオンライン軌道再計画ですが、宇宙環境に適用する際には特有の課題を考慮する必要があります。
通信遅延: 静止軌道の衛星と地上局の間には約0.25秒の通信遅延があり、火星探査では片道3〜22分にもなります。地上からのリアルタイム操縦は不可能であるため、搭載コンピュータ上でのオンライン再計画が不可欠です。
計算資源の制限: 宇宙用コンピュータは放射線耐性（耐放射線設計）が要求されるため、地上の計算機に比べて処理能力が大幅に劣ります。例えば、ISS上のロボットアームの制御コンピュータは地上のPC数世代前の性能に相当します。このため、計算量の少ないポテンシャルフィールド法やDMPの実用的価値は高くなります。
フリーフライング系の反力: 宇宙空間では、ロボットアームの動きに対して母船が反力で回転します（運動量保存則）。このため、関節角度の変化と手先位置の関係が非線形に変わり、地上の固定ベースロボットとは異なる動力学モデルが必要です。MPCで正確な予測を行うには、このフリーフライングダイナミクスを含むモデルが必要です。
回転デブリ捕獲のシナリオ
オンライン軌道再計画の典型的な応用シナリオとして、回転デブリの捕獲があります。
- アプローチフェーズ: MPCまたはオフライン計画で大まかな接近軌道を計画する。デブリの回転状態をビジョンセンサで観測し、回転軸と角速度を推定する
- 待機フェーズ: デブリの把持可能点が手の届く範囲に来るタイミングを予測する。DMPで学習済みの把持動作パターンを準備しておく
- 把持フェーズ: 把持点が近づいたら、DMPの目標位置をリアルタイムに更新しながら手先を把持点に合わせる。予測とずれた場合は即座に軌道を修正する
- 退避フェーズ: 把持に失敗した場合、ポテンシャルフィールド法の斥力でデブリから高速に退避する
各フェーズで異なる手法が適しているのは、必要な精度・速度・安全性のバランスがフェーズごとに異なるためです。このような使い分けが、実際の宇宙ミッションにおけるオンライン軌道再計画の実態です。
ESA e.Deorbit / ClearSpace-1
ESA（欧州宇宙機関）が推進するClearSpace-1ミッションは、軌道上のデブリを自律的に捕獲・除去する世界初の本格的ミッションです。このミッションでは、ターゲットデブリの姿勢推定とリアルタイムのアプローチ軌道修正が核心技術であり、本記事で扱ったオンライン軌道再計画がまさに活用される場面です。
搭載GNC（Guidance, Navigation and Control）システムは、ビジュアルセンサからの情報をもとにMPCベースの接近軌道を更新し、最終接近フェーズではより応答性の高い反応的手法で安全な捕獲を実現する設計が報告されています。
宇宙環境特有の課題と応用例を見てきました。最後に、本記事で学んだ内容を整理しましょう。
まとめ
本記事では、Ch.3「衝突回避と経路計画」の締めくくりとして、動的環境に対応するためのオンライン軌道生成とリアルタイム再計画の手法を解説しました。
- オフライン計画との違い: オフライン計画は「事前に完璧な経路を見つける」アプローチであるのに対し、オンライン計画は「実行しながら修正する」アプローチです。動的環境やモデルの不確かさが避けられない宇宙ロボティクスでは、オンライン再計画が本質的に重要です
- ポテンシャルフィールド法: 引力と斥力の合成場を用いた最も高速な手法です。制御周期ごとの計算は $O(N_{\text{obs}})$ と軽量ですが、局所最適に陥る弱点があり、壁面追従やナビゲーション関数で対策します
- DMP（Dynamic Movement Primitives）: お手本の動作パターンを学習し、目標位置の変更や障害物回避の力項を加えることでリアルタイムに軌道を変形できます。学習が閉形式で高速に行え、実行時の計算負荷も小さい実用的な手法です
- MPC（Model Predictive Control）: 予測ホライズン内の最適化を繰り返し解く、最も体系的な手法です。制約条件（トルクリミット、障害物回避）を明示的に扱える強みがありますが、計算コストが高く、リアルタイム適用には工夫が必要です
- 階層的アーキテクチャ: 実際のシステムでは、ミッション計画（オフライン）、軌道生成（DMP/MPC）、反応的回避（ポテンシャルフィールド）を階層的に組み合わせて運用します
3手法はそれぞれ異なるトレードオフ — 計算速度、解の最適性、制約の扱いやすさ — を持っており、「どれが最良か」ではなく「どの場面で何を使うか」を判断する力が重要です。
次の Ch.4 では、宇宙マニピュレータに特有の力学 — 微小重力下でのフリーフライングダイナミクス — を取り上げます。ロボットアームを動かすと母船が回転してしまうこの独特な状況を、ラグランジュ力学に基づいて定式化します。
次のステップとして、以下の記事も参考にしてください。
- 衝突回避の経路計画（RRT/PRM） — オフラインの経路計画手法（Ch.3前半）
- 計算トルク法による軌道追従制御 — 生成した軌道を追従するための制御手法
- フリーフライングロボットの力学 — Ch.4 宇宙マニピュレータの力学（次の記事）