3차원 라이다 포인트 클라우드를 사용하여 SLAM 수행

다음 제품이 필요합니다.

이 예제에서는 수집된 3차원 라이다 센서 데이터에 포인트 클라우드 처리 알고리즘과 자세 그래프 최적화를 사용하여 SLAM(동시적 위치추정 및 지도작성) 알고리즘을 구현하는 방법을 보여줍니다. 이 예제의 목표는 로봇의 궤적을 추정하고, 3차원 라이다 포인트 클라우드와 추정된 궤적에서 환경의 3차원 점유 맵을 만드는 것입니다.

여기서 보여주는 SLAM 알고리즘은 NDT(Normal Distribution Transform: 정규분포변환)에 기반한 포인트 클라우드 정합 알고리즘을 사용하여 궤적을 추정하며, 로봇이 이전에 갔던 장소를 다시 방문할 때마다 trust-region 솔버를 이용한 SE3 자세 그래프 최적화를 통해 드리프트를 줄입니다.

데이터 불러오기 및 조정 가능형 파라미터 설정하기

주차장에 있는 Clearpath™ Husky 로봇에서 수집된 3차원 라이다 데이터를 불러옵니다. 라이다 데이터는 n×3 행렬로 구성된 셀형 배열을 포함하고 있습니다. 여기서 n은 캡처한 라이다 데이터의 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;

gridStep은 NDT 정합 알고리즘에 사용되는 복셀 그리드 크기를 지정합니다. 로봇이 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);

처리된 포인트 클라우드, 라이다 스캔, 서브맵에 대한 변수를 사전할당합니다. 맵을 신속하게 시각화하기 위해 다운샘플링된 포인트 클라우드 집합을 만듭니다.

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차원 라이다 SLAM 알고리즘을 사용하여 각각의 수집된 3차원 라이다 스캔으로부터 추정됩니다. 3차원 라이다 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로 지정된 작은 반경 내에서 현재 스캔을 이전 스캔과 매칭하여 수행됩니다. 이전 스캔 전체에 대해 탐색하는 것은 많은 시간이 소요됩니다. 여기서는 라이다 오도메트리의 드리프트가 작으므로 좁은 반경 내에서 탐색해도 충분합니다. 루프 폐쇄 쿼리는 다음 단계로 구성됩니다.

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