Введение

Automated Driving Toolbox™ интегрирует Нереальную среду симуляции Engine в Simulink®. Блоки Simulink, связанные с этой средой симуляции, могут быть найдены в drivingsim3d библиотека. Эти блоки обеспечивают способность к:

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

В этом примере вы оцениваете алгоритм восприятия лидара с помощью синтетических данных о лидаре, сгенерированных от среды симуляции. Пример обходит вас через следующие шаги:

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

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

Затем выберите траекторию для транспортного средства, чтобы следовать в сцене. Выбрать пример Waypoints for Unreal Engine Simulation описывает, как в интерактивном режиме выбрать последовательность waypoints от сцены и сгенерировать траекторию транспортного средства. Этот пример использует полученное использование сегмента записанного диска helperSelectSceneWaypoints функция, как описано в waypoint примере выбора.

% Load reference path for recorded drive segment
xData   = load('refPosesX.mat');
yData   = load('refPosesY.mat');
yawData = load('refPosesT.mat');

% Set up workspace variables used by model
refPosesX = xData.refPosesX;
refPosesY = yData.refPosesY;
refPosesT = yawData.refPosesT;

% Display path on scene image
sceneName = 'USCityBlock';
hScene = figure;
helperShowSceneImage(sceneName);
hold on
scatter(refPosesX(:,2), refPosesY(:,2), 7, 'filled')

% Adjust axes limits
xlim([-150 100])
ylim([-125 75])

LidarSLAMIn3DSimulation Модель Simulink сконфигурирована со сценой Городского квартала США с помощью Симуляции 3D Блок Configuration Сцены. Модель помещает транспортное средство в сцену с помощью Симуляции 3D Транспортное средство с блоком Ground Following. Датчик лидара присоединен к транспортному средству с помощью блока Simulation 3D Lidar. В диалоговом окне блока используйте вкладку Mounting, чтобы настроить размещение датчика. Используйте вкладку Parameters, чтобы сконфигурировать свойства датчика симулировать различные датчики лидара. В этом примере лидар смонтирован на центре крыши. Датчик лидара сконфигурирован, чтобы смоделировать типичный датчик Velodyne® HDL-32E.

close(hScene)

if ~ispc
    error(['3D Simulation is only supported on Microsoft', char(174), ' Windows', char(174), '.']);
end

% Open the model
modelName = 'LidarSLAMIn3DSimulation';
open_system(modelName);
snapnow;

Модель записывает и визуализирует синтетические данные о лидаре. Записанные данные доступны посредством симуляции выход и могут использоваться для прототипирования вашего алгоритма в MATLAB. Кроме того, модель использует блок From Workspace (Simulink), чтобы загрузить симулированные измерения от Инерционного датчика навигации (INS). Данные о INS были получены при помощи insSensor объект, и сохраненный в файле MAT.

Остальная часть примера выполняет эти шаги:

Запишите и визуализируйте данные о датчике лидара синтетического продукта

Запись и Визуализирует записи подсистемы синтетические данные о лидаре к рабочей области с помощью блока To Workspace (Simulink). Визуализировать блок MATLAB function Облака точек использует pcplayer объект визуализировать облака точек потоковой передачи. Визуализировать блок MATLAB function Пути к INS визуализирует данные о INS потоковой передачи.

Симулируйте модель. Отображение облака точек потоковой передачи показывает синтетические данные о датчике лидара. Отображение сцены показывает синтетические данные о датчике INS. Если модель завершила симуляцию, simOut переменная содержит структуру с переменными, записанными в рабочую область. helperGetPointCloud функционируйте извлекает данные о датчике в массив pointCloud объекты. pointCloud объект является основной структурой данных, используемой, чтобы содержать данные о лидаре и выполнить обработку облака точек в MATLAB. Кроме того, данные о INS загружаются из файла MAT, который будет позже использоваться, чтобы разработать алгоритм восприятия. Данные о INS были получены с помощью insSensor объект. Данные о INS были обработаны, чтобы содержать [x, y, theta] положения в мировых координатах.

% Update simulation stop time to end when reference path is completed
simStopTime = refPosesX(end,1);
set_param(gcs, 'StopTime', num2str(simStopTime));

% Load INS data from MAT file
data = load('insMeasurement.mat');
insData = data.insMeasurement.signals.values;

% Run the simulation
simOut = sim(modelName);

% Create a pointCloud array from the recorded data
ptCloudArr = helperGetPointCloud(simOut);

Используйте записанные данные, чтобы разработать алгоритм восприятия

Синтетические данные о датчике лидара могут использоваться, чтобы разработать, экспериментировать с и проверить алгоритм восприятия в различных сценариях. Этот пример использует алгоритм, чтобы создать 3D карту среды от потоковой передачи данных о лидаре. Такой алгоритм является базовым блоком для приложений как локализация. Это может также использоваться, чтобы создать карты высокой четкости (HD) для географических областей, которые могут затем использоваться для онлайновой локализации. Карта, создающая алгоритм, инкапсулируется в helperLidarMapBuilder класс. Этот класс использует облако точек и возможности обработки лидара в MATLAB. Для получения дополнительной информации смотрите, что Облако точек Обрабатывает.

