Локализация Lidar с имитацией нереального механизма

В этом примере используются:

В этом примере показано, как разработать и оценить алгоритм локализации лидара с использованием синтетических данных лидара из среды моделирования Unreal Engine ®.

Разработка алгоритма локализации и оценка его эффективности в различных условиях является сложной задачей. Одной из самых больших проблем является установление истины. Хотя с помощью дорогостоящих высокоточных инерциальных навигационных систем (INS) можно получить достоверную информацию о земле, виртуальное моделирование является экономически эффективной альтернативой. Использование моделирования позволяет проводить тестирование в различных сценариях и конфигурациях датчиков. Это также обеспечивает быструю итерацию развития и предоставляет точную достоверную информацию.

В этом примере используется среда моделирования Unreal Engine от Epic Games ® для разработки и оценки алгоритма локализации лидара из известной начальной позы в сценарии парковки.

Настройка сценария в среде моделирования

Парковка транспортного средства в месте стоянки - это сложный маневр, который зависит от точной локализации. Для создания такого сценария используйте предварительно созданную сцену большой парковки. Пример «Выбор ППМ для моделирования нереального двигателя» описывает интерактивный выбор последовательности ППМ из сцены и формирование траектории опорного транспортного средства. В этом примере используется записанная опорная траектория, полученная с использованием подхода, описанного в связанном примере. Сначала визуализируйте опорную траекторию на 2-D виде сцены с высоты птичьего полета.

% Load reference path
data = load('ReferencePathForward.mat');

refPosesX = data.ReferencePathForward.refPosesX;
refPosesY = data.ReferencePathForward.refPosesY;
refPosesT = data.ReferencePathForward.refPosesT;

sceneName = 'LargeParkingLot';
hScene = figure;
helperShowSceneImage(sceneName);
hold on
scatter(refPosesX(:,2), refPosesY(:,2), [], 'filled', 'DisplayName', ...
    'Reference Path');
xlim([0 40])
ylim([-20 10])
legend
hold off

Запись и визуализация данных датчиков

Настройте простую модель с транспортным средством хэтчбека, проходящим указанный справочный путь при помощи Моделирования 3D Транспортное средство с Землей После блока. Установите лидар в центре крыши транспортного средства с помощью блока Simulation 3D Lidar. Запишите и визуализируйте данные датчика. Записанные данные используются для разработки алгоритма локализации.

close(hScene)

if ~ispc
    error("Unreal Engine Simulation is supported only on Microsoft" ...
        + char(174) + " Windows" + char(174) + ".");
end

% Open model
modelName = 'recordAndVisualize';
open_system(modelName);
snapnow;

% Run simulation
simOut = sim(modelName);

Записанные данные датчика возвращаются в simOut переменная.

Разработка алгоритма с использованием записанных данных

В этом примере будет разработан алгоритм, основанный на регистрации облака точек. Регистрация облака точек - это обычный метод локализации, который оценивает относительное движение между двумя облаками точек для получения данных локализации. Накопление относительного движения, подобного этому, по длинным последовательностям может привести к дрейфу, который может быть скорректирован с помощью обнаружения замыкания цикла и оптимизации графика позы, как показано в примере Построение карты из данных Лидара с использованием SLAM. Поскольку в этом примере используется короткий опорный путь, обнаружение замыкания контура опущено.

Извлеките данные лидарного датчика, а также информацию о местоположении и ориентации земли, предоставленную блоком Simulation 3D Lidar. В мировой системе координат (сцене) предусмотрены местоположение и ориентация истинной земли. Извлеките известную начальную позу из данных истинности земли с помощью helperPoseToRigidTransform функция.

close_system(modelName);

% Extract lidar sensor data
ptCloudArr = helperGetPointClouds(simOut);

% Extract ground truth
[lidarLocation, lidarOrientation, simTimes] = helperGetLocalizerGroundTruth(simOut);

% Compute initial pose
initPose = [lidarLocation(1, :) lidarOrientation(1, :)];
initTform = helperPoseToRigidTransform(initPose);

Разработайте алгоритм локализации лидара, используя извлеченные данные датчика. Использовать pcviewset объект обработки и хранения данных одометрии. pcviewset организует данные одометрии в набор представлений и связанные связи между представлениями, где:

Каждый вид имеет абсолютную позу, описывающую жесткое преобразование в некоторую фиксированную опорную рамку.
Каждое соединение имеет относительную позу, описывающую жесткое преобразование между двумя соединительными видами.

Оценка локализации поддерживается в виде абсолютных поз для каждого вида относительно опорного кадра сцены.

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

% Create a view set
vSet = pcviewset;

