ステージ 5 · 第25章 · 理論

25. 軌道計画とロボットアーム制御

Seeed Physical AI Beginner's Course 第25章 — パスとトラジェクトリ、関節空間とデカルト空間での補間、線形・3次・5次多項式、およびフィードフォワード（モデルと重力）とフィードバック誤差補正から構成されるトルク指令。

パス vs トラジェクトリ補間フィードフォワードとフィードバック

25.1 学習目標

ユーザーが、アームがターゲット姿勢からどのように滑らかで安全な連続運動を生成しているかを理解できるようにします。

この章のあとには、次のことができるようになります：

1. 動作をパス（形状）とトラジェクトリ（タイミング）に分け、どちらがタスクによって制約されるかを説明する。
2. 関節空間補間とデカルト空間補間を選び分ける。
3. 線形・3次・5次補間の違いと、開始／停止時の挙動がなぜ重要かを説明する。
4. フィードフォワード、フィードバック、重力補償を説明し、トルク指令がどのように構成されるかを説明する。

25.2 パス vs. トラジェクトリ

自宅からオフィスまで歩くところを想像してください。

パス = あなたが通るルート（形状）

ドアを出て左に曲がり、大通りに出て、橋を渡り、右に曲がって、オフィスに到着する

速く歩いてもゆっくり歩いても、このルート自体は変わりません。

トラジェクトリ = あなたが歩くペース（時間 + 速度）

10秒でドアを出て、橋の上で1分間立ち止まり、1階の信号で30秒待つ

同じ道でも、速く歩くか遅く歩くかは違います。

本質的な違い

次元	パス	トラジェクトリ
説明	どこにいるか	どこにいついるか
定義域	空間	空間 + 時間
重視するもの	形状	時間、速度、加速度
例	「A から B までまっすぐ行く」	「A から B まで5秒で一定速度で行く」

ロボットアームへの対応付け

タスク	パス	トラジェクトリ
溶接	溶接ビードの形状	ビードに沿った移動速度
把持	A からカップの縁まで	いつ到着するか、どれくらい保持するか
塗装	塗装する領域	スプレーガンの移動速度

計画の順序：

1. まずパス（形状）を定義する

2. そのあとトラジェクトリ（いつどこにいるか）を定義する

具体例

アームがカップを点Aから点Bへ運びます。

パス（形状のみ）：

持ち上げる -> 前に伸ばす -> 下ろす

（空間的な曲線）

トラジェクトリ（時間付き）：

1秒かけて持ち上げる -> 0.5秒停止 -> 2秒かけて前に伸ばす -> 1秒かけて下ろす

（各時刻の関節角度）

パスが正しくてもトラジェクトリが間違っていることがあります（例：動きが激しすぎてカップが飛んでしまう）。トラジェクトリを正しくするには、まずパスが正しくなければなりません（パスが間違っていれば、どれだけ正確なトラジェクトリでも意味がありません）。

なぜ分けて議論するのか

段階	重視するもの
パス計画	障害物を避け、実現可能な形状を見つける
トラジェクトリ計画	動きを滑らかにし、振動や速度超過をなくす

パスは幾何学の問題であり、トラジェクトリはタイミングの問題です。

アームのトラジェクトリは「滑らか」でなければならない

単に「A から B へ」だけでなく、次のことも必要です：

• 速度が連続している（0 との間で急に飛ばない）
• 加速度が連続している（ジャークが急に出ない）
• モーターの速度制限内である
• トルク制限内である

これらはすべてトラジェクトリ計画に属します。

25.3 関節空間トラジェクトリとデカルト空間トラジェクトリ

次元	関節空間トラジェクトリ	デカルト空間トラジェクトリ
補間対象	関節角度	エンドエフェクタ姿勢
補間関数	多項式	直線 / 円弧 / 測地線
速度特性	関節速度が一定	エンドエフェクタ速度が一定

