経路計画の全体像と現場で使えるアルゴリズム集：離散・連続空間と動的環境への対応
エバープレイ編集部経路計画とは何か — 概要と位置づけ
経路計画（けいろけいかく、path planning）は、開始点から目的点まで移動するために「どの経路を通るか」「どのような速度・姿勢で移動するか」を決定する技術です。ロボット工学や自動運転、ドローン、物流の自動化だけでなく、ネットワークのルーティングやゲームAIなど幅広いIT分野で重要な役割を担います。問題をどうモデル化するか（離散グラフか連続空間か）、制約（障害物、速度・加速度、動的環境、複数エージェント）をどう扱うかによって手法は大きく変わります。
問題の定式化：空間と制約
経路計画を始める際にはまず「探索空間（configuration space, C-space）」と制約を定義します。C-spaceはロボットの位置・姿勢など全ての自由度を含む空間です。障害物はC-spaceの「許可されない領域（障害領域）」として表現され、許容できる経路はその外側にある点の連続的な軌跡になります。
- 離散化されたグラフ問題：グリッドマップやノード・エッジで表現。古典的な最短経路問題に還元可能。
- 連続空間問題：ロボットの形状や運動方程式（非ホロノミック制約や動的制約）を考慮する必要あり。
- 静的 vs 動的環境：動的障害物が存在する場合はオンラインでの再計画や予測が必要。
代表的アルゴリズム（離散空間）
離散グラフ上の経路計画は多くの古典アルゴリズムで解かれます。以下はよく使われるものです。
- Dijkstra：非負重みのグラフで最短経路を保証する。ヒューリスティクスを使わない全方位探索のため計算量は大きくなりがち。
- A*（エースター）：ヒューリスティック関数 h(n) を導入することで探索を誘導。h が「実際の最短距離を過小評価する（admissible）」なら最適解が得られる。
- Bellman–Ford：負の重みを含むグラフで使用。負のサイクルがあると最短経路は未定義。
- Floyd–Warshall：全点対全点の最短経路を求めるが計算量は O(n^3)。
連続空間・ロボット運動学を考慮した手法
実際のロボットでは位置だけでなく姿勢や運動方程式が重要です。代表的な方法は以下の通りです。
- ポテンシャル場法：目的地を引き寄せるポテンシャルと障害物に反発する力を定義し、勾配降下で移動する。実装が簡単だが「局所最小」に陥る欠点がある。
- サンプリングベース法：連続空間でランダムサンプリングして経路を構成する手法。代表的なものにPRM（Probabilistic Roadmap）とRRT（Rapidly-exploring Random Tree）がある。RRTは探索が広がりやすく、RRT*は漸近的最適性を持つ。
- 最適化ベース（Trajectory Optimization）：初期経路を与え、障害回避や動力学制約を満たすように連続的に最適化する。CHOMP、TrajOptなどが代表例。
- モデル予測制御（MPC）：将来の挙動を予測し、有限時間ホライズンで最適化を繰り返す方式。動的障害や制約のある実時間制御に向く。
- 動力学的手法（Kinodynamic Planning）：位置だけでなく速度・加速度などの運動方程式を直接考慮する。計画と制御を統合する必要がある。
オンライン性と動的環境への対応
動的環境では一度計画した経路がすぐに無効化されることがあります。このため再計画やローカル回避アルゴリズムが重要になります。
- 高速再計画：A*を改良したD*やD*-Liteは環境が変化した際の再計算を効率化します。
- ローカル回避：Dynamic Window Approach（DWA）やORCA（Optimal Reciprocal Collision Avoidance）など、近傍の障害物や他エージェントとの衝突を瞬時に回避する手法。
- 予測と追跡：他者の軌跡を予測して、安全余裕を持った経路を生成することが重要。
複数エージェント（マルチエージェント）経路計画
複数のロボットが同時に動く場合、衝突回避と全体最適性のバランスが難しい課題となります。
- 中央集権型：全体を一つの大きな問題として最適化。最適性は高いが計算コストが急増する。
- 分散型・優先度割当：単純でスケーラブル。優先順位を設定して順に計画するがデッドロックや非最適性の問題がある。
- Conflict-Based Search（CBS）：探索を二層構造にし、衝突を検出して制約を追加しながら解を見つける手法で、最適解を得やすい。
計算量と難しさ（理論的側面）
一般的なモーションプランニング問題は計算的に困難であり、特に高自由度（high-DOF）や動的制約を含む場合はNP困難やPSPACE困難となることがあります。これが実運用で近似手法・ヒューリスティクスが必須となる理由です。
実装上の注意点・実務的な勘所
実システムに組み込む際は以下に注意してください。
- マッピングと自己位置推定（SLAM/Localization）：正確な地図とロボットの自己位置が経路計画の前提。
- コストマップ設計：障害の幅、通行コスト、安定性などを数値化して計画アルゴリズムに与える。
- センサー誤差・遅延の扱い：予測やロバスト性を考慮する。
- シミュレーションでの検証：GazeboやCARLAなどで安全にテストを繰り返す。
- リアルタイム性：デバイス性能に合わせてアルゴリズムや解像度を調整する。
ツール・ライブラリ
実装に便利なオープンソースやミドルウェアがあります。
- ROS（Robot Operating System）：ナビゲーションスタック、MoveItなど豊富なモジュール。
- OMPL（Open Motion Planning Library）：多数のサンプリングベースアルゴリズムを提供。
- MoveIt：ロボットアームの経路計画・運動計画に特化。
- 自動運転向け：Autoware、CARLA（シミュレータ）など。
応用例と最新トレンド
経路計画は以下のような分野で進化を続けています。
- 自動運転：高解像度マップ、行動予測、MPCを組み合わせた複合システム。
- 物流ロボット：多台同時運用と効率最適化、リアルタイム再計画。
- ドローン：3D空間での衝突回避、高度・風の影響を考慮した計画。
- 機械学習の導入：学習したヒューリスティックやポリシーで高速化（学習補助型経路計画）。
まとめ：設計・選定のポイント
経路計画の手法選定は「問題の性質（離散 vs 連続、静的 vs 動的、単体 vs 多数）」「リアルタイム性」「最適性の必要度」「利用できる計算資源」に依存します。理論的な最適性を追求するのか、現場での安定動作を優先するのかを明確にし、シミュレーションと段階的な実機検証を重ねることが成功の鍵です。
参考文献
- Steven M. LaValle, Planning Algorithms（オンライン教科書）
- Path planning — Wikipedia
- A* search algorithm — Wikipedia
- Dijkstra's algorithm — Wikipedia
- Probabilistic Roadmap（PRM） — Wikipedia
- RRT — Wikipedia
- OMPL (Open Motion Planning Library)
- ROS Navigation Stack — ROS Wiki
- Zucker, et al., CHOMP: Covariant Hamiltonian Optimization for Motion Planning
- Kavraki, et al., Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces (1996)
投稿者プロフィール
