Этот пример демонстрирует, как выполнить путь без препятствий между двумя местоположениями на заданной карте в Simulink ®. Путь генерируется с помощью вероятностного алгоритма планирования дорожной карты (PRM) (mobileRobotPRM)
. Команды управления для навигации по этому пути генерируются с помощью блока контроллера Pure Pursuit. Велосипедная кинематическая модель движения моделирует движение робота на основе этих команд.
Загрузка карты в рабочее пространство MATLAB
load exampleMaps.mat
Введите начальное и целевое местоположения
startLoc = [5 5]; goalLoc = [12 3];
Импортированные карты: simpleMap, complexMap and ternaryMap
.
Откройте модель Simulink
open_system('pathPlanningBicycleSimulinkModel.slx')
Модель состоит из четырех основных операций:
Планирование
Контроль
Модель объекта управления
Функциональный блок Planner MATLAB ® использует mobileRobotPRM
планировщик пути и принимает начальное местоположение, местоположение цели и карту как входы. Блоки выводят массив вапоинтов, за которыми следует робот. Запланированные путевые точки используются в нисходящем направлении блоком контроллера Pure Pursuit.
Блок контроллера Pure Pursuit генерирует команды линейной скорости и скорости вращения на основе точек пути и текущего положения робота.
Подсистема «Проверить расстояние до цели» вычисляет текущее расстояние до цели, и если оно находится в пределах порога, симуляция останавливается.
Блок Bicycle Kinematic Model создает модель транспортного средства, чтобы симулировать упрощенную кинематику транспортного средства. Блок принимает линейные и угловые скорости как командные входы от блока контроллера Чистого Преследования и выводит текущие положения и состояния скорости.
Чтобы симулировать модель
simulation = sim('pathPlanningBicycleSimulinkModel.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.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.