点群の時代はまだ終わっていない
We're not done with point clouds (claytonwramsey.com)
要約
この記事では、ロボット工学における衝突判定のための新しいデータ構造「MVT(Multilevel Voxel Table)」を紹介しています。従来のCAPT構造が抱えていた構築時間とメモリ使用量の問題を解決するため、MVTはボクセルグリッドとスパースツリーを採用し、高速かつメモリ効率の良い衝突判定を実現します。Rustでの実装例や、ボクセルサイズの選択に関する考察も含まれています。
全文翻訳
点群の時代はまだ終わっていない
問題解決を待っていると、他の誰かがそれを解決してくれるかもしれません。少なくとも、シンクの皿についてはそう自分に言い聞かせています。
ウィーンでの会議中、ちょうど私のためにその仕事をしてくれた研究者たちを見つけました。彼らは私が2年前に発表した研究を基に、ほとんどすべてのベンチマークで私を凌駕しました。この記事は、彼らの業績に注目を集め、そして少し私利私欲のために、彼らの研究を再実装する過程で私が学んだことについて語るために書いています。要するに、彼らは点群との衝突判定のためのデータ構造を作成し、それは非常に高速に動作し、メモリと構築時間も非常に安価です。
詳細に興味がない場合は、論文またはオリジナルのC++実装に直接ジャンプできます。私は独自の最適化を施したRust実装も公開しており、GitHubでソースコードとcrates.ioのパッケージがあります。
再確認
[画像: Panda robot]
[画像: Spherized panda robot]
Fanka Emika Pandaロボットとその球体化された衝突判定表現。
私は多くの時間をモーションプランニング、つまりロボットが開始状態から目標状態へ衝突のない動きを見つける方法について考えて費やしています。モーションプランニング問題を解決する方法は無数にありますが、十分な論文を読めば、それらはすべて似たようなものに見えます。いくつかのコンフィギュレーションをサンプリングし、それらが有効かどうかをテストし、すべての可能なコンフィギュレーションに対して大規模なパス検索を試みます。それらのアルゴリズムのすべてがコンフィギュレーション検証を必要とします。つまり、ロボットのコンフィギュレーション 𝑞 が与えられたときに、位置 𝑞 にあるロボットが世界のジオメトリと衝突するかどうかを判断します。ロボットは知覚された環境で動作することが多いため、その世界のジオメトリは通常、点群として私たちに提供されます。ロボットのジオメトリが球体の集まりに単純化されている場合、問題を球体衝突判定にさらに単純化できます。つまり、任意のコンフィギュレーションに対して、ロボット上の球体が知覚された点群と衝突するかどうかをチェックするだけです。
問題定義: 与えられた 𝑛 個の点 𝑃 と球のセット 𝑆 に対して、最小時間で 𝑆 のいずれかの球が 𝑃 と衝突するかどうかを判断する。
数年前、私はCAPTと呼ばれるデータ構造を提案しました。これは、点群に対するコンフィギュレーション検証を非常に高速に行うように設計されています。要するに、それは球体と点群間の衝突チェッカーです。これは 𝑘
-dツリーに似た最近傍探索構造ですが、構築時に追加の作業を行い、検索ツリーのバックトラッキングを回避します。その結果、SIMDで高速化された分岐なしクエリをサポートする、バッチ並列検索アルゴリズムを備えた 𝑘
-dツリーが得られます。
CAPTの大きな問題は構築時間でした。密な点群は、バックトラッキングを回避するために多くの重複データが必要になります。点群が十分に密になると、CAPTの構築は 𝑂(𝑛2) にスケールし、これはユーザーが制御ループ周波数でプランニングを行うという希望にとっては壊滅的です。CAPTのデータレイアウトでは、検索ツリーの各リーフ(空間のある領域を表す)が、点群の多くの点の重複コピーを格納する必要があります。それらの重複コピーはデータ構造のフットプリントを支配し始め、それが構築時間を膨張させます。
箱の中で考える
[画像: Voxel-grid collision checking]
Chen and Yehによる、ボクセルベースの衝突判定スキーム。
他のロボット工学の研究者であるChing ChenとTsung-Tai Yehは、自分たちのためにCAPTの問題を解決することにしました。そのために、彼らは最近傍探索ツリーを完全に捨てることから始めました。空間分割ツリーの代わりに、空間をボクセルのグリッドに分割し、それぞれがそれらに含まれる点のリストを格納できます。ここでの利点は二重です。第一に、単純な算術演算でクエリ球がどのボクセルにあるかを判断でき、第二に、隣接するボクセルを見つけることは自明なので、点の重複を避けることができます。
しかし、ワークスペースのすべてのボクセルを単純に格納するだけではうまくいきません。ワークスペースが各次元で100ボクセル長の場合、フィルタリング後に数千点しか含まない単一の点群を記録するために、100万ボクセルの情報を格納する必要があるでしょう。これを制御下に保つために、ChenとYehは、各レイヤーが1つの次元でセグメント化された3層のスパースツリーに、占有されたボクセルのみをスパースに格納します。
いくつかの軸配置バウンディングボックステストと組み合わせると、結果の構造は多レベルボクセルテーブル(MVT)になります。CAPTと同様に、MVTは単一命令複数データ(SIMD)並列処理を使用して並列化できます。任意のボクセルに対して、衝突チェッカーは、ボクセルに含まれるすべての点との大規模なバッチチェックを実行でき、一定の速度で高速化できます。
平坦な層のパッチ適用
ボードのように平坦
MVTの元の実装には、いくつかの厄介なC++の癖がありました。具体的には、ボクセルテーブルはテーブルの各行へのポインタのタペストリーを使用していました。これは一般的に少し不安定であることに加えて、メモリ管理を非常に困難にし、サイズ効率もあまり良くありませんでした。元のC++実装には、多くの奇妙な手動プール管理があり、点群が大きすぎると壊滅的なクラッシュを引き起こします。
struct MVT {
// ボクセルインデックスへのポインタ
using ZLevelTable = uint32_t*;
// zレベルテーブルへのポインタ
using YLevelTable = uint32_t**;
// yレベルテーブルへのポインタ
using XLevelTable = uint32_t***;
XLevelTable x_level_table;
}
Rustでの実装を容易にするために、少し単純化しました。すべてをBox<[]>でバックします。
struct Mvt {
/// グリッドインデックスからボクセルデータを取得する場所を示す
tables: Box<[u32]>,
/// `points`内の点リストを取得する場所を示す
voxels: Box<[u32]>,
/// すべての点データのフラット化されたSoA共有バッファ
points: [Box<[f32]>; 3]
// その他のフィールド...
}
struct Voxel {
/// このボクセルに格納されている最初の点のインデックス (`points`内)
offset: u32,
/// ボクセル内の点の数
count: u32,
/// その他のフィールド...
}
検索ロジックは非常にシンプルになります。tablesを使用して、どのボクセルに属するかを判断し、voxelsでルックアップします。次に、ボクセルを使用して、衝突判定を行う必要があるpointsのどの範囲を見つけるかを確認し、最後にそれらの点に対して総当たりチェックを実行します。
可変性の獲得
検索ロジックを大幅に単純化するだけでなく、新しい検索構造により、各Voxelに独自の点のフィールドを与えることで、MVTを可変にするのが簡単になります。これは、単一の大きなバッファを共有するのではなく、各Voxelに独自の点のフィールドを持たせることで実現されます。
// `points`は`Mvt`から削除されました
struct Voxel {
/// このボクセルに格納されている最初の点のインデックス (`points`内)
offset: u32,
/// ボクセル内の点の数
count: u32,
/// このボクセル内の点のSoAバッファ
points: [Vec<[f32]>; 3]
// その他のフィールド...
}
可変性を追加すると、サイズが約2倍、構築時間が約1.5倍になりますが、便利な機能です。Mvtを単一使用構造として使用するユーザーのために良好なパフォーマンスを維持するために、実装を分割しました。不変のデフォルトMvtとMutableMvt構造の両方を記述しました。
大きなボール、大きな問題
[画像: The spherized model of a robot]
[画像: Robot sphere sizing]
Fetchロボットの球体化(左)と、それぞれ赤、緑、紫で示された半径 𝑟max、𝑟mobile、𝑟query(右)。
MVTを構築するには、ボクセルのサイズを決定する必要があります。ボクセルが大きすぎると、衝突判定クエリは遠くの点を検索するのに無駄な時間を費やしますが、ボクセルが近すぎると、クエリは多数の小さなボクセルに対してフィルタリングする必要が生じます。
しかし、いくつかの妥当な候補があります。各ロボットの球体化されたジオメトリで、最大の球体(半径 𝑟max)を取り出すことができます。あるいは、ロボットの移動リンクのみに限定し、ほとんどのロボットのベースリンクにある大きな球体をスキップすることで、𝑟mobile を得ることができます。最後に、ロボットのバウンディングボリューム階層を見て、衝突判定で使用される最大の球体のサイズである 𝑟query を取り出すことができます。
[画像: Voxel width performance scaling]
ボクセル幅によるクエリ速度のスケーリング。各曲線は、X軸のボクセル幅で生成されたMVTの平均クエリ時間を示しており、ロボットごとに分離されています。𝑟mobile、𝑟robot、𝑟query はそれぞれ ●、■、▲ でマークされています。
元のMVT論文では、𝑟max を使用することが推奨されていました。これは主にクエリ時間について一般的に言及し、その選択でパフォーマンスは十分であると主張していました。しかし、私はより良い答えを得たかったので、経験的に決定することにしました。各ロボットでのシミュレーションワークロードのために、ボクセル幅のパラメータスイープを実行し、平均衝突判定時間を記録しました。その後、上記のプロットに衝突判定パフォーマンスをレンダリングしました。
Fetch、Panda、UR5の場合、𝑟max は確かに妥当なボクセル幅の選択ですが、完全に最適ではありません。しかし、Baxterロボットの場合、𝑟max をボクセル幅として使用すると、最適な選択と比較してクエリ時間が20倍遅くなるという、非常に遅い結果になりました。元のMVTの著者はBaxterでベンチマークを行ったことがないか、行ったとしてもこれに気づいたはずですが、いずれにせよ、私はこの無料のスピードアップを得て先に進みます。
驚くべきことに、すべてのロボットにとって最適なボクセル幅は常に約10〜20 cmの間になります。これは、点群フィルタリングプロセスに起因すると考えられます。特定の点群密度に対して、無駄な作業量を最小限に抑えるための、おおよそ最適なボクセルサイズが存在します。
互いに球で対抗
当然、コードが速いかどうかを判断するには、実際にベンチマークを行う必要があります。そのために、いくつかの楽しいベンチマークを作成しました。多くのモーションプランニング問題を解決し、プランナーが行ったすべての衝突チェックを記録し、それらの衝突チェックを再生して衝突チェックのスループットを測定しました。各問題について、考慮したすべてのデータ構造(私のRustコードと元のC++バージョンのMVT実装、私の古いCAPT実装、および非常に高速な𝑘
-dツリーであるkiddo)のデータ構造構築と衝突チェック時間を記録しました。
[画像: Collision checking structure construction times]
各データ構造の構築時間スケーリング。各線は、点群のバケットに対するデータ構造の平均パフォーマンスを示しています。
最も明白な勝利は構築時間から得られます。CAPTは常に構築が遅く、Rust実装では特に遅かったです。実際、エンドツーエンドのプランニングパイプラインをベンチマークしたとき、CAPTの構築が常に最も遅いステップでした。MVTは構築時にそれほど多くのブックキーピングを行わないため、大きなパフォーマンスの勝利を得られます。また、MVTは線形メモリスケーリングを持つため、その構築時間は、空間分割ツリーの 𝑂(𝑘𝑛log(𝑛)) ではなく、点群サイズ 𝑛 のオーダー 𝑂(𝑛) であり、大規模な点群でも構築時間は優れています。
可変MVTの構築には、1つの大きなプールではなく多くの個別の割り当てを維持する必要があるため、ある程度の構築コストがかかりますが、それでもかなり安価です。
[画像: Collision checking throughput plots]
SIMD並列バッチクエリを含む、各データ構造の衝突チェックスループットのスケーリング。
さらに良いことに、MVTは優れたクエリスループットを持っています。CAPTの10ナノ秒スケールのスループットには非常に誇りを持っていましたが、MVTはそれ以上のことを達成しています。さらに驚くべきことに、可変MVTは不変のものよりもわずかに優れたクエリパフォーマンスを持つようです。パフォーマンスの向上は、キャッシュメモリの何らかの奇妙な挙動によるものである可能性が最も高いです。おそらく、点を個別の割り当てに配置することが役立つのでしょう。
[画像: Motion planning performance plots]
エンドツーエンドのモーションプランニングパフォーマンス。X軸はグラウンドトゥルースのプリミティブジオメトリでのプランニング時間、Y軸はテストされた点群表現でのプランニング時間です。各点は1つの問題のプランニング時間を表します。
実際のモーションプランニング問題を解決する場合、MVTは全体的に堅実なスピードアップを提供します。上記のプロットでは、ほとんどの問題で、MVT点群表現でのプランニングはCAPTよりも高速であり、多くの場合、グラウンドトゥルースのプリミティブジオメトリ表現よりも高速であることがわかります。
大きな問題はまだ解決されていない
MVTは非常にクールなデータ構造です!高速で、安価で、管理が容易です。また、Rust版の実装も楽しかったです。しかし、それらはCAPTの大きな問題のうち、構築時間の問題の1つしか解決していません。
点群データは必然的に不完全です。カメラから取得され、カメラはオブジェクトの片側しか見ることができません。実際のロボットのプランニングを行う際には、これを考慮に入れる必要があります。通常、見えない空間も無効であると仮定します。オクトマップのような知覚データに関する以前の研究はこれを行うことができますが、パフォーマンスの代償としてCAPTの研究でオクルージョン処理を犠牲にしました。現在、ユーザーは安全であるか高速であるかの厳しいトレードオフに直面しています。オクトマップは非常に遅いですが、これまでのところオクルージョンを処理できる唯一のものです。
より一般的な意味で、ロボットが美しい静物画のような世界で動作できるというモーションプランニングの古典的な定式化は間違っていると思います。ロボットの最高の知覚でさえ「悪い」以上のものではなく、したがって、プランニングへのアプローチは、私たちが常に現実世界の近似に対してのみ計画しているという事実に立ち向かうべきです。そのため、私の皿のように、知覚からのプランニングはまだ解決されていません。