ロボットアームに「肩の関節を30度から90度まで動かせ」と指令するとき、ただ始点と終点を与えるだけでは十分ではありません。もし一瞬で90度へジャンプしようとすれば、モータには無限大のトルクが必要になり、機械は壊れてしまいます。逆に、ゆっくりすぎれば作業が終わりません。大切なのは「どう動くか」を時間の関数として設計すること — これが軌道計画（trajectory planning）です。
特に宇宙ロボットでは、軌道計画の重要性は地上以上に高まります。国際宇宙ステーション（ISS）の Canadarm2 が大型モジュールを把持して移動させるとき、急激な加速は宇宙ステーション全体に反力を生みます。宇宙空間では固定された地面がないため、アームの運動はベース衛星の姿勢を乱します。加速度やジャーク（加速度の時間変化率）を滑らかに制御することで、こうした衝撃を最小限に抑えることができます。
軌道計画を理解すると、以下の応用が広がります。
- 産業用ロボット: 組立・溶接ラインでの滑らかな動作生成。衝撃を抑えることでワークの品質と機械の寿命が向上
- 宇宙ロボティクス: 軌道上サービス（衛星修理・燃料補給）やデブリ捕獲での安全な接近動作
- 医療ロボット: 手術支援ロボットでの微細かつ滑らかな制御
- 自動運転: 車両の加減速・操舵のジャーク制限による乗り心地最適化
本記事の内容
- 点間運動の問題設定と軌道計画の数学的定式化
- 3次多項式補間（位置・速度の4境界条件）
- 5次多項式補間（位置・速度・加速度の6境界条件）
- 台形速度プロファイル（加速・等速・減速の3区間）
- S字速度プロファイル（ジャーク制限付き）
- 経由点を通る軌道（via points）とスプライン補間
- 宇宙ロボットにおける制約と考慮事項
- Pythonでの実装と各手法の比較
前提知識
この記事を読む前に、以下の記事を読んでおくと理解が深まります。
- DHパラメータと順運動学 — ロボットアームの関節角と手先位置の関係
なぜ軌道計画が必要か — 始点と終点だけでは足りない
ロボットの「動き方」を設計する
車を運転するとき、出発地と目的地だけでなく「どの道を通るか」「どのくらいのスピードで走るか」を考えます。急ブレーキや急ハンドルは同乗者に不快な衝撃を与えますし、タイヤにも負担がかかります。
ロボットアームの動作設計も同じです。関節角度 $\theta$ を時刻 $t = 0$ での初期値 $\theta_0$ から、時刻 $t = t_f$ での目標値 $\theta_f$ まで遷移させるとき、単に線形に補間する（等速で動かす）と、開始と終了の瞬間に速度が不連続になります。速度の不連続は無限大の加速度を意味し、モータが生成できる有限のトルクでは実現不可能です。
軌道計画の目標は、関節角度の時間関数 $\theta(t)$ を設計して、以下の条件を同時に満たすことです。
- 境界条件: 指定した始点・終点の位置（必要に応じて速度・加速度も）を満たす
- 滑らかさ: 速度や加速度が連続で、機械に衝撃を与えない
- 制約遵守: 関節の速度・加速度・トルクの物理的限界を超えない
- 効率: 可能な限り短い時間で運動を完了する
関節空間 vs 作業空間
軌道計画には大きく分けて2つのアプローチがあります。
関節空間（joint space）での計画は、各関節角度 $\theta_i(t)$ を独立に時間の関数として設計します。関節ごとに軌道を生成するため、計算が単純で特異点の問題が生じにくいのが利点です。
作業空間（task space）での計画は、手先（エンドエフェクタ）の位置・姿勢を時間の関数として直接設計し、逆運動学で関節角度に変換します。直線移動や円弧移動など、幾何学的に意味のある経路を実現できますが、特異点や冗長性の扱いが複雑になります。
本記事では関節空間での軌道計画を取り上げます。関節空間での手法を理解することが、作業空間での計画の土台となります。
それでは、最も基本的な問題設定である点間運動から始めましょう。
点間運動（Point-to-Point）の問題設定
1自由度の関節を考える
軌道計画の本質を理解するために、まず1つの関節（1自由度）について考えます。多関節ロボットでは、各関節に対して独立に同じ手法を適用できます。
問題設定はシンプルです。時刻 $t = 0$ で関節角度が $\theta_0$、時刻 $t = t_f$ で関節角度が $\theta_f$ になるような関数 $\theta(t)$ を求めます。これを点間運動（Point-to-Point motion, PTP）と呼びます。
追加の要求として、始点と終点での速度 $\dot{\theta}$ や加速度 $\ddot{\theta}$ を指定することがあります。典型的には、静止状態から出発して静止状態で到着する「rest-to-rest」運動を考えます。
$$ \begin{equation} \theta(0) = \theta_0, \quad \theta(t_f) = \theta_f, \quad \dot{\theta}(0) = \dot{\theta}_0, \quad \dot{\theta}(t_f) = \dot{\theta}_f \end{equation} $$
この4つの境界条件を満たす最も低次の多項式は何次でしょうか。4つの未知数を決定するには4つの方程式が必要なので、3次多項式（係数が4つ）が最低次の候補となります。
なぜ多項式を使うのか
$\theta(t)$ の形として多項式を選ぶ理由は明快です。
- 微分が容易: 速度 $\dot{\theta}(t)$、加速度 $\ddot{\theta}(t)$ が解析的に求まる
- 境界条件の適用が線形方程式: 係数を連立方程式で一意に決定できる
- 任意の滑らかさを達成可能: 次数を上げれば、より高次の導関数も連続にできる
- 計算コストが低い: リアルタイム制御に向いている
それでは具体的に、3次多項式補間から見ていきましょう。
3次多項式補間 — 位置と速度の境界条件
問題の定式化
3次多項式で関節角度を表現します。
$$ \begin{equation} \theta(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3 \end{equation} $$
この多項式を時間で微分すると、速度と加速度が得られます。
$$ \begin{equation} \dot{\theta}(t) = a_1 + 2a_2 t + 3a_3 t^2 \end{equation} $$
$$ \begin{equation} \ddot{\theta}(t) = 2a_2 + 6a_3 t \end{equation} $$
4つの境界条件を適用します。
位置の初期条件 $\theta(0) = \theta_0$ より:
$$ a_0 = \theta_0 $$
速度の初期条件 $\dot{\theta}(0) = \dot{\theta}_0$ より:
$$ a_1 = \dot{\theta}_0 $$
位置の終端条件 $\theta(t_f) = \theta_f$ より:
$$ a_0 + a_1 t_f + a_2 t_f^2 + a_3 t_f^3 = \theta_f $$
速度の終端条件 $\dot{\theta}(t_f) = \dot{\theta}_f$ より:
$$ a_1 + 2a_2 t_f + 3a_3 t_f^2 = \dot{\theta}_f $$
係数の導出
$a_0$ と $a_1$ は即座に決まりました。残りの2つの条件に $a_0 = \theta_0$, $a_1 = \dot{\theta}_0$ を代入して $a_2, a_3$ を求めます。
終端位置条件に既知の値を代入すると:
$$ \theta_0 + \dot{\theta}_0 t_f + a_2 t_f^2 + a_3 t_f^3 = \theta_f $$
終端速度条件に既知の値を代入すると:
$$ \dot{\theta}_0 + 2a_2 t_f + 3a_3 t_f^2 = \dot{\theta}_f $$
この2元連立方程式を行列形式で書くと:
$$ \begin{bmatrix} t_f^2 & t_f^3 \\ 2t_f & 3t_f^2 \end{bmatrix} \begin{bmatrix} a_2 \\ a_3 \end{bmatrix} = \begin{bmatrix} \theta_f – \theta_0 – \dot{\theta}_0 t_f \\ \dot{\theta}_f – \dot{\theta}_0 \end{bmatrix} $$
行列の行列式は $3t_f^4 – 2t_f^4 = t_f^4 \neq 0$（$t_f > 0$ のとき）なので、逆行列が存在します。クラメルの公式を適用して解くと:
$$ \begin{equation} a_2 = \frac{3(\theta_f – \theta_0) – (2\dot{\theta}_0 + \dot{\theta}_f) t_f}{t_f^2} \end{equation} $$
$$ \begin{equation} a_3 = \frac{-2(\theta_f – \theta_0) + (\dot{\theta}_0 + \dot{\theta}_f) t_f}{t_f^3} \end{equation} $$
rest-to-rest の場合
最も頻繁に使われるのは、静止状態から静止状態への運動（$\dot{\theta}_0 = 0, \dot{\theta}_f = 0$）です。このとき係数は次のように簡略化されます。
$$ a_0 = \theta_0, \quad a_1 = 0, \quad a_2 = \frac{3(\theta_f – \theta_0)}{t_f^2}, \quad a_3 = \frac{-2(\theta_f – \theta_0)}{t_f^3} $$
正規化時間 $\tau = t / t_f$（$\tau \in [0, 1]$）で書くと:
$$ \begin{equation} \theta(\tau) = \theta_0 + (\theta_f – \theta_0)(3\tau^2 – 2\tau^3) \end{equation} $$
この $3\tau^2 – 2\tau^3$ はエルミート基底関数の一つで、$\tau=0$ で0、$\tau=1$ で1をとり、両端で導関数が0になる滑らかなS字カーブを描きます。
3次多項式の限界
3次多項式は位置と速度の連続性を保証しますが、加速度は不連続になり得ます。加速度の不連続はトルクの急変を意味し、機械的な衝撃や振動の原因となります。特にギアやベルト駆動のロボットでは、加速度の急変がバックラッシュによる振動を引き起こします。
この問題を解決するため、加速度の境界条件も追加した5次多項式補間を考えましょう。
5次多項式補間 — 位置・速度・加速度の境界条件
6つの境界条件
始点と終点で位置・速度・加速度の3組、合計6つの境界条件を指定します。
$$ \theta(0) = \theta_0, \quad \dot{\theta}(0) = \dot{\theta}_0, \quad \ddot{\theta}(0) = \ddot{\theta}_0 $$
$$ \theta(t_f) = \theta_f, \quad \dot{\theta}(t_f) = \dot{\theta}_f, \quad \ddot{\theta}(t_f) = \ddot{\theta}_f $$
6つの未知数を決定するために、5次多項式を使います。
$$ \begin{equation} \theta(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3 + a_4 t^4 + a_5 t^5 \end{equation} $$
微分すると:
$$ \dot{\theta}(t) = a_1 + 2a_2 t + 3a_3 t^2 + 4a_4 t^3 + 5a_5 t^4 $$
$$ \ddot{\theta}(t) = 2a_2 + 6a_3 t + 12a_4 t^2 + 20a_5 t^3 $$
係数の導出
初期条件（$t=0$）から $a_0, a_1, a_2$ が即座に決まります。
$$ a_0 = \theta_0, \quad a_1 = \dot{\theta}_0, \quad a_2 = \frac{\ddot{\theta}_0}{2} $$
残り3つの係数 $a_3, a_4, a_5$ は、終端条件3つから決定します。既知の値を代入した終端条件を行列形式で書くと:
$$ \begin{bmatrix} t_f^3 & t_f^4 & t_f^5 \\ 3t_f^2 & 4t_f^3 & 5t_f^4 \\ 6t_f & 12t_f^2 & 20t_f^3 \end{bmatrix} \begin{bmatrix} a_3 \\ a_4 \\ a_5 \end{bmatrix} = \begin{bmatrix} \theta_f – \theta_0 – \dot{\theta}_0 t_f – \frac{\ddot{\theta}_0}{2} t_f^2 \\ \dot{\theta}_f – \dot{\theta}_0 – \ddot{\theta}_0 t_f \\ \ddot{\theta}_f – \ddot{\theta}_0 \end{bmatrix} $$
この連立方程式を解くことで $a_3, a_4, a_5$ が求まります。rest-to-rest（$\dot{\theta}_0 = \dot{\theta}_f = 0$, $\ddot{\theta}_0 = \ddot{\theta}_f = 0$）の場合、右辺が簡潔になり、解は次のようになります。
$h = \theta_f – \theta_0$ と置くと:
$$ \begin{equation} a_3 = \frac{10h}{t_f^3}, \quad a_4 = \frac{-15h}{t_f^4}, \quad a_5 = \frac{6h}{t_f^5} \end{equation} $$
正規化時間 $\tau = t / t_f$ で書くと:
$$ \begin{equation} \theta(\tau) = \theta_0 + h(10\tau^3 – 15\tau^4 + 6\tau^5) \end{equation} $$
この $s(\tau) = 10\tau^3 – 15\tau^4 + 6\tau^5$ は5次エルミート基底関数で、$s(0)=0$, $s(1)=1$ かつ $s'(0)=s'(1)=0$, $s”(0)=s”(1)=0$ を満たします。3次のときよりも「なだらかに」始まりなだらかに終わるS字カーブです。
3次 vs 5次の比較
| 特徴 | 3次多項式 | 5次多項式 |
|---|---|---|
| 境界条件数 | 4（位置+速度） | 6（位置+速度+加速度） |
| 位置の連続性 | $C^0$ 保証 | $C^0$ 保証 |
| 速度の連続性 | $C^1$ 保証 | $C^1$ 保証 |
| 加速度の連続性 | 不連続の可能性 | $C^2$ 保証 |
| 最大速度 | $1.5 h / t_f$ | $1.875 h / t_f$ |
| 最大加速度 | $6 h / t_f^2$ | $10 h / t_f^2$ |
| 計算コスト | 低い | やや高い |
5次多項式は滑らかさの面で優れますが、最大速度と最大加速度が3次より大きいことに注意が必要です。同じ移動量 $h$ と時間 $t_f$ に対して、5次多項式のピーク加速度は3次の約1.67倍になります。モータの出力制限がある場合、これは移動時間を長く設定する必要があることを意味します。
多項式補間は柔軟ですが、「モータの能力を最大限に使う」という観点では必ずしも最適ではありません。加速・等速・減速を明確に分けて、モータの出力を効率的に使う方法が台形速度プロファイルです。
台形速度プロファイル — 最大能力を引き出す設計
直感的な理解
エレベータの動きを想像してください。出発するとき一定の加速度で加速し、目標速度に達したら等速で移動し、到着前に一定の減速度で減速して停止します。速度の時間変化を描くと台形になります。これが台形速度プロファイル（trapezoidal velocity profile）です。
この方法の最大の利点は、加速度が制限値ちょうどの一定値をとることです。モータの能力を最大限に使うため、同じ加速度制限のもとで最も短い時間で目標に到達できます。これは産業用ロボットで広く使われている手法です。
3つの区間
台形速度プロファイルは3つの区間で構成されます。
加速区間（$0 \leq t \leq t_a$）: 一定の加速度 $\ddot{\theta}_{\max}$ で加速
$$ \dot{\theta}(t) = \ddot{\theta}_{\max} \, t $$
$$ \theta(t) = \theta_0 + \frac{1}{2}\ddot{\theta}_{\max} \, t^2 $$
等速区間（$t_a \leq t \leq t_a + t_v$）: 最大速度 $\dot{\theta}_{\max}$ で等速移動
$$ \dot{\theta}(t) = \dot{\theta}_{\max} $$
$$ \theta(t) = \theta(t_a) + \dot{\theta}_{\max}(t – t_a) $$
減速区間（$t_a + t_v \leq t \leq t_f$）: 一定の減速度 $-\ddot{\theta}_{\max}$ で減速
$$ \dot{\theta}(t) = \dot{\theta}_{\max} – \ddot{\theta}_{\max}(t – t_a – t_v) $$
パラメータの決定
加速区間の終了時に最大速度に達する条件から:
$$ \dot{\theta}_{\max} = \ddot{\theta}_{\max} \cdot t_a $$
よって加速時間は:
$$ \begin{equation} t_a = \frac{\dot{\theta}_{\max}}{\ddot{\theta}_{\max}} \end{equation} $$
対称な台形（加速時間 = 減速時間）を仮定すると、全移動量 $h = \theta_f – \theta_0$ は速度プロファイルの面積（台形の面積）で与えられます。
$$ h = \frac{1}{2}\dot{\theta}_{\max} \cdot t_a + \dot{\theta}_{\max} \cdot t_v + \frac{1}{2}\dot{\theta}_{\max} \cdot t_a $$
台形の面積公式を使って整理すると:
$$ \begin{equation} h = \dot{\theta}_{\max}(t_a + t_v) = \dot{\theta}_{\max}\left(t_f – t_a\right) \end{equation} $$
ここで $t_f = 2t_a + t_v$ を使いました。この式から等速区間の時間を求めると:
$$ \begin{equation} t_v = \frac{h}{\dot{\theta}_{\max}} – t_a = \frac{h}{\dot{\theta}_{\max}} – \frac{\dot{\theta}_{\max}}{\ddot{\theta}_{\max}} \end{equation} $$
三角形プロファイル（等速区間がない場合）
移動量が小さい場合、加速中に最大速度に達する前に減速を始める必要があります。このとき $t_v = 0$ で、速度プロファイルは三角形になります。
$t_v \geq 0$ の条件から:
$$ \frac{h}{\dot{\theta}_{\max}} \geq \frac{\dot{\theta}_{\max}}{\ddot{\theta}_{\max}} $$
すなわち:
$$ \begin{equation} h \geq \frac{\dot{\theta}_{\max}^2}{\ddot{\theta}_{\max}} \end{equation} $$
この条件が満たされない場合は、三角形プロファイルを使います。実際に到達する最大速度 $\dot{\theta}_{\text{peak}}$ は:
$$ \dot{\theta}_{\text{peak}} = \sqrt{h \cdot \ddot{\theta}_{\max}} $$
加速・減速の各区間の時間は:
$$ t_a = t_d = \sqrt{\frac{h}{\ddot{\theta}_{\max}}} $$
台形速度プロファイルの長所と短所
長所: – モータの加速度制限を最大限に利用するため、時間効率が良い – 計算が単純で、リアルタイム実装が容易 – パラメータ（最大速度・最大加速度）の物理的意味が明確
短所: – 加速度が不連続（ステップ関数）→ トルクが急変 → 機械的衝撃 – ジャーク（加速度の時間微分）が無限大 → 振動の原因
加速度の不連続は、歯車やベルトの弾性変形を通じて振動を励起します。この問題を解決するために、加速度の変化も滑らかにしたS字速度プロファイルが考案されました。
S字速度プロファイル — ジャーク制限で振動を抑える
ジャークとは何か
コーヒーカップを持って歩くとき、急に立ち止まると中身がこぼれます。これは加速度が急に変わった（ジャークが大きい）からです。ゆっくり減速すれば、加速度が滑らかに変化してコーヒーはこぼれません。
ジャーク（jerk）は加速度の時間微分です。
$$ \begin{equation} j(t) = \dddot{\theta}(t) = \frac{d\ddot{\theta}}{dt} \end{equation} $$
台形速度プロファイルでは、加速区間の始まりと終わりでジャークが無限大になります。S字速度プロファイルは、ジャークを有限の値 $j_{\max}$ に制限することで、加速度を滑らかに変化させます。
7区間の構成
S字速度プロファイルは、台形プロファイルの各加速・減速区間をさらに3分割し、合計7つの区間で構成されます。
| 区間 | ジャーク | 加速度の変化 | 説明 |
|---|---|---|---|
| 1 | $+j_{\max}$ | $0 \to \ddot{\theta}_{\max}$ | 加速度の立ち上がり |
| 2 | $0$ | $\ddot{\theta}_{\max}$ 一定 | 一定加速度 |
| 3 | $-j_{\max}$ | $\ddot{\theta}_{\max} \to 0$ | 加速度の立ち下がり |
| 4 | $0$ | $0$ | 等速区間 |
| 5 | $-j_{\max}$ | $0 \to -\ddot{\theta}_{\max}$ | 減速開始 |
| 6 | $0$ | $-\ddot{\theta}_{\max}$ 一定 | 一定減速度 |
| 7 | $+j_{\max}$ | $-\ddot{\theta}_{\max} \to 0$ | 減速度の解消 |
加速度が台形状（立ち上がり・一定・立ち下がり）になるため、速度のグラフはS字を描きます。
区間1の数学的表現
ジャーク一定 $j(t) = j_{\max}$ の区間（$0 \leq t \leq t_j$）では:
加速度を時間積分すると:
$$ \ddot{\theta}(t) = j_{\max} \, t $$
さらに速度を積分すると:
$$ \dot{\theta}(t) = \frac{1}{2} j_{\max} \, t^2 $$
位置を積分すると:
$$ \theta(t) = \theta_0 + \frac{1}{6} j_{\max} \, t^3 $$
加速度が最大値に達する時刻 $t_j$ は:
$$ \begin{equation} t_j = \frac{\ddot{\theta}_{\max}}{j_{\max}} \end{equation} $$
パラメータ間の関係
S字プロファイルのパラメータは、最大ジャーク $j_{\max}$、最大加速度 $\ddot{\theta}_{\max}$、最大速度 $\dot{\theta}_{\max}$ の3つです。これらから各区間の時間が決定されます。
ジャーク区間の時間:
$$ t_j = \frac{\ddot{\theta}_{\max}}{j_{\max}} $$
加速フェーズで到達する速度の増分は、加速度プロファイル（台形）の面積です。加速度が台形で、一定加速区間の時間を $t_a’$ とすると:
$$ \dot{\theta}_{\max} = \ddot{\theta}_{\max}(t_j + t_a’) $$
S字プロファイルの全パラメータ設計は、台形プロファイルと比べて複雑ですが、本質は同じです。移動量 $h$ が速度プロファイルの面積に等しいという条件から、等速区間の時間 $t_v$ が決まります。
S字プロファイルの最大の利点は振動の抑制です。ジャークを制限することで、構造体の固有振動を励起するリスクが大幅に減少します。宇宙ロボットのように大きなたわみを持つ構造体や、精密な位置決めが必要な応用で特に重要です。
ここまでで、1つの始点から1つの終点へ移動する手法を見てきました。しかし実際のロボット作業では、複数の点を順番に通過する必要があります。次に、経由点を含む軌道計画を考えましょう。
経由点を通る軌道 — via points とスプライン
なぜ経由点が必要か
ロボットの実作業では、始点から終点まで一直線に移動できないことが多くあります。障害物を回避したり、複数の作業点を順番に訪れたり、特定の姿勢を経由する必要があったりします。
$N+1$ 個の点 $\theta_0, \theta_1, \ldots, \theta_N$ を時刻 $t_0, t_1, \ldots, t_N$ に順番に通過する軌道を計画する問題を考えます。各区間で独立に多項式補間を適用するのが基本的なアプローチです。
線形補間と速度の不連続
最も単純な方法は、隣接する2点を直線で結ぶ（線形補間）ことです。区間 $[t_k, t_{k+1}]$ での軌道は:
$$ \theta(t) = \theta_k + \frac{\theta_{k+1} – \theta_k}{t_{k+1} – t_k}(t – t_k) $$
この方法は計算が最も簡単ですが、経由点で速度が不連続になります。各区間の傾き（速度）が異なるため、経由点での速度の接続が滑らかになりません。
区間ごとの3次多項式と速度の接続
各区間 $[t_k, t_{k+1}]$ で3次多項式を使い、経由点での速度を連続にする方法を考えます。$N$ 個の区間があり、各区間に4つの係数があるので、合計 $4N$ 個の未知数があります。
拘束条件は: – 位置の通過条件: $2N$ 個（各区間の始点と終点で位置を指定） – 速度の連続条件: $N-1$ 個（内部の経由点で速度が左右から一致） – 始点・終点の速度: 2個
合計 $2N + (N-1) + 2 = 3N + 1$ 個です。$4N$ 個の未知数に対して $3N + 1$ 個の条件なので、$N – 1$ 個の自由度が残ります。この自由度は、経由点での速度の値を設計者が指定するか、何らかの基準で自動的に決定します。
経由点速度のヒューリスティック
経由点 $\theta_k$ での速度 $\dot{\theta}_k$ を決める一般的なヒューリスティックは:
平均速度法: 前後の区間の平均速度の平均をとる
$$ \begin{equation} \dot{\theta}_k = \frac{1}{2}\left(\frac{\theta_k – \theta_{k-1}}{t_k – t_{k-1}} + \frac{\theta_{k+1} – \theta_k}{t_{k+1} – t_k}\right) \end{equation} $$
折れ曲がり判定: もし関節角度が経由点で方向を変える（$\theta_{k-1} < \theta_k > \theta_{k+1}$ または $\theta_{k-1} > \theta_k < \theta_{k+1}$）なら、$\dot{\theta}_k = 0$ とする。ピーク点では一旦速度0にするのが安全です。
3次スプライン補間
より洗練された方法が3次スプライン補間（cubic spline interpolation）です。各区間で3次多項式を使い、経由点で位置・速度・加速度の連続性を保証します。
$N$ 個の区間、$4N$ 個の未知数に対して: – 位置の通過条件: $2N$ 個 – 速度の連続条件: $N-1$ 個 – 加速度の連続条件: $N-1$ 個
合計 $4N – 2$ 個の条件です。残り2つの自由度は端点条件で決めます。
自然スプライン: 両端で加速度0（$\ddot{\theta}(t_0) = 0$, $\ddot{\theta}(t_N) = 0$）
固定スプライン: 両端で速度を指定（$\dot{\theta}(t_0) = v_0$, $\dot{\theta}(t_N) = v_N$）
3次スプラインは全体として $C^2$ 連続（加速度まで連続）になるため、トルクの急変がなく、滑らかな動作が得られます。ただし、1つの経由点を変更すると全区間に影響が及ぶ（大域的な性質）点に注意が必要です。
経由点を含む軌道計画は、実用上非常に重要な技術です。特に宇宙ロボットでは、障害物回避のための経由点設定が安全な運用の鍵となります。次に、宇宙環境特有の制約と考慮事項を見ていきましょう。
宇宙ロボットにおける特別な考慮事項
微小重力と反力の問題
地上のロボットは固定された台座（ベース）にボルト留めされていますが、宇宙ロボットのベースは衛星本体であり、自由に浮遊しています。ロボットアームが動くと、運動量保存則によりベース衛星は反対方向に回転します。
全系の角運動量保存:
$$ \begin{equation} \bm{H}_{\text{total}} = \bm{H}_{\text{base}} + \bm{H}_{\text{arm}} = \text{const.} \end{equation} $$
アームが時計回りにトルク $\tau$ で動けば、ベースには反時計回りのトルク $-\tau$ が作用します。これは宇宙ステーションの姿勢制御系に外乱を与えます。
軌道計画への影響: 加速度やジャークが大きいほどベースへの反力トルクが大きくなるため、宇宙ロボットの軌道計画では以下の点が重視されます。
- 加速度の制限を厳しく設定: 地上ロボットよりも小さい加速度上限を使う
- ジャークの制限: S字プロファイルの積極的採用
- 反力トルク最小化軌道: 複数関節の加速度を逆方向に組み合わせ、ベースへの正味トルクを相殺する高度な手法
関節速度・加速度・トルクの制約
宇宙ロボットの関節には以下の制約があります。
速度制限 $|\dot{\theta}_i| \leq \dot{\theta}_{i,\max}$: モータの最大回転速度に起因します。減速機のギア比が大きいため、宇宙ロボットの関節速度は典型的に数度/秒と非常に遅いです。
加速度制限 $|\ddot{\theta}_i| \leq \ddot{\theta}_{i,\max}$: モータのトルク定格と慣性モーメントの比で決まります。
トルク制限 $|\tau_i| \leq \tau_{i,\max}$: 加速度制限より厳密な制約です。関節トルクは逆動力学から計算されます。
$$ \begin{equation} \bm{\tau} = \bm{M}(\bm{\theta})\ddot{\bm{\theta}} + \bm{c}(\bm{\theta}, \dot{\bm{\theta}}) + \bm{g}(\bm{\theta}) \end{equation} $$
ここで $\bm{M}$ は慣性行列、$\bm{c}$ はコリオリ・遠心力項、$\bm{g}$ は重力項（宇宙空間では $\bm{g} \approx 0$）です。慣性行列 $\bm{M}$ は姿勢依存なので、同じ加速度でも姿勢によって必要なトルクが異なります。したがって、加速度制限だけでは不十分で、軌道全体にわたってトルク制約を検証する必要があります。
熱的制約
宇宙空間では対流冷却がないため、モータの熱放散は放射と伝導のみです。長時間の連続動作はモータ温度を上昇させ、性能低下や故障の原因となります。このため、軌道計画では:
- デューティサイクルの管理: 動作と休止のサイクルを考慮
- RMS（二乗平均平方根）トルク制限: ピークトルクだけでなく、時間平均のトルクも制限
- 動作時間の余裕: 最短時間軌道ではなく、余裕を持った時間設定
安全マージン
宇宙ロボットの運用では、地上以上に保守的な設計が求められます。修理が困難で、故障は即ミッション喪失につながるためです。一般的には:
- 関節速度の上限を定格の50〜70%に設定
- 加速度の上限を定格の30〜50%に設定
- 予定軌道の前後に停止確認区間を設ける
以上の宇宙特有の考慮事項を踏まえた上で、ここまで解説してきた各種軌道計画手法をPythonで実装し、その挙動を比較してみましょう。
Pythonによる実装と比較
3次多項式補間の実装
まずは3次多項式補間を実装します。rest-to-rest（始点・終点で速度0）の条件で、位置・速度・加速度の時間変化を描画します。
import numpy as np
import matplotlib.pyplot as plt
def cubic_polynomial(theta_0, theta_f, t_f, dt=0.001):
"""3次多項式補間（rest-to-rest）"""
h = theta_f - theta_0
a0 = theta_0
a1 = 0.0
a2 = 3.0 * h / t_f**2
a3 = -2.0 * h / t_f**3
t = np.arange(0, t_f + dt, dt)
theta = a0 + a1 * t + a2 * t**2 + a3 * t**3
dtheta = a1 + 2 * a2 * t + 3 * a3 * t**2
ddtheta = 2 * a2 + 6 * a3 * t
return t, theta, dtheta, ddtheta
# パラメータ設定
theta_0 = 0.0 # 初期角度 [deg]
theta_f = 90.0 # 目標角度 [deg]
t_f = 2.0 # 移動時間 [s]
t, theta, dtheta, ddtheta = cubic_polynomial(theta_0, theta_f, t_f)
fig, axes = plt.subplots(3, 1, figsize=(10, 8), sharex=True)
axes[0].plot(t, theta, 'b-', linewidth=2)
axes[0].set_ylabel('Position [deg]')
axes[0].set_title('Cubic Polynomial Trajectory (Rest-to-Rest)')
axes[0].grid(True, alpha=0.3)
axes[1].plot(t, dtheta, 'r-', linewidth=2)
axes[1].set_ylabel('Velocity [deg/s]')
axes[1].grid(True, alpha=0.3)
axes[2].plot(t, ddtheta, 'g-', linewidth=2)
axes[2].set_ylabel('Acceleration [deg/s²]')
axes[2].set_xlabel('Time [s]')
axes[2].grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('cubic_polynomial.png', dpi=150, bbox_inches='tight')
plt.show()
グラフを観察すると、以下の特徴が読み取れます。
- 位置はS字カーブ — 始点と終点で速度0の条件を満たすため、滑らかに立ち上がり、滑らかに着地します
- 速度は放物線 — 中間点（$t = t_f / 2$）でピーク速度 $1.5h / t_f = 67.5$ deg/s に達します。3次多項式の速度は2次関数なので、放物線を描きます
- 加速度は直線 — 初期加速度は正（加速）、終端加速度は負（減速）で、連続的に変化します。ただし、$t=0$ と $t=t_f$ で加速度が突然0にならないため、端点でジャークの不連続が生じます
5次多項式補間の実装
次に、5次多項式補間を実装し、3次との違いを確認します。
import numpy as np
import matplotlib.pyplot as plt
def quintic_polynomial(theta_0, theta_f, t_f, dt=0.001):
"""5次多項式補間（rest-to-rest）"""
h = theta_f - theta_0
a0 = theta_0
a1 = 0.0
a2 = 0.0
a3 = 10.0 * h / t_f**3
a4 = -15.0 * h / t_f**4
a5 = 6.0 * h / t_f**5
t = np.arange(0, t_f + dt, dt)
theta = a0 + a1*t + a2*t**2 + a3*t**3 + a4*t**4 + a5*t**5
dtheta = a1 + 2*a2*t + 3*a3*t**2 + 4*a4*t**3 + 5*a5*t**4
ddtheta = 2*a2 + 6*a3*t + 12*a4*t**2 + 20*a5*t**3
jerk = 6*a3 + 24*a4*t + 60*a5*t**2
return t, theta, dtheta, ddtheta, jerk
# パラメータ設定
theta_0 = 0.0
theta_f = 90.0
t_f = 2.0
t, theta, dtheta, ddtheta, jerk = quintic_polynomial(theta_0, theta_f, t_f)
fig, axes = plt.subplots(4, 1, figsize=(10, 10), sharex=True)
axes[0].plot(t, theta, 'b-', linewidth=2)
axes[0].set_ylabel('Position [deg]')
axes[0].set_title('Quintic Polynomial Trajectory (Rest-to-Rest)')
axes[0].grid(True, alpha=0.3)
axes[1].plot(t, dtheta, 'r-', linewidth=2)
axes[1].set_ylabel('Velocity [deg/s]')
axes[1].grid(True, alpha=0.3)
axes[2].plot(t, ddtheta, 'g-', linewidth=2)
axes[2].set_ylabel('Acceleration [deg/s²]')
axes[2].grid(True, alpha=0.3)
axes[3].plot(t, jerk, 'm-', linewidth=2)
axes[3].set_ylabel('Jerk [deg/s³]')
axes[3].set_xlabel('Time [s]')
axes[3].grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('quintic_polynomial.png', dpi=150, bbox_inches='tight')
plt.show()
5次多項式のグラフからは以下のことがわかります。
- 加速度が端点で0 — 3次多項式と異なり、$t=0$ と $t=t_f$ で加速度が0から始まり0に戻ります。これによりトルクの急変がなくなります
- ジャークは連続 — 加速度が端点で滑らかに接続されるため、ジャークのプロファイルも連続です。ただし端点でジャークは非ゼロです
- ピーク速度が大きい — 5次多項式のピーク速度 $1.875h/t_f = 84.375$ deg/s は、3次の $67.5$ deg/s よりも約25%大きくなっています。同じ移動量を同じ時間で実現するため、中間で「速く」動く必要があるからです
台形速度プロファイルの実装
続いて、台形速度プロファイルを実装します。最大速度と最大加速度をパラメータとして指定します。
import numpy as np
import matplotlib.pyplot as plt
def trapezoidal_profile(theta_0, theta_f, v_max, a_max, dt=0.001):
"""台形速度プロファイル"""
h = theta_f - theta_0
sign = np.sign(h)
h_abs = abs(h)
# 加速時間
t_a = v_max / a_max
# 三角形プロファイルの判定
if h_abs < v_max**2 / a_max:
# 三角形: 等速区間なし
t_a = np.sqrt(h_abs / a_max)
t_v = 0.0
v_peak = a_max * t_a
else:
# 台形: 等速区間あり
t_v = h_abs / v_max - v_max / a_max
v_peak = v_max
t_f = 2 * t_a + t_v
t = np.arange(0, t_f + dt, dt)
theta = np.zeros_like(t)
dtheta = np.zeros_like(t)
ddtheta = np.zeros_like(t)
for i, ti in enumerate(t):
if ti <= t_a:
# 加速区間
ddtheta[i] = a_max
dtheta[i] = a_max * ti
theta[i] = theta_0 + sign * 0.5 * a_max * ti**2
elif ti <= t_a + t_v:
# 等速区間
ddtheta[i] = 0.0
dtheta[i] = v_peak
theta[i] = (theta_0
+ sign * (0.5 * a_max * t_a**2
+ v_peak * (ti - t_a)))
else:
# 減速区間
t_dec = ti - t_a - t_v
ddtheta[i] = -a_max
dtheta[i] = v_peak - a_max * t_dec
theta[i] = (theta_0
+ sign * (0.5 * a_max * t_a**2
+ v_peak * t_v
+ v_peak * t_dec
- 0.5 * a_max * t_dec**2))
# 符号を反映
dtheta *= sign
ddtheta *= sign
return t, theta, dtheta, ddtheta
# パラメータ設定
theta_0 = 0.0
theta_f = 90.0
v_max = 60.0 # 最大速度 [deg/s]
a_max = 120.0 # 最大加速度 [deg/s²]
t, theta, dtheta, ddtheta = trapezoidal_profile(theta_0, theta_f, v_max, a_max)
fig, axes = plt.subplots(3, 1, figsize=(10, 8), sharex=True)
axes[0].plot(t, theta, 'b-', linewidth=2)
axes[0].set_ylabel('Position [deg]')
axes[0].set_title('Trapezoidal Velocity Profile')
axes[0].grid(True, alpha=0.3)
axes[1].plot(t, dtheta, 'r-', linewidth=2)
axes[1].set_ylabel('Velocity [deg/s]')
axes[1].grid(True, alpha=0.3)
axes[2].plot(t, ddtheta, 'g-', linewidth=2)
axes[2].set_ylabel('Acceleration [deg/s²]')
axes[2].set_xlabel('Time [s]')
axes[2].grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('trapezoidal_profile.png', dpi=150, bbox_inches='tight')
plt.show()
print(f"移動時間: {t[-1]:.3f} s")
print(f"加速時間: {v_max/a_max:.3f} s")
print(f"等速時間: {t[-1] - 2*v_max/a_max:.3f} s")
台形速度プロファイルのグラフからは以下のことが読み取れます。
- 速度が台形 — 加速区間（直線的に増加）、等速区間（一定）、減速区間（直線的に減少）の3つが明確に見えます。これは速度の変化が時間に対して区分線形であることを反映しています
- 加速度がステップ関数 — 加速区間と減速区間の切り替わりで加速度が瞬間的に変化します。$t=0$ で $0 \to +a_{\max}$、$t=t_a$ で $+a_{\max} \to 0$、$t=t_a+t_v$ で $0 \to -a_{\max}$ のように不連続です。これがジャーク無限大の原因です
- 位置は区分的な放物線と直線 — 加速・減速区間では放物線、等速区間では直線となります
S字速度プロファイルの実装
S字速度プロファイルの完全な実装を示します。7区間それぞれの時間を計算し、位置・速度・加速度・ジャークを生成します。
import numpy as np
import matplotlib.pyplot as plt
def s_curve_profile(theta_0, theta_f, v_max, a_max, j_max, dt=0.001):
"""S字速度プロファイル（7区間）"""
h = theta_f - theta_0
sign = np.sign(h)
h_abs = abs(h)
# ジャーク区間の時間
t_j = a_max / j_max
# 加速フェーズで到達する速度（加速度が台形の場合）
# 一定加速区間がある場合
v_acc = a_max * t_j # ジャーク区間のみでの速度増加
if v_max > a_max * t_j:
# 一定加速区間あり
t_a_const = (v_max - a_max * t_j) / a_max
else:
# 一定加速区間なし: 加速度が三角形
t_j = np.sqrt(v_max / j_max)
t_a_const = 0.0
a_max = j_max * t_j # 実効最大加速度
# 加速フェーズの時間
t_acc = 2 * t_j + t_a_const
# 加速フェーズでの移動量
d_acc = 0.5 * v_max * t_acc
# 等速区間の時間
if h_abs > 2 * d_acc:
t_v = (h_abs - 2 * d_acc) / v_max
else:
# 最大速度に到達しない場合（簡略化のため最大速度を下げる）
v_max = np.sqrt(h_abs * a_max - a_max**2 * t_j)
if v_max <= 0:
v_max = np.sqrt(h_abs * j_max * t_j)
t_a_const = max(0, (v_max - a_max * t_j) / a_max)
t_acc = 2 * t_j + t_a_const
d_acc = 0.5 * v_max * t_acc
t_v = max(0, (h_abs - 2 * d_acc) / v_max)
# 全移動時間
t_f = 2 * t_acc + t_v
# 時間配列を生成
t = np.arange(0, t_f + dt, dt)
# 各時刻でのジャーク・加速度・速度・位置を計算
jerk_arr = np.zeros_like(t)
acc_arr = np.zeros_like(t)
vel_arr = np.zeros_like(t)
pos_arr = np.zeros_like(t)
# 区間の境界時刻
t1 = t_j # 区間1終了
t2 = t_j + t_a_const # 区間2終了
t3 = 2*t_j + t_a_const # 区間3終了 = 加速フェーズ終了
t4 = t3 + t_v # 区間4終了 = 等速区間終了
t5 = t4 + t_j # 区間5終了
t6 = t4 + t_j + t_a_const # 区間6終了
t7 = t_f # 区間7終了
for i, ti in enumerate(t):
if ti <= t1:
# 区間1: ジャーク +j_max
tau = ti
jerk_arr[i] = j_max
acc_arr[i] = j_max * tau
vel_arr[i] = 0.5 * j_max * tau**2
pos_arr[i] = (1.0/6.0) * j_max * tau**3
elif ti <= t2:
# 区間2: ジャーク 0, 加速度 a_max
tau = ti - t1
v1 = 0.5 * j_max * t_j**2
p1 = (1.0/6.0) * j_max * t_j**3
jerk_arr[i] = 0.0
acc_arr[i] = a_max
vel_arr[i] = v1 + a_max * tau
pos_arr[i] = p1 + v1 * tau + 0.5 * a_max * tau**2
elif ti <= t3:
# 区間3: ジャーク -j_max
tau = ti - t2
v2 = 0.5 * j_max * t_j**2 + a_max * t_a_const
p2 = ((1.0/6.0) * j_max * t_j**3
+ 0.5 * j_max * t_j**2 * t_a_const
+ 0.5 * a_max * t_a_const**2)
jerk_arr[i] = -j_max
acc_arr[i] = a_max - j_max * tau
vel_arr[i] = v2 + a_max * tau - 0.5 * j_max * tau**2
pos_arr[i] = (p2 + v2 * tau + 0.5 * a_max * tau**2
- (1.0/6.0) * j_max * tau**3)
elif ti <= t4:
# 区間4: 等速
tau = ti - t3
jerk_arr[i] = 0.0
acc_arr[i] = 0.0
vel_arr[i] = v_max
pos_arr[i] = d_acc + v_max * tau
elif ti <= t5:
# 区間5: ジャーク -j_max（減速開始）
tau = ti - t4
p4 = d_acc + v_max * t_v
jerk_arr[i] = -j_max
acc_arr[i] = -j_max * tau
vel_arr[i] = v_max - 0.5 * j_max * tau**2
pos_arr[i] = (p4 + v_max * tau
- (1.0/6.0) * j_max * tau**3)
elif ti <= t6:
# 区間6: ジャーク 0, 加速度 -a_max
tau = ti - t5
v5 = v_max - 0.5 * j_max * t_j**2
p5 = (d_acc + v_max * t_v + v_max * t_j
- (1.0/6.0) * j_max * t_j**3)
jerk_arr[i] = 0.0
acc_arr[i] = -a_max
vel_arr[i] = v5 - a_max * tau
pos_arr[i] = p5 + v5 * tau - 0.5 * a_max * tau**2
else:
# 区間7: ジャーク +j_max（減速終了）
tau = ti - t6
v6 = (v_max - 0.5 * j_max * t_j**2
- a_max * t_a_const)
p6_base = (d_acc + v_max * t_v + v_max * t_j
- (1.0/6.0) * j_max * t_j**3)
v5 = v_max - 0.5 * j_max * t_j**2
p6 = (p6_base + v5 * t_a_const
- 0.5 * a_max * t_a_const**2)
jerk_arr[i] = j_max
acc_arr[i] = -a_max + j_max * tau
vel_arr[i] = v6 - a_max * tau + 0.5 * j_max * tau**2
pos_arr[i] = (p6 + v6 * tau - 0.5 * a_max * tau**2
+ (1.0/6.0) * j_max * tau**3)
# 符号とオフセットを反映
pos_arr = theta_0 + sign * pos_arr
vel_arr *= sign
acc_arr *= sign
jerk_arr *= sign
return t, pos_arr, vel_arr, acc_arr, jerk_arr
# パラメータ設定
theta_0 = 0.0
theta_f = 90.0
v_max = 60.0 # 最大速度 [deg/s]
a_max = 120.0 # 最大加速度 [deg/s²]
j_max = 600.0 # 最大ジャーク [deg/s³]
t, pos, vel, acc, jerk = s_curve_profile(
theta_0, theta_f, v_max, a_max, j_max
)
fig, axes = plt.subplots(4, 1, figsize=(10, 10), sharex=True)
axes[0].plot(t, pos, 'b-', linewidth=2)
axes[0].set_ylabel('Position [deg]')
axes[0].set_title('S-Curve Velocity Profile')
axes[0].grid(True, alpha=0.3)
axes[1].plot(t, vel, 'r-', linewidth=2)
axes[1].set_ylabel('Velocity [deg/s]')
axes[1].grid(True, alpha=0.3)
axes[2].plot(t, acc, 'g-', linewidth=2)
axes[2].set_ylabel('Acceleration [deg/s²]')
axes[2].grid(True, alpha=0.3)
axes[3].plot(t, jerk, 'm-', linewidth=2)
axes[3].set_ylabel('Jerk [deg/s³]')
axes[3].set_xlabel('Time [s]')
axes[3].grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('s_curve_profile.png', dpi=150, bbox_inches='tight')
plt.show()
print(f"移動時間: {t[-1]:.3f} s")
S字プロファイルのグラフからは以下の特徴が読み取れます。
- ジャークが区分的に一定 — $+j_{\max}$, $0$, $-j_{\max}$ の3値をとる矩形波です。これにより加速度は区分線形（台形状）になります
- 加速度が連続 — 台形プロファイルとの最大の違いです。加速度のグラフに不連続な跳びがなく、滑らかに変化しています
- 速度がS字形 — 加速度が台形なので、速度はS字を描きます。台形プロファイルの速度（直線の折れ線）と比べて、角が丸くなっています
- 移動時間が長い — 同じ最大速度・最大加速度に対して、S字プロファイルの移動時間は台形プロファイルより長くなります。ジャーク区間での加速が緩やかなためです。これは滑らかさと時間効率のトレードオフです
全手法の比較
最後に、4つの手法を同一グラフ上で比較します。
import numpy as np
import matplotlib.pyplot as plt
# --- 3次多項式 ---
def cubic_poly(theta_0, theta_f, t_f, dt=0.001):
h = theta_f - theta_0
a2 = 3.0 * h / t_f**2
a3 = -2.0 * h / t_f**3
t = np.arange(0, t_f + dt, dt)
theta = theta_0 + a2 * t**2 + a3 * t**3
dtheta = 2 * a2 * t + 3 * a3 * t**2
ddtheta = 2 * a2 + 6 * a3 * t
return t, theta, dtheta, ddtheta
# --- 5次多項式 ---
def quintic_poly(theta_0, theta_f, t_f, dt=0.001):
h = theta_f - theta_0
a3 = 10.0 * h / t_f**3
a4 = -15.0 * h / t_f**4
a5 = 6.0 * h / t_f**5
t = np.arange(0, t_f + dt, dt)
theta = theta_0 + a3*t**3 + a4*t**4 + a5*t**5
dtheta = 3*a3*t**2 + 4*a4*t**3 + 5*a5*t**4
ddtheta = 6*a3*t + 12*a4*t**2 + 20*a5*t**3
return t, theta, dtheta, ddtheta
# 共通パラメータ
theta_0, theta_f = 0.0, 90.0
t_f_poly = 2.0
# 多項式軌道
t3, p3, v3, a3 = cubic_poly(theta_0, theta_f, t_f_poly)
t5, p5, v5, a5 = quintic_poly(theta_0, theta_f, t_f_poly)
# 台形プロファイル（同じ移動量）
v_max, a_max_trap = 60.0, 120.0
t_trap, p_trap, v_trap, a_trap = trapezoidal_profile(
theta_0, theta_f, v_max, a_max_trap
)
# S字プロファイル（同じ移動量）
j_max = 600.0
t_s, p_s, v_s, a_s, _ = s_curve_profile(
theta_0, theta_f, v_max, a_max_trap, j_max
)
# 比較プロット
fig, axes = plt.subplots(3, 1, figsize=(12, 10), sharex=False)
# 位置
axes[0].plot(t3, p3, 'b-', linewidth=2, label='Cubic')
axes[0].plot(t5, p5, 'r--', linewidth=2, label='Quintic')
axes[0].plot(t_trap, p_trap, 'g-.', linewidth=2, label='Trapezoidal')
axes[0].plot(t_s, p_s, 'm:', linewidth=2.5, label='S-curve')
axes[0].set_ylabel('Position [deg]')
axes[0].set_title('Comparison of Trajectory Planning Methods')
axes[0].legend()
axes[0].grid(True, alpha=0.3)
# 速度
axes[1].plot(t3, v3, 'b-', linewidth=2, label='Cubic')
axes[1].plot(t5, v5, 'r--', linewidth=2, label='Quintic')
axes[1].plot(t_trap, v_trap, 'g-.', linewidth=2, label='Trapezoidal')
axes[1].plot(t_s, v_s, 'm:', linewidth=2.5, label='S-curve')
axes[1].set_ylabel('Velocity [deg/s]')
axes[1].legend()
axes[1].grid(True, alpha=0.3)
# 加速度
axes[2].plot(t3, a3, 'b-', linewidth=2, label='Cubic')
axes[2].plot(t5, a5, 'r--', linewidth=2, label='Quintic')
axes[2].plot(t_trap, a_trap, 'g-.', linewidth=2, label='Trapezoidal')
axes[2].plot(t_s, a_s, 'm:', linewidth=2.5, label='S-curve')
axes[2].set_ylabel('Acceleration [deg/s²]')
axes[2].set_xlabel('Time [s]')
axes[2].legend()
axes[2].grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('trajectory_comparison.png', dpi=150, bbox_inches='tight')
plt.show()
# 特性値の比較
print("=" * 55)
print(f"{'手法':<15} {'移動時間[s]':>10} {'最大速度':>10} {'最大加速度':>12}")
print("=" * 55)
print(f"{'3次多項式':<15} {t3[-1]:>10.3f} {max(v3):>10.1f} {max(abs(a3)):>12.1f}")
print(f"{'5次多項式':<15} {t5[-1]:>10.3f} {max(v5):>10.1f} {max(abs(a5)):>12.1f}")
print(f"{'台形':<15} {t_trap[-1]:>10.3f} {max(v_trap):>10.1f} {max(abs(a_trap)):>12.1f}")
print(f"{'S字':<15} {t_s[-1]:>10.3f} {max(v_s):>10.1f} {max(abs(a_s)):>12.1f}")
print("=" * 55)
比較グラフからは、各手法の特徴的な違いが明確に見えます。
- 多項式手法（3次・5次）は移動時間が固定 — 設計者が $t_f$ を指定するため、速度や加速度の上限は結果として決まります。モータの能力を最大限に活用しているとは限りません
- 台形・S字は制約ベースの設計 — 最大速度と最大加速度（S字はさらにジャーク）を指定するため、モータの能力に直結した設計ができます。移動時間は結果として決まります
- 加速度のプロファイル形状が決定的に異なる — 台形は不連続なステップ、3次多項式は直線、5次多項式は滑らかな曲線、S字は台形状の連続関数です。宇宙ロボットのように振動を嫌う応用では、この違いが大きな意味を持ちます
- S字プロファイルの移動時間が最長 — 同じ速度・加速度制限に対して、ジャーク制限が追加されるため時間的に不利です。しかし、機械的な衝撃や振動の低減という観点では最も優れています
経由点を通る3次スプライン補間の実装
最後に、複数の経由点を通る3次スプライン補間を実装します。
import numpy as np
import matplotlib.pyplot as plt
def cubic_spline_trajectory(t_points, theta_points, dt=0.001):
"""
自然3次スプライン補間による軌道生成
t_points: 経由点の時刻リスト
theta_points: 経由点の角度リスト
"""
n = len(t_points) - 1 # 区間数
h = np.diff(t_points) # 各区間の幅
# 3次スプラインの係数を求める（自然スプライン: 端点で加速度0）
# a_i = theta_i
a = np.array(theta_points, dtype=float)
# c_i を連立方程式で求める
# 三重対角行列を構築
A = np.zeros((n + 1, n + 1))
b = np.zeros(n + 1)
# 自然スプライン端点条件
A[0, 0] = 1.0
A[n, n] = 1.0
for i in range(1, n):
A[i, i-1] = h[i-1]
A[i, i] = 2.0 * (h[i-1] + h[i])
A[i, i+1] = h[i]
b[i] = (3.0 * (a[i+1] - a[i]) / h[i]
- 3.0 * (a[i] - a[i-1]) / h[i-1])
c = np.linalg.solve(A, b)
# b_i, d_i を計算
b_coeff = np.zeros(n)
d_coeff = np.zeros(n)
for i in range(n):
b_coeff[i] = ((a[i+1] - a[i]) / h[i]
- h[i] * (2*c[i] + c[i+1]) / 3.0)
d_coeff[i] = (c[i+1] - c[i]) / (3.0 * h[i])
# 軌道を生成
t_all = np.arange(t_points[0], t_points[-1] + dt, dt)
theta_all = np.zeros_like(t_all)
dtheta_all = np.zeros_like(t_all)
ddtheta_all = np.zeros_like(t_all)
for k, tk in enumerate(t_all):
# 区間を特定
seg = min(np.searchsorted(t_points, tk, side='right') - 1,
n - 1)
seg = max(seg, 0)
tau = tk - t_points[seg]
theta_all[k] = (a[seg] + b_coeff[seg]*tau
+ c[seg]*tau**2 + d_coeff[seg]*tau**3)
dtheta_all[k] = (b_coeff[seg] + 2*c[seg]*tau
+ 3*d_coeff[seg]*tau**2)
ddtheta_all[k] = 2*c[seg] + 6*d_coeff[seg]*tau
return t_all, theta_all, dtheta_all, ddtheta_all
# 経由点の設定（障害物回避を想定）
t_points = [0.0, 1.0, 2.5, 3.5, 5.0]
theta_points = [0.0, 45.0, 30.0, 80.0, 90.0]
t, theta, dtheta, ddtheta = cubic_spline_trajectory(
t_points, theta_points
)
fig, axes = plt.subplots(3, 1, figsize=(10, 9), sharex=True)
# 位置
axes[0].plot(t, theta, 'b-', linewidth=2)
axes[0].plot(t_points, theta_points, 'ro', markersize=10,
label='Via points', zorder=5)
axes[0].set_ylabel('Position [deg]')
axes[0].set_title('Cubic Spline Trajectory through Via Points')
axes[0].legend()
axes[0].grid(True, alpha=0.3)
# 速度
axes[1].plot(t, dtheta, 'r-', linewidth=2)
for tp in t_points[1:-1]:
axes[1].axvline(x=tp, color='gray', linestyle=':', alpha=0.5)
axes[1].set_ylabel('Velocity [deg/s]')
axes[1].grid(True, alpha=0.3)
# 加速度
axes[2].plot(t, ddtheta, 'g-', linewidth=2)
for tp in t_points[1:-1]:
axes[2].axvline(x=tp, color='gray', linestyle=':', alpha=0.5)
axes[2].set_ylabel('Acceleration [deg/s²]')
axes[2].set_xlabel('Time [s]')
axes[2].grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('cubic_spline_via_points.png', dpi=150, bbox_inches='tight')
plt.show()
3次スプライン補間のグラフからは以下のことがわかります。
- 全ての経由点を正確に通過 — 赤い点（via points）を滑らかな曲線が全て通っていることが確認できます。$t=2.5$ s で角度が30度に下がる「迂回」も正しく表現されています
- 速度が連続 — 経由点（灰色の縦線）で速度が連続的に接続されています。線形補間では経由点で速度が不連続になりますが、スプラインではその問題が解消されています
- 加速度も連続 — 自然3次スプラインは $C^2$ 連続なので、加速度もなめらかに変化しています。ただし、端点（$t=0$ と $t=5$）で加速度が0になるのは自然スプラインの端点条件によるものです
- 方向転換時に速度が0を通過 — $t \approx 1.8$ s 付近で速度が0になっています。これは $\theta$ が45度から30度へ反転するためで、物理的に自然な挙動です
手法選択のガイドライン
ここまで5つの手法を解説・実装しました。実際のロボット設計では、応用に応じて適切な手法を選択する必要があります。
| 手法 | 滑らかさ | 時間効率 | 計算量 | 推奨用途 |
|---|---|---|---|---|
| 3次多項式 | $C^1$ | 中 | 最小 | プロトタイピング、教育用 |
| 5次多項式 | $C^2$ | 中 | 小 | 標準的な産業用ロボット |
| 台形速度 | $C^0$（加速度） | 最高 | 小 | 高速ピック&プレース |
| S字速度 | $C^1$（加速度） | 高 | 中 | 精密機器、宇宙ロボット |
| 3次スプライン | $C^2$ | 中 | 中 | 経由点あり、溶接・塗装 |
宇宙ロボットでの推奨: S字速度プロファイルまたは5次以上の多項式が推奨されます。ジャーク制限によりベース衛星への反力トルクが緩和され、姿勢制御系への外乱が最小化されます。経由点が必要な場合は、各区間でS字プロファイルを適用するか、5次スプラインを使います。
まとめ
本記事では、関節空間における軌道計画の基礎を体系的に解説しました。
- 軌道計画の本質: 始点と終点だけでなく「どう動くか」を時間の関数として設計すること。滑らかさ、制約遵守、時間効率のバランスが鍵
- 多項式補間: 3次（4境界条件、$C^1$）と5次（6境界条件、$C^2$）。境界条件の数に応じて次数が決まる。5次多項式は加速度の連続性を保証するが、ピーク値が大きくなる
- 台形速度プロファイル: モータの能力を最大限に使う効率的な手法。ただし加速度が不連続でジャークが無限大
- S字速度プロファイル: ジャーク制限により振動を抑制。7区間構成で計算は複雑だが、宇宙ロボットのような精密応用に不可欠
- 経由点とスプライン: 複数点を通過する滑らかな軌道。3次スプラインは $C^2$ 連続を大域的に保証
- 宇宙ロボットの制約: 微小重力での反力トルク、熱的制約、安全マージンが軌道設計に大きな影響を与える
本記事では各関節を独立に計画しましたが、実際の作業では手先（エンドエフェクタ）の経路を直接指定したい場合があります。例えば「手先を直線で移動させる」「手先を円弧に沿って動かす」といった要求です。次の記事では、作業空間での経路計画と、逆運動学を用いた関節角度への変換について解説します。
次のステップとして、以下の記事も参考にしてください。
- DHパラメータと順運動学 — 関節空間と作業空間の橋渡し
- 作業空間の経路計画 — 手先の直線・円弧補間と逆運動学