SLAM を使用した 2 次元 LiDAR スキャンからのマップの作成

この例では、スキャン処理と姿勢グラフの最適化 (PGO) を使用して、一連の 2 次元 LiDAR スキャンに SLAM アルゴリズムを実装する方法を示します。この例の目標は、ロボットの軌跡を推定し、環境のマップを作成することです。

"SLAM" は、simultaneous localization and mapping (自己位置推定と環境地図作成の同時実行) の略です。

• 位置推定 — 既知の環境におけるロボットの姿勢を推定します。

• 地図作成 — 既知のロボットの姿勢とセンサー データから未知の環境のマップを作成します。

SLAM プロセスでは、ロボットが自身の位置を推定しながら環境のマップを作成します。SLAM は、ロボティクス、自動運転車、UAV などに幅広く応用されています。

オフライン SLAM では、ロボットが環境内を移動してセンサー データを記録します。このデータを SLAM アルゴリズムで処理して環境のマップを計算します。このマップを格納し、実際のロボットの運用時に位置推定やパス計画に使用します。

この例では、2 次元オフライン SLAM アルゴリズムを使用します。記録された LiDAR スキャンをアルゴリズムで段階的に処理し、環境のマップを作成するための姿勢グラフを作成します。推定されるロボットの軌跡で蓄積されるドリフトに対処するために、この例ではスキャン マッチングを使用して前に通った位置を認識し、このループ閉じ込み情報を姿勢の最適化と環境のマップの更新に使用します。姿勢グラフを最適化するために、この例では Navigation Toolbox™ の 2 次元姿勢グラフ最適化関数を使用します。

この例では、次の方法を学習します。

• スキャン レジストレーション アルゴリズムを使用して一連のスキャンからロボットの軌跡を推定する。

• 前に通った位置の特定 (ループ閉じ込み) により、推定されるロボットの軌跡に含まれるドリフトを最適化する。

• スキャンとその絶対姿勢を使用して環境のマップを可視化する。

レーザー スキャンの読み込み

この例では、Clearpath Robotics™ の Jackal™ ロボットを使用して屋内環境で収集されたデータを使用します。このロボットには、最大距離 10 メートルの SICK™ TiM-511 レーザー スキャナーが備えられています。レーザー スキャンを含む wareHouse.mat ファイルをワークスペースに読み込みます。

data = load("wareHouse.mat");
scans = data.wareHouseScans;

ロボットの軌跡の推定

lidarscanmapオブジェクトを作成します。このオブジェクトを使用して次のことが可能です。

• LiDAR スキャンの格納と段階的な追加。

• ループ閉じ込みの検出、追加、削除。

• スキャンの絶対姿勢の特定と更新。

• 姿勢グラフの生成と可視化。

LiDAR の最大距離とグリッド分解能の値を指定します。これらの値を変更して環境のマップを微調整できます。次の値を使用して LiDAR スキャン マップを作成します。

maxLidarRange = 8;
gridResolution = 20;
mapObj = lidarscanmap(gridResolution,maxLidarRange);

関数addScanを使用して、入力データから LiDAR スキャン マップ オブジェクトに段階的にスキャンを追加します。この関数により、そのスキャンが、連続するスキャンと近すぎる場合は棄却されます。

for i = 1:numel(scans)
    isScanAccepted = addScan(mapObj,scans{i});
     if ~isScanAccepted
        continue;
     end
end

LiDAR スキャン マップによって追跡されたスキャンと姿勢をプロットすることにより、シーンを再構成します。

hFigMap = figure;
axMap = axes(Parent=hFigMap);
show(mapObj,Parent=axMap);
title(axMap,"Map of the Environment and Robot Trajectory")

推定されるロボットの軌跡が時間の経過とともにドリフトしていることに注目してください。このドリフトの原因として次のいずれかが考えられます。

• センサーからのノイズを含むスキャンが十分にオーバーラップしていない。

• 環境に十分な特徴がない。

• 初期変換が不正確である。特に回転が大きい場合

推定される軌跡にドリフトがあると、環境のマップが不正確になります。

ドリフト補正

軌跡に含まれるドリフトを補正するには、"ループ"、つまりロボットが前に通って戻ってきた位置を正確に検出します。ループ閉じ込みエッジをlidarscanmapオブジェクトに追加することで、軌跡に含まれるドリフトが姿勢グラフの最適化の際に補正されます。

ループ閉じ込み検出

