Загрузите модель Simulink и карта

Загрузите карту заполнения, которая задает пределы карты и препятствия в рамках карты. exampleMaps.mat содержите несколько карт включая simpleMap, который использует этот пример.

load exampleMaps.mat

Задайте начало и конец locaiton в рамках карты.

startLoc = [5 5];
goalLoc = [20 20];

Обзор модели

Откройте модель Simulink.

open_system('pathPlanningSimulinkModel.slx')

Модель состоит из трех первичных частей:

Планирование

Функциональный блок Planner MATLAB® использует mobileRobotPRM планировщик пути и берет местоположение запуска, целевое местоположение и карту как входные параметры. Блоки выводят массив wapoints, за которым следует робот. Запланированные waypoints используются в нисходящем направлении Чистым контроллером Преследования блок.

Управление

Чистое преследование

Чистый контроллер Преследования блок генерирует линейную скорость и команды скорости вращения на основе waypoints и текущего положения робота.

Проверяйте, достигнута ли Цель

Расстояние Проверки до Целевой подсистемы вычисляет текущее расстояние до цели и если это в пороге, остановках симуляции.

Модель объекта управления

Дифференциальный Диск Кинематический блок Model создает модель транспортного средства, чтобы симулировать упрощенную кинематику транспортного средства. Блок берет линейный и скорости вращения как вводы команд от Чистого диспетчера Преследования блок и выводит скоростные состояния и текущее положение.

Запустите модель

simulation = sim('pathPlanningSimulinkModel.slx');

Визуализируйте движение робота

После симуляции модели визуализируйте робота, управляющего путем без препятствий в карте.

map = binaryOccupancyMap(simpleMap);
robotPose = simulation.Pose;
thetaIdx = 3;

% Translation
xyz = robotPose;
xyz(:, thetaIdx) = 0;

% Rotation in XYZ euler angles
theta = robotPose(:,thetaIdx);
thetaEuler = zeros(size(robotPose, 1), 3 * size(theta, 2));
thetaEuler(:, end) = theta;

% Plot the robot poses at every 10th step.
for k = 1:10:size(xyz, 1) 
    show(map)
    hold on;
    
    % Plot the start location.
    plotTransforms([startLoc, 0], eul2quat([0, 0, 0]))
    text(startLoc(1), startLoc(2), 2, 'Start');
    
    % Plot the goal location.
    plotTransforms([goalLoc, 0], eul2quat([0, 0, 0]))
    text(goalLoc(1), goalLoc(2), 2, 'Goal');
    
    % Plot the xy-locations.
    plot(robotPose(:, 1), robotPose(:, 2), '-b')
    
    % Plot the robot pose as it traverses the path.
    quat = eul2quat(thetaEuler(k, :), 'xyz');
    plotTransforms(xyz(k,:), quat, 'MeshFilePath',...
        'groundvehicle.stl');
    light;
    drawnow;
    hold off;
end

© Copyright 2019 The MathWorks, Inc.