軌道計画（スプライン・多項式補間）

パス計画と軌道計画は別問題

ロボットアームや移動ロボットの動作生成は、しばしば2段階に分かれます。まず障害物を避けて経由点の並びを決める「パス計画」（幾何学的な経路のみを扱い、時間の概念を持たない）、次にその経路上を実際にいつ・どの速度で通過するかを決める「軌道計画（トラジェクトリプランニング）」です。軌道は位置の時間関数 q(t) として表され、その1階微分が速度 q'(t)、2階微分が加速度 q''(t)、3階微分が加加速度（jerk）q'''(t) になります。軌道計画の本質は、この q(t) を経由点群という離散的な拘束条件から滑らかな連続関数として構成することです。

なぜ滑らかさが要るのかは物理的に明確です。加速度が不連続に飛ぶ点では、理想的にはモータートルクが階段状に変化し、実機では有限の応答帯域・ギアのバックラッシ・構造のたわみが振動として励起されます。加加速度が大きいほど、乗員や積載物への衝撃、機構の疲労、追従誤差も増えます。したがって軌道計画は「経由点を通る」という幾何条件と「なめらかに動く」という力学条件を同時に満たす関数を設計する問題です。

3次多項式補間とその限界

もっとも単純な区間補間は3次多項式です。始点・終点の位置と速度という4つの条件に対し、係数が4個ある3次式 q(t) = a0 + a1 t + a2 t² + a3 t³ はちょうど過不足なく決まります（線形方程式を解くだけ）。

区間ごとの3次多項式（1区間の例）:

  拘束条件（4つ）:
    q(0)  = q0     q(T)  = qf
    q'(0) = v0     q'(T) = vf

  未知数（4つ）: a0, a1, a2, a3
  → 4元連立1次方程式で一意に解ける

複数の経由点を3次多項式でつなぐ場合、各区間で始点速度・終点速度を指定すれば連続な軌道が作れますが、素朴に「経由点で速度ゼロ」とすると経由点ごとに一旦静止してしまい非効率です。速度を非ゼロに設定して区間をつなぐことは可能ですが、加速度は各区間内で一定の傾き（線形）にしかならず、区間の境界で加速度が不連続に飛びます。これは3次式の2階微分が1次式（区間ごとに勾配が違う）にしかならないためで、原理的な限界です。

5次多項式と3次スプラインで加速度まで連続にする

加速度まで連続にするには、拘束条件を増やして次数を上げる必要があります。各経由点で位置・速度・加速度の3条件を指定するなら、1区間あたり6つの拘束条件が必要になり、5次多項式 q(t) = a0 + a1 t + ... + a5 t⁵（係数6個）がちょうど条件数と一致します。5次多項式は2階微分（加速度）まで区間内で滑らかな2次式となり、始点・終点で加速度の値を指定して隣接区間と一致させれば、軌道全体で加速度連続（C²連続）が実現します。

一方、多数の経由点を通す場合に実務でよく使われるのが3次スプラインです。各区間は3次多項式ですが、3次多項式そのものの4自由度を使い切るのではなく、区間同士で位置・速度・加速度が一致するという拘束を課すことで、区間ごとに独立には決めず全区間を連立して解きます。

3次スプラインの拘束条件（内部経由点 i、区間 i-1 と i の境界）:

  位置:     Q_{i-1}(t_i) = Q_i(t_i) = q_i    ← 経由点を通る
  速度連続:  Q'_{i-1}(t_i) = Q'_i(t_i)         ← 速度が滑らかにつながる
  加速度連続: Q''_{i-1}(t_i) = Q''_i(t_i)       ← 加速度が滑らかにつながる

  端点（両端）の速度 or 加速度を追加で指定 → 連立方程式が一意に解ける

3次スプラインは、各区間の境界で位置・速度・加速度が一致するという条件を全区間分並べると、未知数（各区間の多項式係数、または等価に各経由点での速度・加速度）に対して連立1次方程式が組め、三重対角行列を解く形に帰着します。これは数値的に安定かつ高速に解けるため、多点経由の軌道生成で広く使われます。

台形速度プロファイルとS字（加加速度制限）プロファイル

