라이다 스캔으로 SLAM(동시적 위치추정 및 지도작성) 구현하기

라이브 스크립트 열기

이 예제는 자세 그래프 최적화를 사용해 수집한 일련의 라이다 스캔에 SLAM(동시적 위치추정 및 지도작성)을 구현하는 방법을 보여줍니다. 목표는 라이다 스캔을 사용하여 환경 맵을 작성하고 로봇의 궤적을 검색하는 것입니다.

환경 맵을 작성하기 위해 SLAM 알고리즘은 라이다 스캔을 증분 방식으로 처리하고 이러한 스캔을 연결하는 자세 그래프를 만듭니다. 로봇은 스캔 매칭을 통해 이전에 방문한 장소를 인식하며 자신의 이동 경로를 따라 하나 이상의 루프 폐쇄를 설정합니다. SLAM 알고리즘은 이러한 루프 폐쇄 정보를 활용하여 맵을 업데이트하고 추정한 로봇 궤적을 조정합니다.

파일에서 레이저 스캔 데이터 불러오기

실내 환경에서 동작을 수행하는 이동 로봇으로부터 수집한 레이저 스캔으로 구성된 다운샘플링된 데이터 세트를 불러옵니다. 두 스캔 간의 평균 변위는 약 0.6미터입니다.

offlineSlamData.mat 파일에는 이 예제에 사용된 모든 레이저 스캔이 들어 있는 scans 변수가 포함되어 있습니다.

load('offlineSlamData.mat');

평면도와 로봇의 근사 경로는 실례를 보여주기 위한 것입니다. 아래 이미지는 맵 작성 대상인 관련 환경과 로봇의 근사 궤적을 보여줍니다.

SLAM 알고리즘을 실행하고, 최적화된 맵을 생성하고, 로봇의 궤적을 플로팅하기

lidarSLAM 객체를 생성하고 맵 해상도와 최대 라이다 거리를 설정합니다. 이 예제에서는 Clearpath Robotics™의 Jackal™ 로봇을 사용합니다. 이 로봇에는 최대 10미터의 거리를 가진 SICK™ TiM-511 레이저 스캐너가 장착되어 있습니다. 레이저 측정값은 최대 거리 근처에서 정확도가 낮아지기 때문에 최대 스캔 거리보다 약간 작게 최대 라이다 거리(8미터)를 설정합니다. 그리드 맵 해상도를 미터당 셀 20개로 설정합니다. 이는 5센티미터의 정확도를 제공합니다.

maxLidarRange = 8;
mapResolution = 20;
slamAlg = lidarSLAM(mapResolution, maxLidarRange);

다음의 루프 폐쇄 파라미터는 경험적으로 설정됩니다. 더 높은 루프 폐쇄 임계값을 사용하면 루프 폐쇄 식별 과정에서 거짓양성을 기각하는 데 도움이 됩니다. 하지만 높은 점수의 매칭이어도 여전히 잘못된 매칭일 수 있다는 사실에 유의하십시오. 예를 들어 유사하거나 반복되는 특징을 가진 환경에서 수집한 스캔은 거짓양성을 생성할 가능성이 높습니다. 더 높은 루프 폐쇄 탐색 반경을 사용하면 알고리즘은 루프 폐쇄에 대한 현재 자세 추정값을 중심으로 더 넓은 범위의 맵을 탐색할 수 있습니다.

slamAlg.LoopClosureThreshold = 210;  
slamAlg.LoopClosureSearchRadius = 8;

초기 10개 스캔을 사용해 맵 작성 과정 관찰하기

스캔을 slamAlg 객체에 점진적으로 추가합니다. 맵에 추가되면 스캔 번호가 인쇄됩니다. 스캔 사이의 간격이 너무 작으면 객체는 스캔을 거부합니다. 우선, 처음 10개 스캔을 추가하여 알고리즘을 테스트하십시오.

for i=1:10
    [isScanAccepted, loopClosureInfo, optimizationInfo] = addScan(slamAlg, scans{i});
    if isScanAccepted
        fprintf('Added scan %d \n', i);
    end
end

Added scan 1 
Added scan 2 
Added scan 3 
Added scan 4 
Added scan 5 
Added scan 6 
Added scan 7 
Added scan 8 
Added scan 9 
Added scan 10

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

figure;
show(slamAlg);
title({'Map of the Environment','Pose Graph for Initial 10 Scans'});

루프 폐쇄와 최적화 과정의 영향 관찰하기

루프에 스캔을 계속 추가합니다. 로봇이 움직이면 루프 폐쇄가 자동으로 감지되어야 합니다. 자세 그래프 최적화는 루프 폐쇄가 식별될 때마다 수행됩니다. 출력값 optimizationInfo에는 자세 그래프 최적화가 수행되고 있음을 나타내는 IsPerformed 필드가 있습니다.

루프 폐쇄가 식별될 때마다 스캔과 자세를 플로팅하여 결과를 시각적으로 확인하십시오. 이 플롯은 겹쳐진 스캔과 첫 번째 루프 폐쇄에 대해 최적화된 자세 그래프를 보여줍니다. 루프 폐쇄 모서리는 빨간색 링크로 추가됩니다.

firstTimeLCDetected = false;

figure;
for i=10:length(scans)
    [isScanAccepted, loopClosureInfo, optimizationInfo] = addScan(slamAlg, scans{i});
    if ~isScanAccepted
        continue;
    end
    % visualize the first detected loop closure, if you want to see the
    % complete map building process, remove the if condition below
    if optimizationInfo.IsPerformed && ~firstTimeLCDetected
        show(slamAlg, 'Poses', 'off');
        hold on;
        show(slamAlg.PoseGraph); 
        hold off;
        firstTimeLCDetected = true;
        drawnow
    end
end
title('First loop closure');

생성한 맵과 로봇의 궤적 시각화하기

모든 스캔을 slamAlg 객체에 추가한 후 최종적으로 작성된 맵을 플로팅합니다. 이전 for 루프는 초기 폐쇄만 플로팅했지만 이제 모든 스캔이 추가되었습니다.

figure
show(slamAlg);
title({'Final Built Map of the Environment', 'Trajectory of the Robot'});

작성한 맵을 원래 평면도와 비교하며 시각적으로 검사하기

스캔과 자세 그래프의 이미지가 원래 평면도에 겹쳐집니다. 모든 스캔을 추가하고 자세 그래프를 최적화한 후에 맵이 원래 평면도와 제대로 일치함을 확인할 수 있습니다.

점유 그리드 맵 구축하기

최적화된 스캔과 자세는 환경을 확률적 점유 그리드로 표현하는 occupancyMap을 생성하는 데 사용할 수 있습니다.

[scans, optimizedPoses]  = scansAndPoses(slamAlg);
map = buildMap(scans, optimizedPoses, mapResolution, maxLidarRange);

레이저 스캔과 최적화된 자세 그래프로 채워진 점유 그리드 맵을 시각화합니다.

figure; 
show(map);
hold on
show(slamAlg.PoseGraph, 'IDs', 'off');
hold off
title('Occupancy Grid Map Built Using Lidar SLAM');