Пакет аппаратной поддержки для TurtleBot

Этот пример дает обзор работы с TurtleBot, используя его собственный интерфейс ROS. Пакет Toolbox™ поддержки ROS для роботов на базе TurtleBot обеспечивает более оптимизированный интерфейс для TurtleBot. Это позволяет вам:

Сбор данных о датчике и отправка команд управления без явного вызова команд ROS

Прозрачная связь с моделируемым роботом в Gazebo или с физическим TurtleBot

Чтобы установить пакет поддержки, откройте дополнения > Get Hardware Support Packages на вкладке MATLAB ® Home и выберите ROS Toolbox™ Support Package для роботов на основе TurtleBot. Кроме того, используйте roboticsAddons (Robotics System Toolbox).

Подключение к TurtleBot

Убедитесь, что TurtleBot работает либо в симуляции через Gazebo ®, либо на реальном оборудовании. Для получения информации о процедуре запуска см. раздел «Начало работы с Gazebo и моделируемым TurtleBot или Начало работы с реальным TurtleBot». Если вы используете оборудование, найдите синий мяч, который будет использоваться для отслеживания. Если вы используете Gazebo ®, синий мяч должен быть в мире перед роботом (убедитесь, что вы используете Gazebo Office world).

Инициализация ROS. Подключиться к TurtleBot путем замены ipaddress с IP-адресом TurtleBot

ipaddress = '192.168.111.134';
rosinit(ipaddress,11311)

Initializing global node /matlab_global_node_52081 with NodeURI http://192.168.111.1:56789/

Убедитесь, что вы запустили камеру Kinect, если вы работаете с реальным оборудованием TurtleBot. Команда запуска камеры:

roslaunch turtlebot_bringup 3dsensor.launch. 

Вы должны ввести это в терминале на TurtleBot.

Создайте подписчиков для цветной камеры, датчика скалы и бампера.

Создайте издатели для излучения звука и для управления сообщениями о скорости робота.

handles.colorImgSub = exampleHelperTurtleBotEnableColorCamera;

Successfully Enabled Camera (raw image)

useHardware = exampleHelperTurtleBotIsPhysicalRobot;
if useHardware
    handles.cliffSub = rossubscriber('/mobile_base/events/cliff','BufferSize', 5);
    handles.bumpSub = rossubscriber('/mobile_base/sensors/bumper_pointcloud', 'BufferSize', 5);
    handles.soundPub = rospublisher('/mobile_base/commands/sound', 'kobuki_msgs/Sound');
    handles.velPub = rospublisher('/mobile_base/commands/velocity');
else
    % Cliff sensor, bumper sensor and sound emitter are only present in
    % real TurtleBot hardware
    handles.cliffSub = [];
    handles.bumpSub = [];
    handles.soundPub = [];
    handles.velPub = rospublisher('/cmd_vel');
end

Настройка обнаружения синего мяча

Установите параметры для фильтрации изображений. Добавьте их в структуру данных, которая будет использоваться в алгоритме.

blueBallParams.blueMax = 120;  % Maximum permissible deviation from pure blue
blueBallParams.darkMin = 30;   % Minimum acceptable darkness value

Попробуйте визуализировать мяч, чтобы убедиться, что параметры нахождения шара могут найти его. Запуск exampleHelperTurtleBotFindBlueBall функция, чтобы увидеть, найден ли кружок. Если да, c и m являются присвоенными значениями. ball - бинарное изображение, созданное путем применения к изображению фильтров голуботы и темноты. Просмотрите ball чтобы увидеть, был ли синий мяч правильно изолирован:

latestImg = readImage(handles.colorImgSub.LatestMessage);
[c,~,ball] = exampleHelperTurtleBotFindBlueBall(latestImg,blueBallParams,useHardware);

Используйте этот пример помощника, чтобы отобразить реальное и бинарное изображение на рисунке и построить красный плюс в центре мяча.

exampleHelperTurtleBotPlotObject(latestImg,ball,c);

Если мяч не найден, попробуйте увеличить или уменьшить blueBallParams.blueMax и blueBallParams.darkMin. Просмотрите график еще раз, пока мяч не будет найден. Этот метод является хорошим способом тонкой настройки алгоритма нахождения мяча перед использованием контроллера.

