宇宙空間でロボットアームがデブリを捕獲する場面を想像してください。回転するデブリは待ってくれません。キャプチャウィンドウは数秒しかなく、ロボットアームは指定された経路上をできるだけ速く動かす必要があります。しかし、関節モータのトルクには上限があり、速度にも制限があります。「この経路を、トルク制約を守りながら最短時間で移動するにはどう速度を割り振ればよいか？」 — これが時間最適軌道計画（Time-Optimal Trajectory Planning）の問題です。
この問題は宇宙ロボティクスだけでなく、非常に広い応用を持ちます。
- 宇宙デブリ捕獲: 回転するデブリの同期ポイントに限られた時間窓でアプローチする必要があり、ロボットアームの動作時間の最小化が直接ミッション成功率に影響します
- 産業用ロボットの生産性向上: 自動車工場の溶接ロボットは経路が決まっており、その経路上での移動時間を最小化することでサイクルタイムを短縮し、生産性を向上させます
- 衛星アンテナの高速指向変更: 通信衛星が異なるターゲットにアンテナを向け直すとき、切り替え時間を最短にすることでデータスループットが向上します
本記事の内容
- 時間最適軌道計画の考え方と経路パラメトリゼーション $s(t)$ の導入
- 位相平面法 $(s, \dot{s})$ による幾何学的な解析
- 加速度制約と切替曲線（switching curve）の構造
- Bobrow-Dubowsky-Gibson のアルゴリズム
- TOPP（Time-Optimal Path Parameterization）— 凸最適化ベースの現代的手法
- Pythonによる位相平面の可視化と最適速度プロファイルの計算
前提知識
この記事を読む前に、以下の記事を読んでおくと理解が深まります。
軌道計画と経路計画の違い — なぜ「時間」を最適化するのか
ロボットの動作計画では、「経路」と「軌道」という2つの言葉が似て非なるものとして使い分けられています。まずこの違いを明確にしておきましょう。
経路（path） とは、ロボットが通る幾何学的な道筋のことです。関節空間で言えば、関節角度ベクトル $\bm{q}$ が描く曲線そのものです。時間の情報は含まれません。「どこを通るか」だけを規定します。
軌道（trajectory） とは、経路に時間情報を付加したものです。つまり、「いつ、どこにいるか」を規定します。同じ経路でも、ゆっくり通過する軌道と高速で通過する軌道は全く異なります。
多くの実用場面では、経路はすでに決まっています。たとえば、障害物回避アルゴリズムで安全な経路が生成された後、「その経路をどれだけ速く辿れるか」を決めるのが時間最適軌道計画です。経路を変えずに速度プロファイルだけを最適化するため、安全性を損なうことなく動作時間を最短にできるのが大きな利点です。
では、この「経路上の速度割り振り」をどのように数学的に定式化すればよいでしょうか？ここで登場するのが経路パラメトリゼーションです。
経路パラメトリゼーション $s(t)$ — 問題を1次元に落とす
直感的な理解
高速道路を走る車を想像してください。道路（経路）はすでに決まっています。車のハンドル操作は不要で、あなたが決めるのはアクセルとブレーキ、つまり「道路上のどの地点にいつ到達するか」だけです。
これをロボットに置き換えると、$n$ 自由度のロボットの関節角度 $\bm{q} \in \mathbb{R}^n$ は $n$ 次元空間の点ですが、経路が決まっていれば自由度は1つしかありません。「経路上のどこにいるか」を表すスカラーパラメータ $s$ だけで全てが決まるのです。
数学的定式化
与えられた経路を、スカラーパラメータ $s \in [0, 1]$ で表します。
$$ \bm{q}(s) : [0, 1] \to \mathbb{R}^n $$
ここで $s = 0$ が経路の始点、$s = 1$ が経路の終点に対応します。時間 $t$ と $s$ の関係を $s(t)$ とすると、$s(t)$ は時間の単調増加関数です（経路上を後戻りしないと仮定）。
関節速度と関節加速度は連鎖律を用いて次のように表されます。まず関節速度について、$\bm{q}(t) = \bm{q}(s(t))$ を時間で微分すると
$$ \dot{\bm{q}} = \frac{d\bm{q}}{dt} = \frac{d\bm{q}}{ds} \cdot \frac{ds}{dt} = \bm{q}'(s) \, \dot{s} $$
ここで $\bm{q}'(s) = d\bm{q}/ds$ は経路の幾何学的な形状（接線方向）を表し、$\dot{s} = ds/dt$ は経路上の「速さ」を表します。
さらに関節加速度を求めるため、もう一度時間で微分します。積の微分法則を適用すると
$$ \ddot{\bm{q}} = \frac{d}{dt}[\bm{q}'(s) \, \dot{s}] = \bm{q}”(s) \, \dot{s}^2 + \bm{q}'(s) \, \ddot{s} $$
ここで $\bm{q}”(s) = d^2\bm{q}/ds^2$ は経路の曲率に関する量、$\ddot{s} = d^2s/dt^2$ は経路上の「加速度」です。
この変換の威力は、$n$ 次元の関節空間の運動が、スカラー量 $s(t)$ という1次元の問題に帰着されることにあります。制御変数は $\ddot{s}$（経路上の加速度）のみとなり、問題が劇的に簡単になります。
動力学方程式の変換
ロボットマニピュレータの運動方程式は、ラグランジュ法から次のように書けます。
$$ \bm{M}(\bm{q})\ddot{\bm{q}} + \bm{C}(\bm{q}, \dot{\bm{q}})\dot{\bm{q}} + \bm{g}(\bm{q}) = \bm{\tau} $$
ここで $\bm{M}(\bm{q})$ は慣性行列、$\bm{C}(\bm{q}, \dot{\bm{q}})$ はコリオリ・遠心力行列、$\bm{g}(\bm{q})$ は重力項、$\bm{\tau}$ は関節トルクベクトルです。
先ほどの $\dot{\bm{q}}$ と $\ddot{\bm{q}}$ の表現を代入して整理します。まず $\dot{\bm{q}} = \bm{q}’ \dot{s}$ と $\ddot{\bm{q}} = \bm{q}” \dot{s}^2 + \bm{q}’ \ddot{s}$ を運動方程式に代入すると
$$ \bm{M}(\bm{q}(s))[\bm{q}”(s) \dot{s}^2 + \bm{q}'(s) \ddot{s}] + \bm{C}(\bm{q}(s), \bm{q}'(s)\dot{s})[\bm{q}'(s)\dot{s}] + \bm{g}(\bm{q}(s)) = \bm{\tau} $$
コリオリ・遠心力項 $\bm{C}(\bm{q}, \dot{\bm{q}})\dot{\bm{q}}$ は $\dot{\bm{q}}$ に対して二次形式であることに注意すると、$\dot{s}^2$ でまとめることができます。したがって、$\ddot{s}$ の項と $\dot{s}^2$ の項を分離して整理すると
$$ \bm{m}(s) \ddot{s} + \bm{c}(s) \dot{s}^2 + \bm{g}_s(s) = \bm{\tau} $$
ここで各係数ベクトルを次のように定義しました。
$$ \bm{m}(s) = \bm{M}(\bm{q}(s)) \bm{q}'(s) $$
$$ \bm{c}(s) = \bm{M}(\bm{q}(s)) \bm{q}”(s) + \bm{C}(\bm{q}(s), \bm{q}'(s)) \bm{q}'(s) $$
$$ \bm{g}_s(s) = \bm{g}(\bm{q}(s)) $$
$\bm{m}(s)$、$\bm{c}(s)$、$\bm{g}_s(s)$ はいずれも $s$ のみの関数であり、経路の幾何学的な形状から計算できます。つまり、動力学方程式が $s$、$\dot{s}$、$\ddot{s}$ だけで記述されるようになりました。
時間最適化問題の定式化
以上の準備により、時間最適軌道計画は次のように定式化されます。
$$ \min_{s(t)} \quad T = \int_0^T dt $$
制約条件は、まず経路の始点と終点の条件として
$$ s(0) = 0, \quad s(T) = 1, \quad \dot{s}(0) = 0, \quad \dot{s}(T) = 0 $$
次に、各関節のトルク制約として
$$ \tau_i^{\min} \leq m_i(s)\ddot{s} + c_i(s)\dot{s}^2 + g_{s,i}(s) \leq \tau_i^{\max}, \quad i = 1, \dots, n $$
そして、経路上を前進するための条件として
$$ \dot{s}(t) \geq 0, \quad \forall t \in [0, T] $$
この問題の構造をよく見てみましょう。目的関数は終端時間 $T$ の最小化、制約は $\ddot{s}$ と $\dot{s}^2$ に関する線形不等式です。制御入力は $\ddot{s}$ のみです。この1次元の最適制御問題を効率的に解く方法が、次に紹介する位相平面法です。
位相平面法 — $(s, \dot{s})$ 平面での幾何学的解析
位相平面の直感
物理で振り子の運動を $(x, \dot{x})$ 平面にプロットすると、エネルギーの等高線として楕円や分離線が現れます。時間最適軌道の問題でも同じアイデアが使えます。
$(s, \dot{s})$ 平面 — 横軸に経路パラメータ $s$、縦軸にその時間微分 $\dot{s}$ をとった2次元平面 — を考えます。この平面上の1点は「経路上のどこにいて（$s$）、どれだけ速く進んでいるか（$\dot{s}$）」を完全に指定します。軌道は $\dot{s} \geq 0$（前進条件）の上半平面にのみ存在します。
時間の経過とともに、状態点はこの平面上を右方向（$s$ が増加する方向）に移動します。移動の「速さ」は $\dot{s}$ の値そのもので決まり、$\dot{s}$ が大きいほど $s$ が速く進みます。移動にかかる全時間は
$$ T = \int_0^1 \frac{ds}{\dot{s}} $$
と表されます。この式は $ds = \dot{s} \, dt$ を変形して得られるものです。この被積分関数 $1/\dot{s}$ から明らかなように、$\dot{s}$ がどこでも大きいほど全体の所要時間 $T$ は短くなります。つまり、時間最適軌道とは $(s, \dot{s})$ 平面上で $\dot{s}$ をできるだけ大きく保つ曲線です。
最大速度曲線（MVC: Maximum Velocity Curve）
トルク制約を満たしながら $\dot{s}$ をどこまで大きくできるかには限界があります。各 $s$ の値に対して、そのトルク制約を満たす $\dot{s}$ の最大値 $\dot{s}_{\max}(s)$ が存在します。この上限を集めた曲線を最大速度曲線（Maximum Velocity Curve, MVC）と呼びます。
MVC は次のように求められます。動力学方程式の各関節について
$$ \tau_i^{\min} \leq m_i(s) \ddot{s} + c_i(s) \dot{s}^2 + g_{s,i}(s) \leq \tau_i^{\max} $$
この不等式が $\ddot{s}$ について解を持つ（つまり、何らかの加速度 $\ddot{s}$ を選べばトルク制約を満たせる）ための条件を求めます。$\ddot{s}$ について整理すると
$$ \frac{\tau_i^{\min} – c_i(s)\dot{s}^2 – g_{s,i}(s)}{m_i(s)} \leq \ddot{s} \leq \frac{\tau_i^{\max} – c_i(s)\dot{s}^2 – g_{s,i}(s)}{m_i(s)} $$
ただし $m_i(s) > 0$ の場合であり、$m_i(s) < 0$ の場合は不等号が逆転します。全関節にわたる制約の交差（共通部分）が空でない条件が MVC を定めます。
各 $s$ における $\ddot{s}$ の許容範囲を $[\ddot{s}_{\min}(s, \dot{s}), \, \ddot{s}_{\max}(s, \dot{s})]$ と書くと、MVC は
$$ \dot{s}_{\text{MVC}}(s) = \sup\{\dot{s} \geq 0 : \ddot{s}_{\min}(s, \dot{s}) \leq \ddot{s}_{\max}(s, \dot{s})\} $$
で定義されます。言い換えれば、MVC は「少なくとも1つの許容加速度 $\ddot{s}$ が存在する」ような $\dot{s}$ の最大値です。
MVC を超える領域は、どのようなトルクを選んでも制約を満たせない「禁止領域」です。したがって、最適軌道は必ずこの曲線の下側を通らなければなりません。
加速度の上下限
位相平面上の各点 $(s, \dot{s})$ において、トルク制約を満たす $\ddot{s}$ の範囲が決まります。全関節のトルク制約を同時に満たす $\ddot{s}$ の上限と下限は
$$ \ddot{s}_{\max}(s, \dot{s}) = \min_{i=1,\dots,n} \begin{cases} \frac{\tau_i^{\max} – c_i(s)\dot{s}^2 – g_{s,i}(s)}{m_i(s)} & \text{if } m_i(s) > 0 \\ \frac{\tau_i^{\min} – c_i(s)\dot{s}^2 – g_{s,i}(s)}{m_i(s)} & \text{if } m_i(s) < 0 \end{cases} $$
$$ \ddot{s}_{\min}(s, \dot{s}) = \max_{i=1,\dots,n} \begin{cases} \frac{\tau_i^{\min} – c_i(s)\dot{s}^2 – g_{s,i}(s)}{m_i(s)} & \text{if } m_i(s) > 0 \\ \frac{\tau_i^{\max} – c_i(s)\dot{s}^2 – g_{s,i}(s)}{m_i(s)} & \text{if } m_i(s) < 0 \end{cases} $$
直感的に言えば、$\ddot{s}_{\max}$ はその瞬間に可能な最大加速、$\ddot{s}_{\min}$ は最大減速（最も強くブレーキをかけたときの減速度）です。
この加速度の上下限は $\dot{s}^2$ に依存するため、$\dot{s}$ が大きくなるほど利用可能な加速度範囲が狭くなります。MVCはまさに $\ddot{s}_{\max} = \ddot{s}_{\min}$ となる点の集合、つまり加速度の選択の余地がゼロになる境界です。
ここまでで、位相平面上のどこでどれだけ加速・減速できるかが分かりました。次に、この情報を使って実際に最適軌道を構成する方法を見ていきましょう。
切替曲線と最適軌道の構造
バンバン制御の直感
最短時間で目的地に到着するには、どうすればよいでしょうか？日常的な感覚でも、「限界まで加速し、ギリギリのタイミングで限界まで減速する」のが最速であることは分かります。信号が赤から青に変わったとき、最速で交差点を渡るにはフルアクセル→フルブレーキです。
これは最適制御理論の一般的な結果と一致します。制御入力に上下限がある線形システムの時間最適制御は、制御入力が常に上限か下限をとるバンバン制御（bang-bang control）になることがポントリャーギンの最大原理から示されます。
時間最適軌道計画でも同様の構造が成り立ちます。最適軌道では、$\ddot{s}$ は常に $\ddot{s}_{\max}$（最大加速）か $\ddot{s}_{\min}$（最大減速）のどちらかをとります。つまり、最適軌道は次の2種類のフェーズの交代で構成されます。
- 最大加速フェーズ: $\ddot{s} = \ddot{s}_{\max}(s, \dot{s})$ — アクセル全開に相当
- 最大減速フェーズ: $\ddot{s} = \ddot{s}_{\min}(s, \dot{s})$ — ブレーキ全開に相当
位相平面上の軌道の構成
位相平面 $(s, \dot{s})$ 上で、最適軌道は次のように構成されます。
始点から最大加速: $(s, \dot{s}) = (0, 0)$ から出発し、$\ddot{s} = \ddot{s}_{\max}$ で加速します。位相平面上で右上に向かう曲線を描きます。
最大加速→最大減速の切り替え: ある点で加速から減速に切り替えます。この切り替え点を通る曲線が切替曲線（switching curve）です。
終点に向かって最大減速: 切替後は $\ddot{s} = \ddot{s}_{\min}$ で減速し、$(s, \dot{s}) = (1, 0)$ に到達します。
単純な場合（後述の2リンクアームなど）では、加速→減速の1回の切り替えで最適軌道が得られます。しかし、一般の多自由度マニピュレータでは、MVC の形状が複雑になり、複数の切替点が現れることがあります。
切替曲線の詳細
切替曲線は、最適軌道上で最大加速から最大減速に（またはその逆に）切り替わる点の軌跡です。切替曲線が現れるのは、典型的には以下の状況です。
ケース1: MVC に接する場合 — 最大加速曲線が MVC に達すると、それ以上加速できません。この近傍で最適軌道は MVC に沿って走り、その後減速に切り替わります。MVC 上では $\ddot{s}_{\max} = \ddot{s}_{\min}$ ですから、「加速でも減速でもない」唯一の加速度が決まります。
ケース2: 動力学的特異点 — $m_i(s) = 0$ となる $s$ の値では、動力学方程式の構造が変化し、$\dot{s}$ の許容最大値が局所的に制限されます。このような特異点の近傍で切替が生じます。
ケース3: 異なる関節の制約が支配的になる切り替わり — ある区間では関節1のトルク制約が律速し、次の区間では関節2のトルク制約が律速するような場合、制約の「支配権」の切り替わり点で切替曲線が現れます。
位相平面上での最適軌道の構造が分かったところで、この軌道を具体的に計算するアルゴリズムを見ていきましょう。まずは歴史的に最初に提案された Bobrow-Dubowsky-Gibson のアルゴリズムです。
Bobrow-Dubowsky-Gibson のアルゴリズム
歴史的背景
1985年、Bobrow, Dubowsky, Gibson は（Shin, McKay と独立に）、経路パラメトリゼーションと位相平面法を用いた時間最適軌道計画のアルゴリズムを発表しました。この研究は、それまで困難とされていた多自由度マニピュレータの時間最適化に対して初めて体系的な数値解法を与えたものであり、現在でもこの分野の基礎となっています。
アルゴリズムの概要
Bobrow-Dubowsky-Gibson のアルゴリズムは、次の4つのステップで構成されます。
ステップ1: 最大速度曲線（MVC）の計算
経路 $s \in [0, 1]$ を細かく離散化し、各 $s$ における $\dot{s}_{\text{MVC}}(s)$ を求めます。前述のとおり、各 $s$ で $\ddot{s}_{\max}(s, \dot{s}) \geq \ddot{s}_{\min}(s, \dot{s})$ を満たす最大の $\dot{s}$ を数値的に探索します。
ステップ2: 前方積分（最大加速）
始点 $(s, \dot{s}) = (0, 0)$ から、$\ddot{s} = \ddot{s}_{\max}(s, \dot{s})$ として位相平面上の微分方程式
$$ \frac{d\dot{s}}{ds} = \frac{\ddot{s}}{\dot{s}} $$
を数値積分します。この式は $\ddot{s} = \dot{s} \cdot d\dot{s}/ds$ の関係（連鎖律 $\ddot{s} = \dot{s} \, d\dot{s}/ds$）から得られます。積分は $\dot{s}$ が MVC に達するか、終点 $s = 1$ に至るまで続けます。
ここで $\dot{s} = 0$ のとき $d\dot{s}/ds$ が定義されない点に注意が必要です。始点 $\dot{s} = 0$ の近傍ではロピタルの定理を適用して
$$ \left.\frac{d\dot{s}}{ds}\right|_{\dot{s}=0} = \sqrt{\ddot{s}_{\max}(s, 0)} $$
として初期化します。これは $\dot{s} \, d\dot{s} = \ddot{s} \, ds$ の両辺を積分して $\dot{s}^2 / 2 = \ddot{s} \cdot \Delta s$（微小区間で $\ddot{s}$ 一定と近似）から $\dot{s} \approx \sqrt{2 \ddot{s} \, \Delta s}$ を得ることで理解できます。
ステップ3: 後方積分（最大減速）
終点 $(s, \dot{s}) = (1, 0)$ から時間を逆転して、$\ddot{s} = \ddot{s}_{\min}(s, \dot{s})$ で積分します。時間を逆転するとは、$s$ を減少させる方向に積分することであり、これは終点から最大減速で到達可能な曲線を逆にたどることに相当します。
具体的には、$s$ を 1 から 0 に向かって減少させながら
$$ \frac{d\dot{s}}{ds} = \frac{\ddot{s}_{\min}(s, \dot{s})}{\dot{s}} $$
を積分します。この後方積分曲線は「終点に停止するために、各 $s$ で最大限の速度で通過できるギリギリの曲線」を表します。
ステップ4: 切替点の決定と軌道の合成
前方積分曲線と後方積分曲線の交点が切替点です。最適軌道は次のように合成されます。
- 始点から切替点まで: 前方積分曲線（最大加速）に沿う
- 切替点から終点まで: 後方積分曲線（最大減速）に沿う
一般の場合、MVC 上に特異点があるとき、単純な前方・後方積分では最適軌道が構成できません。MVC 上の特異点を検出し、その前後で追加の前方・後方積分を実行する必要があります。このとき切替点は複数個になります。
アルゴリズムの計算量
各ステップの計算量を見てみましょう。経路を $N$ 点に離散化した場合
- ステップ1（MVC 計算）: 各点で二分探索を行うと $O(N \log M)$（$M$ は $\dot{s}$ の探索精度）
- ステップ2, 3（前方・後方積分）: $O(N)$ の数値積分
- ステップ4（交点探索）: $O(N)$ の走査
全体で $O(N \log M)$ であり、非常に高速です。この効率性は、問題を1次元に帰着させた経路パラメトリゼーションの恩恵です。
Bobrow-Dubowsky-Gibson のアルゴリズムは直感的で実装もしやすいですが、MVC 上の特異点の処理がやや複雑です。また、速度制約など追加の制約を扱う拡張が難しい面があります。これらの課題を凸最適化の枠組みで解決したのが、次に紹介する TOPP です。
TOPP — 凸最適化ベースの現代的手法
従来手法の課題
Bobrow-Dubowsky-Gibson のアルゴリズムは強力ですが、いくつかの実用上の課題があります。
- 特異点の処理: MVC 上の特異点を正確に検出し、適切に処理する必要があり、実装の堅牢性に影響する
- 制約の追加が困難: トルク制約に加えて、速度制約、加速度制約、ジャーク制約などを同時に扱いたい場合、位相平面法のフレームワークでは追加の工夫が必要
- 数値安定性: 特異点近傍での数値積分が不安定になりやすい
これらの課題に対して、Verscheure ら（2009年）は時間最適軌道計画を凸最適化問題として再定式化する手法を提案しました。これが TOPP（Time-Optimal Path Parameterization）の凸最適化アプローチです。
変数変換による凸化
鍵となるアイデアは、変数変換 $b(s) = \dot{s}^2$ です。$\dot{s} \geq 0$ であるため、$b(s) \geq 0$ かつ $\dot{s} = \sqrt{b(s)}$ です。
この変換の動機を説明します。元の問題で $\dot{s}^2$ は至るところに現れます（動力学方程式の $c_i(s)\dot{s}^2$ の項など）。さらに $\ddot{s}$ と $b(s)$ の関係を見ると
$$ \ddot{s} = \frac{d\dot{s}}{dt} = \frac{d}{dt}\sqrt{b(s)} = \frac{1}{2\sqrt{b}} \cdot \frac{db}{ds} \cdot \dot{s} = \frac{1}{2\sqrt{b}} \cdot \frac{db}{ds} \cdot \sqrt{b} = \frac{1}{2}\frac{db}{ds} $$
つまり $a(s) = \ddot{s}$ とすると
$$ a(s) = \frac{1}{2}\frac{db(s)}{ds} = \frac{1}{2}b'(s) $$
これは驚くべき結果です。元の問題における非線形量 $\ddot{s}$ が、新しい変数 $b(s)$ の微分 $b'(s)/2$ という線形な量に変換されたのです。
凸最適化問題への再定式化
目的関数を新しい変数で書き直しましょう。移動時間は
$$ T = \int_0^1 \frac{ds}{\dot{s}} = \int_0^1 \frac{ds}{\sqrt{b(s)}} $$
です。$1/\sqrt{b}$ は $b > 0$ で凸関数ですから、この目的関数は凸です。
動力学の制約を新しい変数で書き直すと、$\ddot{s} = b'(s)/2$、$\dot{s}^2 = b(s)$ を代入して
$$ \tau_i^{\min} \leq m_i(s) \cdot \frac{b'(s)}{2} + c_i(s) \cdot b(s) + g_{s,i}(s) \leq \tau_i^{\max} $$
左辺と右辺を見ると、$b'(s)$ と $b(s)$ に対して線形（アフィン）です。つまり制約は凸不等式（アフィン不等式）です。
非負条件 $b(s) \geq 0$ と境界条件 $b(0) = 0, \, b(1) = 0$ も線形です。
まとめると、TOPP の凸最適化定式化は次のようになります。
$$ \min_{b(s)} \quad \int_0^1 \frac{ds}{\sqrt{b(s)}} $$
制約条件として
$$ \tau_i^{\min} \leq \frac{1}{2} m_i(s) b'(s) + c_i(s) b(s) + g_{s,i}(s) \leq \tau_i^{\max}, \quad i = 1, \dots, n $$
$$ b(s) \geq 0, \quad \forall s \in [0, 1] $$
$$ b(0) = 0, \quad b(1) = 0 $$
この問題は凸最適化問題であり、局所最適解がそのまま大域最適解となります。したがって、内点法などの標準的な凸最適化ソルバーで効率的に解くことができます。
離散化と数値解法
実際の計算では、経路を $N$ 個の区間に離散化します。$s_k = k/N$（$k = 0, 1, \dots, N$）として、$b_k = b(s_k)$ を決定変数とします。
微分 $b'(s)$ を差分で近似すると
$$ b'(s_k) \approx \frac{b_{k+1} – b_k}{\Delta s}, \quad \Delta s = \frac{1}{N} $$
目的関数は台形公式などで離散化すると
$$ T \approx \sum_{k=0}^{N-1} \frac{2\Delta s}{\sqrt{b_k} + \sqrt{b_{k+1}}} $$
ただし、$b_0 = b_N = 0$ のため分母がゼロになる問題を回避する工夫が必要です。実用的には、$b_0$ と $b_N$ をゼロに厳密に固定するのではなく、微小な正の値 $\epsilon$ とするか、端点近傍を別途処理します。
離散化された問題は二次錐計画（SOCP）に帰着でき、ECOS や MOSEK などの高速ソルバーで解けます。$N = 100$ 程度であれば数ミリ秒で解が得られるため、リアルタイム制御への応用も可能です。
TOPP の利点
凸最適化アプローチの主な利点を整理します。
- 大域最適性の保証: 凸問題であるため、得られる解は確実に大域的最適解です。位相平面法で特異点処理を誤った場合に得られる準最適解のリスクがありません
- 制約の柔軟な追加: 速度制約 $|\dot{q}_i| \leq v_i^{\max}$ は $|q’_i(s)| \sqrt{b(s)} \leq v_i^{\max}$ つまり $b(s) \leq (v_i^{\max}/|q’_i(s)|)^2$ として $b$ の上限制約になり、自然に組み込めます
- 数値安定性: 特異点の明示的な検出が不要で、ソルバーの内部処理に任せられます
- 実装の簡潔さ: cvxpy などの凸最適化ライブラリを用いれば、問題定式化をそのままコードに落とし込めます
TOPP のアルゴリズムはその後も発展を続けており、Pham, Hung（2014年）による TOPP-RA（Time-Optimal Path Parameterization via Reachability Analysis）はさらに高速な解法を提供しています。TOPP-RA は可達集合（Reachability Set）の伝播という視点から、$O(N)$ の計算量で最適解を求めます。
ここまでの理論を踏まえて、Pythonでの具体的な実装に進みましょう。まずは2リンクアームの例で位相平面を可視化し、最適軌道がどのように構成されるかを直感的に理解します。
2リンクアームの動力学モデル
モデル設定
具体的な数値例として、平面2リンクアームを考えます。宇宙用途を想定し、重力はゼロ（$\bm{g} = \bm{0}$）とします。この仮定は宇宙空間での作業を模擬するものであり、問題を簡潔にしつつ本質を失いません。
リンクパラメータは以下のとおりです。
| パラメータ | リンク1 | リンク2 |
|---|---|---|
| リンク長 $l_i$ [m] | 1.0 | 1.0 |
| リンク質量 $m_i$ [kg] | 2.0 | 1.0 |
| 重心までの距離 $l_{c,i}$ [m] | 0.5 | 0.5 |
| 慣性モーメント $I_i$ [kg m²] | 0.167 | 0.083 |
トルク制約は両関節とも $|\tau_i| \leq 10$ Nm とします。
動力学方程式
2リンクアームの慣性行列 $\bm{M}(\bm{q})$ は
$$ \bm{M}(\bm{q}) = \begin{pmatrix} M_{11} & M_{12} \\ M_{21} & M_{22} \end{pmatrix} $$
各成分は（ラグランジュ法から導出すると）
$$ M_{11} = m_1 l_{c1}^2 + I_1 + m_2(l_1^2 + l_{c2}^2 + 2l_1 l_{c2}\cos q_2) + I_2 $$
$$ M_{12} = M_{21} = m_2(l_{c2}^2 + l_1 l_{c2}\cos q_2) + I_2 $$
$$ M_{22} = m_2 l_{c2}^2 + I_2 $$
コリオリ・遠心力項は
$$ \bm{C}(\bm{q}, \dot{\bm{q}}) = \begin{pmatrix} -m_2 l_1 l_{c2} \sin q_2 \cdot \dot{q}_2 & -m_2 l_1 l_{c2} \sin q_2 \cdot (\dot{q}_1 + \dot{q}_2) \\ m_2 l_1 l_{c2} \sin q_2 \cdot \dot{q}_1 & 0 \end{pmatrix} $$
宇宙空間なので重力項は $\bm{g}(\bm{q}) = \bm{0}$ です。
経路の設定
関節空間で以下の直線経路を考えます。
$$ \bm{q}(s) = (1 – s)\bm{q}_{\text{start}} + s \, \bm{q}_{\text{end}} $$
$$ \bm{q}_{\text{start}} = \begin{pmatrix} 0 \\ \pi/4 \end{pmatrix}, \quad \bm{q}_{\text{end}} = \begin{pmatrix} \pi/2 \\ -\pi/4 \end{pmatrix} $$
この経路では $\bm{q}'(s) = \bm{q}_{\text{end}} – \bm{q}_{\text{start}}$ が定数、$\bm{q}”(s) = \bm{0}$ です。関節空間の直線経路はジオメトリックには最も単純ですが、動力学的な制約（トルク上限）が $s$ に依存するため、最適速度プロファイルは自明ではありません。
では、この具体的なモデルを Python で実装し、位相平面上での制約構造を可視化してみましょう。
Python 実装1 — 動力学モデルと位相平面の可視化
まず、2リンクアームの動力学モデルと経路パラメトリゼーションの係数を計算し、位相平面上の最大速度曲線（MVC）と加速度制約を可視化します。
import numpy as np
import matplotlib.pyplot as plt
# --- 2リンクアームのパラメータ ---
m1, m2 = 2.0, 1.0 # リンク質量 [kg]
l1, l2 = 1.0, 1.0 # リンク長 [m]
lc1, lc2 = 0.5, 0.5 # 重心までの距離 [m]
I1, I2 = 0.167, 0.083 # 慣性モーメント [kg m^2]
tau_max = np.array([10.0, 10.0]) # トルク上限 [Nm]
tau_min = -tau_max # トルク下限 [Nm]
# --- 経路の設定 ---
q_start = np.array([0.0, np.pi / 4])
q_end = np.array([np.pi / 2, -np.pi / 4])
qp = q_end - q_start # q'(s) : 定数（直線経路）
qpp = np.array([0.0, 0.0]) # q''(s) = 0（直線経路）
def inertia_matrix(q):
"""慣性行列 M(q) を返す"""
c2 = np.cos(q[1])
M11 = m1 * lc1**2 + I1 + m2 * (l1**2 + lc2**2 + 2*l1*lc2*c2) + I2
M12 = m2 * (lc2**2 + l1*lc2*c2) + I2
M22 = m2 * lc2**2 + I2
return np.array([[M11, M12], [M12, M22]])
def coriolis_matrix(q, qd):
"""コリオリ・遠心力行列 C(q, qd) を返す"""
s2 = np.sin(q[1])
h = m2 * l1 * lc2 * s2
return np.array([[-h * qd[1], -h * (qd[0] + qd[1])],
[ h * qd[0], 0.0]])
def compute_path_dynamics(s):
"""経路パラメトリゼーションの係数 m(s), c(s) を計算"""
q = (1 - s) * q_start + s * q_end
M = inertia_matrix(q)
# c(s) の計算: M(q)*q'' + C(q, q')*q'
# 直線経路なので q'' = 0、第1項は消える
# C(q, q')*q' の計算
C = coriolis_matrix(q, qp) # C(q, q') ← qd = q'（速度は q'*sdot だが、sdot=1として計算）
m_s = M @ qp # m(s) = M(q) * q'(s)
c_s = M @ qpp + C @ qp # c(s) = M(q)*q''(s) + C(q,q')*q'(s)
return m_s, c_s
def compute_sdot_bounds(s, sdot):
"""与えられた (s, sdot) でのトルク制約から sddot の上下限を計算"""
m_s, c_s = compute_path_dynamics(s)
sddot_upper = np.inf
sddot_lower = -np.inf
for i in range(2):
# tau_i = m_i * sddot + c_i * sdot^2
# (重力なしの場合)
rhs_max = tau_max[i] - c_s[i] * sdot**2
rhs_min = tau_min[i] - c_s[i] * sdot**2
if m_s[i] > 1e-10:
sddot_upper = min(sddot_upper, rhs_max / m_s[i])
sddot_lower = max(sddot_lower, rhs_min / m_s[i])
elif m_s[i] < -1e-10:
sddot_upper = min(sddot_upper, rhs_min / m_s[i])
sddot_lower = max(sddot_lower, rhs_max / m_s[i])
else:
# m_i ≈ 0: sdot^2 制約のみ
if rhs_max < 0 or rhs_min > 0:
return -np.inf, -np.inf # 実行不可能
return sddot_lower, sddot_upper
def compute_mvc(s_array):
"""各 s に対して最大速度 sdot_max を二分探索で計算"""
mvc = np.zeros_like(s_array)
for idx, s in enumerate(s_array):
lo, hi = 0.0, 20.0
for _ in range(100): # 二分探索
mid = (lo + hi) / 2.0
lb, ub = compute_sdot_bounds(s, mid)
if lb <= ub:
lo = mid # 実行可能 → もっと大きく
else:
hi = mid # 実行不可能 → もっと小さく
mvc[idx] = lo
return mvc
# --- 位相平面の可視化 ---
N_s = 200
s_array = np.linspace(0, 1, N_s)
mvc = compute_mvc(s_array)
# 加速度場の可視化
N_sdot = 100
sdot_array = np.linspace(0.01, np.max(mvc) * 1.1, N_sdot)
S, SD = np.meshgrid(s_array, sdot_array)
SDDOT_MAX = np.zeros_like(S)
SDDOT_MIN = np.zeros_like(S)
for i in range(N_sdot):
for j in range(N_s):
lb, ub = compute_sdot_bounds(S[i, j], SD[i, j])
SDDOT_MAX[i, j] = ub if ub > lb else np.nan
SDDOT_MIN[i, j] = lb if ub > lb else np.nan
fig, axes = plt.subplots(1, 2, figsize=(14, 6))
# 左: MVC と実行可能領域
ax1 = axes[0]
ax1.fill_between(s_array, 0, mvc, alpha=0.3, color='steelblue', label='Feasible region')
ax1.plot(s_array, mvc, 'b-', linewidth=2, label='MVC (Maximum Velocity Curve)')
ax1.set_xlabel('s (path parameter)')
ax1.set_ylabel(r'$\dot{s}$ (path velocity)')
ax1.set_title('Phase Plane: Feasible Region')
ax1.legend(loc='upper right')
ax1.set_xlim(0, 1)
ax1.set_ylim(0, np.max(mvc) * 1.2)
ax1.grid(True, alpha=0.3)
# 右: 最大加速度のカラーマップ
ax2 = axes[1]
feasible = np.where(np.isnan(SDDOT_MAX), np.nan, SDDOT_MAX)
im = ax2.pcolormesh(S, SD, feasible, cmap='RdYlGn', shading='auto', vmin=-30, vmax=30)
ax2.plot(s_array, mvc, 'k-', linewidth=2, label='MVC')
ax2.set_xlabel('s (path parameter)')
ax2.set_ylabel(r'$\dot{s}$ (path velocity)')
ax2.set_title(r'Maximum Acceleration $\ddot{s}_{\max}$ in Phase Plane')
plt.colorbar(im, ax=ax2, label=r'$\ddot{s}_{\max}$')
ax2.legend(loc='upper right')
ax2.set_xlim(0, 1)
ax2.set_ylim(0, np.max(mvc) * 1.2)
ax2.grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('phase_plane_analysis.png', dpi=150, bbox_inches='tight')
plt.show()
上のグラフから、いくつかの重要な特徴が読み取れます。
- MVC の形状は $s$ に依存して変化する — 経路上の位置によってロボットの姿勢が変わるため、同じトルクで出せる速度が異なります。MVC が低い箇所（ボトルネック）が全体の移動時間を支配します
- 実行可能領域（水色の領域）は MVC で囲まれた閉じた領域 — この領域の外側では、どのようなトルクを選んでも制約を満たせません
- 最大加速度のカラーマップ（右図）で、MVC 近傍は緑→赤に変化 — MVC に近づくほど加速の余裕がなくなり、MVC 上では加速度の選択余地がゼロ（$\ddot{s}_{\max} = \ddot{s}_{\min}$）になることが色の変化から確認できます
この位相平面の構造を踏まえて、次に実際に最適軌道を計算するコードを実装しましょう。
Python 実装2 — Bobrow-Dubowsky-Gibson アルゴリズムによる最適軌道計算
位相平面法に基づくBobrow-Dubowsky-Gibsonアルゴリズムを実装し、最適速度プロファイルを計算します。
import numpy as np
import matplotlib.pyplot as plt
# --- パラメータ（前のコードと同じ） ---
m1, m2 = 2.0, 1.0
l1, l2 = 1.0, 1.0
lc1, lc2 = 0.5, 0.5
I1, I2 = 0.167, 0.083
tau_max_val = np.array([10.0, 10.0])
tau_min_val = -tau_max_val
q_start = np.array([0.0, np.pi / 4])
q_end = np.array([np.pi / 2, -np.pi / 4])
qp = q_end - q_start
qpp = np.array([0.0, 0.0])
def inertia_matrix(q):
c2 = np.cos(q[1])
M11 = m1 * lc1**2 + I1 + m2 * (l1**2 + lc2**2 + 2*l1*lc2*c2) + I2
M12 = m2 * (lc2**2 + l1*lc2*c2) + I2
M22 = m2 * lc2**2 + I2
return np.array([[M11, M12], [M12, M22]])
def coriolis_matrix(q, qd):
s2 = np.sin(q[1])
h = m2 * l1 * lc2 * s2
return np.array([[-h * qd[1], -h * (qd[0] + qd[1])],
[ h * qd[0], 0.0]])
def compute_path_dynamics(s):
q = (1 - s) * q_start + s * q_end
M = inertia_matrix(q)
C = coriolis_matrix(q, qp)
m_s = M @ qp
c_s = M @ qpp + C @ qp
return m_s, c_s
def compute_sdot_bounds(s, sdot):
m_s, c_s = compute_path_dynamics(s)
sddot_upper = np.inf
sddot_lower = -np.inf
for i in range(2):
rhs_max = tau_max_val[i] - c_s[i] * sdot**2
rhs_min = tau_min_val[i] - c_s[i] * sdot**2
if m_s[i] > 1e-10:
sddot_upper = min(sddot_upper, rhs_max / m_s[i])
sddot_lower = max(sddot_lower, rhs_min / m_s[i])
elif m_s[i] < -1e-10:
sddot_upper = min(sddot_upper, rhs_min / m_s[i])
sddot_lower = max(sddot_lower, rhs_max / m_s[i])
else:
if rhs_max < 0 or rhs_min > 0:
return -np.inf, -np.inf
return sddot_lower, sddot_upper
def forward_integrate(s_grid, ds):
"""始点 (0, 0) から最大加速で前方積分"""
sdot = np.zeros(len(s_grid))
# 初期値: sdot ≈ 0 のときの加速度から微小速度を推定
lb0, ub0 = compute_sdot_bounds(s_grid[0], 1e-6)
sddot_init = max(ub0, 0.1)
sdot[0] = np.sqrt(2 * sddot_init * ds) * 0.5 # 微小初速
for k in range(len(s_grid) - 1):
lb, ub = compute_sdot_bounds(s_grid[k], sdot[k])
if ub < lb:
sdot[k+1:] = 0
break
sddot = ub # 最大加速
# オイラー法: sdot_{k+1} = sdot_k + (sddot / sdot_k) * ds * sdot_k
sdot_new = sdot[k] + sddot * ds / max(sdot[k], 1e-8) * sdot[k]
# 上の式を整理すると sdot_new = sdot_k + sddot * ds
# これは dt = ds/sdot から sddot*dt = sddot*ds/sdot
# 速度更新: sdot_new^2 = sdot_k^2 + 2*sddot*ds
sdot_sq = sdot[k]**2 + 2 * sddot * ds
if sdot_sq < 0:
sdot[k+1:] = 0
break
sdot[k+1] = np.sqrt(sdot_sq)
return sdot
def backward_integrate(s_grid, ds):
"""終点 (1, 0) から最大減速で後方積分"""
n = len(s_grid)
sdot = np.zeros(n)
# 終端初期値
lb_end, ub_end = compute_sdot_bounds(s_grid[-1], 1e-6)
sddot_init = min(lb_end, -0.1)
sdot[-1] = np.sqrt(-2 * sddot_init * ds) * 0.5
for k in range(n - 2, -1, -1):
lb, ub = compute_sdot_bounds(s_grid[k+1], sdot[k+1])
if ub < lb:
sdot[:k+1] = np.inf
break
sddot = lb # 最大減速（最小加速度）
# 後方: sdot_k^2 = sdot_{k+1}^2 - 2*sddot*ds
sdot_sq = sdot[k+1]**2 - 2 * sddot * ds
if sdot_sq < 0:
sdot[:k+1] = np.inf
break
sdot[k] = np.sqrt(sdot_sq)
return sdot
# --- 最適軌道の計算 ---
N = 500
s_grid = np.linspace(0, 1, N)
ds = s_grid[1] - s_grid[0]
# MVC の計算
def compute_mvc(s_array):
mvc = np.zeros_like(s_array)
for idx, s in enumerate(s_array):
lo, hi = 0.0, 20.0
for _ in range(100):
mid = (lo + hi) / 2.0
lb, ub = compute_sdot_bounds(s, mid)
if lb <= ub:
lo = mid
else:
hi = mid
mvc[idx] = lo
return mvc
mvc = compute_mvc(s_grid)
# 前方積分と後方積分
sdot_fwd = forward_integrate(s_grid, ds)
sdot_bwd = backward_integrate(s_grid, ds)
# 最適軌道: 両方の min を取り、さらに MVC でクリップ
sdot_opt = np.minimum(sdot_fwd, sdot_bwd)
sdot_opt = np.minimum(sdot_opt, mvc)
sdot_opt = np.maximum(sdot_opt, 0)
# --- 結果の可視化 ---
fig, axes = plt.subplots(2, 2, figsize=(14, 10))
# (a) 位相平面上の最適軌道
ax = axes[0, 0]
ax.fill_between(s_grid, 0, mvc, alpha=0.15, color='steelblue')
ax.plot(s_grid, mvc, 'b-', linewidth=1.5, alpha=0.7, label='MVC')
ax.plot(s_grid, sdot_fwd, 'r--', linewidth=1, alpha=0.6, label='Forward (max accel)')
ax.plot(s_grid, sdot_bwd, 'g--', linewidth=1, alpha=0.6, label='Backward (max decel)')
ax.plot(s_grid, sdot_opt, 'k-', linewidth=2.5, label='Optimal trajectory')
ax.set_xlabel('s')
ax.set_ylabel(r'$\dot{s}$')
ax.set_title('(a) Phase Plane: Optimal Trajectory')
ax.legend(loc='upper right', fontsize=9)
ax.set_xlim(0, 1)
ax.set_ylim(0, np.max(mvc) * 1.2)
ax.grid(True, alpha=0.3)
# (b) 時間プロファイル s(t) の計算
dt_array = ds / np.maximum(sdot_opt[:-1], 1e-8)
t_array = np.concatenate([[0], np.cumsum(dt_array)])
T_total = t_array[-1]
ax = axes[0, 1]
ax.plot(t_array, s_grid, 'k-', linewidth=2)
ax.set_xlabel('Time t [s]')
ax.set_ylabel('s')
ax.set_title(f'(b) Path Parameter s(t) [T = {T_total:.3f} s]')
ax.grid(True, alpha=0.3)
# (c) 速度プロファイル sdot(t)
ax = axes[1, 0]
ax.plot(t_array, sdot_opt, 'k-', linewidth=2)
ax.set_xlabel('Time t [s]')
ax.set_ylabel(r'$\dot{s}$')
ax.set_title('(c) Velocity Profile $\\dot{s}(t)$')
ax.grid(True, alpha=0.3)
# (d) トルクプロファイル
tau_profile = np.zeros((N, 2))
for k in range(N):
m_s, c_s = compute_path_dynamics(s_grid[k])
if k < N - 1 and sdot_opt[k] > 1e-8:
sddot = (sdot_opt[min(k+1, N-1)]**2 - sdot_opt[k]**2) / (2 * ds)
else:
sddot = 0
tau_profile[k] = m_s * sddot + c_s * sdot_opt[k]**2
ax = axes[1, 1]
ax.plot(t_array, tau_profile[:, 0], 'r-', linewidth=1.5, label='Joint 1')
ax.plot(t_array, tau_profile[:, 1], 'b-', linewidth=1.5, label='Joint 2')
ax.axhline(y=10, color='gray', linestyle='--', alpha=0.5, label=r'$\tau_{\max}$')
ax.axhline(y=-10, color='gray', linestyle='--', alpha=0.5, label=r'$\tau_{\min}$')
ax.set_xlabel('Time t [s]')
ax.set_ylabel('Torque [Nm]')
ax.set_title('(d) Joint Torques')
ax.legend(loc='upper right', fontsize=9)
ax.grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('optimal_trajectory_result.png', dpi=150, bbox_inches='tight')
plt.show()
print(f"最適移動時間 T = {T_total:.4f} s")
この4枚のプロットから、時間最適軌道の特徴が明確に読み取れます。
- 位相平面（図a）: 最適軌道（黒実線）は、赤の前方積分曲線（最大加速）と緑の後方積分曲線（最大減速）の下側のエンベロープとして構成されています。始点から加速し、ある切替点で減速に切り替わり、終点で停止する「バンバン」構造が確認できます。最適軌道は MVC（青線）には達していません。これは、この経路パラメータでは MVC のボトルネックが十分広いためです
- 経路パラメータ $s(t)$（図b）: S字カーブを描いており、中間で $s$ の変化率（= $\dot{s}$）が最大になっていることが分かります。これは加速フェーズで速度が増加し、切替後の減速フェーズで速度が減少する構造に対応しています
- 速度プロファイル $\dot{s}(t)$（図c）: 山型のプロファイルが得られています。頂点が切替点に対応し、この点の左側が加速フェーズ、右側が減速フェーズです
- トルクプロファイル（図d）: 各関節のトルクは常にトルク上限 $\pm 10$ Nm の近傍で変動しており、少なくとも一方の関節が常にトルク限界に張り付いていることが確認できます。これはバンバン制御の特徴であり、「常にどこかの関節が全力で動いている」ことを意味します
次に、TOPP の凸最適化アプローチを実装し、Bobrow-Dubowsky-Gibson のアルゴリズムとの比較を行いましょう。
Python 実装3 — TOPP 凸最適化アプローチ
凸最適化を用いた TOPP の実装を行います。ここでは scipy.optimize の線形計画法ベースの近似実装を使います。$b(s) = \dot{s}^2$ の変数変換により、制約が線形になることを利用します。
import numpy as np
import matplotlib.pyplot as plt
from scipy.optimize import linprog
# --- パラメータ（前と同じ） ---
m1, m2 = 2.0, 1.0
l1, l2 = 1.0, 1.0
lc1, lc2 = 0.5, 0.5
I1, I2 = 0.167, 0.083
tau_max_val = np.array([10.0, 10.0])
tau_min_val = -tau_max_val
q_start = np.array([0.0, np.pi / 4])
q_end = np.array([np.pi / 2, -np.pi / 4])
qp = q_end - q_start
qpp = np.array([0.0, 0.0])
def inertia_matrix(q):
c2 = np.cos(q[1])
M11 = m1 * lc1**2 + I1 + m2 * (l1**2 + lc2**2 + 2*l1*lc2*c2) + I2
M12 = m2 * (lc2**2 + l1*lc2*c2) + I2
M22 = m2 * lc2**2 + I2
return np.array([[M11, M12], [M12, M22]])
def coriolis_matrix(q, qd):
s2 = np.sin(q[1])
h = m2 * l1 * lc2 * s2
return np.array([[-h * qd[1], -h * (qd[0] + qd[1])],
[ h * qd[0], 0.0]])
def compute_path_dynamics(s):
q = (1 - s) * q_start + s * q_end
M = inertia_matrix(q)
C = coriolis_matrix(q, qp)
m_s = M @ qp
c_s = M @ qpp + C @ qp
return m_s, c_s
def solve_topp_lp(N_grid=100):
"""
TOPP を線形計画法で近似的に解く。
b_k = sdot^2 を決定変数とし、移動時間を最小化する。
目的関数: sum(1/sqrt(b_k)) の凸性を利用し、
逐次線形近似（SLP）で解く。
"""
ds = 1.0 / N_grid
s_vals = np.linspace(0, 1, N_grid + 1)
# 経路動力学の係数を前計算
m_coeffs = np.zeros((N_grid + 1, 2))
c_coeffs = np.zeros((N_grid + 1, 2))
for k in range(N_grid + 1):
m_s, c_s = compute_path_dynamics(s_vals[k])
m_coeffs[k] = m_s
c_coeffs[k] = c_s
# 初期推定: 一様速度（低速）
b = np.ones(N_grid + 1) * 0.5
b[0] = 1e-4 # 始点: sdot ≈ 0
b[-1] = 1e-4 # 終点: sdot ≈ 0
# 逐次線形近似（SLP）
for iteration in range(50):
b_old = b.copy()
# 目的関数の線形近似: d/db (1/sqrt(b)) = -1/(2*b^{3/2})
# T ≈ sum ds/sqrt(b_k) ≈ sum [ds/sqrt(b_k^old) + ds*(-1/(2*b_k^old^{3/2}))*(b_k - b_k^old)]
# 最小化 → 線形項: -ds/(2*b_k^old^{3/2}) * b_k
c_obj = np.zeros(N_grid + 1)
for k in range(N_grid + 1):
b_ref = max(b_old[k], 1e-6)
c_obj[k] = -ds / (2.0 * b_ref**1.5)
# 不等式制約の構築
# トルク制約: tau_min <= m_i * (b_{k+1} - b_k)/(2*ds) + c_i * b_k <= tau_max
# 各区間 k (0 <= k < N_grid) に対して、各関節 i に対して 2つの不等式
A_ub_list = []
b_ub_list = []
for k in range(N_grid):
for i in range(2):
mi = m_coeffs[k, i]
ci = c_coeffs[k, i]
# 上限: mi * (b_{k+1} - b_k)/(2*ds) + ci * (b_k + b_{k+1})/2 <= tau_max
# 中間点近似を使用
row = np.zeros(N_grid + 1)
row[k] = -mi / (2 * ds) + ci / 2.0
row[k+1] = mi / (2 * ds) + ci / 2.0
A_ub_list.append(row)
b_ub_list.append(tau_max_val[i])
# 下限: -(mi * (b_{k+1} - b_k)/(2*ds) + ci * (b_k + b_{k+1})/2) <= -tau_min
A_ub_list.append(-row)
b_ub_list.append(-tau_min_val[i])
A_ub = np.array(A_ub_list)
b_ub = np.array(b_ub_list)
# 変数の境界: b_k >= 0
bounds = [(1e-4, 1e-4)] + [(1e-6, None)] * (N_grid - 1) + [(1e-4, 1e-4)]
# 信頼領域制約: |b_k - b_k^old| <= delta
delta = max(0.5, 2.0 / (iteration + 1))
for k in range(1, N_grid):
lo = max(1e-6, b_old[k] - delta)
hi = b_old[k] + delta
bounds[k] = (lo, hi)
# 線形計画法を解く
result = linprog(c_obj, A_ub=A_ub, b_ub=b_ub, bounds=bounds, method='highs')
if result.success:
b = result.x
else:
break
# 収束判定
if np.max(np.abs(b - b_old)) < 1e-6:
break
sdot_topp = np.sqrt(np.maximum(b, 0))
return s_vals, sdot_topp
# --- TOPP の実行 ---
N_topp = 200
s_topp, sdot_topp = solve_topp_lp(N_topp)
# 移動時間の計算
ds_topp = s_topp[1] - s_topp[0]
dt_topp = ds_topp / np.maximum(sdot_topp[:-1], 1e-8)
t_topp = np.concatenate([[0], np.cumsum(dt_topp)])
T_topp = t_topp[-1]
# Bobrow法の結果（前のセクションの再計算）
N_bob = 500
s_bob = np.linspace(0, 1, N_bob)
ds_bob = s_bob[1] - s_bob[0]
def compute_sdot_bounds_2(s, sdot):
m_s, c_s = compute_path_dynamics(s)
sddot_upper = np.inf
sddot_lower = -np.inf
for i in range(2):
rhs_max = tau_max_val[i] - c_s[i] * sdot**2
rhs_min = tau_min_val[i] - c_s[i] * sdot**2
if m_s[i] > 1e-10:
sddot_upper = min(sddot_upper, rhs_max / m_s[i])
sddot_lower = max(sddot_lower, rhs_min / m_s[i])
elif m_s[i] < -1e-10:
sddot_upper = min(sddot_upper, rhs_min / m_s[i])
sddot_lower = max(sddot_lower, rhs_max / m_s[i])
else:
if rhs_max < 0 or rhs_min > 0:
return -np.inf, -np.inf
return sddot_lower, sddot_upper
def forward_int(s_grid, ds):
sdot = np.zeros(len(s_grid))
lb0, ub0 = compute_sdot_bounds_2(s_grid[0], 1e-6)
sdot[0] = np.sqrt(max(2 * max(ub0, 0.1) * ds, 0)) * 0.5
for k in range(len(s_grid) - 1):
lb, ub = compute_sdot_bounds_2(s_grid[k], sdot[k])
if ub < lb:
sdot[k+1:] = 0; break
sdot_sq = sdot[k]**2 + 2 * ub * ds
if sdot_sq < 0:
sdot[k+1:] = 0; break
sdot[k+1] = np.sqrt(sdot_sq)
return sdot
def backward_int(s_grid, ds):
n = len(s_grid)
sdot = np.zeros(n)
lb_end, ub_end = compute_sdot_bounds_2(s_grid[-1], 1e-6)
sdot[-1] = np.sqrt(max(-2 * min(lb_end, -0.1) * ds, 0)) * 0.5
for k in range(n - 2, -1, -1):
lb, ub = compute_sdot_bounds_2(s_grid[k+1], sdot[k+1])
if ub < lb:
sdot[:k+1] = np.inf; break
sdot_sq = sdot[k+1]**2 - 2 * lb * ds
if sdot_sq < 0:
sdot[:k+1] = np.inf; break
sdot[k] = np.sqrt(sdot_sq)
return sdot
def compute_mvc_2(s_array):
mvc = np.zeros_like(s_array)
for idx, s in enumerate(s_array):
lo, hi = 0.0, 20.0
for _ in range(100):
mid = (lo + hi) / 2.0
lb, ub = compute_sdot_bounds_2(s, mid)
lo, hi = (mid, hi) if lb <= ub else (lo, mid)
mvc[idx] = lo
return mvc
mvc_bob = compute_mvc_2(s_bob)
sdot_fwd_bob = forward_int(s_bob, ds_bob)
sdot_bwd_bob = backward_int(s_bob, ds_bob)
sdot_opt_bob = np.minimum(np.minimum(sdot_fwd_bob, sdot_bwd_bob), mvc_bob)
sdot_opt_bob = np.maximum(sdot_opt_bob, 0)
dt_bob = ds_bob / np.maximum(sdot_opt_bob[:-1], 1e-8)
t_bob = np.concatenate([[0], np.cumsum(dt_bob)])
T_bob = t_bob[-1]
# --- 比較プロット ---
fig, axes = plt.subplots(1, 2, figsize=(14, 5))
# 位相平面の比較
ax = axes[0]
ax.fill_between(s_bob, 0, mvc_bob, alpha=0.1, color='steelblue')
ax.plot(s_bob, mvc_bob, 'b-', linewidth=1, alpha=0.5, label='MVC')
ax.plot(s_bob, sdot_opt_bob, 'k-', linewidth=2, label=f'Bobrow (T={T_bob:.3f}s)')
ax.plot(s_topp, sdot_topp, 'r--', linewidth=2, label=f'TOPP-LP (T={T_topp:.3f}s)')
ax.set_xlabel('s')
ax.set_ylabel(r'$\dot{s}$')
ax.set_title('Phase Plane: Bobrow vs TOPP')
ax.legend(fontsize=10)
ax.set_xlim(0, 1)
ax.set_ylim(0, np.max(mvc_bob) * 1.2)
ax.grid(True, alpha=0.3)
# 速度の時間プロファイル比較
ax = axes[1]
ax.plot(t_bob, sdot_opt_bob, 'k-', linewidth=2, label=f'Bobrow (T={T_bob:.3f}s)')
ax.plot(t_topp, sdot_topp, 'r--', linewidth=2, label=f'TOPP-LP (T={T_topp:.3f}s)')
ax.set_xlabel('Time t [s]')
ax.set_ylabel(r'$\dot{s}$')
ax.set_title('Velocity Profile: Bobrow vs TOPP')
ax.legend(fontsize=10)
ax.grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('bobrow_vs_topp.png', dpi=150, bbox_inches='tight')
plt.show()
print(f"Bobrow法 最適移動時間: T = {T_bob:.4f} s")
print(f"TOPP-LP 最適移動時間: T = {T_topp:.4f} s")
2つの手法の比較から、以下の知見が得られます。
- 位相平面上の速度プロファイル（左図）: Bobrow 法の結果（黒実線）と TOPP-LP の結果（赤破線）はほぼ一致しています。これは同じ最適解に収束していることを示しており、両手法の正当性を相互に検証しています。TOPP-LP は逐次線形近似を用いているため離散化の粗さに応じたわずかな差異が生じますが、全体的な形状は一致しています
- 速度の時間プロファイル（右図）: 両手法とも山型の速度プロファイルを示しており、加速→減速のバンバン構造が確認できます。最適移動時間もほぼ同じ値が得られています
- 実装上の差異: Bobrow 法は前方・後方積分という直感的なアプローチであり、実装が比較的容易です。一方、TOPP-LP は凸最適化の定式化に基づいており、追加制約（速度制限やジャーク制限）を不等式として自然に追加できる柔軟性があります
数値計算の結果を確認できたところで、ここまでの理論と実装のまとめに進みましょう。
宇宙ロボティクスへの応用 — デブリ捕獲シナリオ
宇宙空間特有の課題
宇宙ロボティクスにおける時間最適軌道計画には、地上の産業用ロボットにはない特有の課題があります。
反力の問題: 宇宙空間では母機（衛星本体）が浮遊状態にあるため、ロボットアームの動作が母機に反力を与え、母機の姿勢が変化します。時間最適軌道はトルクが常に上限に張り付くバンバン制御であるため、反力も最大になります。これは母機の姿勢制御系に大きな外乱として作用します。
微小重力環境: 重力項 $\bm{g}(\bm{q}) = \bm{0}$ となるため、本記事の数値例はそのまま宇宙環境に適用できます。ただし、地上試験では重力補償が必要です。
通信遅延: 地球からの遠隔操作には数秒〜数十分の通信遅延があるため、時間最適軌道は事前にオンボードで計算し、自律的に実行する必要があります。計算の高速性が求められる理由がここにあります。
デブリ捕獲のタイムライン
回転するデブリを捕獲する典型的なシナリオを考えます。
- ランデブーフェーズ: 母機がデブリの近傍（数メートル）に接近し、相対位置を保持する
- タンブリング解析: デブリの回転状態（角速度ベクトル、主軸方向）を観測し、把持可能な時間窓を特定する
- アプローチフェーズ: ロボットアームが把持点に向かって移動する — ここで時間最適軌道が適用される
- 把持・固定フェーズ: エンドエフェクタがデブリを把持し、デブリのタンブリングを制動する
アプローチフェーズの所要時間は、デブリの回転周期と把持可能な姿勢角の範囲から決まる時間窓に収まらなければなりません。たとえば、デブリが10 rpm（6秒/回転）で回転し、把持可能な姿勢角が $\pm 15°$ の場合、時間窓は
$$ \Delta t_{\text{window}} = \frac{30°}{360°} \times 6 \, \text{s} = 0.5 \, \text{s} $$
わずか0.5秒です。この厳しい制約の中で、ロボットアームは経路に沿って最速で移動する必要があります。
実装上の工夫
実際の宇宙ロボティクスでは、以下の工夫が加えられます。
安全マージン: トルク制約をハードリミットの80〜90%に設定し、外乱に対するマージンを確保します。これは単にトルク上限値を小さく設定するだけで対応できます。
再計画: デブリの状態推定が更新されるたびに、新しい経路に対して時間最適軌道を再計算します。TOPP の計算速度（数ミリ秒）が活きる場面です。次の記事で扱うオンライン軌道再計画とも密接に関連します。
ジャーク制限: バンバン制御のトルク急変は構造的な振動を引き起こすため、実用的にはジャーク（トルクの時間変化率）にも制約を加えます。TOPP の凸最適化定式化では、この制約を自然に追加できます。
まとめ
本記事では、ロボットマニピュレータの時間最適軌道計画について、基礎理論からアルゴリズム、Python実装まで体系的に解説しました。
- 経路パラメトリゼーション $s(t)$ により、$n$ 自由度の軌道計画が1次元のスカラー最適制御問題に帰着される。この変換が時間最適軌道計画の計算を tractable にする鍵である
- 位相平面法 $(s, \dot{s})$ による幾何学的解析により、最大速度曲線（MVC）、加速度制約、切替曲線の構造が視覚的に理解できる。最適軌道は $\dot{s}$ を MVC の範囲内で最大化する曲線として構成される
- バンバン制御構造 — 時間最適軌道ではトルクが常に上限か下限に張り付き、最大加速と最大減速が切り替わる。ポントリャーギンの最大原理から導かれるこの結果は、Bobrow-Dubowsky-Gibson のアルゴリズムの基礎となる
- TOPP（凸最適化アプローチ） — 変数変換 $b = \dot{s}^2$ により問題が凸最適化に帰着され、大域最適性の保証、制約の柔軟な追加、数値安定性が得られる
- 宇宙ロボティクスへの応用 — デブリ捕獲などの厳しい時間制約のあるミッションでは、時間最適軌道計画が直接的にミッション成功率に影響する
時間最適軌道計画は「与えられた経路上をいかに速く動くか」に特化した手法であり、経路が事前に決まっていることを前提としています。しかし、実際のデブリ捕獲ミッションでは、デブリの状態推定が随時更新され、経路自体の変更が必要になることがあります。
次のステップとして、以下の記事も参考にしてください。
- オンライン軌道再計画 — 環境変化に応じてリアルタイムに軌道を修正する手法
- 関節空間の軌道計画 — 時間最適化の前段階となる経路計画の基礎