addScan

LiDAR SLAM マップへのスキャンの追加

ページ内をすべて折りたたむ

構文

addScan(slamObj,currScan)
addScan(slamObj,currScan,relPoseEst)
[isAccepted,loopClosureInfo,optimInfo] = addScan(___)

説明

addScan(slamObj,currScan) は、LiDAR スキャン currScan を LiDAR SLAM オブジェクト slamObj に追加します。この関数はスキャン マッチングを使用して、このスキャンと最新のスキャンを相互に関連付け、slamObj で定義される姿勢グラフに追加します。スキャンが受け入れられると、addScan はループ閉じ込みを検出し、slamObj の設定に基づいて最適化します。

また、addScan(slamObj,currScan,relPoseEst) は slamObj の最新の LiDAR スキャンの姿勢を基準とした相対姿勢を指定します。この相対姿勢により、スキャン マッチングが向上します。

メモ

lidarSLAM オブジェクトの ScanRegistrationMethod プロパティが 'PhaseCorrelation' に設定されている場合、relPoseEst 入力は無視されます。

例

[isAccepted,loopClosureInfo,optimInfo] = addScan(___) は SLAM オブジェクトへのスキャンの追加に関する詳細情報を出力します。isAccepted はスキャンが追加されるか棄却されるかを表します。loopClosureInfo と optimInfo はループ閉じ込みが検出されているか、あるいは姿勢グラフが最適化されているかを表します。

例

すべて折りたたむ

ライブ スクリプトを開く

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.

入力引数

すべて折りたたむ

LIDAR SLAM オブジェクト。lidarSLAM オブジェクトとして指定します。このオブジェクトには、マップの作成に使用される、SLAM アルゴリズム パラメーター、センサー データ、基となる姿勢グラフが格納されています。

LIDAR スキャンの読み取り値。lidarScan オブジェクトとして指定します。このスキャンはスキャン マッチングを使用して、slamObj の最新のスキャンと相互に関連付けられます。

スキャンの相対姿勢推定値。[x y theta] ベクトルとして指定します。この相対姿勢により、スキャン マッチングが向上します。

出力引数

すべて折りたたむ

スキャンが受け入れられたかどうかを示します。true または false として返されます。スキャン間の相対姿勢が slamObj の MovementThreshold プロパティを下回る場合、スキャンは棄却されます。既定では、すべてのスキャンが受け入れられます。

ループ閉じ込みの詳細。次のフィールドをもつ構造体として返されます。

  • EdgeIDs –– 姿勢グラフで新しく接続されたエッジの ID。ベクトルとして返されます。

  • Edges –– 新しく追加されたループ閉じ込みエッジ。各エッジが接続するノード ID の n 行 2 列の行列として返されます。

  • Scores –– スキャン マッチングから返される、姿勢グラフで新しく接続されたエッジのスコア。ベクトルとして返されます。

メモ

slamObj で LoopClosureAutoRollback プロパティが true に設定されている場合、ループ閉じ込みエッジは姿勢グラフから削除できます。このプロパティは、最適化後に残差誤差が大きく変化する場合にループ閉じ込みを棄却します。そのため、この構造体にリストされているエッジ ID の一部が実際の姿勢グラフに存在しない場合があります。

姿勢グラフの最適化の詳細。次のフィールドをもつ構造体として返されます。

  • IsPerformed –– このスキャンの追加時に最適化が実行されたかどうかを示す boolean。最適化のパフォーマンスは slamObj の OptimizationInterval プロパティに依存します。

  • IsAccepted –– ResidualError に基づいて最適化が受け入れられたかどうかを示す boolean。

  • ResidualError –– 最適化に関連付けられた誤差。スカラーとして返されます。

  • LoopClosureRemoved –– 最適化中に削除されたループ閉じ込みエッジの ID のリスト。ベクトルとして返されます。

拡張機能

すべて展開する

バージョン履歴

R2019b で導入

参考

|

トピック