В Gazebo используемые параметры могут не найти мяч, потому что пороговые значения слишком щедрые. Изображение Gazebo (левые рисунки) включает части стенки и другие объекты в свободном пространстве. Реальное изображение (правые рисунки) выглядит очень насыщенным белым. Попробуйте изменить параметры так, чтобы они были более ограничительными:

blueBallParams.blueMax = 200; % Maximum permissible deviation from pure blue
blueBallParams.darkMin = 220; % Minimum acceptable darkness value
latestImg = readImage(handles.colorImgSub.LatestMessage);
[c,~,ball] = exampleHelperTurtleBotFindBlueBall(latestImg,blueBallParams,useHardware);

Используйте этот пример вспомогательного устройства для отображения рисунков.

exampleHelperTurtleBotPlotObject(latestImg,ball,c);

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

  blueBallParams.blueMax = 30; % Maximum permissible deviation from pure blue
  blueBallParams.darkMin = 90; % Minimum acceptable darkness value
  latestImg = readImage(handles.colorImgSub.LatestMessage);
  [c,~,ball] = exampleHelperTurtleBotFindBlueBall(latestImg,blueBallParams, useHardware);

Используйте этот пример вспомогательного устройства для отображения рисунков.

 exampleHelperTurtleBotPlotObject(latestImg,ball,c);

Настройка порогов цвета является сложной задачей по сравнению с их настройкой в моделируемом окружении, такой как Gazebo.

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

  handles.params = blueBallParams;

Тестируйте контроллер фиксированного расстояния

Установите коэффициент усиления контроллера для TurtleBot. TurtleBot использует ПИД-регулятор, чтобы держаться на постоянном расстоянии от мяча.

Первый набор усилений контроллера хорош для TurtleBot в Gazebo. Второй набор хорош для TurtleBot на реальном оборудовании. Отрегулируйте коэффициент усиления, как вы считаете нужным.

Вот компактный способ присвоения значений struct.

