SLAM マップ ビルダー

LiDAR ベースの SLAM を使用して 2 次元グリッド マップを作成

R2019b 以降

説明

SLAM マップ ビルダー アプリは、記録された LiDAR スキャンおよびオドメトリ センサーのデータを読み込み、自己位置推定と環境地図作成の同時実行 (SLAM) アルゴリズムを使用して 2 次元占有グリッドを作成します。インクリメンタル スキャン マッチングは、マップを作成するためにスキャンを揃えて重ね合わせます。ループ閉じ込み検出は、以前に通った位置を検出してマップ全体を調整することにより、ビークルのオドメトリのドリフトを調整します。スキャン マッチング アルゴリズムとループ閉じ込み検出について、手動による調整が必要になることがあります。アプリを使用して手動でスキャンを揃え、ループ閉じ込みを変更することで、マップ全体の精度を高めることができます。自動マップ作成を改善するために SLAM アルゴリズムの設定を調整することもできます。

アプリの使用:

	rosbag ログ ファイルを読み込むには、[インポート] 、 [rosbag からインポート] を選択します。rosbag ファイルを選択し、[開く] をクリックします。[インポート] タブが開きます。詳細については、rosbag のインポートとフィルター処理を参照してください。 ワークスペースからデータを読み込むには、[インポート] 、 [ワークスペースからインポート] を選択します。提供されるドロップ ダウンを使用して、[スキャン] と [姿勢] の変数を選択します。変数は関数 slamMapBuilder で指定することもできます。プログラムでの使用を参照してください。
	SLAM アルゴリズムの設定を調整するには、[SLAM の設定] を使用します。既定値が提供されますが、センサーやデータに応じてそれらの設定の調整が必要になることがあります。調整が必要な最も重要な値は [ループ閉じ込みのしきい値] です。詳細については、SLAM の設定の調整を参照してください。
	SLAM マップ作成プロセスを開始するには、[作成] をクリックします。作成プロセスでは、インクリメンタル スキャン マッチングを使用してマップのスキャンが揃えられ、以前と同じ位置を通るとループ閉じ込みが識別され、姿勢が調整されます。マップ作成プロセス中は、いつでも [一時停止] をクリックして、手動でインクリメンタル スキャンを揃えたり、ループ閉じ込みを変更したりできます。
	現在選択されているフレームの相対姿勢を変更したり、スキャンを前のスキャンに揃えたりするには、[インクリメンタル マッチ] をクリックします。現在のフレームの検出されたループ閉じ込みを変更または無視するには、[ループ閉じ込み] をクリックします。下部のスライダーを使用して、スキャン マッチングやループ閉じ込みが正確でない領域までスクロールして戻ります。スキャンやループ閉じ込みはいくつでも変更できます。詳細については、インクリメンタル スキャンとループ閉じ込みの変更を参照してください。
	マップを変更した後、[同期] をクリックしてスキャン マップ内のすべての姿勢を更新します。[同期] には、新しいループ閉じ込みを検索する [同期] と、ループ閉じ込みの検索をスキップしてスキャン マップの更新だけを行う [高速同期] の 2 つのオプションがあります。詳細については、マップの同期を参照してください。
	マップが満足のいく外観になったら、[占有グリッドのエクスポート] をクリックしてマップを M ファイルにエクスポートするか、マップをワークスペースに保存します。マップは 2 次元確率占有グリッドとして occupancyMap オブジェクトに出力されます。
	[セッションを開く] を使用して、保存してある既存のアプリ セッションを開くことができます。[マップ ビルダー] タブで、[セッションの保存] を使用して進行状況を M ファイルに保存できます。

×

SLAM マップ ビルダー アプリを開く

• MATLAB^® ツールストリップ:[アプリ] タブの [ロボティクスと自律システム] にある [SLAM マップ ビルダー] をクリックします。

• MATLAB コマンド ウィンドウ: 「slamMapBuilder」と入力します。

例

SLAM での LiDAR スキャンを使用したマップの作成と調整

