SLAM을 사용하여 2차원 라이다 스캔에서 맵 작성하기

이 예제에서는 스캔 처리와 PGO(자세 그래프 최적화)를 사용해 일련의 2차원 라이다 스캔에 대해 SLAM 알고리즘을 구현하는 방법을 보여줍니다. 이 예제의 목표는 로봇의 궤적을 추정하고 환경의 맵을 작성하는 것입니다.

SLAM은 동시적 위치추정 및 지도작성(Simultaneous Localization And Mapping)을 의미합니다.

• 위치추정 - 알려진 환경에서 로봇의 자세를 추정합니다.

• 지도작성 - 알려진 로봇 자세와 센서 데이터를 사용하여 미지 환경의 맵을 작성합니다.

SLAM 과정에서 로봇은 자신의 위치를 추정하는 동시에 환경 맵을 생성합니다. SLAM은 로보틱스, 자율주행 차량, UAV와 같은 다양한 응용 분야에서 활용됩니다.

오프라인 SLAM에서 로봇은 환경 속을 조향해 나가며 센서 데이터를 기록합니다. SLAM 알고리즘은 이 데이터를 처리하여 환경 맵을 계산합니다. 맵은 저장되어 실제 로봇 작동 중 위치추정과 경로 계획에 사용됩니다.

이 예제에서는 2차원 오프라인 SLAM 알고리즘을 사용합니다. 이 알고리즘은 기록된 라이다 스캔을 단계적으로 처리하고 자세 그래프를 작성하여 환경 맵을 만듭니다. 추정 로봇 궤적에 누적된 드리프트로 인한 문제를 해결하기 위해 이 예제에서는 스캔 매칭을 사용하여 이전에 방문한 지점들을 인식한 다음, 이 루프 폐쇄 정보를 사용하여 자세를 최적화하고 환경 맵을 업데이트합니다. 또한 자세 그래프를 최적화하기 위해 Navigation Toolbox™의 2차원 자세 그래프 최적화 함수를 사용합니다.

이 예제에서는 다음을 수행하는 방법을 배웁니다.

• 스캔 정합 알고리즘을 사용하여 일련의 스캔에서 로봇 궤적을 추정합니다.

• 이전에 방문한 지점(루프 폐쇄)을 식별하여 추정 로봇 궤적의 드리프트를 최적화합니다.

• 스캔과 각 스캔의 절대 자세를 사용하여 환경 맵을 시각화합니다.

레이저 스캔 불러오기

이 예제에서는 Clearpath Robotics™의 Jackal™ 로봇을 사용하여 실내 환경에서 수집된 데이터를 사용합니다. 이 로봇에는 최대 10미터의 거리를 스캔할 수 있는 SICK™ TiM-511 레이저 스캐너가 장착되어 있습니다. 레이저 스캔이 포함되어 있는 wareHouse.mat 파일을 작업 공간으로 불러옵니다.

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

로봇 궤적 추정하기

lidarscanmap 객체를 만듭니다. 이 객체를 사용하여 다음을 수행할 수 있습니다.

• 라이다 스캔을 단계적으로 저장하고 추가합니다.

• 루프 폐쇄를 검출, 추가, 삭제합니다.

• 스캔의 절대 자세를 찾고 업데이트합니다.

• 자세 그래프를 생성하고 시각화합니다.

최대 라이다 거리와 그리드 해상도 값을 지정합니다. 이러한 값을 수정하여 환경 맵을 미세 조정할 수 있습니다. 이러한 값을 사용하여 라이다 스캔 맵을 만듭니다.

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

addScan 함수를 사용하여 입력 데이터의 스캔을 라이다 스캔 맵 객체에 단계적으로 추가합니다. 이 함수는 연속된 스캔이 서로 너무 가까울 경우 스캔을 거부합니다.

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

스캔과 라이다 스캔 맵에 의해 추적된 자세를 플로팅하여 장면을 복원합니다.

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 함수를 사용하여 드리프트가 수정된 라이다 스캔 맵으로부터 자세 그래프 객체를 만듭니다. 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

관련 항목

• Implement Point Cloud SLAM in MATLAB
• Build a Map with Lidar Odometry and Mapping (LOAM) Using Unreal Engine Simulation
• Aerial Lidar SLAM Using FPFH Descriptors