Плавьте несколько датчиков лидара Используя слои карты
Карты заполнения предлагают простой все же устойчивый способ представлять среду для автоматизированных приложений путем отображения непрерывного мирового пробела со структурой дискретных данных. Отдельные ячейки сетки могут содержать бинарную или вероятностную информацию об информации о препятствии. Однако автономная платформа может использовать множество датчиков, которые, возможно, должны быть объединены, чтобы оценить и текущее состояние платформы и состояние окружающей среды.
Этим примером особое внимание при интеграции множества датчиков, чтобы оценить состояние среды и значений заполнения хранилища являются различные слои карты. Пример показывает как multiLayerMap объект может использоваться, чтобы визуализировать, отладить, и объединить данные, собранные от трех датчиков лидара, смонтированных к автономному транспортному средству. Показания датчика в этом примере симулированы с помощью набора lidarPointCloudGenerator Объекты (Automated Driving Toolbox), которые получают показания от сопроводительного drivingScenario Объект (Automated Driving Toolbox).
Каждый лидар обновляет свой собственный 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) и один широкий Лидар, расположенный "против движения", FOV. Перекрывающаяся область и обращенных к передней стороне датчиков должна помочь быстро указать и подтвердить свободное пространство перед транспортным средством, тогда как область значений датчика, расположенная "против движения", помогает сопоставить пересеченную область.
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]);
Визуализация Setup
Постройте эгоцентрические слои рядом с восстановленной картой. Используйте функцию exampleHelperShowEgoMap, чтобы отобразить каждую локальную карту.
% 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 (theta) из кватерниона.Переместите
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возразите функции, чтобы получить ячейки, прослеженные каждым лучом eminating. Если набор ячеек был получен, значения могут быть явным образом присвоены на основе более комплексных методов.Чтобы уменьшать доверие стареющих ячеек, используйте дополнительные слои карты, которые отслеживают метки времени для каждой ячейки. Эти метки времени могут использоваться, чтобы поместить большую важность в недавно ячейки обновлений и медленно игнорировать более старые показания.