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

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

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

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

Чтобы установить пакет поддержки, откройте меню Add-on > Get Hardware Support Packages на вкладке MATLAB ® Home и выберите ROS Toolbox™ Support Package for TurtleBot based Robots. В качестве альтернативы используйте roboticsAddons(Панель инструментов системы робототехники).

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

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

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

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

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

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

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

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

  handles.params = blueBallParams;

Испытательный контроллер с фиксированным расстоянием

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

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

Ниже приведен компактный способ присвоения значений структуры.

Эффективный выигрыш для моделирования беседки:

  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 структура к handles переменная.

handles.gains = gains;

Определите таймер для выполнения поведения отслеживания мяча через обратный вызов. Определите функцию остановки для отключения АФК. Включите дескрипторы в функцию обратного вызова для таймера:

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 структура. Этот пример включает в себя 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 представляют собой цветное изображение и структуру параметров поиска мяча. Выходные аргументы представляют собой центр, величину и двоичное изображение искомого объекта. Входные аргументы для exampleHelperTurtleBotPointController - центр объекта, величина (хотя величина не используется в примере), размер изображения и усиление контроллера (структура). Выходными аргументами являются линейные и угловые скорости.

Базовый конечный автомат, используемый в 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, которая является частью панели инструментов обработки изображений.

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

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

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

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