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

Загрузите прореженный набор данных, состоящий из лазерных сканов, собранных из мобильного робота во внутренней среде. Среднее смещение между каждыми двумя сканами составляет приблизительно 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');