Введение

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

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

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

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

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

Затем выберите траекторию для транспортного средства, чтобы следовать в сцене. Выбрать Waypoints для 3D примера Симуляции описывает, как в интерактивном режиме выбрать последовательность 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])

DesignLidarSLAMAlgorithmUsing3DSimulationEnvironment Модель 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 = 'DesignLidarSLAMAlgorithmUsing3DSimulationEnvironment';
open_system(modelName);
snapnow;

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

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

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

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

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

% 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. Для получения дополнительной информации смотрите Лидар и Облако точек, Обрабатывающее (Computer Vision Toolbox).

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', 3);

% 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 89 and 2 with RMSE 4.352160...
Rejected
Loop closure candidate found between view Id 107 and 68 with RMSE 5.316331...
Rejected
Loop closure candidate found between view Id 108 and 67 with RMSE 4.156444...
Rejected
Loop closure candidate found between view Id 134 and 100 with RMSE 4.299887...
Rejected
Loop closure candidate found between view Id 138 and 106 with RMSE 4.211920...
Rejected
Loop closure candidate found between view Id 172 and 121 with RMSE 9.887250...
Rejected
Loop closure candidate found between view Id 175 and 1 with RMSE 4.919369...
Rejected
Loop closure candidate found between view Id 189 and 156 with RMSE 4.660765...
Rejected
Loop closure candidate found between view Id 190 and 156 with RMSE 5.640325...
Rejected
Loop closure candidate found between view Id 198 and 154 with RMSE 6.151727...
Rejected
Loop closure candidate found between view Id 199 and 157 with RMSE 4.490933...
Rejected
Loop closure candidate found between view Id 207 and 174 with RMSE 4.646351...
Rejected
Loop closure candidate found between view Id 209 and 1 with RMSE 2.230219...
Accepted
Loop closure candidate found between view Id 210 and 2 with RMSE 1.864653...
Accepted
Loop closure candidate found between view Id 211 and 3 with RMSE 2.266311...
Accepted
Loop closure candidate found between view Id 212 and 4 with RMSE 2.328991...
Accepted

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

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

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

Используйте 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 массив pointCloud объекты.

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.

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

helperCompareScanContextFeatures сравнивает функции контекста сканирования от облака точек.

helperFeatureSearcher создает объект, который может использоваться, чтобы искать самые близкие соответствия функции.

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

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

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