"SLAM マップ ビルダー" アプリでは、自己位置推定と環境地図作成の同時実行 (SLAM) アルゴリズムを使用して LiDAR スキャンから占有グリッドを作成できます。スキャン マッチングで姿勢を推定し、姿勢グラフの最適化にループ閉じ込みを使用することでマップが作成されます。この例では、LiDAR スキャン データの rosbag を読み込み、データをフィルター処理してマップを作成するためのワークフローを示します。インクリメンタル スキャン マッチを調整し、ループ閉じ込みを変更することでスキャン マップを調整します。

LiDAR スキャン データの読み込み

サンプル .mat ファイルをワークスペースに読み込みます。これには、変数 scans が lidarScan オブジェクトの cell 配列として格納されています。

load slamLidarScans.mat

アプリを開く

[アプリ] タブの [ロボティクスと自律システム] にある [SLAM マップ ビルダー] をクリックします。

また、関数 slamMapBuilder を呼び出すこともできます。

slamMapBuilder

LiDAR スキャンのインポート

[インポート]、[ワークスペースからインポート] をクリックしてスキャンを読み込みます。rosbag として格納されたデータは、ROS Toolbox ライセンスで読み込むことができます。

[スキャン] ドロップ ダウンで変数 scans を選択します。

ツール バーで [% までダウンサンプリング] を 10 に設定します。データのサンプルを均等にダウンサンプリングすることで、SLAM アルゴリズムの計算時間が短くなります。この例では、10% は 5 番目のスキャンごとです。[適用] をクリックします。

下部のスライダーまたは矢印キーを使用してスキャンをプレビューします。

インポートが完了したら、[閉じる] をクリックします。

SLAM の設定の調整

SLAM アルゴリズムは、[SLAM の設定] ダイアログを使用して調整できます。センサーの仕様、環境、および用途に応じてパラメーターを調整してください。この例では、[ループ閉じ込みのしきい値] を 200 から 300 に増やします。このようにしきい値を大きくすると、検出されたループ閉じ込みを受け入れて使用される可能性が低くなります。[最適化の間隔] は 10 に設定します。ループ閉じ込みを 10 個受け入れるたびに、ドリフトを考慮して姿勢グラフが最適化されます。

マップの作成

データのフィルター処理と SLAM アルゴリズム設定の設定が完了したら、[作成] をクリックします。マップを作成するためのスキャンの処理がアプリで開始されます。スライダーが進行し、マップにスキャンが重ねられていく様子が表示されるはずです。推定されるロボットの軌跡が同じスキャン マップにプロットされます。インクリメンタル スキャン マッチは [インクリメンタル マッチ] ペインに表示されます。ループ閉じ込みが検出されるたびに、重ね合わされた 2 つのスキャンが [ループ閉じ込み] ペインに表示されます。

スキャン マッチまたはループ閉じ込みの調整

作成プロセス中にマップに歪みが見られる場合や、インクリメンタル マッチまたはループ閉じ込みがオフになっているように思われる場合は、気付いた時点で [一時停止] をクリックして調整が必要なスキャンを選択します。スキャンの変更は作成プロセスの最後にも実行できます。矢印キーまたはスライダーを使用して、ファイル内で歪みが最初に生じたポイントに移動します。[インクリメンタル マッチ] ボタンまたは [ループ閉じ込み] ボタンをクリックして、現在表示されているスキャンの姿勢を調整します。このセクションで示している不適切なループ閉じ込みは、例示のみを目的とした人為的なものです。

[ループ閉じ込み] ボタンをクリックします。ループ閉じ込みの相対姿勢を変更するためのタブが開きます。

ループ閉じ込みを完全に無視するには、[無視] をクリックします。それ以外の場合は、スキャンの相対姿勢をスキャンが揃うまで手動で変更します。

[スキャンの移動] または [スキャンの回転] をクリックし、Figure 内でクリックしてドラッグすることで 2 つのスキャンを揃えます。完了したら [確定] をクリックします。これは複数のスキャンについて実行できます。

