Введение

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

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

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

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

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

Затем выберите траекторию следования транспортного средства в сцене. В примере Select Waypoints for Unreal Engine Simulation описано, как в интерактивном режиме выбрать последовательность путевых точек из сцены и сгенерировать траекторию транспортного средства. Этот пример использует записанный сегмент диска, полученный с помощью 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])

The LidarSLAMIn3DSimulation Simulink модель сконфигурирована со сценой US City Block с помощью блока Simulation 3D Scene Configuration. Модель помещает транспортное средство на сцену с помощью блока Simulation 3D Vehicle with Ground Following. Датчик лидара присоединяется к транспортному средству с помощью блока Симуляции 3D Lidar. В диалоговом окне блока используйте вкладку Монтаж, чтобы настроить размещение датчика. Используйте вкладку Parameters, чтобы сконфигурировать свойства датчика, чтобы симулировать различные датчики лидара. В этом примере лидар установлен на центре крыши. Датчик лидара сконфигурирован, чтобы смоделировать типовой датчик HDL-32E Velodyne ®.

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 файла.

Остальная часть примера следует следующим шагам:

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

Подсистема Record and Visualize записывает синтетические данные лидара в рабочую область с помощью блока To Workspace (Simulink). Блок Visualize Point Cloud MATLAB Function использует pcplayer объект, чтобы визуализировать потоковые облака точек. Блок Visualize INS Path MATLAB Function визуализирует потоковые данные INS.

Симулируйте модель. На отображении потокового облака точек показаны синтетические данные о датчике лидара. На отображении сцены показаны синтетические данные о датчике INS. Когда модель завершила симуляцию, simOut переменная содержит структуру с переменными, записанными в рабочую область. The helperGetPointCloud функция извлекает данные датчика в массив pointCloud объекты. The 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. Для получения дополнительной информации см. раздел «Обработка облака точек».

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

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

Такой алгоритм восприимчив к дрейфу при накоплении карты на длинных последовательностях. Чтобы уменьшить дрейф, типично обнаружить закрытие цикла и использовать graph SLAM для коррекции дрейфа. Для получения подробной информации смотрите Build a Map from Lidar Data Using SLAM example. The 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)

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

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.

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

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

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

helper Update Polyline обновляет положение полилинии, используемое в сочетании с helper Show Scene Image.