occupancyMap3D

3 次元占有マップの作成

説明

occupancyMap3D オブジェクトは、3 次元マップとマップ情報を格納します。マップは、八分木データ構造に確率値として格納されます。このクラスは任意の環境に対応し、観測入力に基づいて動的にそのサイズを拡張します。観測値を点群または特定の xyz の位置として追加できます。これらの観測値は確率値を更新します。確率値は位置の占有を表します。八分木データ構造では、データを適切に平衡化してメモリとディスク両方の効率を維持します。

作成

構文

omap = occupancyMap3D

omap = occupancyMap3D(res)

omap = occupancyMap3D(res,Name,Value)

説明

omap = occupancyMap3D は、観測値をもたない既定のプロパティ値を使用した空の 3 次元占有マップを作成します。

omap = occupancyMap3D(res) は、マップの分解能 (セル数/メートル) を指定し、Resolution プロパティを設定します。

omap = occupancyMap3D(res,Name,Value) は、1 つ以上の名前と値の引数を使用してプロパティを指定します。たとえば、"FreeThreshold",0.25 では、セルを障害物なしと見なすしきい値を 0.25 の確率値として設定します。

プロパティ

Resolution — グリッド分解能

1 (既定値) | 正のスカラー

グリッド分解能 (1 メートルあたりのセル数)。スカラーとして指定します。構築時の分解能を指定します。この値を超える精度の観測を挿入すると丸められ、この分解能で適用されます。

FreeThreshold — セルを障害物なしとして見なすしきい値

0.2 (既定値) | 正のスカラー

セルを障害物なしとして見なすしきい値。正のスカラーとして指定します。このしきい値未満の確率値は、障害物なしと見なされます。

OccupiedThreshold — セルを占有状態として見なすしきい値

0.65 (既定値) | 正のスカラー

セルを占有状態として見なすしきい値。正のスカラーとして指定します。このしきい値を上回る確率値は、占有状態と見なされます。

ProbabilitySaturation — 確率値の飽和制限

[0.001 0.999] (既定値) | [min max] ベクトル

確率値の飽和制限。[min max] ベクトルとして指定します。この飽和値を上回る値または下回る値は、min 値または max 値に設定されます。このプロパティは、複数の観測値を組み込む際に、セルの飽和過剰を軽減します。

オブジェクト関数

checkOccupancy	位置がフリーか占有かをチェック
getOccupancy	位置の占有確率を取得
inflate	各占有位置のインフレーション
insertPointCloud	3 次元の点または点群の観測値をマップに挿入
rayIntersection	Find intersection points of rays and occupied map cells
setOccupancy	位置の占有確率を設定
show	Display 3-D occupancy map
updateOccupancy	特定位置の占有確率を更新

例

3 次元占有マップの作成と点のインフレーション

occupancyMap3D オブジェクトは、センサーの観測値を使用して環境のマップを作成して、障害物を 3 次元空間に格納します。マップを作成し、点群から障害物を特定する点を追加します。次にマップ内の障害物をインフレートして、障害物の周囲の安全な動作空間を確保します。

10 セル数/メートルのマップ解像度で occupancyMap3D オブジェクトを作成します。

map3D = occupancyMap3D(10);

一連の 3 次元の点を姿勢 [x y z qw qx qy qz] からの観測値として定義します。この姿勢はこれらの点を観測するセンサー用で、中心が原点上にあります。複数の観測値を挿入する 2 組の点を定義します。

pose = [ 0 0 0 1 0 0 0];

points = repmat((0:0.25:2)', 1, 3);
points2 = [(0:0.25:2)' (2:-0.25:0)' (0:0.25:2)'];
maxRange = 5;

insertPointCloud を使用して 1 組目の点を挿入します。この関数は、センサーの姿勢と指定された点を使用して、観測値をマップに挿入します。表示されている色は、わかりやすく説明する目的のためにのみ点の高さと関連します。

insertPointCloud(map3D,pose,points,maxRange)
show(map3D)

2 組目の点を挿入します。センサーの姿勢 (原点) とこれらの点の間の光線は、前に挿入した点とオーバーラップします。したがって、センサーと新しい点の間の自由空間が更新されて、自由空間とマークされます。

insertPointCloud(map3D,pose,points2,maxRange)
show(map3D)

マップをインフレートして、障害物の周囲の安全な動作のためにバッファー ゾーンを追加します。ビークル半径と安全距離を定義し、これらの値の合計を使ってマップのインフレーション半径を定義します。

vehicleRadius = 0.2;
safetyRadius = 0.3;
inflationRadius = vehicleRadius + safetyRadius;
inflate(map3D, inflationRadius);

show(map3D)

アルゴリズム

八分木データ構造

八分木データ構造は、1 つの環境を "ボクセル" と呼ばれる 3 次ボリュームに分割するために使用される階層構造です。特定のマップ ボリュームでは、目的のマップ分解能 (ボクセル サイズ) に達するまで空間が 8 つのボクセルに再帰的に分割されます。このサブ分割はツリーとして表すことができ、マップ内の位置の確率値を格納します。

ツリー内の確率値には対数オッズ表現を使用できます。この表現により、位置を動的な測定から簡単に回復でき、小さな確率による数値誤差が少なくなります。この対数オッズ表現を使用して同じ占有値を共有する場合、メモリの効率を維持するために、構造内でツリーの下位の分岐が枝刈りされます。

このオブジェクトは、分岐の枝刈りなど、このデータ構造の編成を内部で処理します。setOccupancy、getOccupancy、insertPointCloud などの関数を使う場合、すべての観測値を空間座標系として指定します。オブジェクトでのツリーへの挿入、ツリー内の移動は、空間座標系およびマップの分解能に基づいて決まります。

参照

[1] Hornung, Armin, Kai M. Wurm, Maren Bennewitz, Cyrill Stachniss, and Wolfram Burgard. "OctoMap: an efficient probabilistic 3D mapping framework based on octrees." Autonomous Robots 34, no. 3 (April 2013): 189–206. https://doi.org/10.1007/s10514-012-9321-0.

拡張機能

C/C++ コード生成

MATLAB® Coder™ を使用して C および C++ コードを生成します。

occupancyMap3D は、packType 引数を "flat" に設定した場合、関数 packNGo (MATLAB Coder) を使用したコード生成をサポートしません。

バージョン履歴

R2019b で導入

参考

オブジェクト

• occupancyMap | occupancyMap3DCollisionOptions | binaryOccupancyMap

関数

• checkMapCollision | insertPointCloud | inflate | setOccupancy | show | rosReadOccupancyMap3D (ROS Toolbox)