最新のリリースでは、このページがまだ翻訳されていません。 このページの最新版は英語でご覧になれます。

LIDAR スキャンを使用した SLAM (位置推定とマッピングの同時実行) の実装

この例では、姿勢グラフ最適化を使用して、収集された一連の LIDAR スキャンに SLAM (位置推定とマッピングの同時実行) アルゴリズムを実装する方法を示します。この例の目標は、LIDAR スキャンを使用して環境のマップを構築し、ロボットの軌跡を取得することです。

環境のマップを構築するために、SLAM アルゴリズムは LIDAR スキャンを段階的に処理し、これらのスキャンをリンクする姿勢グラフを構築します。ロボットはスキャン マッチングによって以前に訪れた場所を認識し、移動するパスに合わせて 1 つ以上のループ クロージャを確立する場合があります。SLAM アルゴリズムはループ クロージャ情報を活用してマップを更新し、ロボットの推定軌跡を調整します。

ファイルからのレーザー スキャン データの読み込み

屋内環境のモバイル ロボットから収集されたレーザー スキャンで構成される、ダウンサンプリングされたデータセットを読み込みます。スキャン 2 回ごとの平均変位は 0.6 メートル前後です。

offlineSlamData.mat ファイルには、変数 scans が含まれています。この変数には、この例で使用されるすべてのレーザー スキャンが含まれています。

load('offlineSlamData.mat');

説明をわかりやすくするために、フロア プランとロボットのおおよそのパスを示しています。次の図は、マッピングされる相対環境と、ロボットのおおよその軌跡を示しています。

SLAM アルゴリズムの実行、最適化されたマップの構築、ロボットの軌跡のプロット

robotics.LidarSLAM オブジェクトを作成し、マップの分解能と最大 LIDAR 範囲を設定します。この例では Clearpath Robotics™ の Jackal™ ロボットを使用しています。ロボットは、最大距離 10 メートルの SICK™ TiM-511 レーザー スキャナーを備えています。最大 LIDAR 距離を最大スキャン距離 (8 m) よりわずかに小さい値に設定します。これは、最大距離付近ではレーザーの読み取り値の正確性が低くなるためです。グリッド マップの分解能を 1 メートルあたり 20 セルに設定します。この場合、精度は 5 cm です。

maxLidarRange = 8;
mapResolution = 20;
slamAlg = robotics.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'});

構築されたマップを元のフロア プランと比較して視覚的に検査

スキャンと姿勢グラフのイメージが、元のフロア プランにオーバーレイされています。すべてのスキャンを追加して姿勢グラフを最適化した後でも、マップが元のフロア プランと一致することがわかります。

占有グリッド マップの構築

最適化されたスキャンと姿勢を使用して robotics.OccupancyGrid を生成できます。これは、環境を確率的占有グリッドとして表したものです。

[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');