Минимизируйте Область значений поиска в основанных на сетке Сканах лидара соответствия с помощью БИНС

Открыть Live Script

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

Загрузка записанных данных

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

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

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

Синхронизация индексов времени IMU с индексами времени Лидара

Обновление БИНС и лидара с различными частотами дискретизации. Создайте массив, который сопоставляет лидар с индексами IMU.

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

Оценка рыскание от БИНС

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

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 функция с двумя различными опциями:

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

Повторите все показания сканов лидара, выполнив 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')

На основе результатов синхронизации установка показаний датчика БИНС в качестве оценки алгоритму сопоставления сканов улучшает время каждой итерации. В качестве последнего шага можно проверить, что различие в оценочном положении не значительно. В данном примере все положения из matchScansGrid те же самые.

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

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