Загрузите модель Simulink и карта

Карта распределения памяти при загрузке в рабочем пространстве MATLAB

load exampleMaps.mat

Войдите запускаются и целевые местоположения

startLoc = [5 5];
goalLoc = [12 3];

Импортированные карты: simpleMap, complexMap and ternaryMap.

Откройте модель Simulink

open_system('pathPlanningBicycleSimulinkModel.slx')

Обзор модели

Модель состоит из четырех первичных операций:

Планирование

Это блокируется, берет местоположение запуска, целевое местоположение и карту как вводы и выводы массив wapoints, за которым будет следовать робот. Запланированные waypoints используются в нисходящем направлении Чистым контроллером преследования, который выводит угловые и линейные скорости, учитывая текущее положение робота и запланированного waypoints как входные параметры.

Проверяйте, достигнута ли Цель

Если робот достиг целевого местоположения, simulaion останавливается.

Контроллер

Контроллер выводит линейную скорость и скорость вращения на основе waypoints и текущего положения робота. Блок Pure Pursuit Controller используется в том же самом.

Велосипед кинематическая модель

Велосипед Кинематическая Модель создает модель транспортного средства, чтобы симулировать упрощенную динамику аппарата. Это берет линейный и скорости вращения, как введено от блока Pure Pursuit Controller, и выходных параметров т.е. состояния и stateDot, которые являются состоянием робота и производной времени состояния робота, соответственно. Робот' состояние также используется, чтобы вычислить расстояние до цели и проверки, если робот достиг целевого местоположения.

Запустите модель

Симулировать модель

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 = 304×3

    5.0000    5.0000         0
    5.0002    5.0000   -0.0002
    5.0012    5.0000   -0.0012
    5.0062    5.0000   -0.0062
    5.0313    4.9995   -0.0313
    5.1563    4.9877   -0.1569
    5.7068    4.7074   -0.7849
    5.8197    4.6015   -0.6638
    5.9427    4.5193   -0.5157
    6.6589    4.4144    0.2249
      ⋮

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.