「関節 vs デカルト」は両方のレベルで議論されますが、最もよく問われるのはパスレベルです — それがアームが「何をするか」を決める鍵だからです。

パスの方が注目される理由

タスク	どのパスを選ぶか	理由
溶接	デカルト（直線）	溶接ビードが直線だから
塗装	デカルト（特定の曲線）	塗装面をきちんと覆う必要があるから
パレタイジング	どちらでも（関節空間）	終端の経路は重要でない
自由搬送	関節空間	終端の経路を気にする必要がない
ピック後の配置	関節空間	直線経路の要求がない

警告

デカルト空間トラジェクトリはタスクを記述するうえでは自然ですが、実行するうえでは計算コストが高くなります：サンプリングされた各姿勢ごとに IK を解く必要があります。デカルト空間で計画し、その結果得られる関節トラジェクトリが関節リミットを守り、特異点から離れているかを確認する — あるいは、パスをデカルト空間で生成し、タイミングを関節空間で生成します。 execute it: every sampled pose needs an IK solution. Plan in Cartesian space, then check that the resulting joint trajectory respects the joint limits and stays away from singularities — or generate the path in Cartesian space and the timing in joint space.

25.4 線形補間

関節空間トラジェクトリ：

$$\theta(t) = \theta_{start} + (\theta_{end} - \theta_{start})\,t$$

関節は 0 度から 90 度まで滑らかに動き、その途中を 10% ごとに取った値になります。

位置空間トラジェクトリ：

$$\mathbf{p}(t) = \mathbf{p}_{start} + (\mathbf{p}_{end} - \mathbf{p}_{start})\,t$$

エンドは $(0, 0, 0)$ から $(1, 0, 0)$ まで直線的に動きます；中間位置は $\mathbf{p}_{start} + s\,(\mathbf{p}_{end} - \mathbf{p}_{start})$ で表され、比率 $s$ は 0 から 1 まで変化します。

長所と短所

問題は速度プロファイルです：関節は一定の非ゼロ速度で動き、最後に瞬時に停止します。

$$\dot{\theta}(t) = \frac{\theta_{end} - \theta_{start}}{T} \ne 0, \qquad \ddot{\theta}(t) = 0$$

長所：

• シンプルで計算が容易
• 1 行のコードで実装できる
• リアルタイム性能が高い

短所：

• 開始と終了で速度が飛ぶ（そこで速度がゼロではない）
• アームが開始／停止時に「ガクッ」とする
• 高精度タスクには不向き

開始／停止時の「ガクッ」が本質的な問題です — そのため、より精密なトラジェクトリでは 3 次／5 次多項式が使われます。

25.5 多項式補間

なぜ多項式が必要なのか

本質的な違い

次元	線形	3次	5次
開始／停止時の速度	飛ぶ	0	0
開始／停止時の加速度	飛ぶ	飛ぶ	0
滑らかさ	低い	中程度	高い
計算負荷	最小	中程度	中程度
最適な用途	荒い動き	汎用	高精度

ロボットアームでの用途

シナリオ	どれを使うか
荒い搬送、パレタイジング	3次
溶接、組立	5次
協働ロボット	5次（人のそばで安定している必要がある）
高速動作	3次（十分）
研究 / デモ	5次（最も滑らか）

$$\begin{aligned} \text{線形:}\quad \theta(t) &= a_0 + a_1 t \\ \text{3次:}\quad \theta(t) &= a_0 + a_1 t + a_2 t^2 + a_3 t^3 \\ \text{5次:}\quad \theta(t) &= a_0 + a_1 t + a_2 t^2 + a_3 t^3 + a_4 t^4 + a_5 t^5 \end{aligned}$$

静止から静止への境界条件を持つ 5 次 — アームは静止状態から始まり、静止状態で終わります：

