Минимизируйте поисковую область значений в основанном на сетке соответствии скана лидара Используя IMU

Скрипт Open Live Script

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

Загрузите записанные данные

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

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

Оцените рыскание от IMU

Оцените ориентацию от акселерометра и показаний гироскопа как кватернион с помощью 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 функция с двумя различными вариантами:

Исходное предположение по умолчанию и поисковая область значений
Исходное предположение на основе показаний датчика 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')

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