Выполните локализацию и картографию, используя сканы лидара
lidarSLAM класс выполняет одновременную локализацию и картографию (SLAM) для входных параметров датчика скана лидара. Алгоритм SLAM берет в сканах лидара и присоединяет их к узлу в базовом графике положения. Алгоритм затем сопоставляет сканы используя соответствия сканирования. Это также ищет закрытия цикла for, где сканы перекрывают ранее сопоставленные области, и оптимизирует положения узла в графике положения.
slamObj = lidarSLAM
slamObj = lidarSLAM(mapResolution,maxLidarRange)MapResolution и MaxLidarRange свойства на основе входных параметров.
slamObj = lidarSLAM(mapResolution,maxLidarRange,maxNumScans)maxNumScans положительное целое число. Этот предел скана только требуется при генерации кода.
addScan | Добавьте сканы в лидарную карту SLAM |
copy | Скопируйте лидарный объект 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 цикл, добавляют сканы к объекту 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 ЛИДАРЕ SLAM". 2 016 международных конференций IEEE по вопросам робототехники и автоматизации (ICRA). 2016.