$$\begin{aligned} \theta(0) &= 0, & \dot{\theta}(0) &= 0, & \ddot{\theta}(0) &= 0 \\ \theta(T) &= 90^\circ, & \dot{\theta}(T) &= 0, & \ddot{\theta}(T) &= 0 \end{aligned}$$

6 つの条件と 6 つの未知数 $a_0, a_1, \ldots, a_5$ — 解は一意に定まります。

25.6 フィードフォワード、フィードバック、重力補償

モーターに送られるトルクは、2 つの要素の合計です：

$$\tau_{cmd} = \underbrace{\tau_{feedforward}}_{\text{事前に計算される。重力補償を含む}} + \underbrace{\tau_{feedback}}_{\text{誤差から補正され、起きた問題を修正する}}$$

フィードフォワード：モデルに基づき事前に送る

トラジェクトリとアームのパラメータが与えられれば、各瞬間にどれだけのトルクが必要かを計算できます：

モデルに含まれる項	目的
慣性項	加速時に慣性を打ち消す
コリオリ項	回転時の結合効果を打ち消す
重力項	アームが静止していても存在し、重力を直接打ち消す

重力項は特に重要です — アームは静止している間もこれを送り続けなければなりません。そうしないと重力で下に引っ張られてしまいます。

たとえ話：今日は 5 km の上り坂があると分かっているので、あらかじめ力を入れておくイメージです。

フィードバック：問題が起きたときに修正する

どれだけモデルが正確でも、誤差や外乱は存在し、→ 実際の位置はずれていきます。

フィードバックはリアルタイムで比較し、リアルタイムで補正することです：

• どれだけずれたか -> その分だけ足す（位置フィードバック）
• どれくらいの速さでずれているか -> その分だけ減衰させる（速度フィードバック）

たとえ話：今日は上り坂があると分かっているが、道に穴が空いているので — 見えたらその都度ハンドルを切って避けるイメージです。

2 つがどのように協調するか

ソース	役割	割合（典型値）
フィードフォワード	トルクの大部分	80~95%
フィードバック	小さな誤差を補償	5~20%

フィードフォワードが大部分を処理し、フィードバックが残りの小さな部分を処理する — この2つを組み合わせることで、高速かつ高精度になります。

比較

次元	フィードフォワード	フィードバック
タイミング	事前に計算する	リアルタイムで補正する
依存するもの	モデルの精度	センサの読み取り値
応答	即時	1フレーム遅れる
外乱除去	弱い	強い
含まれるもの	重力補償を含む	含まない

実際のコントローラの形

$$\begin{aligned} \tau_{cmd} &= \tau_{feedforward} + \tau_{feedback} \\ &= \underbrace{\big[ M(q)\,\ddot{q} + C(q, \dot{q})\,\dot{q} + G(q) \big]}_{\text{feedforward (incl. gravity)}} + \underbrace{\big[ K_p\,e + K_d\,\dot{e} \big]}_{\text{feedback}} \end{aligned}$$

重力はフィードフォワードの中に含まれている — 別途計算する必要はありません。

一文でまとめると

フィードフォワードはモデル（重力を含む）に基づいてトルクを事前に送出し、フィードバックは誤差に基づいてリアルタイムに補償する；重力を別に言及する必要はない — アームが静止しているときでさえ計算しなければならない、フィードフォワード内のサブ項だからです。

• パス = 幾何、トラジェクトリ = タイミング；まずパスを計画し、その後にタイミングを計画する。
• 関節空間補間は計算コストが低く特異点もない；デカルト空間補間はタスクが通常要求するものです。
• 線形補間はガクッとする；3次は速度の不連続を取り除き、5次は加速度の不連続も取り除きます。
• トルク指令は フィードフォワード（モデル + 重力）とフィードバック（誤差補正）の和 であり、重力はフィードフォワードの中に存在します。
• 次の章では、これらすべてを Pinocchio と MeshCat を用いて実際の reBot Arm モデル上で動かします。