Создайте эгоцентрические карты заполнения Используя датчики области значений

Скрипт Open Live Script

Карты заполнения предлагают простой все же устойчивый способ представлять среду для автоматизированных приложений путем отображения непрерывного мирового пробела со структурой дискретных данных. Отдельные ячейки сетки могут содержать бинарную или вероятностную информацию, где 0 указывает, что свободное пространство, и 1 указывает на занятое место. Можно создавать эту информацию в зависимости от времени с помощью измерений датчика и эффективно хранить их в карте. Эта информация также полезна для более усовершенствованных рабочих процессов, такова как обнаружение столкновений и планирование пути.

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

Загрузите предварительно созданную карту заполнения основной истины

Создайте неэгоцентрическую карту из ранее сгенерированного файла данных, который считается основной истиной для симулированного лидара. Загрузите карту, mapData, который содержит Data поле как вероятностная матрица и преобразует его в двоичные значения.

Создайте binaryOccupancyMap объект с бинарной матрицей и задает разрешение карты.

% Load saved map information
load mapData_rayTracingTrajectory
binaryMatrix = mapData.Data > 0.5;
worldMap = binaryOccupancyMap(binaryMatrix,mapData.Resolution);

Установите местоположение нижнего левого угла карты как мировой источник.

worldMap.LocalOriginInWorld = mapData.GridLocationInWorld;

Постройте основную истину. Этот пример настраивает подграфик для показа двух карт рядом друг с другом.

set(gcf,'Visible','on')
worldAx = subplot(1,2,1);
worldHandle = show(worldMap,'Parent',worldAx);
hold all

Создайте симулированный лидар

Создайте rangeSensor объект, который может использоваться, чтобы собрать показания лидара от симуляции. Можно изменить различные свойства на rangeSensor более точно представлять конкретную модель лидара или добавить в шуме датчика, чтобы протестировать робастность вашего решения. В данном примере установите [min max] расположитесь и шумовой параметр. После создания объекта получите и постройте чтение от датчика путем обеспечения [x y theta] позируйте относительно мировой системы координат. Помощники в качестве примера строят робота, и показания лидара выше worldMap.

% Create rangeSensor
lidar = rangeSensor;
lidar.Range = [0 5];
lidar.RangeNoise = 0;
pos = [0 0 0];

% Show lidar readings in world map
[ranges, angles] = lidar(pos, worldMap);
hSensorData = exampleHelperPlotLidar(worldAx, pos, ranges, angles);

% Show robot in world map
hRobot = exampleHelperPlotRobot(worldAx, pos);

Инициализируйте эгоцентрическую карту

Создайте occupancyMap объект представлять эгоцентрическую карту. Установите локальный источник на центральное местоположение сетки. Переключите начало координат сетки наполовину ширина границ.

% By default, GridOriginInLocal = [0 0]
egoMap = occupancyMap(10,10,worldMap.Resolution);

% Offset the GridOriginInLocal such that the "local frame" is located in the
% center of the "map-window"
egoMap.GridOriginInLocal = -[diff(egoMap.XWorldLimits) diff(egoMap.YWorldLimits)]/2;

Постройте эгоцентрическую карту рядом с мировой картой в подграфике.

% Update local plot
localAx = subplot(1,2,2);
show(egoMap,'Parent',localAx);
hold all
localMapFig = plot(localAx,egoMap.LocalOriginInWorld+[0 1], egoMap.LocalOriginInWorld+[0 0],'r-','LineWidth',3);

Запланируйте путь между точками

Мы можем теперь использовать нашу карту основной истины, чтобы запланировать путь между двумя свободными точками. Создайте копию мировой карты и раздуйте ее на основе размера робота и желаемого разрешения. Этот пример использует подобного автомобилю робота, который имеет неголономные ограничения, заданные с stateSpaceDubins объект. Это пространство состояний используется планировщиком пути для того, чтобы случайным образом произвести выполнимые состояния для робота. Наконец, создайте validatorOccupancyMap объект, который использует карту, чтобы подтвердить сгенерированные состояния и движения, которые соединяют их путем проверки соответствующих ячеек на заполнение.

% Copy the world map and inflate it.
binaryMap = binaryOccupancyMap(worldMap);
inflate(binaryMap, 0.1);

% Create a state space object.
stateSpace = stateSpaceDubins;

