Этот пример демонстрирует, как управлять роботом, чтобы следовать за желаемым путем с помощью Средства моделирования Робота. Пример использует Чистый контроллер следования траектории Преследования, чтобы управлять моделируемым роботом вдоль предопределенного пути. Желаемый путь является набором waypoints заданное явным образом или вычисленное использование планировщика пути (обратитесь к Планированию пути в Среде Различной Сложности). Чистый контроллер следования траектории Преследования для моделируемого робота с дифференциальным приводом создается и вычисляет команды управления, чтобы следовать за данным путем. Вычисленные команды управления используются, чтобы управлять моделируемым роботом вдоль желаемой траектории, чтобы следовать за желаемым путем на основе Чистого контроллера Преследования.
Примечание: Начиная в R2016b, вместо того, чтобы использовать метод шага, чтобы выполнить операцию, заданную Системой object™, можно вызвать объект с аргументами, как будто это была функция. Например, y = step(obj,x) и y = obj(x) выполняют эквивалентные операции.
Задайте набор 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;
Контроллер следования траектории обеспечивает сигналы элемента управления вводом для робота, который использование робота управлять собой вдоль желаемого пути.
Задайте целевой радиус, который является желаемым порогом расстояния между итоговым местоположением робота и целевым местоположением. Если робот на этом расстоянии от цели, это остановится. Кроме того, вы вычисляете текущее расстояние между местоположением робота и целевым местоположением. Это расстояние постоянно проверяется по целевому радиусу и остановкам робота, когда это расстояние является меньше, чем целевой радиус.
Обратите внимание на то, что слишком маленькое значение целевого радиуса может заставить робота пропускать цель, которая может привести к неожиданному поведению около цели.
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)
Если желаемый набор 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);