動作計画（RRT・PRM）

なぜ格子探索では足りないのか

ロボットアームや移動ロボットの動作計画は、障害物を避けながらスタート姿勢からゴール姿勢まで、ロボットが取りうる**配置空間（コンフィギュレーション空間、C空間）**上で経路を求める問題です。C空間の次元はロボットの自由度（関節数）と一致し、6軸アームなら6次元、移動ロボット単体でも姿勢を含めれば3次元になります。

A*やダイクストラ法のような格子探索は、この空間を格子状に離散化し、各格子点をノードとしてグラフ探索を行えば最短経路を保証します。しかし格子点の数は次元数に対して指数的に増えます。1軸あたり100分割しても、6次元なら100の6乗、すなわち1兆ノードに達し、メモリにも計算時間にも収まりません。これが次元の呪いで、産業用アームのような多自由度ロボットでは格子探索は事実上使えません。

サンプリングベース動作計画は、空間全体を網羅的に離散化する代わりに、C空間からランダムに点をサンプリングし、それらを衝突のない直線（局所プランナ）でつなぐことで経路を探します。空間全体を陽に表現しないため、次元が上がっても衝突判定の回数さえ許容できれば計算量が破綻しにくいのが利点です。代表的な2手法が**RRT（Rapidly-exploring Random Tree、急速展開ランダム木）とPRM（Probabilistic Roadmap、確率的ロードマップ）**です。

RRT：スタートから木を急速展開する

RRTは、スタート点を根としてツリー（木構造）をC空間内に伸ばしていくことで経路を探します。1回のクエリ（スタートとゴールの組）ごとに専用の木を1本作る、使い捨て型のアルゴリズムです。

RRT の擬似コード:

  T ← 木を初期化、根 = スタート姿勢

  repeat（規定回数または解が見つかるまで）:
    1. x_rand ← C空間からランダムに1点サンプリング
       （一定確率でゴール点そのものをサンプリングするゴールバイアスを使うことが多い）
    2. x_near ← 木 T の中で x_rand に最も近いノード
    3. x_new  ← x_near から x_rand の方向へ、
                 ステップ幅 delta だけ進んだ点
    4. x_near → x_new の線分が衝突しなければ:
         x_new を木 T に追加（x_near を親とする辺を張る）
         x_new がゴール近傍に達したら経路を復元して終了

要点は手順3の「ステップ幅を制限して伸ばす」操作です。ランダム点そのものへ木を伸ばすのではなく、既存の木から一定距離だけ進んだ点を新ノードとするため、木は空間の未探索領域へ向かって偏りなく急速に広がります。ランダム点は空間全体から一様に選ばれるため、木が疎な（まだ広がっていない）領域ほど「最も近いノード」に選ばれやすくなり、結果として木は自然に空間全体を埋めるように成長します。これがRRTの「急速展開」という名前の由来です。

実務ではさらに、得られた経路の折れ線をなめらかにする**経路平滑化（ショートカット法やスプライン化）**を後処理として必ず行います。RRTが返す経路はステップ幅delta刻みのジグザグな折れ線であり、そのままではロボットの実行に適さないためです。

PRM：ロードマップを事前に作っておく

PRMは、クエリ（スタートとゴールの組）に依存しないロードマップをあらかじめ空間全体に構築し、個々のクエリではロードマップ上のグラフ探索だけを行う、2段階のアルゴリズムです。

PRM の擬似コード（学習フェーズ）:

  V ← 空集合（ノード集合）, E ← 空集合（辺集合）

  repeat（規定数のサンプルに達するまで）:
    1. q ← C空間からランダムに1点サンプリング
    2. q が衝突していなければ q を V に追加
    3. q の近傍ノード群（k近傍や半径r以内）それぞれについて:
         q との間の線分が衝突しなければ、その辺を E に追加

  → グラフ G = (V, E) がロードマップとして完成

クエリフェーズ（スタート・ゴールごとに毎回）:
  1. スタート点・ゴール点をそれぞれロードマップに接続
  2. グラフ G 上で A* やダイクストラ法により最短経路を探索

学習フェーズはクエリと無関係にオフラインで一度だけ行えるため、同じ環境（工場の棚配置など障害物が変わらない環境）で何度も経路計画を行うロボットアームのような用途に向きます。クエリフェーズはロードマップという疎なグラフ上の探索に帰着するため高速です。

PRMの弱点は狭い通路問題です。2つの部屋を結ぶ細い通路のような領域は、ランダムサンプリングでは衝突しない点が生成される確率自体が低く、ロードマップに反映されにくくなります。対策として、障害物近傍を優先的にサンプリングするガウシアンサンプリングやブリッジサンプリング（障害物内の2点の中点を試す手法）など、一様サンプリングを偏らせる派生手法が使われます。

完全性という評価軸

動作計画アルゴリズムの理論的性質を比較するとき、**完全性（completeness）**という概念が軸になります。

RRT・PRMはいずれも確率完全性しか持ちません。つまり「解が存在するのに永遠に見つからない」ことは実用上は稀ですが、理論上は有限時間内に解を発見できるという保証がない点、また基本形は最短経路を返す保証がない点が、Aのような格子探索との決定的な違いです。Aは探索空間を格子に落とし込んだ時点でその格子上の最短性は保証しますが、格子の粗さが真の最適解を歪める点、そして先述の次元の呪いにより高次元では現実的な時間で終わらない点が弱点です。RRT*/PRM*は確率完全性に加えて漸近最適性を持ちますが、有限サンプルでの最適性は依然として保証されません。

まとめ

RRTとPRMは、C空間を陽に離散化せずランダムサンプリングと局所的な衝突判定だけで経路を探すことで、格子探索が次元の呪いに阻まれる多自由度ロボットの動作計画を現実的な計算量で解く手法です。要点は、(1) RRTはスタートから木を急速展開する使い捨て型でオンラインの1回クエリに向くこと、(2) PRMはクエリ非依存のロードマップを事前構築し同一環境での繰り返しクエリに向くこと、(3) 両者ともサンプル数を無限に増やせば解を見つける確率完全性しか持たず、Aが持つ解像度完全性や最短性の保証は基本形にはないこと、(4) RRT/PRMはrewireや遅延評価などの派生で漸近最適性や高速化を獲得してきたこと。狭い通路問題や非ホロノミック拘束といった実務上の壁を理解した上で使い分けることが、実機への適用では欠かせません。