Этот пример демонстрирует, как выполнить путь без препятствий между двумя местами на данной карте в Simulink®. Путь сгенерирован с помощью вероятностного плана действий (PRM), планируя алгоритм (mobileRobotPRM)
. Команды управления для навигации по этому пути сгенерированы с помощью Чистого контроллера Преследования блок. Велосипед кинематическая модель движения симулирует движение робота на основе тех команд.
Карта распределения памяти при загрузке в рабочем пространстве MATLAB
load exampleMaps.mat
Войдите запускаются и целевые местоположения
startLoc = [5 5]; goalLoc = [12 3];
Импортированные карты: simpleMap, complexMap and ternaryMap
.
Откройте модель Simulink
open_system('pathPlanningBicycleSimulinkModel.slx')
Модель состоит из четырех первичных операций:
Планирование
Управление
Модель объекта управления
Функциональный блок Planner MATLAB® использует mobileRobotPRM
планировщик пути и берет местоположение запуска, целевое местоположение и карту как входные параметры. Блоки выводят массив wapoints, за которым следует робот. Запланированные waypoints используются в нисходящем направлении Чистым контроллером Преследования блок.
Чистый контроллер Преследования блок генерирует линейную скорость и команды скорости вращения на основе waypoints и текущего положения робота.
Расстояние Проверки до Целевой подсистемы вычисляет текущее расстояние до цели и если это в пороге, остановках симуляции.
Велосипед Кинематический блок Model создает модель транспортного средства, чтобы симулировать упрощенную кинематику транспортного средства. Блок берет линейный и скорости вращения как вводы команд от Чистого диспетчера Преследования блок и выводит скоростные состояния и текущее положение.
Симулировать модель
simulation = sim('pathPlanningBicycleSimulinkModel.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.BicyclePose
robotPose = 362×3
5.0000 5.0000 0
5.0001 5.0000 -0.0002
5.0007 5.0000 -0.0012
5.0036 5.0000 -0.0062
5.0181 4.9997 -0.0313
5.0902 4.9929 -0.1569
5.4081 4.8311 -0.7849
5.5189 4.6758 -1.1170
5.5366 4.6356 -1.1930
5.5512 4.5942 -1.2684
⋮
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-2020 The MathWorks, Inc.