В этом примере показано, как записать синтетический продукт лоцируют данные о датчике из 3D среды симуляции, и разработать алгоритм одновременной локализации и отображения (SLAM) с помощью записанных данных.
Automated Driving Toolbox™ интегрирует 3D среду симуляции в Simulink®. 3D среда симуляции использует Нереальный Engine® Epic Games®. Блоки Simulink, связанные с 3D средой симуляции, могут быть найдены в drivingsim3d библиотека. Эти блоки обеспечивают способность к:
Выберите различные сцены в 3D среде симуляции
Поместите и переместите транспортные средства в сцену
Присоедините и сконфигурируйте датчики на транспортных средствах
Симулируйте данные о датчике на основе среды вокруг транспортного средства
Этот мощный инструмент симуляции может использоваться, чтобы добавить действительные данные при разработке, тестируя, и проверяя производительность автоматизированных ведущих алгоритмов, позволяя к сценариям тестирования, которые затрудняют, чтобы воспроизвести в реальном мире.
В этом примере вы оцениваете алгоритм восприятия лидара с помощью синтетических данных о лидаре, сгенерированных от 3D среды симуляции. Пример обходит вас через следующие шаги:
Запишите и визуализируйте синтетические данные о датчике лидара из 3D среды симуляции.
Разработайте алгоритм восприятия, чтобы создать карту с помощью SLAM в MATLAB®.
Во-первых, настройте сценарий в 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
Остальная часть примера выполняет эти шаги:
Симулируйте модель, чтобы записать синтетические данные о лидаре, сгенерированные датчиком и сохранить его в рабочую область.
Используйте данные о датчике, сохраненные в рабочую область, чтобы разработать алгоритм восприятия в MATLAB. Алгоритм восприятия создает карту среды с помощью SLAM.
Визуализируйте результаты созданной карты.
Запись и Визуализирует записи подсистемы синтетические данные о лидаре к рабочей области с помощью блока To Workspace. Визуализировать блок MATLAB function Облака точек использует pcplayer объект визуализировать облака точек потоковой передачи. Визуализировать блок MATLAB function Пути к INS визуализирует данные о INS потоковой передачи.
Симулируйте модель. Отображение облака точек потоковой передачи показывает синтетические данные о датчике лидара. Отображение сцены показывает синтетические данные о датчике INS. Если модель завершила симуляцию, simOut переменная содержит структуру с переменными, записанными в рабочую область. helperGetPointCloud функционируйте извлекает данные о датчике в массив pointCloud объекты. pointCloud объект является основной структурой данных, используемой, чтобы содержать данные о лидаре и выполнить обработку облака точек в MATLAB. Кроме того, данные о INS загружаются из MAT-файла, который будет позже использоваться, чтобы разработать алгоритм восприятия. Данные о INS были получены с помощью insSensor
% 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 класс берет входящие облака точек из датчика лидара и прогрессивно создает карту с помощью следующих шагов:
Предварительно обработайте облако точек: Предварительно обработайте каждое входящее облако точек, чтобы удалить наземную плоскость и автомобиль, оборудованный датчиком.
Облака точек регистра: Укажите входящее облако точек с последним облаком точек с помощью регистрационного алгоритма нормального распределения преобразовывает (NDT). pcregisterndt функция выполняет регистрацию. Улучшить точность и КПД регистрации, pcdownsample используется, чтобы проредить облако точек до регистрации. Начальная оценка преобразования может существенно улучшать регистрационную производительность. В этом примере измерения INS используются, чтобы выполнить это.
Выровняйте облака точек: Используйте предполагаемое преобразование, полученное из регистрации, чтобы преобразовать входящее облако точек к системе отсчета карты.
Представление обновления установило: Добавьте входящее облако точек и предполагаемое абсолютное положение как представление в pcviewset объект. Добавьте связь между текущим и предыдущим представлением с относительным преобразованием между ними.
updateMap метод helperLidarMapBuilder класс выполняет эти шаги. helperEstimateRelativeTransformationFromINS функция вычисляет первоначальную оценку для регистрации от симулированных показаний датчика INS.
Такой алгоритм восприимчив к дрейфу при накоплении карты по длинным последовательностям. Чтобы уменьшать дрейф, это типично, чтобы обнаружить закрытия цикла и график использования SLAM, чтобы откорректировать дрейф. Смотрите Сборку Карта из Данных о Лидаре Используя пример SLAM для подробной обработки. configureLoopDetector метод helperLidarMapBuilder класс конфигурирует обнаружение закрытия цикла. Если это сконфигурировано, обнаружение закрытия цикла происходит каждый раз updateMap вызывается, с помощью следующих функций помощника и классов:
helperExtractScanContextFeatures: Извлечения сканируют дескрипторы функции контекста от обнаружения закрытия цикла for облака точек.
helperCompareScanContextFeatures: Вычисляет расстояние функции между дескрипторами функции контекста сканирования.
helperFeatureSearcher: Хранилища вычислили дескрипторы функции и ищут самые близкие соответствия функции. Самые близкие соответствия функции являются кандидатами закрытия цикла. Расстояние функции вычисляется с помощью helperCompareScanContextFeatures.
helperLoopClosureDetector: Обнаруживает закрытия цикла. Кандидаты закрытия цикла идентифицированы путем вычисления функций контекста сканирования каждого входящего облака точек и нахождения самых близких соответствий функции. Затем регистрация облака точек используется, чтобы принять или отклонить кандидатов закрытия цикла.
% 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.