制御工学入門：軌道生成 — 最小ジャーク、5次多項式、台形速度プロファイル

経路（path）は空間上の通り道、軌道（trajectory）はその通り道をいつ、どの速度・加速度・ジャークで通るかである。同じ廊下の中心線でも、急停止できない搬送車、高速で動くアーム、コップを運ぶ協働ロボットでは許される時間軌道が違う。経路計画器が衝突しない折れ線を返しても、それを瞬時に曲がる速度指令へ変換すれば、車輪は滑り、関節は飽和し、積載物はこぼれる。軌道生成は、幾何学的な可行性を、時間と動力学を持つ実行可能性へ翻訳する層である。

本稿では台形・三角速度プロファイル、5次多項式、最小ジャークを比較し、制約、安全、ROS 2と経路計画への接続、実装の確認方法を説明する。グローバル／局所経路の役割は経路計画入門、ROS 2での時間・制御境界はROS 2入門、位置推定の遅れはVisual SLAM入門を参照してほしい。

結論：位置だけを滑らかにしても、安全な軌道にはならない

• 台形速度プロファイルは最大速度・最大加速度を明快に守りやすく、短距離では加速と減速だけの三角形プロファイルになる。加速度が段差状に変わるため、ジャークは理想化すれば無限大であり、柔軟機構や積載物には注意が必要である。
• 5次多項式は始点・終点の位置、速度、加速度を同時に満たせる。ゼロ速度・ゼロ加速度でつなげば、位置から加速度まで連続な滑らかな接続を作れる。
• 最小ジャーク軌道は \int j(t)^2dt を小さくする境界値問題で、固定時間・固定端点条件では5次多項式になる。これは「必ず最も安全」ではなく、指定した評価の下で滑らかという意味である。
• 実機では位置、速度、加速度、ジャーク、関節範囲、電流、停止距離、追従誤差、地図鮮度を同時に監視する。軌道生成器だけに衝突回避・非常停止の責任を負わせない。

経路の時間パラメータ化

空間経路を p(s)、経路に沿った進行度を s\in[0,1]、時間を t とする。時間軌道は p(s(t)) である。連鎖律から

となる。曲率の大きい場所では、同じ \dot s でも横加速度が増える。移動ロボットなら概念的に a_{lat}=v^2\kappa（\kappa は曲率）であり、許容横加速度 a_{lat,max} から v\le\sqrt{a_{lat,max}/|\kappa|} という速度上限が出る。経路の角を丸めず速度だけを上げると、追従器に不可能な操舵を要求する。

アームでも同じである。手先経路が滑らかでも、逆運動学で得た関節経路が特異点近くを通れば関節速度が急増する。軌道生成は座標系を一つ選んで終わりではなく、手先、車体、関節、アクチュエータの各制約を往復する。

台形速度プロファイル：最も実装しやすい時間則

移動距離を D、最大速度を v_{max}、最大加速度の大きさを a_{max} とする。加速時間は t_a=v_{max}/a_{max}、加速・減速だけで必要な距離は D_{tri}=v_{max}^2/a_{max} である。D\ge D_{tri} なら最大速度へ達し、巡航時間は

となる台形プロファイルになる。D<D_{tri} なら巡航が消え、ピーク速度 v_p=\sqrt{Da_{max}} の三角形プロファイルになる。

図: Duskcoil作成の概念図。理想台形では加速度の不連続によりジャークはインパルス状となる。実測波形・機械応答ではない。

台形は直感的で、PLC、モータドライバ、簡易搬送機構で広く使いやすい。一方、加速度を0から a_{max} へ瞬時に切り替える仮定は、ベルトのたるみ、ギヤのバックラッシュ、アームのたわみ、液体の揺れを励起する。ジャーク上限 j_{max} を持つS字速度プロファイルは、加速度をランプ状に変え、振動と衝撃を抑える代わりに、同じ距離・速度上限で時間が延びる。仕様に「最大加速度」しかない場合も、許容ジャークを実験で確認する価値がある。

5次多項式と最小ジャーク

1次元の5次多項式を