多項式補間が「位置の関数形」から出発するのに対し、産業用ロボットやCNCで広く使われるのが速度プロファイルから設計する方式です。もっとも基本的なのが台形速度プロファイルで、加速→定速→減速の3区間から成り、加速度は一定値を取ります。

台形速度プロファイル（3フェーズ）:

  フェーズ1（加速）: 速度が0から v_max まで一定加速度 a_max で増加
  フェーズ2（定速）: v_max を維持
  フェーズ3（減速）: v_max から0まで一定加速度 -a_max で減少

  移動距離が短い場合はフェーズ2が消え「三角形速度プロファイル」になる

台形プロファイルは実装が単純ですが、加速度自体は各フェーズの切り替わり（速度線図の角）で瞬時にステップ変化します。これは前述の3次多項式と同じ弱点で、**加速度は連続でも加加速度は無限大（デルタ関数的）**になり、機構への衝撃は解消されません。

これを解決するのが**S字プロファイル（加加速度制限プロファイル、jerk-limited profile）**です。加速度自体にも台形形状を持たせ、加加速度を有限値 j_max に制限した7フェーズ（加加速度一定で加速度が立ち上がる→加速度一定→加加速度が逆符号で加速度が定速に収束→…と対称に7段階）で速度を変化させます。

S字（7フェーズ）プロファイルの加加速度パターン:

  フェーズ1: jerk = +j_max   （加速度が0からa_maxへ立ち上がる）
  フェーズ2: jerk = 0        （加速度 a_max を維持、速度が線形に増加）
  フェーズ3: jerk = -j_max   （加速度がa_maxから0へ収束、速度が目標値に漸近）
  フェーズ4: jerk = 0        （定速走行）
  フェーズ5〜7: フェーズ1〜3の減速版（符号反転で対称）

時間最適化と滑らかさのトレードオフ

軌道計画には常に「できるだけ速く動きたい」という時間最適化の要求と、「できるだけ滑らかに動きたい」という滑らかさの要求が同居し、この二つは本質的にトレードオフの関係にあります。

移動距離と経由点列が固定されているとき、動作時間を切り詰めれば、有限時間内により大きな速度変化を詰め込む必要が生じ、必要な最大加速度・最大加加速度は増大します。逆に加加速度の上限を厳しく制限すれば、同じ距離を移動するのに必要な時間は必ず伸びます。これはS字プロファイルの各フェーズの継続時間が a_max と j_max の比で決まることからも分かります。

時間最小化問題の典型的な定式化:

  最小化: 総動作時間 T
  制約:   |q'(t)|  ≤ v_max   （速度制約）
          |q''(t)| ≤ a_max   （加速度制約 = トルク・推力の限界に対応）
          |q'''(t)| ≤ j_max  （加加速度制約 = 衝撃・振動の限界に対応）
          q(0) = q_start, q(T) = q_goal, 経由点をすべて指定時刻までに通過

多くの実装では、まず速度・加速度・加加速度それぞれの制約を満たす最短時間を区間ごとに計算し、そのうち最も厳しい（時間のかかる）制約が支配的な区間の所要時間を採用します。産業用ロボットの多くは、各関節軸で独立にこの時間最小化を行った後、全軸のうち最も遅い軸の所要時間に他の軸を同期させる（タイムスケーリング）ことで、複数軸が同時に動作を終える協調動作を実現します。

まとめ

軌道計画は、経由点という離散的な幾何条件から、位置・速度・加速度・加加速度が求める滑らかさの水準を満たす連続関数 q(t) を構成する問題です。3次多項式は実装が単純な反面、加速度が経由点で不連続になり、5次多項式や3次スプラインを使うことで加速度まで連続な軌道が得られます。速度プロファイルの観点からは、台形速度プロファイルが加速度をステップ変化させてしまうのに対し、S字（加加速度制限）プロファイルは加加速度そのものに上限を課すことで機構への衝撃を抑えます。そしてどの方式を選んでも、動作時間を切り詰めることと軌道を滑らかに保つことは表裏一体のトレードオフであり、速度・加速度・加加速度それぞれの制約のうち最も厳しいものが実際の所要時間を決めます。この時間最適化と滑らかさの両立こそが、ロボットアームの高速ピッキングから精密搬送、協調多軸動作まで、実用的な軌道生成アルゴリズム設計の中心課題です。