lidarSLAM

LIDAR スキャンを使用した位置推定とマッピングの実行

説明

lidarSLAM クラスは、LIDAR スキャン センサー入力の SLAM (位置推定とマッピングの同時実行) を行います。SLAM アルゴリズムは、LIDAR スキャンを取り込んで、基となる姿勢グラフのノードに付加します。アルゴリズムは次に、スキャン マッチングを使用してスキャンを関連付けます。また、以前にマップされた領域とスキャンがオーバーラップするループ クロージャを検索し、姿勢グラフ内のノードの姿勢を最適化します。

作成

説明

slamObj = lidarSLAM は、LIDAR SLAM オブジェクトを作成します。占有マップの既定のサイズは 1 メートルあたり 20 セルです。各 LIDAR スキャンの最大範囲は 8 メートルです。

slamObj = lidarSLAM(mapResolution,maxLidarRange) は、LIDAR SLAM オブジェクトを作成し、入力に基づいて MapResolution プロパティと MaxLidarRange プロパティを設定します。

slamObj = lidarSLAM(mapResolution,maxLidarRange,maxNumScans) は、コード生成時に受け入れ可能なスキャン数の上限を指定します。maxNumScans は正の整数です。このスキャンの上限は、コード生成時にのみ必要です。

プロパティ

すべて展開する

スキャンを接続する基となる姿勢グラフ。poseGraph オブジェクトとして指定します。lidarSLAM にスキャンを追加すると、この姿勢グラフが更新されます。ループ クロージャが検出されると、OptimizationFcn を使用して姿勢グラフが最適化されます。

占有グリッド マップの分解能。1 メートルあたりのセル数を示す正の整数として指定します。構築時のマップ分解能を指定します。

LIDAR センサーの最大範囲。メートル単位の正のスカラーとして指定します。構築時の最大範囲を指定します。

姿勢グラフの最適化関数。関数ハンドルとして指定します。既定で、アルゴリズムは関数 optimizePoseGraph を呼び出します。独自の最適化手法を指定するには、クラスに次の関数シグネチャが必要です。

[updatedPose,stat] = myOptimizationFcn(poseGraph)
poseGraphposeGraph オブジェクトです。updatedPose は、連続するノード ID 順にリストされた姿勢 [x y theta] の n 行 3 列のベクトルです。stat は、ResidualError フィールドを正のスカラーとして含む構造体です。stat 構造体を使用して、最適化に関連するその他の情報を含めます。

スキャン マッチング アルゴリズムのスコアに関するループ クロージャ受け入れのしきい値。正のスカラーとして指定します。しきい値を高くするとよりよい一致が得られますが、スコアはセンサー データに基づいて変わります。

ループ クロージャ検出の探索半径。正のスカラーとして指定します。この半径を大きくすると、探索時間が長くなるため、パフォーマンスに影響します。実際の環境と予測される車両の軌跡に基づいて、この距離を調整します。

ループ クロージャ検出の試行回数。正の整数として指定します。試行回数を増やすと、探索時間が長くなるため、パフォーマンスに影響します。

追加されたループ クロージャの自動ロールバックを許可するかどうか。true または false として指定します。SLAM オブジェクトは、OptimizationFcn によって返される残差誤差を追跡します。このプロパティが true の場合、残差誤差の突然の変化が検出されたときに、ループ クロージャを却下 (ロール バック) します。

最適化トリガーが受け入れられるループ クロージャの数。正の整数として指定します。既定では、lidarSLAM がループ クロージャを 1 つ追加するたびに PoseGraph が最適化されます。

スキャン処理に必要な姿勢の最小変化。ベクトル [translation rotation] として指定します。新しく追加されたスキャンの相対姿勢の変化は [x y theta] として計算されます。xy 位置の並進または theta の回転がこれらのしきい値を超えると、lidarSLAM オブジェクトはスキャンを受け入れ、PoseGraph に姿勢が追加されます。

オブジェクト関数

addScanAdd scan to lidar SLAM map
copyCopy lidar SLAM object
removeLoopClosures Remove loop closures from pose graph
scansAndPoses Extract scans and corresponding poses
showPlot scans and robot poses

すべて折りたたむ

lidarSLAM オブジェクトを使用して LIDAR スキャンの追加および比較を繰り返し、ロボットの軌跡の最適化された姿勢グラフを作成します。関連付けられている姿勢およびスキャンから占有マップを取得するには、関数buildMapを使用します。

データの読み込みと SLAM アルゴリズムの設定

lidarScanオブジェクトの cell 配列を読み込みます。この LIDAR スキャンは、ある駐車場で、ClearPath Robotics® 製の Husky® ロボットで収集されました。通常、LIDAR スキャンは高周波数で取得され、個々のスキャンが SLAM に必要なわけではありません。したがって、40 番目、次の 40 番目というようにスキャンを繰り返し選択して、スキャンをダウン サンプリングします。

load garage_fl1_southend.mat scans
scans = scans(1:40:end);

SLAM アルゴリズムを設定するには、LIDAR の範囲、マップの分解能、ループ クロージャのしきい値および探索半径を指定します。これらのパラメーターを、実際のロボットと環境に合わせて調整します。次のパラメーターを指定して lidarSLAM オブジェクトを作成します。

maxRange = 19.2; % meters
resolution = 10; % cells per meter

slamObj = lidarSLAM(resolution,maxRange);
slamObj.LoopClosureThreshold = 360;
slamObj.LoopClosureSearchRadius = 8;

スキャンを繰り返し追加

for ループを使用して、SLAM オブジェクトにスキャンを追加します。オブジェクトでは、スキャン マッチングを使用して、追加された各スキャンを以前に追加されたスキャンと比較します。マップを改善するために、オブジェクトはループ クロージャを検出するたびに姿勢グラフを最適化します。スキャン 10 個ごとに、保存された姿勢とスキャンを表示します。

for i = 1:numel(scans)

    addScan(slamObj,scans{i});
    
    if rem(i,10) == 0
        show(slamObj);
    end
end

占有マップの表示

すべてのスキャンを SLAM オブジェクトに追加した後、スキャンと姿勢を指定してbuildMapを呼び出すことにより、occupancyMapマップを作成します。SLAM オブジェクトで使用したものと同じマップ分解能および最大範囲を使用します。

[scansSLAM,poses] = scansAndPoses(slamObj);
occMap = buildMap(scansSLAM,poses,resolution,maxRange);
figure
show(occMap)
title('Occupancy Map of Garage')

詳細

すべて展開する

参照

[1] Hess, Wolfgang, Damon Kohler, Holger Rapp, and Daniel Andor. "Real-Time Loop Closure in 2D LIDAR SLAM." 2016 IEEE International Conference on Robotics and Automation (ICRA). 2016.

拡張機能

R2019b で導入