absPose = initTform;
relPose = rigid3d;
viewId  = 1;

% Define rigid transformation between the lidar sensor mounting position
% and the vehicle reference point.
lidarToVehicleTform = helperPoseToRigidTransform(single([0 0 -1.57 0 0 0]));

% Process the point cloud frame
ptCloud = helperProcessPointCloud(ptCloudArr(1));

% Initialize accumulated point cloud map
ptCloudAccum = pctransform(ptCloud, absPose);

% Add first view to the view set
vSet = addView(vSet, viewId, absPose, 'PointCloud', ptCloud);

% Configure display
xlimits = [  0  50];
ylimits = [-25  10];
zlimits = [-30  30];
player = pcplayer(xlimits, ylimits, zlimits);
estimatePathHandle = [];
truthPathHandle    = [];

% Specify vehicle dimensions
centerToFront = 1.104;
centerToRear  = 1.343;
frontOverhang = 0.828;
rearOverhang  = 0.589;
vehicleWidth  = 1.653;
vehicleHeight = 1.513;
vehicleLength = centerToFront + centerToRear + frontOverhang + rearOverhang;
hatchbackDims = vehicleDimensions(vehicleLength,vehicleWidth,vehicleHeight, ...
'FrontOverhang',frontOverhang,'RearOverhang',rearOverhang);

vehicleDims   = [hatchbackDims.Length, hatchbackDims.Width, hatchbackDims.Height];
vehicleColor  = [0.85 0.325 0.098];

% Initialize parameters
skipFrames    = 5;      % Number of frames to skip to accumulate sufficient motion
prevViewId    = viewId;
prevPtCloud   = ptCloud;

% Loop over lidar sensor frames and localize
for viewId = 6 : skipFrames : numel(ptCloudArr)
    % Process frame
    ptCloud = helperProcessPointCloud(ptCloudArr(viewId));

    % Register current frame to previous frame
    relPose = pcregistericp(ptCloud, prevPtCloud, 'MaxIterations', 40, ...
        'Metric', 'pointToPlane');

    % Since motion is restricted to a 2-D plane, discard motion along Z to
    % prevent accumulation of noise.
    relPose.Translation(3) = 0;

    % Update absolute pose
    height = absPose.Translation(3);
    absPose = rigid3d( relPose.T * absPose.T );
    absPose.Translation(3) = height;

    % Add new view and connection to previous view
    vSet = addView(vSet, viewId, absPose, 'PointCloud', ptCloud);
    vSet = addConnection(vSet, prevViewId, viewId, relPose);

    % Accumulated point cloud map
    ptCloudAccum = pccat([ptCloudAccum, pctransform(ptCloud, absPose)]);

    % Compute ground truth and estimate position
    localizationEstimatePos = absPose.Translation;
    localizationTruthPos    = lidarLocation(viewId, :);

    % Update accumulated point cloud map
    view(player, ptCloudAccum);

    % Set viewing angle to top view
    view(player.Axes, 2);

    % Convert current absolute pose of sensor to vehicle frame
    absVehiclePose = rigid3d( lidarToVehicleTform.T * absPose.T );

    % Draw vehicle at current absolute pose
    helperDrawVehicle(player.Axes, absVehiclePose, vehicleDims, 'Color', vehicleColor);

    % Draw localization estimate and ground truth points
    helperDrawLocalization(player.Axes, ...
        localizationEstimatePos, estimatePathHandle, vehicleColor, ...
        localizationTruthPos, truthPathHandle, [0 1 0]);

    prevPtCloud = ptCloud;
    prevViewId  = viewId;
end

Увеличьте изображение хвостовой части траектории, чтобы проверить оценку локализации по сравнению с достоверностью на земле.

xlim(player.Axes, [0 15]);
ylim(player.Axes, [-15 0]);
zlim(player.Axes, [0 15]);

snapnow;

% Close player
hide(player);

Полезным результатом алгоритма локализации, основанного на регистрации облака точек, является карта пересекаемой среды. Эту карту можно получить, объединив все облака точек в общую опорную рамку. pccat используется в каждой итерации цикла выше, наряду с pctransform, для инкрементного объединения зарегистрированных облаков точек. Кроме того, можно использовать pcalign для выравнивания всех облаков точек по общей опорной системе координат за один снимок в конце.

Наложите карту облака точек на изображение сцены сверху, чтобы визуально увидеть, насколько она напоминает элементы сцены.

hMapOnScene = helperSuperimposeMapOnSceneImage(sceneName, ptCloudAccum);

snapnow;

% Close the figure
close(hMapOnScene);