Эффективные усиления для симуляции Gazebo:

  gains.lin = struct('pgain',1/100,'dgain',1/100,'igain',0,'maxwindup',0','setpoint',0.65);
  gains.ang = struct('pgain',1/400,'dgain',1/500,'igain',0,'maxwindup',0','setpoint',0.5);
 

Эффективные усиления для оборудования TurtleBot:

gains.lin = struct('pgain',1/100,'dgain',1/1000,'igain',0,'maxwindup',0','setpoint',0.75);
gains.ang = struct('pgain',1/100,'dgain',1/3000,'igain',0,'maxwindup',0','setpoint',0.5);

Убедитесь, что добавили gains struct к handles переменная.

handles.gains = gains;

Определите таймер, чтобы выполнить поведение отслеживания мяча через коллбэк. Определите функцию stop, чтобы выключить ROS. Включите указатели в функцию обратного вызова для таймера:

timer2 = timer('TimerFcn',{@exampleHelperTurtleBotTrackingTimer,handles,useHardware},'Period',0.1,'ExecutionMode','fixedSpacing');
timer2.StopFcn = {@exampleHelperTurtleBotStopCallback};

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

start(timer2);
pause(1);

Датчик отбойника не активируется в симуляции, поэтому TurtleBot может не восстановиться при ударе о стенку.

Если вы хотите переместить синий мяч вокруг, используйте следующие команды для применения силы:

g = ExampleHelperGazeboCommunicator();
    ballhandle = ExampleHelperGazeboSpawnedModel('unit_sphere_1',g)
    duration = 2;
    forceVector = [0 4 0];
    applyForce(ballhandle,'link',duration,forceVector)

Для дальнейшего изучения контроля Gazebo над симуляцией см. раздел «Добавление, сборка и удаление объектов в Gazebo».

Остановите Движение Робота

Чтобы остановить таймер и автономное поведение, используйте следующую команду:

stop(timer2);

Если таймер удален из рабочей области перед остановкой, необходимо удалить его другим способом. Чтобы остановить все таймеры (даже таймеры в фоновом режиме), выполните следующую команду:

delete(timerfindall)

Очистить рабочую область издателей, подписчиков и других связанных с ROS объектов, когда вы закончите с ними

clear

Дополнительная информация

ПРИМЕЧАНИЕ. Код в этом разделе не предназначен для выполнения командной строки MATLAB

В этом примере организация вспомогательных файлов обеспечивает большую гибкость при настройке и повторном назначении кода. Можно изменить параметры нахождения мяча и усиления контроллера путем изменения значений в handles struct. Этот пример включает в себя timer который управляет всеми аспектами алгоритма управления. Этот таймер является exampleHelperTurtleBotTrackingTimer. Этот таймер имеет пары "имя-значение" Period и ExecutionMode которые заданы, чтобы определить, как часто вызывается коллбэк таймера. Кроме того, используется стоповый коллбэк. Вы можете включать дополнительные функции обратного вызова, если хотите.

Указатели, переданные в таймер, включают params для нахождения мяча и gains для контроллера.

Структура exampleHelperTurtleBotTrackingTimer это просто. Это основной конечный автомат с некоторыми шагами инициализации. Функция инициализации определяет, какой алгоритм отслеживания и какой контроллер использовать, когда нет в состоянии восстановления скалы или бампера. Функция является:

function [objectTrack, imgControl] = initControl()
   % INITCONTROL - Initialization function to determine which control
   % and object detection algorithms to use
   objectTrack = @exampleHelperTurtleBotFindBlueBall;
   imgControl = @exampleHelperTurtleBotPointController;     

В этом примере функция отслеживания exampleHelperTurtleBotFindBlueBall и контроллер exampleHelperTurtleBotPointController Можно заменить эту функцию и контроллер любыми пользовательскими функциями, которые имеют ту же структуру входных и выходных аргументов. Входные параметры для exampleHelperTurtleBotFindBlueBall являются цветным изображением и struct параметров нахождения шара. Выходными аргументами являются центр, величина и бинарное изображение искомого объекта. Входные параметры для exampleHelperTurtleBotPointController являются центром объекта, величины (хотя величина не используется в примере), размером изображения и усилениями контроллера (a struct). Выходные аргументы являются линейной и угловой скоростями.

Базовый конечный автомат, используемый в exampleHelperTurtleBotTrackingTimer является:

switch state
        case ExampleHelperTurtleBotStates.Seek
            % Object-finding state
            [center, scale] = findObject(handles.Tbot.ImColor,handles.params);
            % Wander if no circle is found, target the circle if it exists
            if isempty(center)
                [linearV, angularV] = exampleHelperTurtleBotWanderController();
            else
                [linearV, angularV] = imageControl(center, scale, size(handles.Tbot.ImColor),handles.gains);
                setSound(handles.Tbot,2);
            end
            state = ExampleHelperTurtleBotStates.Seek;           
        case ExampleHelperTurtleBotStates.Bumper
            % Bumper contact state
        case ExampleHelperTurtleBotStates.Spin
            % Spin state            
        case ExampleHelperTurtleBotStates.Cliff
            % Cliff avoidance
end

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

Алгоритм нахождения мяча является модульным и изменяемым. Он использует два фильтра изображений (один на темноте и один на голубости), маскированных вместе, чтобы изолировать синий мяч. Вместо этого можно изменить маски, чтобы найти красный или зеленый мяч. Если вы хотите исследовать другие формы отслеживания фигур, основной рабочий процесс работы остается неизменным.

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

blueImg = img(:,:,1)/2 + img(:,:,2)/2 - img(:,:,3)/2;
blueThresh = blueImg < params.blueMax;

Эти команды изолируют обратную часть синего цвета (с другим масштабированием) и подчеркивают темноту. Применяется порог.

darkIso = -img(:,:,1)/2 - img(:,:,2)/2 + 3*img(:,:,3) - 2*rgb2gray(img);
darkThresh = darkIso > params.darkMin;

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

ball1 = blueThresh & darkThresh;

Константы и коэффициенты масштабирования на изображении определяются пользователем, чтобы изолировать определенный цвет. Можно экспериментировать с различными комбинациями.

Можно также найти смежные области в отфильтрованном изображении, используя regionprops, который является частью Image Processing Toolbox.

s = regionprops(ball1, {'Centroid','Area','EquivDiameter'});

Есть дополнительные шаги, чтобы найти мяч из этой области, который вы можете найти в exampleHelperTurtleBotFindBlueBall.

The exampleHelperTurtleBotPointController функция использует ExampleHelperPIDControl Класс для хранения заданной точки (в этом случае местоположения центра мяча) в точном месте внутри изображения.

Модульность и гибкость примера кода позволяет экспериментировать с собственными алгоритмами и функциями.