Выполните локализацию и картографию, используя сканы лидара
The lidarSLAM класс выполняет одновременную локализацию и картографию (SLAM) для входов датчика скана лидара. Алгоритм SLAM принимает сканы лидара и присоединяет их к узлу в базовом графике положения. Затем алгоритм коррелирует сканы с помощью сопоставления сканов. Он также ищет замыкания цикла, где сканы перекрывают ранее отображенные области, и оптимизирует положения узла в графике положения.
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 LIDAR SLAM. 2016 IEEE International Conference on Robotics and Automation (ICRA). 2016.