3 次元 LiDAR 点群を使用した SLAM の実行

この例では次を使用します。

この例では、点群処理アルゴリズムと姿勢グラフの最適化を使用して、収集された 3 次元 LiDAR センサーデータに "自己位置推定と環境地図作成の同時実行" (SLAM) アルゴリズムを実装する方法を説明します。この例の目標は、ロボットの軌跡を推定し、3 次元 LiDAR 点群と推定された軌跡から環境の 3 次元占有マップを作成することです。

ここで示す SLAM アルゴリズムでは、正規分布変換 (NDT) に基づく点群レジストレーションアルゴリズムを使用して軌跡を推定し、ロボットが同じ場所を通るたびに信頼領域ソルバーを使用した SE3 姿勢グラフの最適化を使用してドリフトを減らします。

データの読み込みと調整可能なパラメーターの設定

ある駐車場で Clearpath™ Husky ロボットから収集した 3 次元 LiDAR データを読み込みます。この LiDAR データには、"n" 行 3 列の行列の cell 配列が格納されています。ここで、"n" は取得された LiDAR データ内の 3 次元の点の数で、列は取得された各点に関連付けられている "xyz" 座標を表します。

outputFolder = fullfile(tempdir,'ParkingLot');

dataURL = ['https://ssd.mathworks.com/supportfiles/lidar/data/' ...
            'NatickParkingLotLidarData.tar'];

pClouds = helperDownloadData(outputFolder,dataURL);

点群レジストレーションアルゴリズムのパラメーター

点群レジストレーションアルゴリズムを使用した軌跡の推定のパラメーターを指定します。maxLidarRange で 3 次元レーザースキャナーの最大範囲を指定します。

maxLidarRange = 20;

屋内環境で取得された点群データには床面と天井面の点が含まれていて、点群レジストレーションアルゴリズムで混同されます。次のパラメーターにより、一部の点が点群から削除されます。

referenceVector - 床面の法線方向。
maxDistance - 床面と天井面を削除するときのインライアの最大距離。
maxAngularDistance - 床面と天井面を当てはめるときの基準ベクトルからの最大角度偏差。

referenceVector = [0 0 1];
maxDistance = 0.5;
maxAngularDistance = 15;

レジストレーションアルゴリズムの効率と精度を高めるために、randomSampleRatio で指定したサンプル比の無作為抽出を使用して点群がダウンサンプリングされます。

randomSampleRatio = 0.25;

NDT レジストレーションアルゴリズムで使用されるボクセルのグリッドサイズを gridStep で指定します。スキャンは、ロボットの移動距離が distanceMovedThreshold を超えた後にのみ受け入れられます。

gridStep = 2.5;
distanceMovedThreshold = 0.3;

これらのパラメーターを、実際のロボットと環境に合わせて調整します。

ループ閉じ込み推定アルゴリズムのパラメーター

ループ閉じ込み推定アルゴリズムのパラメーターを指定します。ループ閉じ込みの探索は、現在のロボットの位置を中心とする loopClosureSearchRadius で指定した半径の中でのみ行われます。

loopClosureSearchRadius = 3;

ループ閉じ込みアルゴリズムは、2 次元のサブマップとスキャンマッチングに基づきます。サブマップは、スキャンが nScansPerSubmap 個 (サブマップあたりのスキャン数) 受け入れられるたびに作成されます。ループ閉じ込みアルゴリズムでループ候補を探す際、最新の subMapThresh 個のスキャンが無視されます。

nScansPerSubmap = 3;
subMapThresh = 50;

annularRegionLimits で指定した "z" 軸範囲の環状領域が点群から抽出されます。この範囲から外れる床と天井の点は、点群の平面当てはめアルゴリズムで関心領域が特定された後に削除されます。

annularRegionLimits = [-0.75 0.75];

ループ候補間の相対姿勢の推定で許容される最大平方根平均二乗誤差 (RMSE) を rmseThreshold で指定します。正確なループ閉じ込みエッジを推定するために小さい値を選択してください。これは、姿勢グラフの最適化に大きく影響します。

rmseThreshold = 0.26;

スキャンマッチングスコアに基づくループ閉じ込み受け入れのしきい値を loopClosureThreshold で指定します。姿勢グラフの最適化は、姿勢グラフに optimizationInterval 個のループ閉じ込みエッジが挿入された後に呼び出されます。

