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

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

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

load('offlineSlamData.mat');

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

Выполнение алгоритма SLAM, построение оптимизированной карты и графика траектории робота

Создать lidarSLAM и задайте разрешение карты и максимальный диапазон лидаров. В этом примере используется робот- Jackal™ из Clearpath Robotics™. Робот оснащен лазерным сканером SICK™ TiM-511 с максимальным диапазоном 10 метров. Установите максимальный диапазон лидара немного меньше максимального диапазона сканирования (8m), так как показания лазера менее точны вблизи максимального диапазона. Установите разрешение карты сетки 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 loop должен был добавить все сканирования, несмотря только на печать начального замыкания цикла.

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