LIDAR スキャンでの自己位置推定と環境地図作成の同時実行 (SLAM) の実装
この例では、姿勢グラフの最適化を使用して、一連の収集された LIDAR スキャンに自己位置推定と環境地図作成の同時実行 (SLAM) アルゴリズムを実装する方法を説明します。この例の目標は、LIDAR スキャンを使用して環境のマップを作成し、ロボットの軌跡を取得することです。
環境のマップを作成するために、SLAM アルゴリズムは LIDAR スキャンを段階的に処理し、これらのスキャンをリンクする姿勢グラフを作成します。ロボットは、以前に訪れた場所をスキャン マッチングによって認識し、自身の移動パスに沿って 1 つ以上のループ閉じ込みを設置する場合があります。SLAM アルゴリズムは、ループ閉じ込みの情報を利用してマップを更新し、推定されるロボットの軌跡を調整します。
ファイルからのレーザー スキャン データの読み込み
屋内環境でモバイル ロボットから収集されたレーザー スキャンで構成される、ダウンサンプリングされたデータ セットを読み込みます。2 つのスキャン間の平均変位はおよそ 0.6 メートルです。
offlineSlamData.mat
ファイルには変数 scans
が含まれており、これにはこの例で使用するすべてのレーザー スキャンが含まれています。
load('offlineSlamData.mat');
フロア プランおよびロボットのおおよそのパスが、説明目的で提示されています。このイメージは、地図作成対象の相対的環境と、ロボットのおおよその軌跡を示しています。
SLAM アルゴリズムの実行、最適化されたマップの作成、ロボットの軌跡のプロット
lidarSLAM
オブジェクトを作成し、マップの分解能と LIDAR の最大距離を設定します。この例では、Clearpath Robotics™ の Jackal™ ロボットを使用します。このロボットには、最大距離 10 メートルの SICK™ TiM-511 レーザー スキャナーが備えられています。LIDAR の最大距離を、スキャンの最大距離 (8 m) よりわずかに小さく設定します。レーザーの読み取りは最大距離近辺で精度が低下するためです。グリッド マップの分解能を 1 メートルあたり 20 セルに設定します。これにより、5 cm の精度が得られます。
maxLidarRange = 8; mapResolution = 20; slamAlg = lidarSLAM(mapResolution, maxLidarRange);
以下のループ閉じ込みパラメーターは経験的に設定されます。高めのループ閉じ込みしきい値を使用すると、ループ閉じ込みの識別プロセスで誤検知を棄却しやすくなります。ただし、高スコアの一致でも不適切な一致となる場合もあることを覚えておいてください。たとえば、類似した、または繰り返される特徴をもつ環境で収集されたスキャンでは、誤検知の可能性が高くなります。ループ閉じ込みの探索半径を大きくすると、アルゴリズムはループ閉じ込みについて、現在の姿勢推定の周囲でマップのより広い範囲を探索できるようになります。
slamAlg.LoopClosureThreshold = 210; slamAlg.LoopClosureSearchRadius = 8;
最初の 10 スキャンでのマップ作成プロセスの観察
slamAlg
オブジェクトにスキャンを段階的に追加します。マップに追加される場合は、スキャン番号が表示されます。スキャン間の距離が小さすぎる場合、オブジェクトはスキャンを棄却します。まず、最初の 10 件のスキャンを追加してアルゴリズムをテストします。
for i=1:10 [isScanAccepted, loopClosureInfo, optimizationInfo] = addScan(slamAlg, scans{i}); if isScanAccepted fprintf('Added scan %d \n', i); end end
Added scan 1 Added scan 2 Added scan 3 Added scan 4 Added scan 5 Added scan 6 Added scan 7 Added scan 8 Added scan 9 Added scan 10
slamAlg
によって追跡されたスキャンと姿勢をプロットすることにより、シーンを再構成します。
figure; show(slamAlg); title({'Map of the Environment','Pose Graph for Initial 10 Scans'});
ループ閉じ込みの効果と最適化プロセスの観察
引き続きループでのスキャンを追加します。ロボットが移動すると、ループ閉じ込みは自動的に検出されます。ループ閉じ込みが識別されるたびに、姿勢グラフの最適化が実行されます。出力 optimizationInfo
には IsPerformed
というフィールドがありますが、これは、姿勢グラフの最適化が行われるタイミングを示します。
ループ閉じ込みが識別されるたびにスキャンと姿勢をプロットし、結果を視覚的に検証します。このプロットは、オーバーレイされたスキャンと、最初のループ閉じ込みの最適化された姿勢グラフを示しています。ループ閉じ込みエッジが赤のリンクとして追加されています。
firstTimeLCDetected = false; figure; for i=10:length(scans) [isScanAccepted, loopClosureInfo, optimizationInfo] = addScan(slamAlg, scans{i}); if ~isScanAccepted continue; end % visualize the first detected loop closure, if you want to see the % complete map building process, remove the if condition below if optimizationInfo.IsPerformed && ~firstTimeLCDetected show(slamAlg, 'Poses', 'off'); hold on; show(slamAlg.PoseGraph); hold off; firstTimeLCDetected = true; drawnow end end title('First loop closure');
構築されたマップとロボットの軌跡の可視化
すべてのスキャンが slamAlg
オブジェクトに追加されたら、最終の作成されたマップをプロットします。前の for
ループでは最初の閉じ込みをプロットしただけですが、すべてのスキャンが追加されています。
figure show(slamAlg); title({'Final Built Map of the Environment', 'Trajectory of the Robot'});
作成されたマップを元のフロア プランと比較して視覚的に検査
スキャンと姿勢グラフのイメージを、元のフロア プランにオーバーレイします。すべてのスキャンを追加し、姿勢グラフを最適化した後は、マップが元のフロア プランとよく一致していることが確認できます。
占有グリッド マップの作成
最適化されたスキャンと姿勢を使用して、occupancyMap
を生成できます。これは、環境を確率占有グリッドとして表します。
[scans, optimizedPoses] = scansAndPoses(slamAlg); map = buildMap(scans, optimizedPoses, mapResolution, maxLidarRange);
レーザー スキャンと最適化された姿勢グラフを取り込んだ占有グリッド マップを可視化します。
figure; show(map); hold on show(slamAlg.PoseGraph, 'IDs', 'off'); hold off title('Occupancy Grid Map Built Using Lidar SLAM');
参考
poseGraph
| lidarSLAM
| occupancyMap