Алгоритм локализации, описанный выше, инкапсулирован в helperLidarRegistrationLocalizer класс помощника. Этот класс может использоваться в качестве инфраструктуры для разработки трубопровода локализации с использованием регистрации облака точек.

Используйте 'ProcessFcnHandle' и 'ProcessFcnArguments' аргументы пары «имя-значение» для настройки обработки облаков точек перед регистрацией.
Используйте 'RegisterFcnHandle' и 'RegisterFcnArguments' аргументы пары «имя-значение» для настройки способа регистрации облаков точек.

Оценка точности локализации

Чтобы количественно оценить эффективность локализации, измерьте отклонение в оценках поступательного и вращательного движения по сравнению с достоверностью основания. Поскольку транспортное средство движется по плоской поверхности земли, этот пример касается только движения в плоскости X-Y.

hFigMetrics = helperDisplayMetrics(vSet, lidarLocation, lidarOrientation, simTimes);

Моделирование в цикле

Несмотря на то, что необходимы такие метрики, как отклонение в оценках поступательного и вращательного движения, производительность системы локализации может оказывать влияние на последующие процессы. Например, изменения в точности или рабочих характеристиках системы локализации могут влиять на контроллер транспортного средства, что требует повторного использования коэффициентов усиления контроллера. Поэтому крайне важно иметь замкнутую систему проверки, которая включает последующие компоненты. localizeAndControlUsingLidar модель демонстрирует эту структуру путем включения алгоритма локализации, контроллера транспортного средства и подходящей модели транспортного средства.

Модель имеет следующие основные компоненты:

Блок локализации является функциональным блоком MATLAB, который инкапсулирует алгоритм локализации, реализованный с помощью helperLidarRegistrationLocalizer класс. Этот блок принимает облако точек лидара, генерируемое блоком Simulation 3D Lidar, и исходную известную позу в качестве входных данных и создает оценку локализации. Оценка возвращается как, $(x, y, \theta)$ которая представляет 2-D положение лидара в опорном кадре карты.
Подсистема «План» загружает предварительно спланированную траекторию из рабочей области с помощью refPoses, directions, curvatures и velocities переменные рабочей области. Блок сглаживания траектории использовался для вычисления refPoses, directions и curvatures переменные. Блок Velocity Profiler рассчитал velocities переменная.
Блок анализатора вспомогательного пути использует опорную траекторию и текущую позу для подачи соответствующего опорного сигнала на контроллер транспортного средства.
Подсистема контроллера транспортного средства управляет рулевым управлением и скоростью транспортного средства с помощью бокового и продольного контроллера для получения команды управления рулевым управлением и ускорением или замедлением. Для этого используются блоки бокового контроллера Стэнли и продольного контроллера Стэнли. Эти команды подаются в модель транспортного средства для моделирования динамики транспортного средства в среде моделирования с использованием блока 3DOF кузова транспортного средства.

close(hFigMetrics);

% Load workspace variables for preplanned trajectory
refPoses   = data.ReferencePathForward.Trajectory.refPoses;
directions = data.ReferencePathForward.Trajectory.directions;
curvatures = data.ReferencePathForward.Trajectory.curvatures;
velocities = data.ReferencePathForward.Trajectory.velocities;
startPose  = refPoses(1, :);

% Open model
modelName = 'localizeAndControlUsingLidar';
open_system(modelName);
snapnow;

% Run simulation
sim(modelName);

close_system(modelName);

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

Чтобы выбрать другой сценарий, используйте блок «Моделирование» 3D «Конфигурация сцены». Выберите из существующих готовых сцен или создайте пользовательскую сцену в редакторе Unreal ®.
Чтобы создать другую траекторию привязки, используйте helperSelectSceneWaypoints , как показано в примере «Выбор ППМ для моделирования нереального двигателя».
Для изменения конфигурации датчика используйте блок Simulation 3D Lidar. Вкладка «Монтаж» содержит опции для указания различных мест установки сенсора. На вкладке Параметры (Parameters) представлены параметры изменения параметров датчика, такие как диапазон обнаружения, поле зрения и разрешение.

Вспомогательные функции

helperGetPointClouds Извлечение массива pointCloud объекты, содержащие данные лидарного датчика.

function ptCloudArr = helperGetPointClouds(simOut)

% Extract signal
ptCloudData = simOut.ptCloudData.signals.values;

% Create a pointCloud array
ptCloudArr = pointCloud(ptCloudData(:,:,:,3)); % Ignore first 2 frames
for n = 4 : size(ptCloudData,4)
    ptCloudArr(end+1) = pointCloud(ptCloudData(:,:,:,n));  %#ok<AGROW>
end
end

helperGetLocalizerGroundTruth Извлеките истинное местоположение и ориентацию земли.

