Задайте Waypoints

Задайте набор waypoints для желаемого пути для робота

path = [2.00    1.00;
        1.25    1.75;
        5.25    8.25;
        7.25    8.75;
        11.75   10.75;
        12.00   10.00];

Установите текущее местоположение и целевое местоположение робота, как задано путем.

robotInitialLocation = path(1,:);
robotGoal = path(end,:);

Примите начальную ориентацию робота (ориентация робота является углом между заголовком робота и положительной Осью X, измеренной против часовой стрелки).

initialOrientation = 0;

Задайте текущее положение для робота [x y тета]

robotCurrentPose = [robotInitialLocation initialOrientation]';

Создайте кинематическую модель робота

Инициализируйте модель робота и присвойте начальное положение. Симулированный робот имеет кинематические уравнения для движения двухколесного робота с дифференциальным приводом. Входные параметры к этому симулированному роботу линейны и скорости вращения.

robot = differentialDriveKinematics("TrackWidth", 1, "VehicleInputs", "VehicleSpeedHeadingRate");

Визуализируйте желаемый путь

figure
plot(path(:,1), path(:,2),'k--d')
xlim([0 13])
ylim([0 13])

Задайте контроллер следования траектории

На основе пути, заданного выше и модель движения робота, вам нужен контроллер следования траектории, чтобы управлять роботом вдоль пути. Создайте контроллер следования траектории, использующий controllerPurePursuit объект.

controller = controllerPurePursuit;

Используйте путь, заданный выше, чтобы установить желаемый waypoints для контроллера

controller.Waypoints = path;

Установите контроллер следования траектории параметры. Желаемая линейная скорость установлена в 0,3 метра/секунда для этого примера.

controller.DesiredLinearVelocity = 0.6;

Максимальная скорость вращения действует как предел насыщения для вращательной скорости, которая установлена в 2 радианах/секунда для этого примера.

controller.MaxAngularVelocity = 2;

Как правило предварительное расстояние должно быть больше, чем желаемая линейная скорость для сглаженного пути. Робот может сократить углы, когда предварительное расстояние является большим. В отличие от этого маленькое предварительное расстояние может привести к нестабильному поведению следования траектории. Значение 0,5 м было выбрано для этого примера.

controller.LookaheadDistance = 0.3;

Используя контроллер следования траектории, управляйте роботом по желаемому Waypoints

Контроллер следования траектории обеспечивает сигналы элемента управления вводом для робота, который использование робота управлять собой вдоль желаемого пути.

Задайте целевой радиус, который является желаемым порогом расстояния между итоговым местоположением робота и целевым местоположением. Если робот на этом расстоянии от цели, это остановится. Кроме того, вы вычисляете текущее расстояние между местоположением робота и целевым местоположением. Это расстояние постоянно проверяется по целевому радиусу и остановкам робота, когда это расстояние меньше целевого радиуса.

Обратите внимание на то, что слишком маленькое значение целевого радиуса может заставить робота пропускать цель, которая может привести к неожиданному поведению около цели.

goalRadius = 0.1;
distanceToGoal = norm(robotInitialLocation - robotGoal);

controllerPurePursuit объект вычисляет команды управления для робота. Управляйте роботом с помощью этих команд управления, пока он не достигнет в целевом радиусе. Если вы используете внешнее средство моделирования или физического робота, то контроллер, выходные параметры должны быть применены к роботу и системе локализации, может быть обязан обновлять положение робота. Контроллер достигает 10 Гц.

% Initialize the simulation loop
sampleTime = 0.1;
vizRate = rateControl(1/sampleTime);

% Initialize the figure
figure

% Determine vehicle frame size to most closely represent vehicle with plotTransforms
frameSize = robot.TrackWidth/0.8;