helperLidarMapBuilder класс берет входящие облака точек из датчика лидара и прогрессивно создает карту с помощью следующих шагов:

updateMap метод helperLidarMapBuilder класс выполняет эти шаги. helperEstimateRelativeTransformationFromINS функция вычисляет первоначальную оценку для регистрации от симулированных показаний датчика INS.

Такой алгоритм восприимчив к дрейфу при накоплении карты по длинным последовательностям. Чтобы уменьшать дрейф, это типично, чтобы обнаружить закрытия цикла и график использования SLAM, чтобы откорректировать дрейф. Смотрите Сборку Карта из Данных о Лидаре Используя пример SLAM для подробной обработки. configureLoopDetector метод helperLidarMapBuilder класс конфигурирует обнаружение закрытия цикла. Если это сконфигурировано, обнаружение закрытия цикла происходит каждый раз updateMap вызывается, с помощью следующих функций и классов:

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

% Set the random seed for example reproducibility
rng(0);

% Create a lidar map builder
mapBuilder = helperLidarMapBuilder('DownsamplePercent', 0.25, ...
    'RegistrationGridStep', 3.5, 'Verbose', true);

% Configure the map builder to detect loop closures
configureLoopDetector(mapBuilder, ...
    'LoopConfirmationRMSE',  2, ...
    'SearchRadius',          0.15, ...
    'DistanceThreshold',     0.15);

% Loop through the point cloud array and progressively build a map
skipFrames = 5;
numFrames  = numel(ptCloudArr);
exitLoop   = false;

prevInsMeas = insData(1, :);
for n = 1 : skipFrames : numFrames

    insMeas = insData(n, :);

    % Estimate initial transformation using INS
    initTform = helperEstimateRelativeTransformationFromINS(insMeas, prevInsMeas);

    % Update map with new lidar frame
    updateMap(mapBuilder, ptCloudArr(n), initTform);

    % Update top-view display
    isDisplayOpen = updateDisplay(mapBuilder, exitLoop);

    % Check and exit if needed
    exitLoop = ~isDisplayOpen;

    prevInsMeas = insMeas;
end

snapnow;

% Close display
closeDisplay = true;
updateDisplay(mapBuilder, closeDisplay);

Loop closure candidate found between view Id 211 and 4 with RMSE 1.019180...
Accepted

Накопленный дрейф прогрессивно увеличивается в зависимости от времени получившийся в неприменимой карте.

Если достаточные закрытия цикла обнаруживаются, накопленный дрейф может быть откорректирован с помощью оптимизации графика положения. Это выполняется optimizeMapPoses метод helperLidarMapBuilder класс, который использует createPoseGraph создать график положения и optimizePoseGraph (Navigation Toolbox), чтобы оптимизировать график положения.

После того, как график положения был оптимизирован, восстановите карту с помощью обновленных положений. Это выполняется rebuildMap метод helperLidarMapBuilder использование pcalign.

Используйте optimizeMapPoses и rebuildMap откорректировать для дрейфа и восстановить карту. Визуализируйте набор представления до и после оптимизации графика положения.

% Visualize viewset before pose graph optimization
hFigViewset = figure;
hG = plot(mapBuilder.ViewSet);
view(hG.Parent, 2);
title('Viewset Display')

% Optimize pose graph and rebuild map
optimizeMapPoses(mapBuilder);
rebuildMap(mapBuilder);

% Overlay viewset after pose graph optimization
hold(hG.Parent, 'on');
plot(mapBuilder.ViewSet);
hold(hG.Parent, 'off');

legend(hG.Parent, 'before', 'after')

Optimizing pose graph...done
Rebuilding map...done

Визуализируйте вычисленное использование карты накопленного облака точек записанных данных.

close(hFigViewset)

hFigMap = figure;
pcshow(mapBuilder.Map)

% Customize axes labels and title
xlabel('X (m)')
ylabel('Y (m)')
zlabel('Z (m)')
title('Point Cloud Map')

helperMakeFigurePublishFriendly(hFigMap);

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

% Close windows
close(hFigMap)
close_system(modelName)

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

Извлечение helperGetPointCloud массив showPointCloud объекты.

function ptCloudArr = helperGetPointCloud(simOut)

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

% Create a pointCloud array
ptCloudArr = pointCloud(ptCloudData(:,:,:,1));

for n = 2 : size(ptCloudData,4)
    ptCloudArr(end+1) = pointCloud(ptCloudData(:,:,:,n));  %#ok<AGROW>
end
end

helperMakeFigurePublishFriendly Настраивают фигуру так, чтобы снимок экрана, полученный, опубликовал, правильно.

function helperMakeFigurePublishFriendly(hFig)

if ~isempty(hFig) && isvalid(hFig)
    hFig.HandleVisibility = 'callback';
end
end

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

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

helperEstimateRelativeTransformationFromINS оценивает относительное преобразование из данных о INS.

helperShowSceneImage отображает изображение вида сверху Нереальной сцены.

helperUpdatePolyline обновляет полилинейное положение, используемое в сочетании с helperShowSceneImage.