В этом примере показано, как смоделировать различные модели кинематики робота в среде и сравнить их.
Существует много способов смоделировать кинематику мобильных роботов. Все диктуют, как скорости колеса связаны с состоянием робота: [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)
