Main Content

このページの翻訳は最新ではありません。ここをクリックして、英語の最新版を参照してください。

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

Figure contains an axes object. The axes object with title Map of the Environment Pose Graph for Initial 10 Scans contains 11 objects of type line.

ループ クロージャの効果と最適化プロセスの観察

引き続きループでのスキャンを追加します。ロボットが移動すると、ループ クロージャは自動的に検出されます。ループ クロージャが識別されるたびに、姿勢グラフの最適化が実行されます。出力 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');

Figure contains an axes object. The axes object with title First loop closure contains 42 objects of type line, text.

構築されたマップとロボットの軌跡の可視化

すべてのスキャンが slamAlg オブジェクトに追加されたら、最終の作成されたマップをプロットします。前の for ループでは最初のクロージャをプロットしただけですが、すべてのスキャンが追加されています。

figure
show(slamAlg);
title({'Final Built Map of the Environment', 'Trajectory of the Robot'});

Figure contains an axes object. The axes object with title Final Built Map of the Environment Trajectory of the Robot contains 73 objects of type line.

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

スキャンと姿勢グラフのイメージを、元のフロア プランにオーバーレイします。すべてのスキャンを追加し、姿勢グラフを最適化した後は、マップが元のフロア プランとよく一致していることが確認できます。

占有グリッド マップの作成

最適化されたスキャンと姿勢を使用して、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');

Figure contains an axes object. The axes object with title Occupancy Grid Map Built Using Lidar SLAM contains 4 objects of type image, line.