Лоцируйте локализацию с нереальной симуляцией Engine

Этот пример использует:

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

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

Этот пример использует Нереальную среду симуляции Engine от Epic Games®, чтобы разработать и оценить алгоритм локализации лидара от известного начального положения в сценарии парковки.

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

Парковка транспортного средства в место для парковки является сложным маневром, который использует точную локализацию. Используйте предварительно созданную Большую сцену Парковки, чтобы создать такой сценарий. Выбрать пример Waypoints for Unreal Engine Simulation описывает, как в интерактивном режиме выбрать последовательность waypoints от сцены и как сгенерировать ссылочную траекторию транспортного средства. Этот пример использует полученное использование записанной ссылочной траектории подхода, описанного в соединенном примере. Во-первых, визуализируйте ссылочный путь на 2D виде с высоты птичьего полета на сцену.

% 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 Транспортное средство с блоком Ground Following. Смонтируйте лидар на центре крыши транспортного средства с помощью блока 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 модель демонстрирует эту среду путем слияния алгоритма локализации, контроллера транспортного средства и подходящей модели транспортного средства.

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

Блоком Localize является блок MATLAB function, который инкапсулирует алгоритм локализации - реализованное использование helperLidarRegistrationLocalizer класс. Этот блок берет облако точек лидара, сгенерированное блоком Simulation 3D Lidar, и известная начальная буква изображают из себя входные параметры, и производит оценку локализации. Оценка возвращена как $(x, y, \theta)$ , который представляет 2D положение лидара в системе координат карты.
Подсистема Плана загружает предварительно запланированную траекторию из рабочей области с помощью refPoses, directions, curvatures и velocities переменные рабочей области. Блок Path Smoother Spline был использован для расчета refPoses, directions и curvatures переменные. Блок Velocity Profiler вычислил velocities переменная.
Блок Helper Path Analyzer использует ссылочную траекторию и текущее положение, чтобы накормить соответствующим опорным сигналом контроллер транспортного средства.
Подсистема контроллера Транспортного средства управляет регулированием и скоростью транспортного средства при помощи бокового и продольного контроллера, чтобы произвести регулирование и команду замедления или ускорение. Боковой диспетчер Стэнли и Продольные блоки диспетчера Стэнли используются, чтобы реализовать это. Эти команды питаются модель транспортного средства, чтобы симулировать динамику транспортного средства в среде симуляции с помощью блока Vehicle Body 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 Блок Configuration Сцены. Выберите из существующих предварительно созданных сцен или создайте пользовательскую сцену в Редакторе Unreal®.
Чтобы создать различную ссылочную траекторию, используйте helperSelectSceneWaypoints инструмент, как показано в Выбрать примере Waypoints for Unreal Engine Simulation.
Чтобы изменить настройку датчика используют блок Simulation 3D Lidar. Вкладка Mounting предоставляет возможности для определения различных размещений монтирования датчика. Вкладка 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

Смотрите также

Функции

pccat | pctransform | pcalign | pcviewset | pcplayer

Блоки

Simulation 3D Scene Configuration | Simulation 3D Vehicle with Ground Following | Simulation 3D Lidar

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