Введение

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

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

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

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

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

Затем выберите траекторию движения транспортного средства в сцене. Пример «Выбор ППМ для моделирования нереального двигателя» описывает интерактивный выбор последовательности ППМ из сцены и формирование траектории транспортного средства. В этом примере используется записанный сегмент дисковода, полученный с помощью helperSelectSceneWaypoints , как описано в примере выбора ППМ.

% 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 конфигурируется со сценой US City Block с использованием блока Simulation 3D Scene Configuration. Модель помещает транспортное средство в сцену, используя Моделирование 3D Транспортное средство с Землей После блока. Датчик лидара крепится к транспортному средству с помощью блока Simulation 3D Lidar. В диалоговом окне блока используйте вкладку «Монтаж» для настройки расположения датчика. Используйте вкладку Параметры (Parameters), чтобы настроить свойства датчика для моделирования различных лидарных датчиков. В этом примере лидар устанавливается в центре крыши. Лидарный датчик предназначен для моделирования типичного датчика Velodine ® 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. Кроме того, модель использует блок «Из рабочего пространства» (Simulink) для загрузки смоделированных измерений с инерциального навигационного датчика (INS). Данные INS были получены с использованием insSensor и сохранен в MAT-файле.

В остальном примере выполняются следующие шаги:

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

Подсистема записи и визуализации записывает синтетические данные лидара в рабочую область с помощью блока «В рабочую область» (Simulink). Функциональный блок «Визуализация облака точек MATLAB» использует pcplayer объект для визуализации облаков потоковых точек. Функциональный блок Visualize INS Path MATLAB визуализирует потоковые данные 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 для коррекции дрейфа. Подробное описание процедуры см. в разделе Построение карты из данных Lidar с помощью 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.8, ...
    'MatchThreshold',       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 210 and 2 with RMSE 1.176767...
Accepted
Loop closure candidate found between view Id 211 and 3 with RMSE 2.901281...
Rejected

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

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

После оптимизации графика позы перестроите карту, используя обновленные позы. Это выполняется 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)

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

helperGetStartCloud Извлечение массива 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 постепенно создает карту-лидар с помощью сканирования облака точек. Каждое облако точек обрабатывается для удаления нулевой плоскости и эго-носителя и регистрируется относительно предыдущего облака точек. Затем постепенно строится карта облаков точек путем выравнивания и объединения облаков точек.

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

helperFeatureSearcher создает объект, который можно использовать для поиска ближайших совпадений элементов.

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

helPerSunImagedImage отображает изображение сцены Unreal в виде сверху.

helperStartPolyline обновляет положение полилинии, используемое совместно с helperStartExecutionImage.