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

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

Создайте 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 объект для представления эгоцентрической карты. Смещение начала сетки относительно локального начала путем установки GridOriginInLocal свойство в половину различия между мировыми пределами. Это смещает границы карты так, чтобы локальный центр был центрирован.

% 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 м/с. Чтобы получить временные метки для каждой точки, вычислите расстояния между точками, суммируйте их совокупно, затем разделите это на линейную скорость, чтобы получить монотонно увеличивающийся массив временных меток, 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, задающие состояния пути, соответствующие временные метки и желаемую частоту дискретизации 10Hz.

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));

Основными операциями цикла симуляции являются:

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

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