Определение ППМ

Определение набора ППМ для требуемого пути робота

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;

Используя контроллер «Путь следования», наведите робота на нужные ППМ

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

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

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

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

См. также