Задайте путевые точки

Задайте набор путевых точек для нужного пути для робота

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 theta]

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;

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

controller.Waypoints = path;

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

controller.DesiredLinearVelocity = 0.6;

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

controller.MaxAngularVelocity = 2;

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

controller.LookaheadDistance = 0.3;

Используя Path Follow Controller, Управляйте роботом по желаемым путевым точкам

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

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

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

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

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

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);

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

release(controller);
controller.Waypoints = path;

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

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

Предположим, что начальная ориентация робота

initialOrientation = 0;

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

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

См. также