Этот пример демонстрирует, как выполнить путь без препятствий между двумя местами на данной карте в Simulink®. Путь сгенерирован с помощью вероятностного плана действий (PRM), планируя алгоритм (mobileRobotPRM)
. Команды управления для навигации по этому пути сгенерированы с помощью Чистого контроллера Преследования блок. Одноколесный велосипед кинематическая модель движения симулирует движение робота на основе тех команд.
Загрузите карту заполнения, которая задает пределы карты и препятствия в рамках карты. exampleMaps.mat
содержите несколько карт включая simpleMap
, который использует этот пример.
load exampleMaps.mat
Задайте запуск и закончите locaiton в рамках карты.
startLoc = [5 5]; goalLoc = [12 3];
Откройте модель Simulink
open_system('pathPlanningUnicycleSimulinkModel.slx')
Модель состоит из трех первичных частей:
Планирование
Управление
Модель объекта управления
Функциональный блок Planner MATLAB® использует mobileRobotPRM
планировщик пути и берет местоположение запуска, целевое местоположение и карту как входные параметры. Блоки выводят массив waypoints, за которым следует робот. Запланированные waypoints используются в нисходящем направлении Чистым контроллером Преследования блок.
Чистый контроллер Преследования блок генерирует линейную скорость и команды скорости вращения на основе waypoints и текущего положения робота.
Расстояние Проверки до Целевой подсистемы вычисляет текущее расстояние до цели и если это в пороге, остановках симуляции.
Одноколесный велосипед Кинематический блок Model создает модель транспортного средства, чтобы симулировать упрощенную кинематику транспортного средства. Блок берет линейный и скорости вращения как вводы команд от Чистого диспетчера Преследования блок и выводит скоростные состояния и текущее положение.
Симулировать модель
simulation = sim('pathPlanningUnicycleSimulinkModel.slx');
После симуляции модели визуализируйте робота, управляющего путем без препятствий в карте.
map = binaryOccupancyMap(simpleMap)
map = binaryOccupancyMap with properties: GridLocationInWorld: [0 0] XWorldLimits: [0 27] YWorldLimits: [0 26] DataType: 'logical' DefaultValue: 0 Resolution: 1 GridSize: [26 27] XLocalLimits: [0 27] YLocalLimits: [0 26] GridOriginInLocal: [0 0] LocalOriginInWorld: [0 0]
robotPose = simulation.UnicyclePose
robotPose = 428×3
5.0000 5.0000 0
5.0000 5.0000 -0.0002
5.0001 5.0000 -0.0012
5.0006 5.0000 -0.0062
5.0031 5.0000 -0.0313
5.0156 4.9988 -0.1569
5.0707 4.9707 -0.7849
5.0945 4.9354 -1.1140
5.1075 4.9059 -1.1828
5.1193 4.8759 -1.2030
⋮
numRobots = size(robotPose, 2) / 3; 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; for k = 1:size(xyz, 1) show(map) hold on; % Plot Start Location plotTransforms([startLoc, 0], eul2quat([0, 0, 0])) text(startLoc(1), startLoc(2), 2, 'Start'); % Plot Goal Location plotTransforms([goalLoc, 0], eul2quat([0, 0, 0])) text(goalLoc(1), goalLoc(2), 2, 'Goal'); % Plot Robot's XY locations plot(robotPose(:, 1), robotPose(:, 2), '-b') % Plot Robot's pose as it traverses the path quat = eul2quat(thetaEuler(k, :), 'xyz'); plotTransforms(xyz(k,:), quat, 'MeshFilePath',... 'groundvehicle.stl'); pause(0.01) hold off; end
© Copyright 2019 The MathWorks, Inc.