スキャンの姿勢をインクリメンタル マッチとループ閉じ込みに応じて変更したら、[同期] をクリックして変更を適用します。既にすべてのスキャンの処理が完了している場合は、[高速同期] で新しいループ閉じ込みを検索せずにマップを更新すると計算時間を短縮できます。

占有グリッドのエクスポート

変更を同期してマップの作成を終了すると、完全に重ねられたスキャン マップがロボットの軌跡と共に表示されるはずです。

[占有グリッドのエクスポート] をクリックして、環境の最終的な占有マップをoccupancyMapオブジェクトとして取得します。変数名を指定してマップをワークスペースにエクスポートします。エクスポートする前に目的のフレームまでスクロールして戻り、[現在選択されているスキャンまで] を選択すると、スキャンのサブセットからマップを作成できます。

格納したマップで show を呼び出して占有マップを可視化します。

show(myOccMap)

[セッションの保存] ボタンを使用して SLAM マップ ビルダー アプリ セッションを保存することもできます。アプリの現在の状態が .mat ファイルに書き込まれ、後で [セッションを開く] を使用して読み込むことができます。

関連する例

• LIDAR スキャンでの自己位置推定と環境地図作成の同時実行 (SLAM) の実装
• LiDAR スキャンでのオンラインの自己位置推定と環境地図作成の同時実行 (SLAM) の実装

パラメーター

スキャン — ワークスペースからインポートする LiDAR スキャン

lidarScan オブジェクトの N 要素 cell 配列

ワークスペースからインポートする LiDAR スキャン。lidarScan オブジェクトの N 要素 cell 配列として指定します。

姿勢 (オドメトリ) — ワークスペースからインポートする姿勢

N 行 3 列の行列 | 3 要素の行ベクトルの N 要素 cell 配列

ワークスペースからインポートする姿勢。N 行 3 列の行列、または 3 要素の行ベクトルの N 要素 cell 配列として指定します。N 行 3 列の行列の各行と cell 配列の 3 要素の各行ベクトルは、[x y theta] の形式で姿勢を表します。x と y はメートル単位の位置を構成し、theta はラジアン単位の方向です。

例: [0 0 0; 1 1 pi/4; 2 2 pi/2]

例: {[0 0 0],[1 1 pi/4],[2 2 pi/2]}

プログラムでの使用

slamMapBuilder(bag)

slamMapBuilder(bag) は、SLAM マップ ビルダー アプリを開き、bag で指定された rosbag ログ ファイルをインポートします。これは、関数 rosbag (ROS Toolbox) を使用して作成された BagSelection オブジェクトです。rosbag のセンサー データをフィルター処理するための [インポート] タブが開きます。

slamMapBuilder(sessionFile)

slamMapBuilder(sessionFile) は、sessionFile で指定された名前の保存されているセッション ファイルから SLAM マップ ビルダー アプリを開きます。アプリ セッション ファイルは、アプリのツールストリップの [セッションの保存] ボタンから作成されます。

slamMapBuilder(scans)

slamMapBuilder(scans) は、SLAM マップ ビルダー アプリを開き、scans で指定されたスキャンをインポートします。これは、lidarScan オブジェクトの cell 配列です。スキャンのプレフィルター処理が完了していると仮定され、インポート プロセスはスキップされます。[作成] をクリックするとマップの作成が開始されます。

slamMapBuilder(scans,poses)

slamMapBuilder(scans,poses) は、SLAM マップ ビルダー アプリを開き、スキャンと姿勢をインポートします。scans は lidarScan オブジェクトの cell 配列として指定します。poses は scans の姿勢に対応する [x y theta] ベクトルの行列です。スキャンのプレフィルター処理が完了していると仮定され、インポート プロセスはスキップされます。[作成] をクリックするとマップの作成が開始されます。

詳細

rosbag のインポートとフィルター処理

