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

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

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

load('offlineSlamData.mat');

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

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

Создайте lidarSLAM Объекту и установите разрешение карты и максимальную область значений лидара. Этот пример использует робота- Jackal™ из Robotics™ Clearpath. Робот оснащен 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 цикл должен был добавить все сканы, несмотря только на графическое изображение закрытия начального цикла.

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