Локализация и сопоставление с помощью сканирования lidar
lidarSLAM класс выполняет одновременную локализацию и отображение (SLAM) для входов датчика сканирования лидара. Алгоритм SLAM выполняет лидарные сканирования и присоединяет их к узлу в базовом графе позы. Затем алгоритм коррелирует сканирование с помощью сопоставления сканирования. Он также выполняет поиск замыканий цикла, где сканирование перекрывает ранее отображенные области, и оптимизирует положения узлов на графике позы.
slamObj = lidarSLAM
slamObj = lidarSLAM(mapResolution,maxLidarRange)MapResolution и MaxLidarRange свойства на основе входных данных.
slamObj = lidarSLAM(mapResolution,maxLidarRange,maxNumScans)maxNumScans является положительным целым числом. Этот предел сканирования требуется только при создании кода.
addScan | Добавить сканирование на карту lidar SLAM |
copy | Копировать объект lidar SLAM |
removeLoopClosures | Удалить замыкания петель из графика позы |
scansAndPoses | Извлечь сканы и соответствующие позы |
show | Сканировать графики и позы роботов |
Использовать lidarSLAM объект для итеративного добавления и сравнения лидарных сканирований и построения оптимизированного графика позы траектории робота. Чтобы получить карту занятости из связанных поз и сканирований, используйте buildMap функция.
Загрузить данные и настроить алгоритм 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')
[1] Хесс, Вольфганг, Деймон Колер, Хольгер Рапп и Даниэль Андор. «Замыкание петли в режиме реального времени в 2D LIDAR SLAM.» 2016 Международная конференция IEEE по робототехнике и автоматизации (ICRA). 2016.