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

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

Кинематическая модель Unicycle

Простейший способ представления кинематики подвижного робота-транспортного средства - с одноколесной моделью, которая имеет скорость вращения колеса, заданную вращением вокруг центральной оси, и может поворачиваться вокруг своей оси 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 = [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 Purescuit. Учитывая набор ППМ, текущее состояние робота и некоторые другие параметры, контроллер выдает скорость транспортного средства и скорость курса.

% 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)