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

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

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