はじめに
以前の記事で、Intel CPU 内蔵グラフィックス機能を GPGPU として活用するための手法として Level Zero API (oneAPI/L0) を紹介しました。
Level Zero API を用いて ROS Navigation stack の一部を GPGPU 対応したところ、大きなマップに対するコスト値算出処理の高速化を実現することができました。
CPU 向けの処理を半ば強引に GPU 向けに修正しているなどこなれていない部分もありますが、組込み向けシステムにおける Level Zero API の適用事例として本記事で紹介したいと思います。
最初は Intel N100 内蔵 GPU の性能が未知数で、むしろ CPU 処理よりも遅くなる可能性もあると考えていたのですが、結果としてシングルスレッドの CPU 処理よりも 10 倍以上高速化することができて驚きました。外付け GPU を使用できない環境でも、使い方によっては内蔵 GPU を十分実用的に活用できると感じました。
ベースとなる環境は ROS Noetic を使用します。
ROS Navigation stack
ROS Navigation stack は、ROS においてロボットの自律移動を実現するための枠組みです。
下記の図は http://wiki.ros.org/navigation/Tutorials/RobotSetup より引用
ここで costmap_2d ( global_costmap および local_costmap ) により、障害物との衝突判定などに使用するコスト値の算出を行います。
しかし、サイズが大きく (5000x5000px など) 障害物の多いマップを使用する場合、1回のコスト値算出処理に数秒以上かかり、十分な性能が出ないという課題が見られました。
常にマップ全体を処理するのではなく、必要な範囲のみ処理する仕組みも用意されていますが、ロボットの移動に伴い処理対象範囲が広がるため、徐々に処理が重くなってしまいます。
詳しく確認すると、global_costmap に対する Inflation Costmap Plugin の updateCosts 処理が特に重く、ボトルネックとなっていることが分かりました。
InflationLayer コスト値算出処理の概要
もともとの InflationLayer::updateCosts() における処理の概要を下記に示します。
- 障害物からの距離とセル情報 (座標 x, y など) を管理するためのコンテナを用意する
std::map<double, std::vector<CellData>> inflation_cells_; - master_grid から全ての障害物セル (LETHAL_OBSTACLE) を探し、「距離 0.0」としてセル情報を inflation_cells_ に追加する
- inflation_cells_ から、距離の小さいものから順にセル情報を取得し、以下の処理を行う
- 当該セルに対して、障害物からの距離を用いてコスト値を算出し master_grid を更新
- 近傍セル (上/下/左/右) に対して、それぞれ以下の処理を行う
- 既に処理済みのセルの場合、何もしない
- 障害物からの距離 d を算出し、閾値 (cell_inflation_radius) を超える場合は何もしない
- それ以外の場合、障害物からの距離 d とセル情報を inflation_cells_ に追加する
これにより、以下のように障害物からの距離を求めることができます。
詳しく見てみると、以下のように順に処理を行うことが分かります。
前の処理の結果を使って後の処理を行うため、単純に並列化することはできません。
また、std::map はキー（ここでは距離）によって自動的にソートされるため、要素の数が増えると処理が重くなります。ソートを行わない std::unordered_map もありますが、距離順に処理を行う必要があるため、単純に置き換えることはできません。
コスト値算出処理の GPU 対応
GPU を用いて効率的に処理させるためには、処理を並列化させる必要があります。しかし、前述したように距離順に処理を行う必要があるため、そのままでは難しそうです。
ネットを検索したところ、CUDA で実装された GPUアクセラレーションを使用したコストマップの高速更新方法 が見つかりました。障害物ごとにコスト値を算出して、最も近い障害物のものを採用する仕組みのようです。しかし、サイズが大きく障害物の多いマップに対して適用するのは難しそうだと感じました。
そのため、ここでは 5000x5000px のような大きなマップを 32x32px といったブロック単位に分割し、各ブロックを並列処理する方法を検討します。
ブロック単位に分割した GPU 処理
左側の破線は InflationLayer::updateCosts() の引数で指定された範囲、左側の実線は cell_inflation_radius を加味した処理対象範囲を示します。右側の図は、マップ全体をブロック単位に分割し、赤い範囲のブロックに対して処理を行うことを示します。
各処理は全てブロック単位で行い、処理対象セルの一覧といった管理データもブロックごとに持ちます。
ここで、標準ライブラリの std::map は GPU で使えないので、inflation_cells_ の代わりに距離やセル情報を管理する簡易的な仕組みを自前で用意しました。
また、距離がバラバラの値だと扱いづらいため、以下のように distance_id として対応付けて使用します。distance_id は、距離として取り得る値 (最大値 cell_inflation_radius) に対して小さいものから順に連番を振ったものです。
例えば cell_inflation_radius=20 の場合、max_distance_id は 146 となります。
| 障害物からの距離 | distance_id |
|---|---|
| 0.0 | 1 |
| 1.0 | 2 |
| 1.414 | 3 |
| ... | ... |
| 20.0 | 146 |
ブロックをまたぐケースは、いったん置いておいて後で考えることにします。
処理の全体像（たたき台）を下記に示します。
障害物セル (LETHAL_OBSTACLE) の抽出
最初に master_grid から障害物セル (LETHAL_OBSTACLE) を抽出します。
- procCells により、各セルにおける障害物からの距離を管理する
- 障害物セルの場合は 0、未処理の場合は最大値 (0xffff) とする
- obstacleCellArrayBuf により、セル情報のリストを管理する
- セル情報は、座標 x, y および障害物との相対位置 diff_x, diff_y を含む
距離に対応するセル一覧の取得
ここで obstacleCellArrayBuf から、距離順にセル情報を取り出します。
毎回ソートを行うと処理が重くなるため、ここでは別のバッファ obstacleCellArrayTarget を用意して、以下のように対応します。
- obstacleCellArrayBuf から、特定の距離 (distance_id) に対応するセル情報を抽出する
- 対象の場合、obstacleCellArrayTarget にコピーし、obstacleCellArrayBuf から削除する
- 対象外の場合、obstacleCellArrayBuf に残す
例えば「距離 1.414」に対応するセル情報を抽出する場合、下記のようになります。
最初は「距離 0.0 (distance_id=1)」のセル情報を抽出し、後述する所定の処理を行います。
その後、distance_id をインクリメントしながら同様に繰り返します。
近傍セルの処理
ここでは obstacleCellArrayTarget として抽出された特定の距離 (distance_id) に対応するセル情報に対して、それらのセルの近傍セルを処理します。
- obstacleCellArrayTarget から、格納されている順にセル情報を取得し、以下の処理を行う
- 近傍セル (上/下/左/右) に対して、それぞれ以下の処理を行う
- （近傍セルを基準にした）障害物からの距離 d を算出する
- 距離 d が procCells に記録された距離よりも小さい場合、以下の処理を行う
- （近傍セルを基準とした）セル情報を obstacleCellArrayBuf に追加
- procCells に記録された距離を d で置き換える
- 近傍セル (上/下/左/右) に対して、それぞれ以下の処理を行う
master_grid に対するコスト値の更新
最後に、procCells に記録された距離を用いてコスト値を算出し、master_grid に対してコスト値の更新を行います。
ブロックをまたぐケースへの対応
ここで問題となるのが、以下のように近傍セルがブロックをまたぐケースです。
ここでは Block A 内の障害物セルが Block B に影響を与える可能性があります。コスト値算出処理が連鎖的に行われるため、ブロック端のセル a だけではなく、セル b やセル c についても考慮する必要があります。さらに、Block B から Block A についても同様です。
他のブロックが管理する obstacleCellArrayBuf に対して勝手に書き込むことはできないので、ブロックをまたぐ近傍セルは別に管理することにします。
例えば Block A から Block B にまたぐ近傍セルの場合、以下のような流れとなります。
- Block A の処理において、Block B の influenceCellArray にセル情報を追加
- Block B の処理において、自身の influenceCellArray を確認し、ブロック内のセルと同様に処理する
近傍セルの処理 (ブロックをまたぐケースを含む)
ブロックをまたぐセル情報のマージ処理
各ブロックの処理の同期
しかし、この方法でも課題が残ります。
ブロック内の障害物セルの数などによって、各ブロックの処理時間は変動します。そのため Block A が Block B の influenceCellArray にセル情報を追加した時点で、Block B は既に処理が完了しているかもしれません。
例えば、以下のようなケースを考えます。
ここでは「セル a : 距離 2.0」「セル b : 距離 3.0」が適切ですが、もし Block B の処理が先に進んだ場合、「セル b : 距離 4.0」「セル a : 距離 5.0」となる可能性があります。
これを防ぐためには、処理中の distance_id を全体で共通にして、全てのブロックの処理が完了してから次の距離の処理に進む必要があります。また、influenceCellArray にセル情報を追加してから確認するという順番も守る必要があります。
すこし雲行きが怪しくなってきました。
処理の全体像を下記に示します。
Level Zero API の Command List を用いた実装
細かい処理を1つずつ GPU に任せていたら、オーバーヘッドが非常に大きくなってしまうため、Command List を用いて一連の処理をまとめて GPU に実行させます。
必要な kernel 関数を用意して zeCommandListAppendLaunchKernel() により順に実行させます。ここで、kernel 関数からの出力データは zeCommandListAppendMemoryRangesBarrier() によりバリアを設定します。Command List の準備手順を下記に示します。
/* 障害物セルの抽出 */
LaunchKernel(detectObstacles);
Barrier(detectObstaclesBuf);
LaunchKernel(extractObstaclesArray);
Barrier(extractObstaclesArrayBuf);
for (i=1; i <= max_distance_id; i++)
{
/* distance_id インクリメント */
LaunchKernel(updateTargetDistance);
Barrier(updateTargetDistanceBuf);
/* 距離に対応するセル一覧の取得 */
LaunchKernel(checkCellArrayDistance);
Barrier(checkCellArrayDistanceBuf);
/* 近傍セルの処理 (ブロックをまたぐケースを含む) */
LaunchKernel(extractNeighbors);
Barrier(extractNeighborsBuf);
/* ブロックをまたぐセル情報のマージ処理 */
LaunchKernel(mergeNeighbors);
Barrier(mergeNeighborsBuf);
}
/* master_grid に対するコスト値の更新 */
LaunchKernel(updateMasterCosts);
Barrier(updateMasterCostsBuf);
例えば cell_inflation_radius=20 の場合、max_distance_id=146 なので、コマンドの数は 1174 となります。
Level Zero API 仕様での最大コマンド数は分かりませんが、コマンドの数が 30000 を超えるケースでも、期待通りに Command List を用意して実行できることが確認できました。
( cell_inflation_radius=120 の場合、max_distance_id=3860、コマンドの数は 30886 )
各ブロックは並列処理できるとはいえ、全体的にかなり力技でシーケンシャルな処理になってしまいました。本当に GPU を用いて高速化できるのだろうか...？
検証
GPU 対応前と対応後における InflationLayer コスト値算出処理の処理時間を計測します。
- 動作確認環境 を使用
- costmap_2d_node と必要なノード (tf, map_server) のみ起動する launch ファイルを用意
- マップは 5000x5000px (そのうち障害物セルが約65万セル) を使用
- cell_inflation_radius=20
- 処理対象範囲 (
InflationLayer::updateCosts()の引数min_i,min_j,max_i,max_j) はマップ全体とする
GPU 対応後では、InflationLayer コスト値算出処理に対応する以下の処理時間をまとめて確認します。
- master_grid から GPU 処理用の共有メモリにコピー
- Command List の準備
- Command Queue に対して実行を要求し、処理が完了するのを待つ
- GPU 処理用の共有メモリから master_grid に処理結果をコピー
GPU 対応前と対応後の比較
GPU 対応前と対応後における InflationLayer コスト値算出処理の処理時間を示します。
| No | GPU対応前 [ms] | GPU対応後 [ms] |
|---|---|---|
| 1 | 6546 | 290 |
| 2 | 6724 | 287 |
| 3 | 6897 | 287 |
| 4 | 6751 | 287 |
| 5 | 6873 | 292 |
| 平均 | 6758 | 289 |
なんと、処理時間が 1/20 以下になりました！
既存の CPU 処理がシングルスレッドであること、Intel N100 の CPU コアが省電力向け（Eコア）であることを考慮しても、あまりの差に驚きました。
まとめ
本記事では、組込み向けシステムにおける Level Zero API の適用事例として ROS Navigation stack (costmap) に対する GPGPU 対応について紹介しました。
今回示した手法は、サイズが大きく障害物の多いマップにおいて、マップ全体のコスト値算出処理が必要となる、ある意味ワーストケースのような状況を想定しています。CPU 処理では数秒以上かかるような場合でも、GPU を用いて高速化できることを示しました。
なお、マップのサイズがそれほど大きくない場合や障害物セルが少ない場合、あるいは処理対象範囲が狭い場合、CPU 処理でも 10ms 以下で処理することができます。一方、GPU 処理ではメモリコピーなどのオーバーヘッドが 20ms ほどかかるため、CPU 処理よりも遅くなってしまいます。状況によって使い分けるのが良いですね。
参考文献
付録
動作確認環境
docker を用いて、ホスト環境 (Ubuntu 22.04 LTS) の上で ROS Noetic のコンテナを動かす。
オーバークロック設定は UFEI (BIOS) で無効化。
| 項目 | 内容 |
|---|---|
| CPU | Intel N100 (Alder Lake-N) |
| GPU | Intel UHD Graphics (Intel N100 CPU 内蔵グラフィックス) |
| RAM | 8GB |
| OS (ホスト) | Ubuntu 22.04 LTS |
| OS (コンテナ) | ROS Noetic |
| Kernel | HWEカーネル 5.19 |
| GPUドライバ | NEO Driver Version 23.35.27191.9 |
| Level Zero Loader | Level Zero v1.14 |