% Reduce the turning radius to better fit the size of map and obstacle
% density.
stateSpace.MinTurningRadius = 0.5;

% Create a state validator object.
validator = validatorOccupancyMap(stateSpace);
validator.Map = binaryMap;
validator.ValidationDistance = 0.1;

Используйте RRT* планирование алгоритма как plannerRRTStar возразите и задайте пространство состояний и утвердите блок проверки допустимости как входные параметры. Задайте местоположения начала и конца для планировщика и сгенерируйте путь.

% Create our planner using the previously created StateSpace and
% StateValidator objects.
planner = plannerRRTStar(stateSpace, validator);
planner.MaxConnectionDistance = 2;
planner.MaxIterations = 20000;

% Set a seed for the randomly generated path for reproducible results.
rng(1, 'twister')

% Set the start and end points.
startPt = [-6   -5      0];
goalPt  = [ 8    7   pi/2];

% Plan a path between start and goal points.
path = plan(planner, startPt, goalPt);
interpolate(path, size(path.States,1)*10);
plot(worldAx, path.States(:,1),path.States(:,2),'b-');

Сгенерируйте траекторию вдоль пути

Планировщик сгенерировал набор состояний, но выполнить траекторию, времена для состояний необходимы. Цель этого примера состоит в том, чтобы переместить робота вдоль пути с постоянной линейной скоростью 0.5 m/s. Чтобы получить метки времени для каждой точки, вычислите расстояния между точками, суммируйте их кумулятивно, затем разделите это на линейную скорость, чтобы получить монотонно увеличивающийся массив меток времени, tStamps.

% Get distance between each waypoint
pt2ptDist = distance(stateSpace,path.States(1:end-1,:),path.States(2:end,:))

pt2ptDist = 129×1

    0.2000
    0.2000
    0.2000
    0.2000
    0.2000
    0.2000
    0.2000
    0.2000
    0.2000
    0.2000
      ⋮

linVel = 0.5; % m/s
tStamps = cumsum(pt2ptDist)/linVel;

Сгенерируйте симулированную траекторию финала с waypointTrajectory, задавая состояния пути, соответствующие метки времени и желаемую частоту дискретизации 10 Гц.

traj = waypointTrajectory(path.States, [0; tStamps], 'SampleRate', 10);

Симулируйте показания датчика и создайте карту

Наконец, переместите робота вдоль траектории при обновлении карты с симулированными показаниями Лидара.

Чтобы инициализировать симуляцию, сбросьте траекторию, установите локальный источник на первую точку xy на траектории и очистите карту.

reset(traj);
robotCurrentPose = path.States(1,:);
move(egoMap, robotCurrentPose(1:2));
setOccupancy(egoMap,repmat(egoMap.DefaultValue,egoMap.GridSize));

Первичные операции цикла симуляции:

Получите следующее положение в траектории от traj и извлеките ориентацию оси z (theta) из кватерниона.
Переместите egoMap к новому [x y theta] положение.
Получите данные о датчике от лидара.
Обновите локальную карту с данными о датчике с помощью insertRay.
Обновите визуализацию.

Обратите внимание, что робот проезжает среда, симулируя показания датчика, и создавая карту заполнения, когда это идет.

while ~isDone(traj)
    % Increment robot along trajectory
    [pts, quat] = step(traj);
    
    % Get orientation angle from quaternion
    rotMatrix = rotmat(quat,'point');
    orientZ = rotm2eul(rotMatrix);
    
    % Move the robot to the new location
    robotCurrentPose = [pts(1:2) orientZ(1)];
    move(egoMap, robotCurrentPose(1:2),'MoveType','Absolute');
    
    % Retrieve sensor information from the lidar and insert it into the egoMap
    [ranges, angles] = lidar(robotCurrentPose, worldMap);
    insertRay(egoMap, robotCurrentPose,ranges,angles,lidar.Range(2));

    % Update egoMap-centric plot
    show(egoMap, 'Parent', localAx, 'FastUpdate', 1);
    
    % Update orientation vector
    set(localMapFig, 'XData', robotCurrentPose(1)+[0 cos(robotCurrentPose(3))], 'YData', robotCurrentPose(2)+[0 sin(robotCurrentPose(3))]);

    % Update world plot
    exampleHelperUpdateRobotAndLidar(hRobot, hSensorData, robotCurrentPose, ranges, angles);

    % Call drawnow to push updates to the figure
    drawnow limitrate
end

Документация