В этом примере показано, как разработать и оценить алгоритм локализации лидара с помощью данных о лидаре синтетического продукта из Нереальной среды симуляции 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 (Computer Vision Toolbox)pcviewset организует данные об одометрии в набор представлений и связанных связей между представлениями, где:
Каждое представление имеет абсолютное положение, описывающее твердое преобразование к некоторой фиксированной системе координат.
Каждая связь имеет относительное положение, описывающее твердое преобразование между двумя соединяющимися представлениями.
Оценка локализации обеспечена в форме абсолютных положений для каждого представления относительно системы координат сцены.
Используйте pcplayer (Computer Vision Toolbox)
% 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 (Computer Vision Toolbox)pctransform (Computer Vision Toolbox)pcalign (Computer Vision Toolbox)
Наложите карту облака точек на изображение вида сверху сцены, чтобы визуально исследовать, как тесно это напоминает функции в сцене.
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, и известная начальная буква изображают из себя входные параметры, и производит оценку локализации. Оценка возвращена как
, который представляет 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]$](../../examples/driving/win64/LidarLocalizationInUnrealExample_eq13678673112243762356.png)
.
function angle = helperWrapToPi(angle) idx = (angle < -pi) | (angle > pi); angle(idx) = helperWrapTo2Pi(angle(idx) + pi) - pi; end
helperWrapTo2Pi Перенесите углы, чтобы быть в области значений![$[-2\pi, 2\pi]$](../../examples/driving/win64/LidarLocalizationInUnrealExample_eq04792388212521125941.png)
.
function angle = helperWrapTo2Pi(angle) pos = (angle>0); angle = mod(angle, 2*pi); angle(angle==0 & pos) = 2*pi; end
pcalign (Computer Vision Toolbox) | pccat (Computer Vision Toolbox) | pcplayer (Computer Vision Toolbox) | pctransform (Computer Vision Toolbox) | pcviewset (Computer Vision Toolbox)