とする。速度、加速度、ジャークは微分して得る。始点と終点で位置・速度・加速度の6条件、すなわち p(0),\dot p(0),\ddot p(0),p(T),\dot p(T),\ddot p(T) を与えれば、6係数が決まる。よく使う静止から静止への条件 p(0)=0,p(T)=D,\dot p(0)=\dot p(T)=\ddot p(0)=\ddot p(T)=0 では、\tau=t/T として

となる。これにより位置、速度、加速度が端点で滑らかにつながる。

最小ジャークは

を、端点の位置・速度・加速度制約の下で最小化する。Euler–Lagrange方程式から6階微分がゼロとなり、解が5次多項式になる。ここで「最小」はジャーク二乗積分だけについてであり、エネルギー、最大トルク、衝突リスク、到着時刻、他者の快適性を同時に最小化する意味ではない。T を短くすると速度・加速度・ジャークが急増するため、端点が同じでも時間選択が安全性を左右する。

図: Duskcoil作成。5次多項式は端点で速度・加速度をゼロにする例で、すべての機械で台形より短時間または低エネルギーになるという実測比較ではない。

実装例：関節一軸と移動ロボット

関節一軸では、目標角 q_0 から q_f までの5次軌道をサンプル周期ごとに q_d,\dot q_d,\ddot q_d として生成し、低位の位置・速度・トルク制御器へ渡す。追従器は目標位置だけでなく望ましい速度・加速度をフィードフォワードとして使える。だが関節角を個別に補間すると、手先が障害物へ近づく場合がある。手先経路、逆運動学、関節上限、自己衝突を統合して確認する必要がある。

移動ロボットでは、A*やRRTが返す経路をスプライン等で滑らかにし、曲率に合わせて速度上限を作る。次に台形・S字・最小ジャークの時間則で v(t) を定め、差動二輪なら左右輪速度、車両なら操舵と加速へ変換する。局所コストマップに人が現れたら、元の「最小ジャーク到着」を守るより、減速・停止・再計画を優先する。経路計画入門のグローバル経路は、局所追従と安全監視を置き換えない。

ROS 2では、経路・軌道・制御コマンドの時刻を分けて扱う。未来時刻のtrajectory messageを受け取ったコントローラが、クロックの飛び・遅延・キャンセル時にどう振る舞うかを決める。古い軌道を再送して実行しない、目標置換時に速度を不連続にしない、制御周期を逃したら安全な減速へ移る、という規則が必要である。ROS 2入門のライフサイクルとハードウェア境界は、この責務を分ける助けになる。

制約と安全：軌道を捨てる判断を仕様にする

制約は単に |v|\le v_{max}、|a|\le a_{max}、|j|\le j_{max} では終わらない。関節位置範囲、トルク・電流、熱、車輪摩擦、操舵速度、荷重、視界、通信遅れ、停止距離、他者の予測不能な運動も含む。停止距離の概算を d_{stop}\simeq v^2/(2a_{brake})+v t_{latency} と書けば、速度が2倍なら制動距離項は4倍になり、検出・計算遅れの項も残ることが分かる。速度計画は地図の空きだけでなく、停止できる余裕を持つ必要がある。

軌道生成器が可行解を返せない、目標が急に変わる、自己位置が不確か、追従誤差が大きい、電流が飽和する、局所センサが障害物を検出する場合に、何をするかを決める。一般に、目標を後ろ倒しする、速度を落とす、ホールドする、制御トルクを解除する、非常停止する、操作者へ引き継ぐ、という選択がある。落下物や人への接触が起こり得る装置では、非常停止・トルク制限・機械的リミットを軌道生成のソフトウェアから独立させる。

実装チェックリスト

1. 経路と軌道、座標系、目標時刻、制御周期を区別して定義したか。
2. v,a,j の上限を実測のアクチュエータ・荷重・停止距離から設定したか。
3. 短距離で台形が三角形へ切り替わる条件、目標置換時の境界条件を試験したか。
4. 5次多項式の時間 T を短縮した際の最大速度・加速度・ジャークを検査したか。
5. 地図更新、自己位置ジャンプ、追従逸脱、通信断、障害物検出で古い軌道を破棄できるか。
6. 非常停止・トルク制限・可動域リミットが軌道生成器の故障時にも働くか。

参考資料

• Flash and Hogan, 1985: The coordination of arm movements
• ROS 2 joint_trajectory_controller documentation
• 経路計画入門
• ROS 2入門、Visual SLAM入門