経路計画 — 安全で滑らかな道の見つけ方
現在地からゴールまで、障害物を避けながらどう進むか。地図をグラフとして探索する A*、ランダムに木を伸ばす RRT、候補軌道をコストで選ぶ局所計画 — 自動運転の「道の選び方」を3つの道具で動かして理解します。
1. 計画は階層で解く — ルートから軌道まで
「自宅から会社まで」と「3秒後にハンドルを何度切るか」を1つの問題として解くのは無理があります。実際のシステムは計画を階層に分けます。
- 大域計画（ルート計画）：道路ネットワークのグラフ上で目的地までの経路を探索。数 km〜数百 km スケール。カーナビと同じ問題で、ダイクストラ法や A* の世界。
- 挙動計画：車線変更する・右折レーンに入る・譲る、といった離散的な判断。
- 局所計画（軌道計画）：数秒先までの具体的な軌道（位置＋速度の時系列）を生成。障害物回避と乗り心地はここで決まる。
この章では、大域で使うグラフ探索、幾何的に複雑な空間で使うサンプリング法、局所で使うコスト最適化の順に見ていきます。
2. グラフ探索 — ダイクストラ法と A*
地図をグリッドやグラフとみなせば、経路計画は「最短経路問題」です。ダイクストラ法はスタートからの距離 g が小さい順に律儀に広げるので、最短性は保証されますが全方位に膨らみます。A* はそこに「ゴールまでの残り距離の見積もり h（ヒューリスティック）」を足し、ゴールに向かう方向を優先します。
f(n) = g(n) + h(n)
g = スタートから n までの実コスト、h = n からゴールまでの見積もり。h が実コストを超えない（許容的）なら A* は最短経路を保証する
ダイクストラ vs A* — 探索の広がりを競走させる
左＝ダイクストラ（w=0）、右＝重みつき A*（f = g + w·h）。塗られたセルが展開済みノードです。w=1 の A* は最短性を保ったまま展開数が激減。w を上げると貪欲になりさらに速い一方、「コの字の罠」では袋小路に突っ込み、経路も遠回りになることがあります。
POINT — h は「嘘をつかない楽観主義者」がベスト
h がゴールまでの真のコストを決して超えない（許容的な）見積もりなら、A* は最短経路を保証しつつ無駄な展開を省ける。グリッドならマンハッタン距離や直線距離がその代表。w>1 の重みつき A* は保証を「最短の w 倍以内」に緩める代わりに速度を買う実務的なテクニック。
3. サンプリングで探す — RRT
グリッド探索は次元が上がると破綻します（位置×向き×速度…でセル数が爆発）。そこで空間をランダムにサンプリングして探索木を伸ばすのが RRT（Rapidly-exploring Random Tree）です。手順は単純：①ランダムな点を打つ → ②木の中で最も近いノードを探す → ③そこから一定距離だけ点の方向へ枝を伸ばす（障害物に当たるなら捨てる）→ ①へ戻る。
RRT — ランダムな木が狭い通路を抜けてゴールへ
木は空間の「空いている所」へ勝手に広がり、狭い通路もいつかは通り抜けます。ゴールバイアス＝サンプルをゴール自身にする確率。0% だと純粋な探査、上げるとゴールへ一直線になりやすい反面、障害物の裏に回り込みにくくなります。到達後、橙＝生の経路、緑＝ショートカット平滑化後。
注意 — RRT の経路はそのままでは走れない
RRT が返す経路はランダムの継ぎ接ぎでガタガタ。車はその場で向きを変えられない（曲率制約がある）ため、平滑化や、車両の運動モデルに沿った枝だけを伸ばす変種（kinodynamic RRT）が必須。また RRT は「見つかる」ことは保証しても「最短」は保証しない — 漸近的に最適へ近づく RRT* がその改良版。
4. 局所計画 — 候補軌道とコストマップ
走行中の軌道計画で広く使われるのが「たくさんの候補軌道を生成して、コストが最小のものを選ぶ」方式です。コストは複数の項の重みつき和で、この重みの設計がそのまま車の性格になります。
J = wobs·C障害物 + wlane·C車線 + wcurv·C曲率
障害物に近いほど、車線中心から外れるほど、急ハンドルなほどコスト増。重みのバランスが「安全・規律・乗り心地」の優先順位を決める
候補軌道とコスト — 重みを変えると選ばれる道が変わる
前方に停車車両。緑＝選ばれた軌道、赤＝衝突する候補、灰＝その他の候補。標準設定では隣車線へのスムーズな車線変更が選ばれます。wobs を下げ wlane を上げると停車車両ギリギリを通る危険な軌道に、wcurv を上げるとゆるやかな（＝早めに始める）回避になります。
POINT — コスト設計は「価値観の設計」
アルゴリズムは与えられたコストを最小化するだけ。「安全マージンと車線遵守のどちらを優先するか」「多少の急操舵を許してでも距離を取るか」を決めるのは重みを設計する人間だ。自動運転の意思決定の倫理は、実装上はこのコスト関数の中に宿る。
5. まとめ — 3つの道具の使い分け
| 手法 | 保証 | 得意な場面 | 弱点 |
|---|---|---|---|
| ダイクストラ / A* | 最短（h が許容的なら） | 道路グラフ・グリッドの大域計画 | 高次元で爆発 |
| RRT / RRT* | 確率的完全性（RRT* は漸近最適） | 駐車など幾何的に複雑・高次元 | 経路が粗く平滑化必須 |
| 候補軌道＋コスト | 候補集合内で最良 | 走行中の局所計画（リアルタイム） | 候補に無い動きは選べない |
- 計画は大域（ルート）→ 挙動 → 局所（軌道）の階層で解く。
- A* の本質は f = g + h。良いヒューリスティックが探索を「細く」する。
- RRT はランダムサンプリングで高次元・複雑空間を攻める。
- 局所計画はコスト最小化。重みの設計が車の振る舞いを決める。
一歩先へ — 実車で使われる発展形
車は横に平行移動できないため、向き・曲率まで状態に含めた ハイブリッド A*（駐車場で有名）や、走行向きに沿った候補を格子状に張る State Lattice が使われる。近年は候補選択のあと二次計画法（QP）で軌道を数値最適化して仕上げるのが定番で、さらに学習ベースのプランナが候補生成やコストそのものを置き換えつつある。