loopClosureThreshold = 150;
optimizationInterval = 2;

変数の初期化

姿勢グラフ、占有マップ、および必要な変数を設定します。

% 3D Posegraph object for storing estimated relative poses
pGraph = poseGraph3D;
% Default serialized upper-right triangle of 6-by-6 Information Matrix
infoMat = [1,0,0,0,0,0,1,0,0,0,0,1,0,0,0,1,0,0,1,0,1];
% Number of loop closure edges added since last pose graph optimization and map refinement
numLoopClosuresSinceLastOptimization = 0; 
% True after pose graph optimization until the next scan
mapUpdated = false;
% Equals to 1 if the scan is accepted
scanAccepted = 0;

% 3D Occupancy grid object for creating and visualizing 3D map
mapResolution = 8; % cells per meter
omap = occupancyMap3D(mapResolution);

処理された点群、LiDAR スキャン、およびサブマップの変数を事前に割り当てます。マップをすばやく可視化するために、ダウンサンプリングした点群のセットを作成します。

pcProcessed = cell(1,length(pClouds));
lidarScans2d = cell(1,length(pClouds)); 
submaps = cell(1,length(pClouds)/nScansPerSubmap);

pcsToView = cell(1,length(pClouds));

表示用の変数を作成します。

% Set to 1 to visualize created map and posegraph during build process
viewMap = 1; 
% Set to 1 to visualize processed point clouds during build process
viewPC = 0;

無作為抽出の整合性を確保するために乱数シードを設定します。

rng(0);

必要に応じて Figure ウィンドウを初期化します。

% If you want to view the point clouds while processing them sequentially
if viewPC==1
    pplayer = pcplayer([-50 50],[-50 50],[-10 10],'MarkerSize',10);
end

% If you want to view the created map and posegraph during build process
if viewMap==1
    ax = newplot; % Figure axis handle
    view(20,50);
    grid on;
end

軌跡の推定と姿勢グラフの最適化を使用した調整

ロボットの軌跡は、ロボットの姿勢 (3 次元空間における位置と向き) の集合です。ロボットの姿勢は、3 次元 LiDAR スキャンを取得する各インスタンスで 3 次元 LiDAR SLAM アルゴリズムを使用して推定されます。3 次元 LiDAR SLAM アルゴリズムには次のステップがあります。

点群のフィルター処理
点群のダウンサンプリング
点群のレジストレーション
ループ閉じ込みのクエリ
姿勢グラフの最適化

点群を反復的に処理して軌跡を推定します。

count = 0; % Counter to track number of scans added
disp('Estimating robot trajectory...');

Estimating robot trajectory...

for i=1:length(pClouds)
    % Read point clouds in sequence
    pc = pClouds{i};

点群のフィルター処理

点群のフィルター処理は、取得されたスキャンから関心領域を抽出するために行います。この例では、床と天井を削除した環状領域が関心領域です。

最大範囲から外れた無効な点とロボットの背後の運転する人間に対応する不要な点を削除します。

    ind = (-maxLidarRange < pc(:,1) & pc(:,1) < maxLidarRange ...
        & -maxLidarRange  < pc(:,2) & pc(:,2) < maxLidarRange ...
        & (abs(pc(:,2))>abs(0.5*pc(:,1)) | pc(:,1)>0));
    
    pcl = pointCloud(pc(ind,:));

床面の点を削除します。

    [~, ~, outliers] = ...
        pcfitplane(pcl, maxDistance,referenceVector,maxAngularDistance);
    pcl_wogrd = select(pcl,outliers,'OutputSize','full');

天井面の点を削除します。

    [~, ~, outliers] = ...
        pcfitplane(pcl_wogrd,0.2,referenceVector,maxAngularDistance);
    pcl_wogrd = select(pcl_wogrd,outliers,'OutputSize','full');

環状領域の点を選択します。

    ind = (pcl_wogrd.Location(:,3)<annularRegionLimits(2))&(pcl_wogrd.Location(:,3)>annularRegionLimits(1));
    pcl_wogrd = select(pcl_wogrd,ind,'OutputSize','full');

点群のダウンサンプリング

