ロボットを出発点から目的地へ動かすとき、最短の直線を引けばよい場面はほとんどない。壁、通れない幅、ロボットの胴体、旋回半径、自己位置の誤差、地図の古さ、人や台車の動き、停止距離を扱う必要がある。経路計画は「どこを通るか」を決めるグローバルな探索であり、「その経路をどの速度・操舵で追うか」という軌道生成・制御とは別の層である。安全なロボットは、この区別を保ちながら互いの制約を渡す。
本稿では、グリッド上のDijkstraとA、連続空間のRRT/RRT、複数問い合わせに強いPRMを比較する。SLAMで作った地図とNav2のコストマップへどう接続するか、障害物膨張、ヒューリスティック、サンプリング、計算量、安全を順に説明する。地図推定の誤差はVisual SLAM入門、ROS 2の実行構成はROS 2入門、LiDAR等の入力はセンサフュージョン入門も参照してほしい。
結論：アルゴリズム選定の前に自由空間を正しく作る
- Dijkstraは重み付きグラフの最短路を保証するが、目的地と無関係な方向にも広がる。A*は許容的ヒューリスティックで探索を目的地へ向け、同じ最適性を保てる。
- RRTは高次元・連続空間で素早く可行路を見つけやすい。RRT*はサンプルが増えるにつれて最適解へ近づくが、近傍探索と再配線の計算を要する。PRMは静的環境で多数の出発・目的問い合わせを処理しやすい。
- 地図上の障害物をロボット半径、位置誤差、停止距離で膨張させずに出た「最短経路」は、実機には衝突経路である。
- 計画器が経路を返しても安全とは限らない。地図鮮度、局所センサ、制御追従誤差、動的障害物、再計画の遅延、非常停止を独立に監視する。
問題を定義する：地図、状態、自由空間、コスト
状態空間を \mathcal X、障害物領域を \mathcal X_{obs}、通行可能な自由空間を \mathcal X_{free}=\mathcal X\setminus\mathcal X_{obs} とする。出発 x_s から目的 x_g への経路は、\sigma:[0,1]\to\mathcal X_{free} で、\sigma(0)=x_s,\sigma(1)=x_g を満たす連続写像である。二次元の点ロボットなら x=(x,y) で足りるが、車両では姿勢 \theta、速度、操舵角、アームでは全関節角を含む。状態を簡略化すれば探索は軽くなる一方、後段の制御器が実現できない曲がり方を選ぶ。
障害物膨張（inflation）は、点として地図を探索するために、実ロボットを障害物側へ畳み込む操作である。半径 r_{robot}、位置推定の不確かさ r_{loc}、追従・停止余裕 r_{safe} があれば、少なくとも概念的には
を使う。これは単なる画像処理の太線ではない。センサの死角、人が近づく速度、速度別停止距離、地図の解像度で必要余裕が変わる。余裕を過小評価すれば衝突し、過大にすれば本来通れる狭路を塞ぐ。
図: Duskcoil作成の概念図。セル、膨張幅、経路は実測地図ではない。実機では地図解像度とロボット外形・不確かさからコストを設計する。
グリッド探索：DijkstraとA*
占有地図をセルまたはグラフ G=(V,E) とし、辺コスト c(u,v)\ge0 を持たせる。Dijkstraは出発からの既知最小コスト g(n) が最小のノードを優先キューから取り出し、隣接辺を緩和する。非負コストなら、確定したノードの g は最短である。二分ヒープ実装で代表的に O((|V|+|E|)\log|V|) 程度であり、目的地の方角を知らないため、均一地図では同心円状に探索が広がる。
A*は優先度を
とする。h(n) はノードから目的地までの下界で、真の残りコストを過大評価しない許容的（admissible）ヒューリスティックなら、A*は最短路を保つ。4近傍グリッドにはマンハッタン距離、8近傍にはチェビシェフ距離やユークリッド距離が候補となる。さらに h(n)\le c(n,n')+h(n') を満たす一貫性があれば、ノードの再展開を抑えやすい。
ヒューリスティックを w>1 倍するweighted A*は、より速く目的地へ向かう代わりに最適性を緩める。災害時の緊急避難のように「少し長くても即座に可行路」が価値を持つ場面はあるが、最適性の保証を失うことを運用側が理解していなければならない。コストマップの危険セル、旋回、狭路、エネルギーを辺コストへ入れれば、距離最短でなくリスク・時間・余裕を含む最小コスト経路になる。
連続・高次元空間：RRT、RRT*、PRM
関節6軸アームの状態は6次元、車両は位置と姿勢だけでも連続量であり、細かなグリッドは爆発的に大きくなる。RRT（Rapidly-exploring Random Tree）は自由空間からランダム標本 x_{rand} を取り、最も近い木の頂点 x_{near} から一定距離だけ x_{new} へ伸ばし、衝突がなければ追加する。未探索の大きな領域へ伸びやすく、可行解を早く見つける確率的完全性（十分な標本数で解があれば見つける確率が1へ近づく）を持つ。
RRTは最初の可行路に偏り、最短性を保証しない。RRT*は新点の近傍候補から最小コストの親を選び、周囲の頂点を再配線することで、標本数が増えると最適解へ漸近する（asymptotically optimal）。その代償は近傍探索、衝突判定、再配線である。有限時間での最適性を約束するのでなく、締切に対し「どれだけよい経路を返せるか」を測るべきである。
PRM（Probabilistic Roadmap）は、多数の自由標本を取り、近傍同士を衝突のない局所経路で結んだ道路地図（roadmap）を先に作る。静的な工場、同じアームが多数のピック位置へ向かう状況では、前処理を複数問い合わせで償却できる。動的障害物が多いなら道路の辺がすぐ無効になり、更新・再接続が必要となる。狭い通路では一様サンプリングだけでは標本が入りにくく、障害物境界・経路候補周りのバイアス、タスク固有のサンプリングが有効になる。
図: Duskcoil作成の概念図。ランダム標本・接続・障害物判定を簡略化しており、実測性能や最終経路を示すものではない。
| 手法 | 空間・代表計算量 | 解の性質 | 向く状況 | 主な落とし穴 |
|---|---|---|---|---|
| Dijkstra | グラフ、O((V+E)\log V) | 最短路（非負コスト） | ヒューリスティックなし、全域コスト | 目的地以外も広く探索 |
| A* | グラフ、最悪はDijkstra同程度 | 許容hで最短路 | グリッド地図の一点問い合わせ | 過大h、不適切な近傍・コスト |
| RRT | 連続・高次元、標本数依存 | 確率的完全、最適ではない | 速い可行路、複雑な自由空間 | 細長い通路、粗い衝突判定 |
| RRT* | 連続・高次元、近傍・再配線増 | 漸近最適 | 時間をかけて改善できる | 締切、計算・メモリ |
| PRM | 前処理＋問い合わせ探索 | 十分な標本で確率的完全 | 静的環境・多数問い合わせ | 動的障害物、道路地図の陳腐化 |
SLAM、Nav2、局所計画との接続
SLAMは地図と自己位置を推定するが、経路計画器が欲しいのは時刻整合した座標変換と、占有・コストの意味が明確な地図である。ループ閉鎖で地図座標が補正され、ロボット位置が飛んだとき、古い経路をそのまま追わせれば危険になり得る。Visual SLAM入門で扱う推定不確かさ、再局在化、地図更新は、plannerの再計画条件へ渡すべき状態である。
Nav2のようなROS 2ナビゲーション構成では、グローバルコストマップで出発から目標までの大域経路を作り、ローカルコストマップと局所コントローラが直近の障害物・速度を扱う。大域A*が通路を選んでも、動く人の前で減速・停止・迂回するのは局所層の責務である。反対に局所層だけで遠い目的地を目指せば、袋小路や局所最小に迷いやすい。計画器、コントローラ、回復行動、地図更新の周期・タイムアウト・優先度をROS 2上で明示する。ROS 2入門の通信は、リアルタイム安全の代替ではない。
実装チェックと安全
最初に、ロボットの外形、積載物、センサの視野、最大速度、最大減速度、位置誤差、地図解像度を測る。次に、膨張半径とコスト減衰、未知セルの扱い、斜め移動、地図外、到達判定を設定する。探索結果は、連続的な軌道とロボットの運動学に照らして衝突検査する。グリッドの隣接セルへ瞬間移動できても、差動二輪・自動車・アームがその曲率を実行できるとは限らない。
動的障害物に対しては、センサ検出の鮮度、相対速度、停止距離、再計画時間を測り、計画器が遅れたときに進まない規則を置く。地図にない人・穴・透明物・センサ故障を「無障害物」と仮定しない。経路がなくなった、局所経路が安全でない、自己位置の共分散が大きい、地図が古い、制御追従誤差が閾値を超えた場合は、減速・停止・操作者引継ぎのどれかへ遷移する。非常停止はplannerの出力とは独立に動作する必要がある。
- 状態空間、ロボット外形、座標系、地図解像度、未知セルの意味を定義したか。
- 膨張が位置誤差・速度・停止距離を含むか。狭路で実機外形を試験したか。
- ヒューリスティックが許容的か、最適性を緩める設定を運用側が理解しているか。
- RRT/PRMの衝突判定分解能、乱数種、締切、可行解なしの挙動を記録するか。
- SLAM再局在化、地図更新、センサ欠損、動的障害物、通信遅延を注入試験したか。
- 経路なし・古い地図・追従逸脱で安全停止でき、ログで原因を追えるか。