Отследите и следуйте за объектом

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

Выполнение этого примера требует Image Processing Toolbox™.

Предпосылки: свяжитесь с TurtleBot, исследуйте основное поведение TurtleBot, управляйте TurtleBot с дистанционным управлением, объездом препятствий с TurtleBot

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

Этот пример дает обзор работы с TurtleBot с помощью его нативного интерфейса ROS. Пакет Поддержки Robotics System Toolbox™ для Находящихся в TurtleBot Роботов обеспечивает более оптимизированный интерфейс к TurtleBot. Это позволяет вам:

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

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

Чтобы установить пакет поддержки, откройтесь, Дополнения> Получают Пакеты Аппаратной поддержки на вкладке Home MATLAB® и выбирают "TurtleBot-Based Robots". Также используйте roboticsAddons команду.

Соединитесь с TurtleBot

Убедитесь, что у вас есть TurtleBot, запускающийся или в симуляции через Gazebo® или на действительном оборудовании. Обратитесь к Запуску с Gazebo и Моделируемым TurtleBot или Запуску с Действительным TurtleBot для процедуры запуска. Если вы используете оборудование, найдите, что синий шар использует для отслеживания. Если вы используете Gazebo®, синий шар должен быть в мире перед роботом (убедитесь, что вы используете Gazebo TurtleBot World).

Инициализируйте ROS. Соединитесь с TurtleBot, заменив ipaddress на IP-адрес TurtleBot

ipaddress = '192.168.203.129';
rosinit(ipaddress)
Initializing global node /matlab_global_node_46760 with NodeURI http://192.168.203.1:53697/

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

roslaunch turtlebot_bringup 3dsensor.launch. 

Необходимо ввести это в терминал на TurtleBot.

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

handles.colorImgSub = exampleHelperTurtleBotEnableColorCamera;
Successfully Enabled Camera (raw image)
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');

Настройте синее обнаружение шара

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

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

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

exampleHelperTurtleBotPlotObject(latestImg,ball,c);

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

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

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

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

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

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

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

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

handles.gains = gains;

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

timer2 = timer('TimerFcn',{@exampleHelperTurtleBotTrackingTimer,handles},'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 симуляции, относятся к Read Model and Simulation Properties из Gazebo и Добавляют, Сборка, и Удаляют Объекты в Gazebo.

Остановите движение робота

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

stop(timer2);

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

delete(timerfindall)

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

clear

Больше информации

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

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

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

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

Для просмотра документации необходимо авторизоваться на сайте