Загрузить данные лазерного сканирования из файла

load lidarScans.mat

Данные лазерного сканирования были собраны мобильным роботом в помещении. Приблизительный план пола области вместе с траекторией робота через пространство показан на следующем рисунке.

Постройте график двух лазерных сканирований

Выберите два лазерных сканирования, из которых необходимо выполнить сканирование lidarScans. Они должны иметь общие черты, будучи близкими друг к другу в последовательности.

referenceScan = lidarScans(180);
currentScan = lidarScans(202);

Отображение двух сканирований. Обратите внимание, что есть поступательные и вращательные смещения, но некоторые элементы по-прежнему совпадают.

currScanCart = currentScan.Cartesian;
refScanCart = referenceScan.Cartesian;
figure
plot(refScanCart(:,1),refScanCart(:,2),'k.');
hold on
plot(currScanCart(:,1),currScanCart(:,2),'r.');
legend('Reference laser scan','Current laser scan','Location','NorthWest');

Запустить алгоритм сопоставления сканирования и отобразить преобразованное сканирование

Передайте эти два сканирования функции сопоставления сканирования. matchScans вычисляет относительную позу текущего сканирования относительно эталонного сканирования.

transform = matchScans(currentScan,referenceScan)

transform = 1×3

    0.5348   -0.0065   -0.0336

Чтобы визуально убедиться, что относительная поза вычислена правильно, преобразуйте текущее сканирование по расчетной позе с помощью transformScan. Это преобразованное лазерное сканирование может быть использовано для визуализации результата.

transScan = transformScan(currentScan,transform);

Отображение эталонного сканирования вместе с преобразованным текущим лазерным сканированием. Если проверка прошла успешно, два сканирования должны быть хорошо выровнены.

figure
plot(refScanCart(:,1),refScanCart(:,2),'k.');
hold on
transScanCart = transScan.Cartesian;
plot(transScanCart(:,1),transScanCart(:,2),'r.');
legend('Reference laser scan','Transformed current laser scan','Location','NorthWest');

Построить сетку занятости с помощью итеративного сопоставления сканирования

Если сопоставление сканирования применяется к последовательности сканирований, его можно использовать для восстановления грубой карты среды. Используйте occupancyMap класс для построения вероятностной карты заполняемости среды.

Создайте объект матрицы занятости для площади 15 метров на 15 метров. Установите начало координат карты [-7.5 -7.5].

map = occupancyMap(15,15,20);
map.GridLocationInWorld = [-7.5 -7.5]

map = 
  occupancyMap with properties:

   mapLayer Properties
        OccupiedThreshold: 0.6500
            FreeThreshold: 0.2000
    ProbabilitySaturation: [0.0010 0.9990]
                LayerName: 'probabilityLayer'
                 DataType: 'double'
             DefaultValue: 0.5000
      GridLocationInWorld: [-7.5000 -7.5000]
        GridOriginInLocal: [0 0]
       LocalOriginInWorld: [-7.5000 -7.5000]
               Resolution: 20
                 GridSize: [300 300]
             XLocalLimits: [0 15]
             YLocalLimits: [0 15]
             XWorldLimits: [-7.5000 7.5000]
             YWorldLimits: [-7.5000 7.5000]

Предварительно распределите массив для захвата абсолютного движения робота. Инициализировать первую позу как [0 0 0]. Все остальные позы относятся к первому измеренному сканированию.

numScans = numel(lidarScans);
initialPose = [0 0 0];
poseList = zeros(numScans,3);
poseList(1,:) = initialPose;
transform = initialPose;

Создайте цикл для обработки сканирований и сопоставления области. Лазерное сканирование обрабатывают попарно. Определите первое сканирование как эталонное сканирование, а второе сканирование как текущее сканирование. Затем два сканирования передаются в алгоритм согласования сканирования, и вычисляется относительная поза между двумя сканированиями. exampleHelperComposeTransform используется для вычисления кумулятивной абсолютной позы робота. Данные сканирования вместе с абсолютной позицией робота затем могут быть переданы в insertRay функция сетки занятости.

% Loop through all the scans and calculate the relative poses between them
for idx = 2:numScans
    % Process the data in pairs.
    referenceScan = lidarScans(idx-1);
    currentScan = lidarScans(idx);
    
    % Run scan matching. Note that the scan angles stay the same and do 
    % not have to be recomputed. To increase accuracy, set the maximum 
    % number of iterations to 500. Use the transform from the last
    % iteration as the initial estimate.
    [transform,stats] = matchScans(currentScan,referenceScan, ...
        'MaxIterations',500,'InitialPose',transform);
    
    % The |Score| in the statistics structure is a good indication of the
    % quality of the scan match. 
    if stats.Score / currentScan.Count < 1.0
        disp(['Low scan match score for index ' num2str(idx) '. Score = ' num2str(stats.Score) '.']);
    end
    
    % Maintain the list of robot poses. 
    absolutePose = exampleHelperComposeTransform(poseList(idx-1,:),transform);
    poseList(idx,:) = absolutePose;
       
    % Integrate the current laser scan into the probabilistic occupancy
    % grid.
    insertRay(map,absolutePose,currentScan,10);

end

Визуализация карты

Визуализируйте сетку занятости, заполненную лазерным сканированием.

figure
show(map);
title('Occupancy grid map built using scan matching results');

Постройте график абсолютных поз робота, которые были рассчитаны алгоритмом сопоставления сканирования. Это показывает путь, который робот прошел по карте окружающей среды.

hold on
plot(poseList(:,1),poseList(:,2),'bo','DisplayName','Estimated robot position');
legend('show','Location','NorthWest')

Ссылки

[1] П. Бибер, В. Штрассер, «Преобразование нормальных распределений: новый подход к сопоставлению лазерного сканирования», в Трудах Международной конференции IEEE/RSJ по интеллектуальным роботам и системам (IROS), 2003, стр. 2743-2748