function [lidarLocation, lidarOrientation, simTimes] = helperGetLocalizerGroundTruth(simOut)

lidarLocation    = squeeze(simOut.lidarLocation.signals.values)';
lidarOrientation = squeeze(simOut.lidarOrientation.signals.values)';
simTimes         = simOut.lidarLocation.time;

% Ignore first 2 frames
lidarLocation(1:2, :)    = [];
lidarOrientation(1:2, :) = [];
simTimes(1:2, :)         = [];
end

helperDrawLocalization Нарисуйте оценку локализации и истинность грунта на осях.

function [estHandle,truthHandle] = helperDrawLocalization(axesHandle, ...
    est, estHandle, estColor, truth, truthHandle, truthColor)

% Create scatter objects and adjust legend
if isempty(estHandle) || isempty(truthHandle)
    markerSize = 6;
    hold(axesHandle, 'on');
    estHandle   = scatter3(axesHandle, NaN, NaN, NaN, markerSize, estColor, 'filled');
    truthHandle = scatter3(axesHandle, NaN, NaN, NaN, markerSize, truthColor, 'filled');
    %legend(axesHandle, {'Points', 'Estimate', 'Truth'}, ...
    %    'Color', [1 1 1], 'Location', 'northeast');
    hold(axesHandle, 'off');
end

estHandle.XData(end+1) = est(1);
estHandle.YData(end+1) = est(2);
estHandle.ZData(end+1) = est(3);

truthHandle.XData(end+1) = truth(1);
truthHandle.YData(end+1) = truth(2);
truthHandle.ZData(end+1) = truth(3);
end

helperSuperimposeMapOnSceneImage Наложение карты облака точек на изображение сцены

function hFig = helperSuperimposeMapOnSceneImage(sceneName, ptCloudAccum)

hFig = figure('Name', 'Point Cloud Map');
hIm = helperShowSceneImage(sceneName);

hold(hIm.Parent, 'on')
pcshow(ptCloudAccum);
hold(hIm.Parent, 'off')

xlim(hIm.Parent, [-10 50]);
ylim(hIm.Parent, [-30 20]);
end

helperDisplayMetrics Отображение метрик для оценки качества локализации.

function hFig = helperDisplayMetrics(vSet, lidarLocation, lidarOrientation, simTimes)

absPoses = vSet.Views.AbsolutePose;
translationEstimates = vertcat(absPoses.Translation);
rotationEstimates    = pagetranspose(cat(3, absPoses.Rotation));

viewIds = vSet.Views.ViewId;
viewTimes = simTimes(viewIds);

xEst   = translationEstimates(:, 1);
yEst   = translationEstimates(:, 2);
yawEst = euler(quaternion(rotationEstimates, 'rotmat', 'point'), 'ZYX', 'point');
yawEst = yawEst(:, 1);

xTruth   = lidarLocation(viewIds, 1);
yTruth   = lidarLocation(viewIds, 2);
yawTruth = lidarOrientation(viewIds, 3);

xDeviation   = abs(xEst - xTruth);
yDeviation   = abs(yEst - yTruth);
yawDeviation = abs(helperWrapToPi(yawTruth - yawEst));

hFig = figure('Name', 'Metrics - Absolute Deviation');
subplot(3,1,1)
plot(viewTimes, xDeviation, 'LineWidth', 2);
ylim([0 1])
grid on
title('X')
xlabel('Time (s)')
ylabel('Deviation (m)')

subplot(3,1,2)
plot(viewTimes, yDeviation, 'LineWidth', 2);
ylim([0 1])
grid on
title('Y')
xlabel('Time (s)')
ylabel('Deviation (m)')

subplot(3,1,3)
plot(viewTimes, yawDeviation, 'LineWidth', 2);
ylim([0 pi/20])
grid on
title('Yaw')
xlabel('Time (s)')
ylabel('Deviation (rad)')
end

helperWrapToPi Углы обертывания должны находиться в диапазоне. $[-\pi, \pi]$

function angle = helperWrapToPi(angle)

idx = (angle < -pi) | (angle > pi);
angle(idx) = helperWrapTo2Pi(angle(idx) + pi) - pi;
end

helperWrapTo2Pi Углы обертывания должны находиться в диапазоне. $[-2\pi, 2\pi]$

function angle = helperWrapTo2Pi(angle)

pos = (angle>0);
angle = mod(angle, 2*pi);
angle(angle==0 & pos) = 2*pi;
end

См. также

Функции

pcalign | pccat | pcplayer | pctransform | pcviewset

Блоки

Моделирование 3D Лидар | Моделирование 3D конфигурация сцены | Имитация 3D транспортного средства со следующим грунтом

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