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

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