Минимизация диапазона поиска при сопоставлении Lidar Scan на основе сетки с помощью IMU

Открыть сценарий в реальном времени

В этом примере показано, как использовать инерциальный измерительный блок (IMU) для минимизации диапазона поиска угла поворота для алгоритмов сопоставления сканирования. Показания датчиков IMU используются для оценки ориентации транспортного средства и указываются в качестве первоначального предположения для matchScansGrid функция. Этот способ начальной оценки позы сравнивается с базовым алгоритмом с предположением начальной догадки [0 0 0].

Загрузить зарегистрированные данные

Загрузите MAT-файл, loggedLidarAndIMUData.mat.В этом файле содержатся данные сканирования llidar, показания акселерометра и гироскопы, а также соответствующие временные метки.

rng(1); % Fixed RNG seed for repeatibility
load('loggedLidarAndIMUData', ...
    'tLidar', 'lidarScans', ...
    'imuFs', 'tIMU', 'accel', 'gyro');

startIdx = 1;
endIdx = numel(lidarScans)-1;

Синхронизировать индексы времени IMU с индексами времени Лидара

IMU и лидар обновляются с различной частотой выборки. Создайте массив, сопоставляющий лидар индексам IMU.

lidarToIMUIndices = zeros(size(tLidar));
for i = 1:numel(tLidar)
    [~, lidarToIMUIndices(i)] = min(abs(tLidar(i) - tIMU));
end

Оценка Yaw от IMU

Оценка ориентации по показаниям акселерометра и гироскопа как кватерниона с помощью imufilter объект. Затем вычислите относительные значения yaws между последовательными сканированиями лидара путем преобразования кватернионов в углы Эйлера.

orientFilt = imufilter('SampleRate', imuFs);
q = orientFilt(accel, gyro);

% Calculate relative yaws
eulerAngs = euler(q(lidarToIMUIndices(1+(startIdx:endIdx))) ...
    .* conj(q(lidarToIMUIndices(startIdx:endIdx))), 'ZYX', 'frame');
imuYaws = eulerAngs(:,1);

Выполнить сопоставление сканирования и результаты журнала

Запустить matchScansGrid функция с двумя различными опциями:

Начальное предположение и диапазон поиска по умолчанию
Начальное предположение на основе показаний датчика IMU с небольшим диапазоном поиска

Итерация по всем показаниям сканирования лидара, выполнение matchScansGrid с каждой парой последовательных сканирований. Запишите время обработки для каждого вызова функции и относительные выходные данные позы из сопоставления сканирования. Для визуализации преобразованных сканирований на основе решения установите plotSolutions кому 1. Однако в этом примере разница в позе между двумя различными вариантами не заметна.

smallSearchRange = pi/8;
plotSolutions = 0;

% Initialize time values and relative pose arrays
timeDefaultSearch = NaN(endIdx - startIdx + 1,1);
timeSmallSearchWithIMU = NaN(endIdx - startIdx + 1,1);
allRelPosesDefault = NaN(endIdx - startIdx + 1,3);
allRelPosesIMU = NaN(endIdx - startIdx + 1,3);

for idx = startIdx:endIdx
    scan1 = lidarScans(idx);
    scan2 = lidarScans(idx+1);
    
    yaw = imuYaws(idx);
    initGuess = [0 0 yaw];
    
    % Run scan matching with default values.
    tic;
    relPose = matchScansGrid(scan2, scan1);
    timeDefaultSearch(idx) = toc;
    
    allRelPosesDefault(idx,:) = relPose;
    
    % Run scan matching with IMU-based initial yaw and small search range.
    tic;
    relPose = matchScansGrid(scan2, scan1, 'InitialPose', initGuess, ...
        'RotationSearchRange', smallSearchRange);
    timeSmallSearchWithIMU(idx) = toc;
    allRelPosesIMU(idx,:) = relPose;
    
    % Set plot solutions to 1 to turn on scan visualization.
    if plotSolutions == 1
        figure(cfg,'Visibile','on')
        plot(scan1)
        hold on
        plot(transformScan(scan2, allRelPosesDefault(idx,:)))
        plot(transformScan(scan2, allRelPosesIMU(idx,:)))
        hold off
        legend('Ref Scan','Default', ...
            'Small Search Range + IMU',...
            'Location','northwest')
        title(sprintf('Matched Lidar Scans %d and %d', i, i+1))
    end
    
end

Сравнить результаты

Визуализация и сравнение результатов проверки соответствия. Отображение общего времени обработки в виде гистограммы. Затем сравните время каждой итерации.

figure
title('Scan Matching Processing Time')
bar(categorical({'Default','IMU + Small Search'}), ...
    [sum(timeDefaultSearch),sum(timeSmallSearchWithIMU)])
ylabel('time (s)')

figure
title('Difference in Interation Time')
plot(startIdx:endIdx,(timeDefaultSearch - timeSmallSearchWithIMU))
ylabel('Time (seconds)')
xlabel('Iteration')

На основе результатов синхронизации задание показаний датчика IMU в качестве оценки алгоритма согласования сканирования улучшает время каждой итерации. В качестве последнего шага можно проверить несущественную разницу в позе оценки. Для этого примера все позы из matchScansGrid одинаковые.

figure
title('Difference in Pose Values')
plot(allRelPosesDefault-allRelPosesIMU)
legend('X','Y','Theta')

Документация