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

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

Определение модели

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

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см. раздел Определение функции события в конце этого примера.

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

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

Определение функции события

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

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