Карты занятости предлагают простой, но надежный способ представления среды для роботизированных приложений, отображая непрерывное мировое пространство на дискретную структуру данных. Отдельные ячейки сетки могут содержать двоичную или вероятностную информацию о препятствиях. Однако автономная платформа может использовать множество датчиков, которые могут нуждаться в объединении для оценки как текущего состояния платформы, так и состояния окружающей среды.
Этот пример фокусируется на интеграции различных датчиков для оценки состояния окружающей среды и сохранения значений заполненности различными слоями карты. В примере показано, как multiLayerMap объект может быть использован для визуализации, отладки и запала данных, собранных с трех лидарных датчиков, установленных на автономном транспортном средстве. Показания датчиков в этом примере моделируются с использованием набора lidarPointCloudGenerator (Automated Driving Toolbox) объекты, которые записывают показания из сопровождающих drivingScenario (Автоматическая панель инструментов управления).
Каждый лидар обновляет свой собственный validatorOccupancyMap3D объект, позволяющий визуализировать локальную карту, созданную каждым датчиком в изоляции. Эти локальные карты могут использоваться для быстрой идентификации источников шума или ошибки монтажа и могут помочь в выборе подходящего метода слияния. multiLayerMap содержит четвертый mapLayer , который использует пользовательскую функцию обратного вызова для слияния данных, содержащихся в каждом слое занятости. Наконец, карта с предохранителем используется для обновления соответствующего субрегиона карты мира, когда автономное транспортное средство движется по заранее спланированному пути.
Сначала создайте drivingScenario объект и заполните сцену несколькими зданиями с помощью функции поддержки примера. Функция также визуализирует сцену.
scene = drivingScenario; groundTruthVehicle = vehicle(scene,'PlotColor',[0 0.4470 0.7410]); % Add a road and buildings to scene and visualize. exampleHelperPopulateScene(scene,groundTruthVehicle);
Создайте траекторию, которая следует по главной дороге в сцене, используя waypointTrajectory объект.
sampleRate = 100;
speed = 10;
t = [0 20 25 44 46 50 54 56 59 63 90].';
wayPoints = [ 0 0 0;
200 0 0;
200 50 0;
200 230 0;
215 245 0;
260 245 0;
290 240 0;
310 258 0;
290 275 0;
260 260 0;
-15 260 0];
velocities = [ speed 0 0;
speed 0 0;
0 speed 0;
0 speed 0;
speed 0 0;
speed 0 0;
speed 0 0;
0 speed 0;
-speed 0 0;
-speed 0 0;
-speed 0 0];
traj = waypointTrajectory(wayPoints,'TimeOfArrival',t,...
'Velocities',velocities,'SampleRate',sampleRate);Для сбора показаний лидара из сценария вождения создайте три lidarPointcloudGenerator объекты, использующие функцию примера-помощника. Это транспортное средство было сконфигурировано с двумя передними, узкими лидарами поля зрения (FOV) и одним широким Lidar, обращенным назад. Перекрывающаяся область обоих датчиков, обращенных спереди, должна помочь быстро регистрировать и подтверждать свободное пространство перед транспортным средством, в то время как диапазон датчиков, обращенных сзади, помогает отображать пересекающуюся область.
lidarSensors = exampleHelperCreateVehicleSensors(scene, groundTruthVehicle); disp(lidarSensors)
Columns 1 through 2
{1x1 lidarPointCloudGenerator} {1x1 lidarPointCloudGenerator}
Column 3
{1x1 lidarPointCloudGenerator}
Создать multiLayerMap объект, состоящий из трех occupancyMap объекты и один общий mapLayer объект. Каждый локальный occupancyMap обновляется соответствующим лидарным датчиком. Объединение данных из всех карт в mapLayer объект, установите GetTransformFcn аргумент «имя-значение» для exampleHelperFuseOnGet функция, сохраненная как дескриптор fGet. exampleHelperFuseOnGet функция объединила все три значения данных карты, вызвав getMapData функция на каждом и с использованием логарифмического суммирования значений.
% Define map and parameters. res = 2; width = 100*2; height = 100*2; % Define equal weights for all sensor readings. weights = [1 1 1]; % Create mapLayers for each sensor. fLeftLayer = occupancyMap(width,height,res,'LayerName','FrontLeft'); fRightLayer = occupancyMap(width,height,res,'LayerName','FrontRight'); rearLayer = occupancyMap(width,height,res,'LayerName','Rear'); % Create a get callback used to fuse data in the three layers. fGet = @(obj,values,varargin)... exampleHelperFuseOnGet(fLeftLayer,fRightLayer,rearLayer,... weights,obj,values,varargin{:}); % Create a generic mapLayer object whose getMapData function fuses data from all % three layers. fusedLayer = mapLayer(width,height,'Resolution',res,'LayerName','FuseLayer',... 'GetTransformFcn',fGet,'DefaultValue',0.5); % Combine layers into a multiLayerMap. egoMap = multiLayerMap({fLeftLayer, fRightLayer, rearLayer, fusedLayer}); % Set map grid origin so that the robot is located at the center. egoMap.GridOriginInLocal = -[diff(egoMap.XLocalLimits) diff(egoMap.YLocalLimits)]/2;
Создайте пустую карту мира. Эта карта периодически обновляется с использованием данных из слоя слияния. Используйте эту карту, чтобы указать, насколько хорошо работает метод слияния лидаров.
% Create an empty reconstruction layer covering the same area as world map. reconLayer = occupancyMap(400,400,res,... % width,height,resolution 'LayerName','FuseLayer','LocalOriginInWorld',[-25 -50]);
Постройте график эгоцентрических слоев рядом с реконструированной картой. Используйте функцию exampleHelperSunEgoMap для отображения каждой локальной карты.
% Setup the display window. axList = exampleHelperSetupDisplay(groundTruthVehicle,lidarSensors); % Display the reconstructionLayer and submap region. show(reconLayer,'Parent', axList{1}); hG = findobj(axList{1},'Type','hggroup'); egoOrientation = hG.Children; egoCenter = hgtransform('Parent',hG); egoOrientation.Parent = egoCenter; gridLoc = egoMap.GridLocationInWorld; xLimits = egoMap.XLocalLimits; yLimits = egoMap.YLocalLimits; rectangle('Parent',egoCenter,... 'Position',[gridLoc diff(xLimits) diff(yLimits)],... 'EdgeColor','r'); % Display the local maps built by each sensor alongside the reconstruction map. exampleHelperShowEgoMap(axList,egoMap,[0 0 0],{'FrontLeft Lidar','FrontRight Lidar','Rear Lidar','Fused'});
Перемещайте робота вдоль траектории, обновляя карту с помощью смоделированных показаний Лидара.
Для запуска сценария управления вызовите exampleHelperResetSimulation функция помощника. При этом выполняется сброс моделирования и траектории, очистка карты и перемещение эгоцентрических карт обратно в первую точку траектории.
exampleHelperResetSimulation(scene,traj,lidarSensors,egoMap,reconLayer)
Позвоните в exampleHelperRunSimulation для выполнения моделирования.
Основными операциями контура моделирования являются:
Получите следующую позу на траектории из traj и извлекают ориентацию оси Z (тета) из кватерниона.
Переместить egoMap к новому [x y theta] позу.
Извлеките данные датчика из lidarPointCloudGenerators.
Обновление локальных карт с использованием данных датчиков insertRay.
Обновление глобальной карты с помощью mapLayer слитый результат.
Обновите визуализацию.
exampleHelperRunSimulation(scene,traj,groundTruthVehicle,egoMap,lidarSensors,reconLayer,axList)
Отображенные результаты показывают, что передний правый датчик вводит большое количество шума в конденсированную карту. Обратите внимание, что правая стенка имеет большую изменчивость по всей траектории. Вы не хотите полностью сбрасывать показания этого датчика, так как датчик по-прежнему обнаруживает свободное пространство спереди. Вместо этого уменьшите вес этих показаний датчиков во время слияния и заново создайте полную многослойную карту. Затем сбросьте и снова запустите моделирование.
% Construct a new multiLayerMap with a different set of fusion weights updatedWeights = [1 0.25 1]; egoMap = exampleHelperConstructMultiLayerEgoMap(res,width,height,updatedWeights); % Rerun the simulation exampleHelperResetSimulation(scene,traj,lidarSensors,egoMap,reconLayer) exampleHelperRunSimulation(scene,traj,groundTruthVehicle,egoMap,lidarSensors,reconLayer,axList)
После моделирования снова обратите внимание на несколько вещей о карте:
Области, покрытые только шумным датчиком, все еще могут обнаруживать свободное пространство с небольшим шумом.
Хотя шум все еще присутствует, показания других датчиков перевешивают показания датчиков шума. На карте показаны четкие границы препятствий (черные квадраты) в областях перекрытия датчиков.
Шум за пределами отдельных границ остается, потому что шумный лидар является единственным датчиком, который сообщает о показаниях в этих областях, но не подключается к другому свободному пространству.
В этом примере показан простой метод сплавления показаний. Далее можно настроить слияние с помощью следующих рекомендаций:
Чтобы настроить вес на основе достоверности датчика до слияния слоев, укажите пользовательскую обратную модель датчика при использовании insertRay объектная функция в examplerHelperUpdateEgoMap функция.
Чтобы назначить значения заполняемости на основе более сложного доверительного распределения, такого как гауссова обратная модель, используйте raycast функция объекта для извлечения ячеек, отслеживаемых каждым эминирующим лучом. После извлечения набора ячеек значения могут быть явно назначены на основе более сложных методов.
Для снижения достоверности устаревших ячеек используйте дополнительные уровни карты, которые отслеживают временные метки для каждой ячейки. Эти метки времени могут использоваться для того, чтобы уделять больше внимания недавно обновленным ячейкам и медленно игнорировать старые показания.