Задайте мобильных роботов с кинематическими ограничениями

Существует много способов смоделировать кинематику мобильных роботов. Все диктуют, как скорости колеса связаны с состоянием робота: [x y theta], как координаты xy и заголовок робота, theta, в радианах.

Одноколесный велосипед кинематическая модель

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

unicycle = unicycleKinematics("VehicleInputs","VehicleSpeedHeadingRate");

Дифференциальный диск кинематическая модель

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

diffDrive = differentialDriveKinematics("VehicleInputs","VehicleSpeedHeadingRate");

Чтобы дифференцировать поведение от модели одноколесного велосипеда, добавьте скоростное ограничение скорости колеса в дифференциальный диск кинематическая модель

diffDrive.WheelSpeedRange = [-10 10]*2*pi;

Велосипед кинематическая модель

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

bicycle = bicycleKinematics("VehicleInputs","VehicleSpeedHeadingRate","MaxSteeringAngle",pi/8);

Другие модели

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

carLike = ackermannKinematics;

Настройте Параметры Симуляции

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

waypoints = [0 0; 0 10; 10 10; 5 10; 11 9; 4 -5];
% Define the total time and the sample rate
sampleTime = 0.05;               % Sample time [s]
tVec = 0:sampleTime:20;          % Time array

initPose = [waypoints(1,:)'; 0]; % Initial pose (x y theta)

Создайте контроллер транспортного средства

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

% Define a controller. Each robot requires its own controller
controller1 = controllerPurePursuit("Waypoints",waypoints,"DesiredLinearVelocity",3,"MaxAngularVelocity",3*pi);
controller2 = controllerPurePursuit("Waypoints",waypoints,"DesiredLinearVelocity",3,"MaxAngularVelocity",3*pi);
controller3 = controllerPurePursuit("Waypoints",waypoints,"DesiredLinearVelocity",3,"MaxAngularVelocity",3*pi);

Симулируйте модели Используя решатель ОДУ

Модели симулированы с помощью derivative функционируйте, чтобы обновить состояние. Этот пример использует решатель для обыкновенных дифференциальных уравнений (ОДУ), чтобы сгенерировать решение. Иначе должен был бы обновить состояние с помощью цикла, как показано в Следовании траектории для Робота с дифференциальным приводом.

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

goalPoints = waypoints(end,:)';
goalRadius = 1;

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

% Compute trajectories for each kinematic model under motion control
[tUnicycle,unicyclePose] = ode45(@(t,y)derivative(unicycle,y,exampleHelperMobileRobotController(controller1,y,goalPoints,goalRadius)),tVec,initPose);
[tBicycle,bicyclePose] = ode45(@(t,y)derivative(bicycle,y,exampleHelperMobileRobotController(controller2,y,goalPoints,goalRadius)),tVec,initPose);
[tDiffDrive,diffDrivePose] = ode45(@(t,y)derivative(diffDrive,y,exampleHelperMobileRobotController(controller3,y,goalPoints,goalRadius)),tVec,initPose);

Постройте результаты

Результаты решателя ОДУ могут быть легко просмотрены на одном графике с помощью plotTransforms визуализировать результаты всех траекторий целиком.

Положение выходные параметры должно сначала быть преобразовано в индексируемые матрицы переводов и кватернионов.

unicycleTranslations = [unicyclePose(:,1:2) zeros(length(unicyclePose),1)];
unicycleRot = axang2quat([repmat([0 0 1],length(unicyclePose),1) unicyclePose(:,3)]);

bicycleTranslations = [bicyclePose(:,1:2) zeros(length(bicyclePose),1)];
bicycleRot = axang2quat([repmat([0 0 1],length(bicyclePose),1) bicyclePose(:,3)]);

diffDriveTranslations = [diffDrivePose(:,1:2) zeros(length(diffDrivePose),1)];
diffDriveRot = axang2quat([repmat([0 0 1],length(diffDrivePose),1) diffDrivePose(:,3)]);

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

figure
plot(waypoints(:,1),waypoints(:,2),"kx-","MarkerSize",20);
hold all
plotTransforms(unicycleTranslations(1:10:end,:),unicycleRot(1:10:end,:),'MeshFilePath','groundvehicle.stl',"MeshColor","r");
plotTransforms(bicycleTranslations(1:10:end,:),bicycleRot(1:10:end,:),'MeshFilePath','groundvehicle.stl',"MeshColor","b");
plotTransforms(diffDriveTranslations(1:10:end,:),diffDriveRot(1:10:end,:),'MeshFilePath','groundvehicle.stl',"MeshColor","g");
view(0,90)