while( distanceToGoal > goalRadius )
    
    % Compute the controller outputs, i.e., the inputs to the robot
    [v, omega] = controller(robotCurrentPose);
    
    % Get the robot's velocity using controller inputs
    vel = derivative(robot, robotCurrentPose, [v omega]);
    
    % Update the current pose
    robotCurrentPose = robotCurrentPose + vel*sampleTime; 
    
    % Re-compute the distance to the goal
    distanceToGoal = norm(robotCurrentPose(1:2) - robotGoal(:));
    
    % Update the plot
    hold off
    
    % Plot path each instance so that it stays persistent while robot mesh
    % moves
    plot(path(:,1), path(:,2),"k--d")
    hold all
    
    % Plot the path of the robot as a set of transforms
    plotTrVec = [robotCurrentPose(1:2); 0];
    plotRot = axang2quat([0 0 1 robotCurrentPose(3)]);
    plotTransforms(plotTrVec', plotRot, "MeshFilePath", "groundvehicle.stl", "Parent", gca, "View","2D", "FrameSize", frameSize);
    light;
    xlim([0 13])
    ylim([0 13])
    
    waitfor(vizRate);
end

Используя контроллер следования траектории наряду с PRM

Если желаемый набор waypoints вычисляется планировщиком пути, контроллер следования траектории может использоваться тем же способом. Во-первых, визуализируйте карту

load exampleMaps
map = binaryOccupancyMap(simpleMap);
figure
show(map)

Можно вычислить path использование алгоритма планирования пути PRM. Смотрите Планирование пути в Среде Различной Сложности для деталей.

mapInflated = copy(map);
inflate(mapInflated, robot.TrackWidth/2);
prm = robotics.PRM(mapInflated);
prm.NumNodes = 100;
prm.ConnectionDistance = 10;

Найдите путь между местоположением начала и конца. Обратите внимание на то, что path будет отличаться из-за вероятностной природы алгоритма PRM.

startLocation = [4.0 2.0];
endLocation = [24.0 20.0];
path = findpath(prm, startLocation, endLocation)

path = 8×2

    4.0000    2.0000
    3.1703    2.7616
    7.0797   11.2229
    8.1337   13.4835
   14.0707   17.3248
   16.8068   18.7834
   24.4564   20.6514
   24.0000   20.0000

Отобразите расширенную карту, планы действий и итоговый путь.

show(prm);

Вы задали контроллер следования траектории, выше которого можно снова использовать для вычисления команд управления робота на этой карте. Чтобы снова использовать контроллер и переопределить waypoints при сохранении другой информации тем же самым, используйте release функция.

release(controller);
controller.Waypoints = path;

Установите начальное местоположение и цель робота, как задано путем

robotInitialLocation = path(1,:);
robotGoal = path(end,:);

Примите начальную ориентацию робота

initialOrientation = 0;

Задайте текущее положение для движения робота [x y тета]

robotCurrentPose = [robotInitialLocation initialOrientation]';

Вычислите расстояние до целевого местоположения

distanceToGoal = norm(robotInitialLocation - robotGoal);

Задайте целевой радиус

goalRadius = 0.1;

Управляйте роботом с помощью контроллера выход на данной карте, пока это не достигнет цели. Контроллер достигает 10 Гц.

reset(vizRate);

% Initialize the figure
figure

while( distanceToGoal > goalRadius )
    
    % Compute the controller outputs, i.e., the inputs to the robot
    [v, omega] = controller(robotCurrentPose);
    
    % Get the robot's velocity using controller inputs
    vel = derivative(robot, robotCurrentPose, [v omega]);
    
    % Update the current pose
    robotCurrentPose = robotCurrentPose + vel*sampleTime; 
    
    % Re-compute the distance to the goal
    distanceToGoal = norm(robotCurrentPose(1:2) - robotGoal(:));
    
    % Update the plot
    hold off
    show(map);
    hold all

    % Plot path each instance so that it stays persistent while robot mesh
    % moves
    plot(path(:,1), path(:,2),"k--d")
    
    % Plot the path of the robot as a set of transforms
    plotTrVec = [robotCurrentPose(1:2); 0];
    plotRot = axang2quat([0 0 1 robotCurrentPose(3)]);
    plotTransforms(plotTrVec', plotRot, 'MeshFilePath', 'groundvehicle.stl', 'Parent', gca, "View","2D", "FrameSize", frameSize);
    light;
    xlim([0 27])
    ylim([0 26])
    
    waitfor(vizRate);
end

Смотрите также