点群のダウンサンプリングにより、点群レジストレーションアルゴリズムの速度と精度が向上します。ダウンサンプリングは具体的なニーズに合わせて調整が必要です。現在のシナリオに対しては、以下のダウンサンプリングのバリアントから経験的に無作為抽出アルゴリズムを選択しています。

    pcl_wogrd_sampled = pcdownsample(pcl_wogrd,'random',randomSampleRatio);
    
    if viewPC==1
        % Visualize down sampled point cloud
        view(pplayer,pcl_wogrd_sampled);
        pause(0.001)
    end

点群のレジストレーション

点群のレジストレーションでは、現在のスキャンと前のスキャンの間の相対姿勢 (回転と並進) を推定します。最初のスキャンは常に受け入れられますが (さらに処理されて格納されます)、それ以外のスキャンは指定されたしきい値を超えて並進した後にのみ受け入れられます。推定された相対姿勢 (軌跡) が受け入れられると、それらが poseGraph3D を使用して格納されます。

    if count == 0
        % First can
        tform = [];
        scanAccepted = 1;
    else
        if count == 1
            tform = pcregisterndt(pcl_wogrd_sampled,prevPc,gridStep);
        else
            tform = pcregisterndt(pcl_wogrd_sampled,prevPc,gridStep,...
                'InitialTransform',prevTform);
        end
        
        relPose = [tform2trvec(tform.T') tform2quat(tform.T')];
        
        if sqrt(norm(relPose(1:3))) > distanceMovedThreshold
            addRelativePose(pGraph,relPose);
            scanAccepted = 1;
        else
            scanAccepted = 0;
        end
    end

ループ閉じ込みのクエリ

ループ閉じ込みのクエリでは、現在のロボットの位置を前に通ったことがあるかどうかを調べます。この探索は、現在のロボットの位置を中心とする loopClosureSearchRadius で指定された小さい半径の中で、現在のスキャンを過去のスキャンと照合することにより行われます。過去のすべてのスキャンと照合して探索するには時間がかかるため、LiDAR オドメトリはドリフトが少ないことから、小さい半径の中を探索すれば十分です。ループ閉じ込みのクエリは次の手順で構成されます。

連続する nScansPerSubmap 個のスキャンからサブマップを作成します。
現在のスキャンを loopClosureSearchRadius の範囲内でサブマップと照合します。
一致スコアが loopClosureThreshold を超えたら一致を受け入れます。受け入れられたサブマップを表すすべてのスキャンが可能性のあるループ候補と見なされます。
可能性のあるループ候補と現在のスキャンの間の相対姿勢を推定します。相対姿勢は、RMSE が rmseThreshold 未満の場合のみ、ループ閉じ込みの拘束として受け入れられます。

    if scanAccepted == 1
        count = count + 1;
        
        pcProcessed{count} = pcl_wogrd_sampled;
        
        lidarScans2d{count} = exampleHelperCreate2DScan(pcl_wogrd_sampled);
        
        % Submaps are created for faster loop closure query. 
        if rem(count,nScansPerSubmap)==0
            submaps{count/nScansPerSubmap} = exampleHelperCreateSubmap(lidarScans2d,...
                pGraph,count,nScansPerSubmap,maxLidarRange);
        end
        
        % loopSubmapIds contains matching submap ids if any otherwise empty.   
        if (floor(count/nScansPerSubmap)>subMapThresh)
            [loopSubmapIds,~] = exampleHelperEstimateLoopCandidates(pGraph,...
                count,submaps,lidarScans2d{count},nScansPerSubmap,...
                loopClosureSearchRadius,loopClosureThreshold,subMapThresh);
            
            if ~isempty(loopSubmapIds)
                rmseMin = inf;
                
                % Estimate best match to the current scan
                for k = 1:length(loopSubmapIds)
                    % For every scan within the submap
                    for j = 1:nScansPerSubmap
                        probableLoopCandidate = ...
                            loopSubmapIds(k)*nScansPerSubmap - j + 1;
                        [loopTform,~,rmse] = pcregisterndt(pcl_wogrd_sampled,...
                            pcProcessed{probableLoopCandidate},gridStep);
                        % Update best Loop Closure Candidate
                        if rmse < rmseMin
                            loopCandidate = probableLoopCandidate;
                            rmseMin = rmse;
                        end
                        if rmseMin < rmseThreshold
                            break;
                        end
                    end
                end
                
                % Check if loop candidate is valid
                if rmseMin < rmseThreshold
                    % loop closure constraint
                    relPose = [tform2trvec(loopTform.T') tform2quat(loopTform.T')];
                    
                    addRelativePose(pGraph,relPose,infoMat,...
                        loopCandidate,count);
                    numLoopClosuresSinceLastOptimization = numLoopClosuresSinceLastOptimization + 1;
                end
                     
            end
        end

