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

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;

Создайте цикл для обработки сканов и отображения области. Лазерные сканы обрабатываются парами. Определите первое сканирование как эталонное, а второе - как текущее. Затем два сканов передаются в алгоритм сопоставления сканов, и вычисляется относительное положение между двумя сканами. The 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] P. Biber, W. Strasser, «Преобразование нормальных распределений: Новый подход к лазерному сопоставлению сканов», в трудах Международной конференции IEEE/RSJ по интеллектуальным роботам и системам (IROS), 2003, стр. 2743-2748