Загрузить данные и настроить алгоритм SLAM

Загрузка массива ячеек lidarScan объекты. Сканирование лидара было собрано в гараже на роботе Husky ® от ClearPath Robotics ®. Как правило, лидарные сканирования выполняются с высокой частотой, и каждое сканирование не требуется для SLAM. Поэтому выполните выборку сканирования, выбирая только каждое 40-е сканирование.

load garage_fl1_southend.mat scans
scans = scans(1:40:end);

Чтобы настроить алгоритм SLAM, укажите диапазон лидара, разрешение карты, порог замыкания цикла и радиус поиска. Настройте эти параметры для конкретного робота и среды. Создать lidarSLAM объект с этими параметрами.

maxRange = 19.2; % meters
resolution = 10; % cells per meter

slamObj = lidarSLAM(resolution,maxRange);
slamObj.LoopClosureThreshold = 360;
slamObj.LoopClosureSearchRadius = 8;

Итеративно добавить сканирования

Использование for loop, добавление сканирований к объекту SLAM. Объект использует сопоставление сканирования для сравнения каждого добавленного сканирования с ранее добавленным. Чтобы улучшить карту, объект оптимизирует график позы всякий раз, когда обнаруживает замыкание цикла. Каждые 10 сканирований отображаются сохраненные позы и сканирования.

for i = 1:numel(scans)

    addScan(slamObj,scans{i});
    
    if rem(i,10) == 0
        show(slamObj);
    end
end

Просмотр карты занятости

После добавления всех сканирований к объекту SLAM создайте occupancyMap карта путем вызова buildMap со сканами и позами. Используйте то же разрешение карты и максимальный диапазон, что и для объекта SLAM.

[scansSLAM,poses] = scansAndPoses(slamObj);
occMap = buildMap(scansSLAM,poses,resolution,maxRange);
figure
show(occMap)
title('Occupancy Map of Garage')