Этот пример демонстрирует, как выполнить путь без препятствий между двумя местоположениями на заданной карте в Simulink ®. Путь генерируется с помощью вероятностного алгоритма планирования дорожной карты (PRM) (mobileRobotPRM)
. Команды управления для навигации по этому пути генерируются с помощью блока контроллера Pure Pursuit. Одноколесная кинематическая модель движения моделирует движение робота на основе этих команд.
Загрузите карту заполнения, которая определяет пределы карты и препятствия в карте. exampleMaps.mat
содержат несколько карт, включая simpleMap
, который использует этот пример.
load exampleMaps.mat
Задайте начальный и конечный локализатор в карте.
startLoc = [5 5]; goalLoc = [12 3];
Откройте модель Simulink
open_system('pathPlanningUnicycleSimulinkModel.slx')
Модель состоит из трех основных частей:
Планирование
Контроль
Модель объекта управления
Функциональный блок Planner MATLAB ® использует mobileRobotPRM
планировщик пути и принимает начальное местоположение, местоположение цели и карту как входы. Блоки выводят массив путевых точек, которые следует робот. Запланированные путевые точки используются в нисходящем направлении блоком контроллера Pure Pursuit.
Блок контроллера Pure Pursuit генерирует команды линейной скорости и скорости вращения на основе точек пути и текущего положения робота.
Подсистема «Проверить расстояние до цели» вычисляет текущее расстояние до цели, и если оно находится в пределах порога, симуляция останавливается.
Блок Unicycle Kinematic Model создает модель транспортного средства, чтобы симулировать упрощенную кинематику транспортного средства. Блок принимает линейные и угловые скорости как командные входы от блока контроллера Чистого Преследования и выводит текущие положения и состояния скорости.
Чтобы симулировать модель
simulation = sim('pathPlanningUnicycleSimulinkModel.slx');
После симуляции модели визуализируйте робота, ведущего путь без препятствий на карте.
map = binaryOccupancyMap(simpleMap)
map = binaryOccupancyMap with properties: mapLayer Properties LayerName: 'binaryLayer' DataType: 'logical' DefaultValue: 0 GridLocationInWorld: [0 0] GridOriginInLocal: [0 0] LocalOriginInWorld: [0 0] Resolution: 1 GridSize: [26 27] XLocalLimits: [0 27] YLocalLimits: [0 26] XWorldLimits: [0 27] YWorldLimits: [0 26]
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.