"ループ閉じ込み検出" では、与えられたスキャンについて、ロボットが現在の位置を前に通ったかどうかを判定します。この探索は、現在のロボットの位置を中心とする指定された半径 loopClosureSearchRadius の中で、現在のスキャンを過去のスキャンと照合することにより行われます。一致スコアが指定されたしきい値 loopClosureThreshold を超えていれば、スキャンを一致として受け入れます。

ループ閉じ込みは、lidarscanmapオブジェクトの関数detectLoopClosureを使用して検出し、関数addLoopClosureを使用してマップ オブジェクトに追加できます。

loopClosureThreshold の値を大きくするとループ閉じ込み検出の誤検知を回避できますが、類似した、または繰り返される特徴をもつ環境では、それでも誤った一致が関数から返されることがあります。これに対処するには、計算時間は増えますが、loopClosureSearchRadius の値を大きくして、現在のスキャンを中心とするループ閉じ込みの探索半径を広げます。

ループ閉じ込みの一致の数 loopClosureNumMatches. を指定することもできます。これらのパラメーターは、いずれもループ閉じ込み検出の微調整に役立ちます。

loopClosureThreshold = 110;
loopClosureSearchRadius = 2;
loopClosureNumMatches = 1;
mapObjLoop = lidarscanmap(gridResolution,maxLidarRange);
for i = 1:numel(scans)
    isScanAccepted = addScan(mapObjLoop,scans{i});
    % Detect loop closure if scan is accepted
     if isScanAccepted
         [relPose,matchScanId] = detectLoopClosure(mapObjLoop, ...
             MatchThreshold=loopClosureThreshold, ...
             SearchRadius=loopClosureSearchRadius, ...
             NumMatches=loopClosureNumMatches);
         % Add loop closure to map object if relPose is estimated
         if ~isempty(relPose)
            addLoopClosure(mapObjLoop,matchScanId,i,relPose);
         end
     end
end

軌跡の最適化

関数poseGraphを使用して、ドリフト補正後の LiDAR スキャン マップから姿勢グラフ オブジェクトを作成します。姿勢グラフの最適化には関数optimizePoseGraph (Navigation Toolbox)を使用します。

pGraph = poseGraph(mapObjLoop);
updatedPGraph = optimizePoseGraph(pGraph);

関数nodeEstimates (Navigation Toolbox)を使用して最適化された絶対姿勢を姿勢グラフから抽出し、軌跡を更新して環境の正確なマップを作成します。

optimizedScanPoses = nodeEstimates(updatedPGraph);
updateScanPoses(mapObjLoop,optimizedScanPoses);

結果の可視化

姿勢グラフの最適化の前と後のロボットの軌跡の変化を可視化します。赤い線はループ閉じ込みエッジを表します。

hFigTraj = figure(Position=[0 0 900 450]);

% Visualize robot trajectory before optimization
axPGraph = subplot(1,2,1,Parent=hFigTraj);
axPGraph.Position = [0.04 0.1 0.45 0.8];
show(pGraph,IDs="off",Parent=axPGraph);
title(axPGraph,"Before PGO")

% Visualize robot trajectory after optimization
axUpdatedPGraph = subplot(1,2,2,Parent=hFigTraj);
axUpdatedPGraph.Position = [0.54 0.1 0.45 0.8];
show(updatedPGraph,IDs="off",Parent=axUpdatedPGraph);
title(axUpdatedPGraph,"After PGO")
axis([axPGraph axUpdatedPGraph],[-6 10 -7 3])
sgtitle("Robot Trajectory",FontWeight="bold")

姿勢グラフの最適化の前と後の環境のマップとロボットの軌跡を可視化します。

hFigMapTraj = figure(Position=[0 0 900 450]);

% Visualize map and robot trajectory before optimization
axOldMap = subplot(1,2,1,Parent=hFigMapTraj);
axOldMap.Position = [0.05 0.1 0.44 0.8];
show(mapObj,Parent=axOldMap);
title(axOldMap,"Before PGO")

% Visualize map and robot trajectory after optimization
axUpdatedMap = subplot(1,2,2,Parent=hFigMapTraj);
axUpdatedMap.Position = [0.56 0.1 0.44 0.8];
show(mapObjLoop,Parent=axUpdatedMap);
title(axUpdatedMap,"After PGO")
axis([axOldMap axUpdatedMap],[-9 18 -10 9])
sgtitle("Map of the Environment and Robot Trajectory",FontWeight="bold")

参考

関数

• lidarscanmap | addScan | detectLoopClosure | addLoopClosure | deleteLoopClosure

関連するトピック

• MATLAB での点群 SLAM の実装
• Build a Map with Lidar Odometry and Mapping (LOAM) Using Unreal Engine Simulation
• Aerial Lidar SLAM Using FPFH Descriptors