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

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

Задайте модель

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

carLike = ackermannKinematics; 

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

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

velo = 5;    % Constant linear velocity 
psidot = 1;  % Constant left steering rate 

% Define the total time and sample rate 
sampleTime = 0.05;                  % Sample time [s]
timeEnd1 = 1.5;                     % Simulation end time for unconstrained robot 
timeEnd2 = 10;                      % Simulation end time for constrained robot 
tVec1 = 0:sampleTime:timeEnd1;      % Time array for unconstrained robot 
tVec2 = 0:sampleTime:timeEnd2;      % Time array for constrained robot  

initPose = [0;0;0;0];               % Initial pose (x y theta phi) 

Создайте структуру опций для решателя ОДУ

В этом примере вы передаете options структура в качестве аргумента к решателю ОДУ. options структура содержит информацию о держащемся угловом пределе. Создать options структура, используйте Events опция odeset и созданная функция события, detectSteeringSaturation. detectSteeringSaturation устанавливает максимальный руководящий угол на 45 градусов.

Для описания того, как задать detectSteeringSaturation, смотрите Функцию События Define в конце этого примера.

options = odeset('Events',@detectSteeringSaturation);

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

Затем вы используете derivative функционируйте и решатель ОДУ, ode45, решить модель и сгенерировать решение.

% Simulate the unconstrained robot 
[t1,pose1] = ode45(@(t,y)derivative(carLike,y,[velo psidot]),tVec1,initPose);

% Simulate the constrained robot 
[t2,pose2,te,ye,ie] = ode45(@(t,y)derivative(carLike,y,[velo psidot]),tVec2,initPose,options);

Обнаружьте держащееся насыщение

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

if te < timeEnd2
    str1 = "Steering angle limit was reached at ";
    str2 = " seconds";
    comp = str1 + te + str2; 
    disp(comp)
end 

Steering angle limit was reached at 0.785 seconds

Симулируйте ограниченного робота с новыми начальными условиями

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

saturatedPsiDot = 0;             % Steering rate after saturation 
cmds = [velo saturatedPsiDot];   % Command vector 
tVec3 = te:sampleTime:timeEnd2;  % Time vector 
pose3 = pose2(length(pose2),:); 
[t3,pose3,te3,ye3,ie3] = ode45(@(t,y)derivative(carLike,y,cmds), tVec3,pose3, options);

Постройте график результатов

Постройте траекторию робота с помощью plot и данные хранимы в pose.

figure(1)
plot(pose1(:,1),pose1(:,2),'--r','LineWidth',2); 
hold on; 
plot([pose2(:,1); pose3(:,1)],[pose2(:,2);pose3(:,2)],'g'); 
title('Trajectory X-Y')
xlabel('X')
ylabel('Y') 
legend('Unconstrained robot','Constrained Robot','Location','northwest')
axis equal

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

Функция события Define

Установите функцию события, таким образом, что интегрирование завершает работу, когда 4-е состояние, theta, равно максимальному руководящему углу.

function [state,isterminal,direction] = detectSteeringSaturation(t,y)
  maxSteerAngle = 0.785;               % Maximum steering angle (pi/4 radians)
  state(4) = (y(4) - maxSteerAngle);   % Saturation event occurs when the 4th state, theta, is equal to the max steering angle    
  isterminal(4) = 1;                   % Integration is terminated when event occurs 
  direction(4) = 0;                    % Bidirectional termination 

end