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

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

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

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

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

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

Парковка транспортного средства на парковочном месте - это сложный маневр, который опирается на точную локализацию. Используйте предварительно построенную сцену «Большая парковка», чтобы создать такой сценарий. В примере Select Waypoints for Unreal Engine Simulation описано, как в интерактивном режиме выбрать последовательность путевых точек из сцены и как сгенерировать опорную траекторию транспортного средства. Этот пример использует записанную эталонную траекторию, полученную с использованием подхода, описанного в связанном примере. Во-первых, визуализируйте ссылку путь на 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

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

Настройте простую модель с транспортным средством, перемещающимся по заданному исходному пути, с помощью блока Simulation 3D Vehicle with 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);

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

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

hMapOnScene = helperSuperimposeMapOnSceneImage(sceneName, ptCloudAccum);

snapnow;

% Close the figure
close(hMapOnScene);

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

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

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

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

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

Симулируйте в цикле

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

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

Блок Localize является блоком MATLAB Function, который инкапсулирует алгоритм локализации - реализованный с помощью helperLidarRegistrationLocalizer класс. Этот блок принимает лидарное облако точек, сгенерированное блоком Simulation 3D Lidar, и начальное известное положение в качестве входов и производит оценку локализации. Оценка возвращается как, $(x, y, \theta)$ которая представляет 2-D положение лидара в опорной системе координат карты.
Подсистема Plan загружает предварительно спланированную траекторию из рабочей области с помощью refPoses, directions, curvatures и velocities переменные рабочей области. Блок Path Smooter Spline использовался для вычисления refPoses, directions и curvatures переменные. Блок Velocity Profiler вычислил velocities переменная.
Блок Helper Пути Analyzer использует траекторию ссылки и текущее положение, чтобы передать соответствующий опорный сигнал на контроллер транспортного средства.
Подсистема Vehicle Controller управляет рулевым управлением и скоростью транспортного средства, используя боковой и продольный контроллер, чтобы получить команду рулевого управления и ускорения или замедления. Для реализации этого используются блоки Lateral Controller Stanley и Longitudinal Controller Stanley. Эти команды подаются на модель транспортного средства, чтобы симулировать динамику транспортного средства в среде симуляции с помощью блока 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);

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

Чтобы выбрать другой сценарий, используйте блок Simulation 3D Scene Configuration. Выберите из существующих предварительно построенных сцен или создайте пользовательскую сцену в редакторе Unreal ®.
Чтобы создать другую ссылку траекторию, используйте helperSelectSceneWaypoints инструмент, как показано на примере Select 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

См. также

Функции

pcalign | pccat | pcplayer | pctransform | pcviewset

Блоки

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

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