[Rust] 四分木による経路探索

ChatGPT の勢いがすごいですね。きっと我々の仕事はなくなるのでしょう。
そんな中、空気を読まずに経路探索のアルゴリズムについて語ります。

この記事は swarm-rs というゲーム実装のノートの一部 (こちら) を翻訳し、少し肉付けしたものです。

まず前提ですが、経路探索するのは二次元の占有グリッド(Occupancy grid)の中です。
地形情報がバイナリイメージとして与えられていると考えても良いです。

経路探索のアルゴリズムとしては、ダイクストラ法とA*が有名ですが、これらについては良い教材が腐るほどあるので触れません。
ただ、どちらも探索空間をグラフとして表現することを前提とします。
占有グリッドをそのまま探索空間とすると、ゴールに至るまでのピクセルのほとんどを評価する必要があるので、探索計算が非常に長くなる傾向があります。
ここでは、バイナリイメージで与えられた探索空間をいかに探索に適した形に軽量化するかを考えます。

トライアンギュレーション

ゲームの世界ではAIの経路探索に使うデータ構造をナビゲーションメッシュなどと呼んだりします。
これはポリゴンの集まりで表現された空間の分割です。
このような分割に使われるアルゴリズムの一つがドロネー三角形分割です。
トライアンギュレーションとも呼ばれます。

探索空間が非常に小さくなり、探索を高速化することができますが、事前にドロネー三角形分割を実施しておく必要があるので、動的に変化する環境に適応するのは得意ではありません。

ドロネー三角形分割の利点は、占有グリッドの輪郭の複雑度に応じて動的にナビゲーションメッシュの細かさが変わることです。このことで、シンプルな形状には疎な三角形を使い、複雑な形状には密な三角形を使うことができ、資源の最適化ができます。
ピクセル単位のギザギザはRDPなどのアルゴリズムで単純化しておいた方が疎な三角形分割になります。

しかし、ドロネー三角形分割は計算コストが高く、毎フレーム実行することはできません。
変化する環境に適応する、特に障害物回避には使えません。

具体的な構築方法については、結局次に述べる四分木メッシュをメインに使うことにしたので詳しくは述べませんが、おおむね次のようなステップになります。

1. 占有グリッドをバイナリイメージとして用意する。(上の図では Perlin noise を使っています)
2. Marching Squares アルゴリズムで境界のピクセルを抽出する
3. RDP アルゴリズムで単純化する
4. ドロネー三角形分割を実施する (delaunator という crate を使っています)
5. 隣り合う三角形の距離に応じて探索コストを定義する

四分木メッシュ

もう一つの方法は、四分木でナビゲーションメッシュを構築することです。
(こちらの論文がベースになっています)。
四分木の利点は、本質的に局所的であることで、構築するのに隣のセルをチェックする必要がありません。
このため、占有グリッドに変化があっても、それの再計算が局所的で済むという特徴があります。

また、ドロネー三角形分割では境界を単純化しておかないと十分に疎なメッシュになりませんでしたが、四分木ではその必要はないのも良いところです。

四分木の欠点は、トライアンギュレーションに比べるとノードの数が多く、メモリ使用量と探索コストの軽量化という点では敵わないところです。

とは言え、それなりに疎な探索グラフは構築できます。

四分木メッシュの構築方法は非常にシンプルです。
下記のような再帰的アルゴリズムで実現できます。

1. 最上位のノード(マップ全てのピクセルを含む)をチェック対象のセルとする
2. チェックするセルに含まれるピクセルを全てスキャンする
3. ピクセルが一様(Homogeneous)であればそのセルを葉ノードとする
4. ピクセルが非一様(Heterogeneous)であればそのセルを四分割したノードとし、4つの子ノードをチェック対象とする
5. 2.に戻る

四分木メッシュの動的更新

四分木の構築は比較的低コストなので、毎フレーム行うこともできます。
これによって動的な障害物を回避するようなグローバルな経路探索も可能になります。^[1]

まず、セルの取りうる状態を、占有と非占有の二通りだけではなく、動的な障害物がある場合も状態に加えます。
swarm-rs では下記のような列挙型にしています。

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum CellState {
    Obstacle,
    Occupied(usize),
    Free,
    Mixed,
}

Occupied(usize) がエージェントが占有しているセルの状態です。ペイロードの usize はエージェントの id を示しています。これは経路探索で自分自身でブロックされてしまうのを回避するためです。

Mixed はノードが混ざった状態ピクセルを持ち、子ノードを持つ(非葉ノードである)ということを示します。

下の図では、緑の正方形は空いている葉で、赤い正方形は静的な障害物で占有された葉で、紫は動的な障害物(エージェント)で占有された葉です。
それぞれのエージェントは別々の経路を取る傾向が見て取れると思います。
これはお互いを障害物として経路探索から除外しているからです。

