Построить карту заполняемости из сканирования lidar
map = buildMap(scans,poses,mapResolution,maxRange)occupancyMap карта путем вставки лидара scans на данном poses. Укажите разрешение результирующей карты, mapResolutionи максимальный диапазон лидарного датчика, maxRange.
buildMap функция принимает показания лидарного сканирования и связанные позы, чтобы построить сетку занятости как lidarScan объекты и связанные с ними [x y theta] позы для построения occupancyMap.
Сканирование нагрузки и оценки позы, собранные с датчиков на роботе в гараже. Собранные данные коррелируются с использованием lidarSLAM алгоритм, который выполняет сопоставление сканирования, чтобы связать сканирования и настроить позы по всей траектории робота. Убедитесь, что сканирования и позы имеют одинаковую длину.
load scansAndPoses.mat
length(scans) == length(poses)ans = logical
1
Постройте карту. Укажите сканирования и позы в buildMap и включают требуемое разрешение карты (10 ячеек на метр) и максимальный диапазон лидара (19,2 метра). Каждое сканирование добавляется в соответствующих позах, и значения вероятности в сетке занятости обновляются.
occMap = buildMap(scans,poses,10,19.2);
figure
show(occMap)
title('Occupancy Map of Garage')
Использовать 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')