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

この例では次を使用します。

この例では、姿勢グラフの最適化を使用して、シミュレーション環境から取得された LiDAR スキャンに自己位置推定と環境地図作成の同時実行 (SLAM) アルゴリズムを実装する方法を説明します。この例には Simulink® 3D Animation™ と Navigation Toolbox™ が必要です。

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

SLAM アルゴリズムの基礎については、LIDAR スキャンでの自己位置推定と環境地図作成の同時実行 (SLAM) の実装の例を参照してください。

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

ロボットの軌跡は、シミュレーション環境でロボットが移動するウェイポイントとして与えられます。この例では、用意されているロボットの軌跡を使用します。

load slamRobotTrajectory.mat

フロアプランおよびロボットのおおよそのパスが、説明目的で提示されています。このイメージは、地図作成対象の環境と、ロボットのおおよその軌跡を示しています。

バーチャル世界の読み込みと表示

この例では、四方を障害物となる壁で囲まれた 2 台の車両と LiDAR スキャナーを搭載したロボットを Simulink 3D Animation ビューアーで表示したバーチャルシーンを使用します。メニューバー、ツールバー、ナビゲーションパネル、マウス、キーボードを使用してバーチャルシーン内を移動できます。

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 オブジェクトの作成

lidarSLAMオブジェクトを作成し、マップの分解能と LiDAR の最大距離を設定します。この例では、シミュレートしたバーチャル環境を使用します。この vrworld のロボットは、範囲が 0 ～ 10 メートルの LiDAR センサーを搭載しています。LiDAR の最大距離をスキャンの最大距離より小さい 8 m に設定します。レーザーの読み取りは最大距離近辺で精度が低下するためです。グリッドマップの分解能を 1 メートルあたり 20 セルに設定します。これにより、5 cm の精度が得られます。これらの 2 つのパラメーターを例全体を通して使用します。

maxLidarRange = 8;
mapResolution = 20;
slamAlg = lidarSLAM(mapResolution,maxLidarRange);

ループ閉じ込みパラメーターは経験的に設定されます。高めのループ閉じ込みしきい値を使用すると、ループ閉じ込みの識別プロセスで誤検知を棄却しやすくなります。高スコアの一致でも不適切な一致となる場合もあることを覚えておいてください。たとえば、類似した、または繰り返される特徴をもつ環境で収集されたスキャンでは、誤検知の可能性が高くなります。ループ閉じ込みの探索半径を大きくすると、アルゴリズムはループ閉じ込みについて、現在の姿勢推定の周囲でマップのより広い範囲を探索できるようになります。

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

ループ閉じ込みの効果と最適化プロセスの観察

ロボットをバーチャルシーン内で動かすループを作成します。軌跡点からのループでロボットの位置が更新されます。ロボットが環境内を移動すると、ロボットからスキャンが取得されます。

ループ閉じ込みは、ロボットの移動に合わせて自動的に検出されます。ループ閉じ込みが検出されるたびに、姿勢グラフの最適化が実行されます。これは、addScan からの出力値 optimizationInfo.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'});

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

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

バーチャルシーンを閉じます。

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

参考

poseGraph | lidarSLAM | occupancyMap