姿勢グラフの最適化

姿勢グラフの最適化は、十分な数のループエッジが受け入れられた後に、軌跡の推定におけるドリフトを減らす目的で実行されます。最適化後は姿勢の推定における不確かさが小さくなることから、ループ閉じ込みの最適化のたびにループ閉じ込みの探索半径を小さくしていきます。

        if (numLoopClosuresSinceLastOptimization == optimizationInterval)||...
                ((numLoopClosuresSinceLastOptimization>0)&&(i==length(pClouds)))
            if loopClosureSearchRadius ~=1
                disp('Doing Pose Graph Optimization to reduce drift.');
            end
            % pose graph optimization
            pGraph = optimizePoseGraph(pGraph);
            loopClosureSearchRadius = 1;
            if viewMap == 1
                position = pGraph.nodes;
                % Rebuild map after pose graph optimization
                omap = occupancyMap3D(mapResolution);
                for n = 1:(pGraph.NumNodes-1)
                    insertPointCloud(omap,position(n,:),pcsToView{n}.removeInvalidPoints,maxLidarRange);
                end
                mapUpdated = true;
                ax = newplot;
                grid on;
            end
            numLoopClosuresSinceLastOptimization = 0;
            % Reduce the frequency of optimization after optimizing
            % the trajectory
            optimizationInterval = optimizationInterval*7;
        end

作成プロセス中にマップと姿勢グラフを可視化します。この可視化はコストが高いため、viewMap を 1 に設定して必要なときにだけ有効にします。可視化が有効になっている場合、スキャンが 15 個追加されるたびにプロットが更新されます。

        pcToView = pcdownsample(pcl_wogrd_sampled, 'random', 0.5);
        pcsToView{count} = pcToView;
        
        if viewMap==1
            % Insert point cloud to the occupance map in the right position
            position = pGraph.nodes(count);
            insertPointCloud(omap,position,pcToView.removeInvalidPoints,maxLidarRange);
            
            if (rem(count-1,15)==0)||mapUpdated
                exampleHelperVisualizeMapAndPoseGraph(omap, pGraph, ax);
            end
            mapUpdated = false;
        else
            % Give feedback to know that example is running
            if (rem(count-1,15)==0)
                fprintf('.');
            end
        end

前の相対姿勢の推定と点群を更新します。

        prevPc = pcl_wogrd_sampled;
        prevTform = tform;
    end
end

Doing Pose Graph Optimization to reduce drift.

3 次元占有マップの作成と可視化

推定されたグローバルな姿勢を使用して occupancyMap3D に点群が挿入されます。すべてのノードを反復した後、完全なマップと推定されたビークルの軌跡が表示されます。

if (viewMap ~= 1)||(numLoopClosuresSinceLastOptimization>0)
    nodesPositions = nodes(pGraph);
    % Create 3D Occupancy grid
    omapToView = occupancyMap3D(mapResolution);
    
    for i = 1:(size(nodesPositions,1)-1)
        pc = pcsToView{i};
        position = nodesPositions(i,:);
        
        % Insert point cloud to the occupance map in the right position
        insertPointCloud(omapToView,position,pc.removeInvalidPoints,maxLidarRange);
    end
    
    figure;
    axisFinal = newplot;
    exampleHelperVisualizeMapAndPoseGraph(omapToView, pGraph, axisFinal);
end

ユーティリティ関数

function pClouds = helperDownloadData(outputFolder,lidarURL)
% Download the data set from the given URL to the output folder.
    lidarDataTarFile = fullfile(outputFolder,'NatickParkingLotLidarData.tar');    
    if ~exist(lidarDataTarFile,'file')
        mkdir(outputFolder);        
        disp('Downloading Lidar data (472 MB)...');
        websave(lidarDataTarFile,lidarURL);
        untar(lidarDataTarFile,outputFolder);
    end    
    % Extract the file.
    if (~exist(fullfile(outputFolder,'NatickParkingLotLidarData'),'dir'))
        untar(lidarDataTarFile,outputFolder);
    end

    pClouds = load(fullfile(outputFolder,'NatickParkingLotLidarData', 'NatickParkingLotLidarData.mat')).pClouds(1:5:end);
end