このページは前リリースの情報です。該当の英語のページはこのリリースで削除されています。

スキャン マッチングによるロボットの姿勢の推定

この例では、正規分布変換 (NDT) アルゴリズム [1] を使用して 2 つのレーザー スキャンを一致させる方法を説明します。スキャン マッチングの目標は、スキャンを行った 2 つのロボット位置間の相対姿勢 (または変換) を検出することです。スキャンは、オーバーラップする特徴の形状を基に揃えることができます。

この姿勢を推定するために、NDT ではレーザー スキャンを 2 次元セルに分割し、各セルに対応する正規分布を割り当てます。この分布は、セル内の位置測定の確率を表します。確率密度が計算されると、最適化手法により現在のレーザー スキャンと基準のレーザー スキャンとの間の相対姿勢が見出されます。手法の収束を早めるために、姿勢の初期推定を指定できます。通常は、ロボットのオドメトリを使用して初期推定が行われます。

一連のスキャンにスキャン マッチングを適用した場合、それを使用して、ロボットが通過する環境の大まかなマップを復元できます。スキャン マッチングは、位置追跡や SLAM (位置推定とマッピングの同時実行) など、他のアプリケーションでも重要な役割を果たします。

ファイルからのレーザー スキャン データの読み込み

load scanMatchingData.mat

レーザー スキャン データは、屋内環境で移動ロボットにより収集されたものです。領域の大まかな見取り図をロボットの空間内のパスと共に次の図に示します。

2 つのレーザー スキャンのプロット

ROS メッセージ laserMsg から、スキャン マッチングを行う 2 つのレーザー スキャンを選択します。これらはシーケンス内で近くにあり、共通の特徴をもっているはずです。

referenceScan = lidarScan(laserMsg{180});
currentScan = lidarScan(laserMsg{202});

2 つのスキャンを表示します。並進と回転のオフセットがありますが、いくつかの特徴は一致しています。

currScanCart = currentScan.Cartesian;
refScanCart = referenceScan.Cartesian;
figure
plot(refScanCart(:,1), refScanCart(:,2), 'k.');
hold on
plot(currScanCart(:,1), currScanCart(:,2), 'r.');
legend('Reference laser scan', 'Current laser scan', 'Location', 'NorthWest');

スキャン マッチング アルゴリズムの実行と変換されたスキャンの表示

これら 2 つのスキャンをスキャン マッチング関数に渡します。matchScans は、基準スキャンに対する現在のスキャンの相対姿勢を計算します。

transform = matchScans(currentScan, referenceScan)
transform = 1×3

    0.5348   -0.0065   -0.0336

相対姿勢が正しく計算されたことを視覚的に確認するには、transformScan を使用して、計算された姿勢により現在のスキャンを変換します。この変換されたレーザー スキャンを使用して結果を可視化できます。

transScan = transformScan(currentScan, transform);

変換された現在のレーザー スキャンと共に基準スキャンを表示します。スキャン マッチングに成功した場合、2 つのスキャンは良く揃ったものになります。

figure
plot(refScanCart(:,1), refScanCart(:,2), 'k.');
hold on
transScanCart = transScan.Cartesian;
plot(transScanCart(:,1), transScanCart(:,2), 'r.');
legend('Reference laser scan', 'Transformed current laser scan', 'Location', 'NorthWest');

反復スキャン マッチングを使用した占有グリッド マップの作成

一連のスキャンにスキャン マッチングを適用した場合、それを使用して環境の大まかなマップを復元できます。robotics.OccupancyGrid クラスを使用して、環境の確率的占有グリッド マップを作成します。

15 m x 15 m の領域の占有グリッド オブジェクトを作成します。マップの原点を [-7.5 -7.5] に設定します。

map = robotics.OccupancyGrid(15, 15, 20);
map.GridLocationInWorld = [-7.5 -7.5]
map = 
  OccupancyGrid with properties:

        OccupiedThreshold: 0.6500
            FreeThreshold: 0.2000
    ProbabilitySaturation: [0.0010 0.9990]
                 GridSize: [300 300]
               Resolution: 20
             XWorldLimits: [-7.5000 7.5000]
             YWorldLimits: [-7.5000 7.5000]
      GridLocationInWorld: [-7.5000 -7.5000]

ロボットの絶対運動を捉える配列を事前に割り当てます。最初の姿勢を [0 0 0] として初期化します。その他すべての姿勢は、最初に測定されるスキャンとの相対的な姿勢です。

numScans = numel(laserMsg);
initialPose = [0 0 0];
poseList = zeros(numScans,3);
poseList(1,:) = initialPose;
transform = initialPose;

スキャンを処理して領域をマッピングするためのループを作成します。レーザー スキャンはペアで処理されます。最初のスキャンを基準スキャン、2 番目のスキャンを現在のスキャンとして定義します。次に、2 つのスキャンはスキャン マッチング アルゴリズムに渡され、2 つのスキャン間の相対姿勢が計算されます。関数 exampleHelperComposeTransform を使用して、ロボットの累積的な絶対姿勢が計算されます。これにより、スキャン データとロボットの絶対姿勢を、占有グリッドの関数 に渡すことができます。

% Loop through all the scans and calculate the relative poses between them
for idx = 2:numScans
    % Process the data in pairs.
    referenceScan = lidarScan(laserMsg{idx-1});
    currentScanMsg = laserMsg{idx};
    currentScan = lidarScan(currentScanMsg);
    
    % Run scan matching. Note that the scan angles stay the same and do 
    % not have to be recomputed. To increase accuracy, set the maximum 
    % number of iterations to 500. Use the transform from the last
    % iteration as the initial estimate.
    [transform, stats] = matchScans(currentScan, referenceScan, ...
        'MaxIterations', 500, 'InitialPose', transform);
    
    % The |Score| in the statistics structure is a good indication of the
    % quality of the scan match. 
    if stats.Score / currentScan.Count < 1.0
        disp(['Low scan match score for index ' num2str(idx) '. Score = ' num2str(stats.Score) '.']);
    end
    
    % Maintain the list of robot poses. 
    absolutePose = exampleHelperComposeTransform(poseList(idx-1,:), transform);
    poseList(idx,:) = absolutePose;
       
    % Integrate the current laser scan into the probabilistic occupancy
    % grid.
    insertRay(map, absolutePose, currentScan, double(currentScanMsg.RangeMax));

end

マップの可視化

レーザー スキャンの入力された占有グリッド マップを可視化します。

figure
show(map);
title('Occupancy grid map built using scan matching results');

スキャン マッチング アルゴリズムにより計算されたロボットの絶対姿勢をプロットします。これは、環境のマップをロボットが移動したパスを示します。

hold on
plot(poseList(:,1), poseList(:,2), 'bo', 'DisplayName', 'Estimated robot position');
legend('show', 'Location', 'NorthWest')

参考

参考文献

[1] P. Biber, W. Strasser, "The normal distributions transform: A new approach to laser scan matching," in Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2003, pp. 2743-2748