[インポート] ボタンをクリックする際は、rosbag のパラメーターとデータをフィルター処理する方法をツールストリップで指定します。Figure で更新されたスキャンを表示するには、[適用] をクリックして設定を適用する必要があります。

• LiDAR スキャンおよびオドメトリの ROS トピックを選択します (利用可能な場合)。

• [オドメトリのトピック] で [Use TF] を選択する場合は、LiDAR スキャン センサーのフレームを [LIDAR フレーム] で指定し、ビークルのベースの固定フレームを [固定フレーム] で指定します。ドロップ ダウン メニューの項目は、rosbag の tf 変換ツリーで使用可能な座標系に基づいて生成されます。アプリでは /tf トピックおよび /tf2 トピックのみがサポートされています。

• rosbag のデータを平衡化する場合は、[開始時間] と [終了時間] を指定します。スライダーを使用するか、手動で時間の値を入力できます。

• スキャンをダウンサンプリングする目的の割合を [(%) までダウンサンプリング] で選択します。これにより、その割合に基づいてスキャンが均等にダウンサンプリングされます。たとえば、値が 25% であれば、4 つのスキャンごとに 1 つだけ選択されます。

• [適用] をクリックすると、フィルター処理された新しいスキャンが表示され、すべての設定が適用されます。完了したら [閉じる] をクリックしてタブを閉じます。

rosbag のスキャンのフィルター処理をさらに細かく制御する場合は、rosbag (ROS Toolbox) を使用して MATLAB に rosbag をインポートします。select (ROS Toolbox) を使用して rosbag をフィルター処理します。フィルター処理したカスタムの rosbag を使用してアプリを開くには、プログラムでの使用を参照してください。

SLAM の設定の調整

SLAM アルゴリズムには、自動マップ作成プロセスを改善するために調整可能なパラメーターがあります。パラメーターを調整するには、[SLAM の設定] をクリックします。スキャン配置やループ閉じ込み検出のプロセスのさまざまな要素については、[LIDAR SLAM のパラメーター] を使用します。また、マップ最適化アルゴリズムでループ閉じ込みに基づいてマップ全体を改善する方法を変更するには、[NLP ソルバー パラメーター] を調整します。

LIDAR SLAM のパラメーター:

• マップの分解能 (セル/m) –– マップの分解能。分解能はスキャン配置の位置の精度に影響を与えるほか、占有グリッドの出力サイズを定義します。

• LIDAR の距離 [min,max] (m) –– LiDAR センサーの読み取り値の範囲。LiDAR の距離の範囲外の読み取り値は、LiDAR スキャンを処理する際に無視されます。

• ループ閉じ込みのしきい値 –– ループ閉じ込み受け入れの単位なしのしきい値。平均的なループ閉じ込みのスコアは LiDAR スキャンによって異なります。作成プロセスでループ閉じ込みが見つからず、ビークルがマップ内の同じ位置を繰り返し通る場合、このしきい値を下げることを検討してください。

• ループ閉じ込みの検索半径 (m) –– ループ閉じ込みを検索する半径。アルゴリズムは、オドメトリ姿勢に基づいて、既存のマップ内のループ閉じ込みを与えられた半径 (メートル単位) 内で検索します。

• ループ閉じ込みの最大試行回数 –– ループ閉じ込み検出の試行回数。この数を増やすと、アルゴリズムによるマップ内でのループ閉じ込み検出の試行回数が増えますが、計算時間が長くなります。

• ループ閉じ込みの自動ロールバック –– ループ閉じ込みの自動除外を許可します。アルゴリズムではマップの最適化による残差誤差を追跡します。このパラメーターが on に設定されている場合、誤差の突然の変化が検出されたときに、ループ閉じ込みが除外されます。

• 最適化の間隔 (ループ閉じ込みの数) –– 最適化トリガーが受け入れられるループ閉じ込みの検出数。既定では、ループ閉じ込みが検出されるたびにマップが最適化されます。

