Загрузите лазерные данные сканирования из файла

Загрузите прореженный набор данных, состоящий из лазерных сканирований, собранных из мобильного робота во внутренней среде. Среднее смещение между каждыми двумя сканированиями составляет приблизительно 0,6 метра.

offlineSlamData.mat файл содержит scans переменная, которая содержит все лазерные сканирования, используемые в этом примере

load('offlineSlamData.mat');

План здания и аппроксимированный путь робота предоставлены для иллюстративных целей. Это изображение показывает относительную сопоставляемую среду и аппроксимированная траектория робота.

Запустите алгоритм SLAM, создайте оптимизированную карту и постройте траекторию робота

Создайте a lidarSLAM объект и набор разрешение карты и макс. область значений лидара. Этот пример использует робота Jackal™ от Clearpath Robotics™. Робот оборудован сканером лазера SICK™ TiM-511 с макс. областью значений 10 метров. Установите макс. область значений лидара, немного меньшую, чем макс. область значений сканирования (8 м), когда лазерные показания являются менее точной близкой макс. областью значений. Установите разрешение карты сетки 20 ячеек на метр, который дает точность на 5 см.

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

Следующие параметры закрытия цикла устанавливаются опытным путем. Используя более высокий цикл порог закрытия помогает отклонить ложные положительные стороны в идентификационном процессе закрытия цикла. Однако имейте в виду, что соответствие высокого счета может все еще быть плохим соответствием. Например, сканирования, собранные в среде, которая имеет подобные или повторные функции, более вероятно, произведут ложные положительные стороны. Используя более высокий цикл радиус поиска закрытия позволяет алгоритму искать более широкую область значений карты вокруг текущих оценочных закрытий цикла for положения.

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'});

Визуально смотрите созданную карту по сравнению с исходным планом здания

Изображение сканирований и графика положения наложено на исходной планировке. Вы видите, что карта совпадает с исходным планом здания много позже добавления всех сканирований и оптимизации графика положения.

Создайте карту сетки заполнения

Оптимизированные сканирования и положения могут использоваться, чтобы сгенерировать a 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');