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

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

% 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

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