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

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

% 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 = 139×1

    0.1818
    0.1818
    0.1818
    0.1818
    0.1818
    0.1818
    0.1818
    0.1818
    0.1818
    0.1818
      ⋮

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

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

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

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