Документация

Задайте 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];

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

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

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

initialOrientation = 0;

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

robotCurrentPose = [robotCurrentLocation initialOrientation];

Инициализируйте средство моделирования робота

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

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

robotRadius = 0.4;
robot = ExampleHelperRobotSimulator('emptyMap',2);
robot.enableLaser(false);
robot.setRobotSize(robotRadius);
robot.showTrajectory(true);
robot.setRobotPose(robotCurrentPose);

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

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

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

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

controller = robotics.PurePursuit;

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

controller.Waypoints = path;

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

controller.DesiredLinearVelocity = 0.3;

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

controller.MaxAngularVelocity = 2;

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

controller.LookaheadDistance = 0.5;

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

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

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

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

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

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

controlRate = robotics.Rate(10);
while( distanceToGoal > goalRadius )
    
    % Compute the controller outputs, i.e., the inputs to the robot
    [v, omega] = controller(robot.getRobotPose);
    
    % Simulate the robot using the controller outputs.
    drive(robot, v, omega);
    
    % Extract current location information ([X,Y]) from the current pose of the
    % robot
    robotCurrentPose = robot.getRobotPose;
    
    % Re-compute the distance to the goal
    distanceToGoal = norm(robotCurrentPose(1:2) - robotGoal);
    
    waitfor(controlRate);
    
end

Моделируемый робот достиг целевого местоположения с помощью контроллера следования траектории вдоль желаемого пути. Закройте симуляцию.

delete(robot)

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

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

robot = ExampleHelperRobotSimulator('simpleMap',2);
robot.enableLaser(false);
robot.setRobotSize(robotRadius);

robot.showTrajectory(true);

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

mapInflated = copy(robot.Map);
inflate(mapInflated,robotRadius);
prm = robotics.PRM(mapInflated);
prm.NumNodes = 100;
prm.ConnectionDistance = 10;

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

startLocation = [2.0 1.0];
endLocation = [12.0 10.0];
path = findpath(prm, startLocation, endLocation)

path = 6×2

    2.0000    1.0000
    1.5851    1.3808
    4.0668    6.7417
    7.0353    8.6624
   11.8252   10.6359
   12.0000   10.0000

Отобразите путь

show(prm, 'Map', 'off', 'Roadmap', 'off');

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

release(controller);
controller.Waypoints = path;

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

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

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

initialOrientation = 0;

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

robotCurrentPose = [robotCurrentLocation initialOrientation];

Сбросьте текущее положение моделируемого робота к запуску пути.

robot.setRobotPose(robotCurrentPose)
show(prm, 'Map', 'off', 'Roadmap', 'off');

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

distanceToGoal = norm(robotCurrentLocation - robotGoal);

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

goalRadius = 0.1;

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

reset(controlRate);
while( distanceToGoal > goalRadius )
    
    % Compute the controller outputs, i.e., the inputs to the robot
    [v, omega] = controller(robot.getRobotPose);
    
    % Simulate the robot using the controller outputs
    drive(robot, v, omega);
    
    % Extract current location information from the current pose
    robotCurrentPose = robot.getRobotPose;
    
    % Re-compute the distance to the goal
    distanceToGoal = norm(robotCurrentPose(1:2) - robotGoal);
    
    waitfor(controlRate);
end

Моделируемый робот достиг целевого местоположения с помощью контроллера следования траектории вдоль желаемого пути. Остановите робота.

drive(robot, 0, 0);

Закройте симуляцию.

delete(robot);

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