lidarSLAM

LiDAR スキャンを使用した位置推定と地図作成の実行

このページをすべて展開する

説明

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

作成

構文

slamObj = lidarSLAM
slamObj = lidarSLAM(mapResolution,maxLidarRange)
slamObj = lidarSLAM(mapResolution,maxLidarRange,maxNumScans)

説明

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)
poseGraph は poseGraph オブジェクトです。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 に姿勢が追加されます。

スキャンのレジストレーションの方法。文字ベクトルとして指定します。

メモ

位相相関法を使用するには Image Processing Toolbox™ が必要です。

インクリメンタル マッチの並進の検索範囲。[x y] 形式の 2 要素ベクトル (メートル単位) として指定します。このプロパティは、ScanRegistrationMethod プロパティが 'BranchAndBound' に設定されている場合にのみ適用可能です。

これらの値は、関数 addScan の引数 relPoseEst で指定された初期並進推定値について検索ウィンドウを定義します。このプロパティの値は、連続する受け入れられたスキャン間の最大予測並進に設定します。

このプロパティは、関数 matchScansGrid の名前と値のペアの引数 'TranslationSearchRange' に似ています。

インクリメンタル マッチの回転の検索範囲。正のスカラー (ラジアン単位) として指定します。このプロパティは、ScanRegistrationMethod プロパティが 'BranchAndBound' に設定されている場合にのみ適用可能です。

この値は、関数 addScan の引数 relPoseEst で指定された初期回転推定値について検索ウィンドウを定義します。このプロパティの値は、連続する受け入れられたスキャン間の最大予測回転に設定します。

このプロパティは、関数 matchScansGrid の名前と値のペアの引数 'RotationSearchRange' に似ています。

オブジェクト関数

addScanLiDAR SLAM マップへのスキャンの追加
copyLiDAR SLAM オブジェクトをコピー
removeLoopClosures 姿勢グラフからのループ閉じ込みの削除
scansAndPoses スキャンおよび対応する姿勢を抽出
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
title("Lidar Scans and Poses")
xlabel("X [meters]")
ylabel("Y [meters]")

Figure contains an axes object. The axes object with title Lidar Scans and Poses, xlabel X [meters], ylabel Y [meters] contains 121 objects of type line.

占有マップの表示

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

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

Figure contains an axes object. The axes object with title Occupancy Map of Garage, xlabel X [meters], ylabel Y [meters] contains an object of type image.

詳細

すべて展開する

参照

[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 で導入

参考

|

トピック