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

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

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

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

carLike = ackermannKinematics;

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

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

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)

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

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

% 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 функция для обновления состояния. Этот пример использует решатель для обыкновенных дифференциальных уравнений (ОДУ), чтобы сгенерировать решение. Другим способом является обновление состояния с помощью цикла, как показано на Пути Following for a Робота с дифференциальным приводом.

Поскольку решатель ОДУ требует, чтобы все выходы были предоставлены как один выход, контроллер чистого преследования должен быть обернут в функцию, которая выводит линейную скорость и скорость вращения курса как один выход. Пример помощника, 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)