scansAndPoses

スキャンおよび対応する姿勢を抽出

構文

[scans,poses] = scansAndPoses(slamObj)

[scans,poses] = scansAndPoses(slamObj,nodeIDs)

説明

[scans,poses] = scansAndPoses(slamObj) は、lidarSLAM オブジェクトで使用されるスキャンを lidarScan オブジェクトとして返し、さらに slamObj の基となる姿勢グラフでそれらに関連付けられている姿勢 [x y theta] を返します。

[scans,poses] = scansAndPoses(slamObj,nodeIDs) は、特定のノード ID のスキャンと姿勢を返します。ノード ID を取得するには、slamObj の基となる poseGraph オブジェクトでノード ID を確認します。

例

LIDAR スキャンを使用した SLAM の実行

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]")

占有マップの表示

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

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

入力引数

slamObj — LIDAR SLAM オブジェクト

lidarSLAM オブジェクト

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

nodeIDs — 姿勢グラフにおけるノード ID

正の整数

姿勢グラフにおけるノード ID。正の整数として指定します。ノードは連続する ID 番号で姿勢グラフに追加されます。ノード ID を取得するには、slamObj の基となる poseGraph オブジェクトでノード ID を確認します。

出力引数

scans — LIDAR スキャンの読み取り値

lidarScan オブジェクト

LIDAR スキャンの読み取り値。lidarScan オブジェクトとして返されます。

poses — 各スキャンの姿勢

n 行 3 列の行列 | [x y theta] ベクトル

各スキャンの姿勢。[x y theta] ベクトルの n 行 3 列の行列として返されます。各行が scans のスキャンに対応する姿勢になります。

拡張機能

C/C++ コード生成

MATLAB® Coder™ を使用して C および C++ コードを生成します。

コード生成用の lidarSLAM オブジェクトを作成するときには、次の構文を使用します。

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

バージョン履歴

R2019b で導入

参考

poseGraph | optimizePoseGraph

トピック

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