実はランダム木ベースの動的障害物回避も試したのですが、計算コストが高くなる傾向にあり、結局は四分木を使うことにしました。
特に swarm-rs では多数のエージェントをシミュレートしますので、ランダム木のようにエージェントごとに探索グラフの再構築を行うアルゴリズムはうまくスケールしません。
四分木はグローバルな探索グラフなので、エージェント間で再利用ができ、フレーム内で一度更新するだけで済みます。

変更のあったセルの効率的な更新

さらに大きなマップに対応するために、最適化をもっと進めることを考えます。

占有グリッドのほとんどのピクセルは変化しません。
エージェントがいるセルだけが変化します。
従って、変化があった部分だけを更新するようにアルゴリズムを最適化すれば、さらに高速になると考えられます。

まず、準備するのは2つのキャッシュマップです。
キャッシュマップとは、占有グリッドと同じ大きさを持つ2次元配列で、要素は前に述べた CellState です。
一つ目のキャッシュマップは現在のマップの状態を表し、二つ目のキャッシュマップは一つ前のマップの状態を表します。
これはコード中では次のような構造体で map および prev_map としてモデル化されています。

pub struct CacheMap {
    /// An internal map having the size (2 ^ toplevel) ^ 2, indicating index into [`cache_buf`]
    map: Vec<u32>,
    /// An array of actual values in [`cache_map`], extracted to reduce the pixel size.
    buf: Vec<CellState>,
    topbit: usize,
    size: usize,
    /// A history of recently updated cells for visualization
    pub fresh_cells: HashMap<[i32; 2], usize>,
    prev_map: Option<Vec<u32>>,
}

ちょっとした最適化を施してあり、キャッシュマップは CellState そのものの二次元配列ではなく、 buf: Vec<CellState> へのインデックスを u32 で表しています。これは CellState のサイズは 64bit コンピュータでは 16 バイトと大きくなりがちなので、大きな配列を避けるための indirection です。 ^[2]
例えば、 512 x 512 ピクセルのマップでは、 16 バイトの要素の配列は 4MB になります。
u32 であれば要素は 4 バイトなので 1MB で済みます。
エージェントの id が 42億を超えるような値にならない限りは、オーバーフローの心配もないでしょう。
メモリ上のマップのサイズを抑えることで、 CPU のキャッシュのヒット率を上げることを期待しています。

さて、差分の検出には、単純に現在のキャッシュマップと前のキャッシュマップを比較して、違いを見つけるということをします。
このためには全てのピクセルを比較しながらスキャンしないといけないので、マップのピクセル数に比例した計算量ですが^[3]、四分木の更新は次に述べるアルゴリズムで変更のあった部分だけに絞られます。

1. 現在のキャッシュマップにエージェントの位置を反映した状態ラベルを書き込む
2. 前のキャッシュマップと現在のそれを比較し、違いのあるピクセルを更新対象とする
3. 更新対象となるピクセルを四分木から見つける
4. 四分木の葉がピクセルではない場合、再帰的に分割してピクセル単位になるまで細分化する
5. ピクセルを新しいラベルに置き換える
6. 置き換えた結果、親のノードが一様になるかどうかを確かめる
7. 一様であればノードをマージして親ノードのみにする
8. 6. から繰り返す

次の GIF アニメでは、四分木が動的に更新されていく様子を示しています。
緑や赤で一瞬光るセルが変化のあったセルで、それを含むノードのみが再計算されています。
計算速度は、私の環境では 1ms ぐらいから 0.05ms ぐらいまで高速化されました。

脚注

1. 多くの実装では障害物回避はローカルな経路探索にすると思います。動的障害物は普通はエージェントの近くにあるので、全経路の再計算までは必要ないことがほとんどです。しかし、ここでは実装の単純化を目的にグローバルな経路探索で動的障害物回避までしてしまいます。ローカルな障害物回避についてはそのうち別記事にするかもしれません。

2. CellState が 16 バイトも使うというのは、ちょっと納得いきがたいところはありますが、Occupied(usize) というバリアントを持つことから、 usize が 64bit コンピュータでは 8 バイトであることと、識別フラグが 1 バイトであってもアラインメントで 8 の倍数に引き上げられてしまうことによります。

3. 四分木のデータ構造から違いを検出することも原理的にはできると思いますが、ここでは実装のシンプルさを優先して2つのフラットなキャッシュマップの比較を行います。また、フラットなマップの比較は CPU にとっては非常に分岐予測を利かせやすく、四分木の構築のようにランダムにメモリを飛び回るのに比べて極めて速いと考えられます。