Карты заполнения предлагают простой, но надежный способ представления окружения для роботизированных приложений путем отображения непрерывного мирового пространства на дискретную структуру данных. Отдельные камеры сетки могут содержать двоичную или вероятностную информацию об информации о препятствиях. Однако автономная платформа может использовать множество датчиков, которые могут потребоваться объединить, чтобы оценить как текущее состояние платформы, так и состояние окружающей среды.
Этот пример особого внимания на интегрировании множества датчиков, чтобы оценить состояние окружения и значений заполнения хранилища, являются различными слоями карты. Пример показывает, как multiLayerMap объект может использоваться для визуализации, отладки и запала данных, собранных с трех датчиков лидара, установленных на автономном транспортном средстве. Показания датчика в этом примере моделируются с помощью набора lidarPointCloudGenerator (Automated Driving Toolbox) объекты, которые захватывают показания из сопровождающего drivingScenario (Automated Driving Toolbox) объект.
Каждый лидар обновляет свои собственные validatorOccupancyMap3D объект, который позволяет нам визуализировать локальную карту, созданную каждым датчиком в изоляции. Эти локальные карты могут использоваться, чтобы быстро идентифицировать источники шума или ошибки монтажа, и могут помочь в выборе подходящего метода слияния. The multiLayerMap содержит четвертое mapLayer объект, который использует пользовательскую функцию обратного вызова, чтобы объединить данные, содержащиеся в каждом слое заполнения. Наконец, сросшаяся карта используется для обновления соответствующего субрегиона карты мира, когда автономное транспортное средство перемещается по предварительно спланированному пути.
Во-первых, создайте drivingScenario объект и заполнение сцены несколькими созданиями с помощью функции helper в качестве примера. Функция также визуализирует сцену.
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. The 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]);
Постройте график эгоцентрических слоев рядом с восстановленной картой. Используйте функцию 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 функция объекта для извлечения камер, прослеживаемых каждым эминирующим лучом. После извлечения набора камер значения могут быть явно назначены на основе более сложных методов.
Чтобы уменьшить доверие стареющих камер, используйте дополнительные слои карты, которые отслеживают временные метки для каждой камеры. Эти временные метки могут использоваться, чтобы придавать большее значение недавно обновленным камерам и медленно игнорировать старые показания.