Этот пример демонстрирует, как выполнить путь без препятствий между двумя местами на данной карте в Simulink®. Путь сгенерирован с помощью вероятностного плана действий (PRM), планируя алгоритм (mobileRobotPRM)
. Команды управления для навигации по этому пути сгенерированы с помощью Чистого контроллера Преследования блок. Дифференциальный диск кинематическая модель движения симулирует движение робота на основе тех команд.
Загрузите карту заполнения, которая задает пределы карты и препятствия в рамках карты. 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.