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

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

この例では、シミュレートされた環境から姿勢グラフ最適化を使用して取得した LIDAR スキャンに SLAM (位置推定とマッピングの同時実行) アルゴリズムを実装する方法を示します。

この例の目標は、ロボット シミュレーターをループで使用し、LIDAR スキャンを使用して環境のマップを作成して、ロボットの軌跡を取得することです。

SLAM アルゴリズムの基本は LIDAR スキャンを使用した SLAM (位置推定とマッピングの同時実行) の実装 の例に記載されています。この例には Simulink® 3D Animation™ と Robotics System Toolbox™ が必要です。

ファイルからのロボットの軌跡の読み込み

ロボットの軌跡は、シミュレートされた環境内で移動するためにロボットに与えられる中間点です。この例では、ロボットの軌跡を提供しています。

load slamRobotTrajectory.mat

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

仮想世界の読み込みと表示

この例では、Simulink 3D Animation Viewer で表示されている、2 台の車両、障害物としての 4 つの壁、LIDAR スキャナーを装備したロボットがある仮想シーンを使用します。メニュー バー、ツール バー、ナビゲーション パネル、マウス、およびキーボードを使用して、仮想シーン内を移動できます。ビューアーの主要な機能を、VR example に示します。

vrworld オブジェクトを作成して開きます。

w = vrworld('slamSimulatedWorld.x3d');
open(w)

仮想シーンを表示する Figure を作成します。

vrf = vrfigure(w)

vrf = 

	vrfigure object: 1-by-1

	Differential Wheeled Robot with LIDAR Sensor

仮想世界でのロボットの位置と回転の初期化

仮想シーンは、Simulink 3D Animation によって使用される VRML ファイルの階層構造として表されます。子オブジェクトの位置と向きは、親オブジェクトに対して相対的に表されます。ロボットの vrnode は、仮想シーン内のロボットの位置と向きを操作するために使用します。

VRML ノードにアクセスするには、適切な vrnode オブジェクトを作成しなければなりません。ノードは名前とそのノードが属するワールドによって識別されます。

仮想環境のロボット用に vrnode ハンドルを作成します。

robotVRNode = vrnode(w,'Robot');

軌跡の最初の点からロボットの初期位置を設定し、初期回転を y 軸に対して 0 ラジアンに設定します。

robotVRNode.children.translation = [trajectory(1,1) 0 trajectory(1,2)];
robotVRNode.children.rotation = [0 1 0 0];

VRNODE を作成することにより、ロボット上の LIDAR センサーのハンドルを作成します。

lidarVRNode = vrnode(w,'LIDAR_Sensor');

シミュレート対象の LIDAR は、合計 240 本のレーザー線を使用しており、これらの線の間の角度は 1.5 度です。

angles  = 180:-1.5:-178.5;
angles = deg2rad(angles)';

仮想シーンの更新と初期化を待機しています。

pause(1)

LIDAR SLAM オブジェクトの作成

robotics.LidarSLAM オブジェクトを作成し、マップの分解能と最大 LIDAR 範囲を設定します。この例ではシミュレートされた仮想環境を使用します。この vrworld のロボットには、距離 0 ~ 10 メートルの LIDAR センサーがあります。最大 LIDAR 距離 (8 m) を最大スキャン距離より小さい値に設定します。これは、最大距離付近ではレーザーの読み取り値の正確性が低くなるためです。グリッド マップの分解能を 1 メートルあたり 20 セルに設定します。この場合、精度は 5 cm です。例全体にわたって、この 2 つのパラメーターを使用します。

maxLidarRange = 8;
mapResolution = 20;
slamAlg = robotics.LidarSLAM(mapResolution, maxLidarRange);

ループ クロージャ パラメーターは経験的に設定します。より大きなループ クロージャしきい値を使用すると、ループ クロージャ識別プロセスにおける誤検知を棄却するのに役立ちます。スコアの高い一致でも、良好でない一致である可能性があることに注意してください。たとえば、類似の特徴や繰り返される特徴のある環境で収集したスキャンは、誤検知が発生する可能性が高くなります。より大きなループ クロージャ検索半径を使用すると、アルゴリズムによって、現在の姿勢推定の周りにおいて、マップのより広い範囲でループ クロージャを検索できるようになります。

slamAlg.LoopClosureThreshold = 200;
slamAlg.LoopClosureSearchRadius = 3;
controlRate = robotics.Rate(10);

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

ループを作成して、ロボットを仮想シーン内で移動させます。ループ内では、軌跡点からロボットの位置が更新されます。ロボットが環境内を移動するのに伴い、そのロボットからスキャンが取得されます。

ロボットが動くのに伴い、ループ クロージャが自動的に検出されます。ループ クロージャが検出されるたびに、姿勢グラフの最適化が実行されます。これは、addScanoptimizationInfo.IsPerformed 出力値を使用して確認できます。

最初のループ クロージャが識別されたときにスキャンと姿勢を示し、また結果を目視で確認できるように、スナップショットが表示されます。以下のプロットは、最初のループ クロージャについて、オーバーレイされたスキャンと最適化された姿勢グラフを示します。

すべてのスキャンが収集され、処理された後で、最終的に構築されたマップが提示されます。

ロボットが仮想シーン内を移動するのに伴い、プロットが継続的に更新されます。

firstLoopClosure = false;
scans = cell(length(trajectory),1);

figure
for i=1:length(trajectory)
    % Use translation property to move the robot. 
    robotVRNode.children.translation = [trajectory(i,1) 0 trajectory(i,2)];
    vrdrawnow;
    
    % Read the range readings obtained from lidar sensor of the robot.
    range = lidarVRNode.pickedRange;
    
    % The simulated lidar readings will give -1 values if the objects are
    % out of range. Make all these value to the greater than
    % maxLidarRange.
    range(range==-1) = maxLidarRange+2;

    % Create a |lidarScan| object from the ranges and angles. 
    scans{i} = lidarScan(range,angles);
    
    [isScanAccepted, loopClosureInfo, optimizationInfo] = addScan(slamAlg, scans{i});
    if isScanAccepted
        % Visualize how scans plot and poses are updated as robot navigates
        % through virtual scene
        show(slamAlg);
        
        % Visualize the first detected loop closure
        % firstLoopClosure flag is used to capture the first loop closure event
        if optimizationInfo.IsPerformed && ~firstLoopClosure
            firstLoopClosure = true;
            show(slamAlg, 'Poses', 'off');
            hold on;
            show(slamAlg.PoseGraph);
            hold off;
            title('First loop closure');
            snapnow
        end
    end

    waitfor(controlRate);
end

slamAlg オブジェクトにすべてのスキャンを追加したら、最終的に構築されたマップをプロットします。

show(slamAlg, 'Poses', 'off');
hold on
show(slamAlg.PoseGraph); 
hold off
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');

仮想シーンを閉じます。

close(vrf);
close(w);
delete(w);