• 移動のしきい値 [Linear,Angular] (m、rad) –– 新しいスキャンの受け入れに必要な姿勢の最小変化。ビークルの姿勢がこのしきい値を超えないと、次のスキャンがマップ作成プロセスから破棄されます。

NLP ソルバー パラメーター:

• 最大反復回数 –– マップの最適化の最大反復回数。この値を大きくすると、マップの精度が向上する可能性がありますが、計算時間が増えます。

• 最大時間 (秒) –– マップの最適化の最大許容時間。秒数で指定します。この値を大きくすると、マップの精度が向上する可能性がありますが、計算時間が増えます。

• 勾配の許容誤差 –– 最適化のコスト関数の勾配ノルムについての下限。この値を小さくすると、局所的最小値を検索する最適化の実行時間が長くなりますが、計算時間が増えます。

• 関数の許容誤差 –– 最適化のコスト関数の変位についての下限。この値を小さくすると、局所的最小値を検索する最適化の実行時間が長くなりますが、計算時間が増えます。

• ステップの許容誤差 –– 最適化のステップ サイズについての下限。この値を小さくすると、局所的最小値を検索する最適化の実行時間が長くなりますが、計算時間が増えます。

• 1 番目のノードの姿勢 [x,y,theta] (m、rad) –– グラフ内の最初のノードの姿勢。マップ内のスキャンの位置をオフセットする必要がある場合、位置 [x y] (メートル単位) と向き theta (ラジアン単位) を指定します。

これらのいずれかの設定を変更した後、新しいパラメーターでマップを作成し直すには、マップ作成プロセスを再実行する必要があります。

インクリメンタル マッチとループ閉じ込みの変更

このアプリでは、インクリメンタル スキャンの変更と検出されたループ閉じ込みの調整を手動で実行できます。マップの作成後にスキャンが適切に揃っていないことに気付いた場合は、[インクリメンタル マッチ] ボタンおよび [ループ閉じ込み] ボタンをクリックして変更タブを開きます。変更ツールストリップ ボタンを使用してスキャン間の相対姿勢を調整します。

• 無視 –– ループ閉じ込みを変更する際、ループ閉じ込みが正しくない場合は単純に無視できます。無視したループ閉じ込みが同じアプリ セッションで検出された場合、アルゴリズムによって常に破棄されます。インクリメンタル スキャン マッチは無視できません。

• スキャンの移動 –– 姿勢を手動でシフトするには、このボタンをクリックします。選択した後、マップ内でクリックしてドラッグすることでスキャンをシフトし、それらを適切に重ね合わせます。満足のいく配置になるまで、スキャンのすべての点を揃えます。[相対姿勢] セクションで [X] と [Y] の位置を手動で指定することもできます。

• スキャンの回転 –– 姿勢を手動で回転するには、このボタンをクリックします。選択した後、マップ内でクリックしてドラッグすることでスキャンを回転し、それらを適切に重ね合わせます。満足のいく配置になるまで、スキャンのすべての点を揃えます。[相対姿勢] セクションで [Theta] の位置を手動で指定することもできます。

マップの同期

[Incremental Scans] と [ループ閉じ込み] を使用してマップ作成プロセスに変更を加えた後は、マップを同期して変更を適用する必要があります。スキャンを適切に揃えるための変更内容に基づいて、変更後にマップ全体で各スキャンのシフトと配置が変わります。変更を加えた後のオプションとして、[同期] と [高速同期] の 2 つがあります。[高速同期] をクリックすると、姿勢に対する変更が自動的に適用されます。マップに対して他の変更は行われません。[同期] は、マップ作成とループ閉じ込み検出のプロセス全体を最初の変更の時点から再実行します。適用されるのは指定した変更ですが、他のスキャンの再配置や新しいループ閉じ込みの検索もアルゴリズムで試行されます。

バージョン履歴

R2019b で導入

参考

関数

• buildMap | matchScans | matchScansGrid | rosbag (ROS Toolbox) | optimizePoseGraph

オブジェクト

• lidarSLAM | lidarScan | occupancyMap | poseGraph