В этом примере показано, как разработать и оценить алгоритм локализации лидара, используя синтетические данные лидара из среды симуляции 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);
Разработайте алгоритм локализации лидара при помощи извлеченных данных о датчике. Использование pcviewsetpcviewset организует данные одометрии в набор представлений и связанных связей между представлениями, где:
Каждый вид имеет абсолютное положение, описывающее твердое преобразование в некоторую фиксированную опорную систему координат.
Каждое соединение имеет относительное положение, описывающее твердое преобразование между двумя соединительными видами.
Оценка локализации поддерживается в форме абсолютных положений для каждого вида относительно опорной системы координат сцены.
Использование 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 pccatpctransformpcalign
Наложите карту облака точек на изображение сцены сверху, чтобы визуально изучить, насколько она тесно напоминает функции сцены.
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, и начальное известное положение в качестве входов и производит оценку локализации. Оценка возвращается как, 
которая представляет 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]$](../../examples/shared_vision_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/shared_vision_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 | pccat | pcplayer | pctransform | pcviewset