Recapt: 点群衝突検出の高速化と Rust による実装改善

ロボット制御において、**構成妥当性検証（Collision Detection）**は運動計画のボトルネックとなることがあります。点群データを扱える高速なデータ構造「Recapt」を開発・再実装し、その技術的詳細を解説します。

1. 背景：衝突検出の問題定義

ロボットを起点から目標へ導く際、経路上での衝突回避が必須です。一般的なアルゴリズムは構成（Configuration）をサンプリングし、幾何形状との干渉をチェックします。

• 入力データ: センサーによって得られる環境の点群（Point Cloud）。
• 解決アプローチ: ロボットの幾何形状を多数の球体として簡略化し、「球体同士の衝突」を検出。
• 課題: 与えられた点リスト

  P

  と球体集合

  S

  に対して、最小限の時間で衝突判定を行う必要がある。

2. CAPT の経緯と限界

筆者が以前提案したCAPTは、R ツリーに類似する近傍探索構造であり、バッチ並列検索および SIMD 加速に対応していました。しかし、以下の理由から構築時間がボトルネックとなりました。

• 高密度点群での課題: 遡回（Backtrack）を避けるために重複データを多数必要とし、メモリフットプリントが膨大になります。
• 計算複雑性: 構築は

  O(N log N)

  のコストを持ちます。
• 実用性: 制御ループ周波数でのリアルタイム計画処理には不向きです。

3. Chen & Yeh の Voxel ベースアプローチ (MVT)

Chen（陳）と Yeh（葉）は、CAPT の課題を解決するために近傍探索ツリーを廃止し、体素（Voxel）グリッドを採用しました。

• 構造: 空間をvoxels に分割し、各 voxel 内に含まれる点のリストを格納。
• 利点:
  • クエリ対象の位置判定が単純な算術演算で可能。
  • 隣接 voxel の検索のために点を重複させる必要がない。
• データ構造 (MVT):
  • Naive な全 voxel 保存はメモリ不足になるため、**占有されている voxels だけを疎な木（Sparse Tree）**に格納。
  • 3 レイヤーの構造を持ち、正交軸に揃った境界ボックステストを組み合わせた**Multilevel Voxel Table (MVT)**を実現。
  • SIMD による並列処理が可能で、定数倍の高速化を達成。

C++ から Rust への実装改善

元の C++ 実装には以下の難問がありました。

• 複雑なポインタ構造:

  uint32_t***

  などの多重ポインタを使用し、メモリ管理が困難で不安定。
• 手動プール管理: 大規模点群でクラッシュを起こしやすい。

Rust 版では構造を簡素化し、安全性と効率性を向上させました。

元の C++ 実装例（複雑なポインタ配列）

struct MVT {
    pointers to voxel indices
    using ZLevelTable = uint32_t*;
    pointers to z-level tables
    using YLevelTable = uint32_t**;
    pointers to y-level tables
    using XLevelTable = uint32_t***;

    XLevelTable x_level_table;
}

Rust 版実装（平坦な配列と Box）

データを

Box<[u32]>

struct Mvt {
    // グリッドインデックスから voxel データを取得するためのテーブル
    tables: Box<[u32]>,
    // `points` 中の点リストを取得するための voxel インデックス
    voxels: Box<[u32]>,
    // すべての点データを平らに格納した SoA（Structure of Arrays）共有バッファ
    points: [Box<[f32]>; 3], 
}

struct Voxel {
    offset: u32,       // voxel の開始インデックス
    count: u32,         // 含まれる点数
    points: [Vec<[f32]>; 3], // SoA バッファ（各次元独立）
}

• 可変性の追加:

  Mvt

  から共有バッファを分離し、個別の

  Vec

  を採用。これによりメモリ使用量は約 2 倍、構築時間は約 1.5 倍増加しますが、パフォーマンス向上というメリットがあります。
• 提供形態: 高速な不変型 (

  Mvt

  ) と可変型 (

  MutableMvt

  ) の両方を提供し、用途に合わせて最適化可能にしました。

4. Voxel サイズの最適化

Voxel のサイズは性能を左右します。

• 大きすぎると: 遠くの点を検索する際に不要な計算が発生。
• 小さすぎると: 多数の微小 voxel に対するフィルタリング処理が増加。

推奨される半径戦略

• r_base

  : ロボット全体の最大の球体。

• r_link1

  : ベースリンクを無視し、移動リンクのみを対象に。

• r_link2

  : バウンディングボリュームから解析して選定。

Chen & Yeh の論文は汎用的な

r_base

• Fetch, Panda, UR5:

  r_base

  も十分機能するが、必ずしも最適ではない。
• Baxter:

  r_base

  を使用するとクエリ時間が 20 倍も悪化。Baxter 専用ベンチマークが不足していたことが原因と推測される。
• 普遍的な結論: すべてのロボットで最適な Voxel 幅は、点群フィルタリングプロセスの影響を考慮し、概ね10〜20 cmの範囲に収まる傾向がある。

5. ベンチマーク結果

運動計画問題に対する全体的なパフォーマンス向上を実証しました。多数の衝突チェックを再生し、各データ構造のスループットを計測。

O(N)

• k-d ツリーと比較しても、MVT は構築コストの低さとメモリエフィシエンスで優位性を示しました。

6. まだ解決されていない課題

MVT は高速、軽量、管理が容易という点で魅力的ですが、依然として根本的な課題が残っています。

• 非視認性の無視: CAPT の最大の問題（構築時間）は解消されましたが、点群データそのものが不完全な問題は無効化されていません。
  • カメラからのため、オブジェクトの裏側や隠れた領域が含まれていません。
  • 計画アルゴリズムは通常、「見えない＝安全」と仮定しますが、これは危険です（例：octomap のような非視認性マップが必要）。
• トレードオフ:

  • octomap

    : 非視認性を扱えるが非常に遅い。

  • MVT/CAPT

    : 高速だが非視認性を無視。 ユーザーは「安全か、速度か」に直面しており、現状では妥協が必要です。
• 認識データの限界: ロボットが動作する世界は完璧な静的環境ではありません。「悪い」レベルには達しないこともあるため、計画アプローチは常に現実世界の近似値と向き合う必要があります。点群からの計画問題は、依然として未解決のままです。