Глава 3. Основы технического зрения
Техническое зрение — направление робототехники, в котором изучаются методы получения изображения с камеры, его обработки и выделения на нем объектов для принятия решений о движении робота. В отличие от датчиков, измеряющих одну величину в одной точке, система технического зрения анализирует изображение целиком и определяет положение, размер и тип объектов в поле зрения камеры.
Для изучения основ технического зрения потребуется ресурсный комплект для кибернетика, в состав которого входит модуль технического зрения TrackingCam. Этот комплект является дополнительным и поставляется отдельно от основного ресурсного комплекта.
Обзор модуля технического зрения TrackingCam
Современные мобильные роботы функционируют в заранее неопределенной, изменяющейся среде, взаимодействуют с объектами в ней. В процессе движения робот должен оценивать обстановку: измерять собственные координаты, классифицировать окружающие его объекты и определять их положение. Для этого используются различные датчики и системы.
Особое место среди оптических локационных систем занимают системы технического зрения (СТЗ). Цифровая камера в отличие от точечного фотореле или датчика цвета, представляет собой двумерный массив из тысяч миниатюрных оптических датчиков, что позволяет получить намного больше сведений об окружающей среде. Сигналы с этого массива преобразуются в яркость и цвет отдельных дискретных элементов — пикселей, образующих цифровое изображение рабочей сцены. В состав СТЗ входят цифровые телекамеры, вычислительное устройство и комплекс программных средств для анализа изображения, в частности для выделения на нем интересующих объектов.
TrackingCam – модуль технического зрения, способный выполнять операции по распознаванию как однотонных объектов, так и составных объектов, состоящих из нескольких цветов.

Внешний вид модуля
Элементы модуля TrackingCam:
Объектив со стандартной резьбой М12.
Калибровочный винт – используется для фиксации объектива после настройки фокуса.
Светодиодный индикатор. Он отображает состояние модуля (красный цвет – объект не распознается, зеленый – распознается, остальные цвета – сервисные).
Разъем Dynamixel – используется для подключения модуля к Dynamixel-совместимым устройствам.
Общий разъем подключения к другим устройствам – содержит выводы питания, синхронизации, интерфейсов UART, I2C, SPI.
Разъем USB – служит для подключения модуля к ПК.
Кнопка – используется для перезагрузки камеры.
Отладочные выводы SWD – используется для сервисного обслуживания модуля.

Внешний вид модуля в корпусе
Программное обеспечение модуля технического зрения TrackingCam
Программное обеспечение модуля TrackingCam можно скачать по ссылке
ПО для работы с модулем и представляет собой интерфейс для задания распознаваемых образов, настройки телекамеры и портов ввода-вывода, захвата изображения и иллюстрации работы алгоритмов.

ПО для настройки модуля TrackingCam
Пусконаладка модуля и распознавание объектов
Настройка камеры для распознавания одноцветной области сводится к заданию ее цветовых и морфологических признаков. Рассмотрим в качестве распознаваемого объекта зеленый мяч для настольного тенниса, расположим его перед камерой таким образом, чтобы он целиком попадал в зону ее видимости. Это необходимо для того, чтобы выбрать все необходимые оттенки цвета, набор которых будет различен, в зависимости от внешнего освещения.

Пример размещения одного однотонного объекта перед камерой
Для выполнения первоначальной настройки необходимо подключить модуль к компьютеру с помощью кабеля mini-USB и запустить приложение «TrackingCam App». В главном окне нажать кнопку «Подключить камеру». В окне «Камера» появится изображение с телекамеры. Убедившись, что модуль видит шарик целиком и зажав кнопку Ctrl на клавиатуре, нажмем левой кнопкой мыши на изображение шарика в окне «Камера». Модуль отделит шарик от фона и применит к нему параметры распознавания по умолчанию. При этом программное обеспечение автоматически подберет набор цветовых оттенков, которые будут использоваться при распознавании. Выбранные оттенки можно наблюдать в окне палитры справа, при необходимости можно расширить область фиксируемых оттенков. Результат поиска выбранных оттенков отобразится в окне иллюстрации бинаризации («Линейные блобы»). Окончательный результат распознавания области с учетом морфологических признаков отобразится в окне «Детектор блобов». Если же в окне «Детектор блобов» не отображается область, соответствующая шарику, либо же видно несколько областей, вызываемых прочими внешними объектами или бликами, необходимо отрегулировать настройки детектора во вкладке «Управление детекцией». После выполнения настройки модуля необходимо выполнить их запись в память модуля, чтобы после выключения питания они сохранялись. Для этого необходимо нажать значок «Сохранить». После нажатия начнется запись настроек в память модуля, которая займет несколько секунд. Таким образом, модуль технического зрения настраивается на распознавание простых однотонных объектов.

Внешний вид интерфейса TrackingCam при настройке на однотонный объект
Модуль TrackingCam способен распознавать до 10 однотонных областей разных оттенков. Для настройки камеры на работу в этом режиме необходимо во вкладке «Цветовые паттерны» выбрать требуемое число шаблонов переключателем «Количество паттернов» — каждый шаблон отвечает за определенный оттенок — и затем по очереди настроить каждый шаблон по аналогии как настраивается модуль на отслеживание и распознавание одной области. Для этого необходимо поочередно выбирать «Активный паттерн» и последовательно выполнять настройки для каждого требуемого цвета (оттенка). В качестве примера представлена настройка камеры на распознавание зеленого и красного шариков.

Пример размещения двух однотонных объектов
перед камерой

Внешний вид интерфейса TrackingCam при настройке на два объекта разного цвета
Первый шаблон будет отвечать за распознавание зеленого шарика, второй – красного шарика. После настройки, для получения информации о распознанных областях можно нажать на каждую область в окне «Детектор блобов» и в правом нижнем углу будет выведена вся информация о выбранной области. Эта информация будет содержать в себе данные о типе (type), индексе (index), положении центра (cx, cy), площади в пикселях (area), значении округлости (circularity), вытянутости/инерции (inertia), выпуклости (convexity) и угле поворота (angle). Применительно к ситуации с шариками информация может выглядеть следующим образом:


Пример вывода информации о распознанных областях
Здесь Тип 0 – область, соответствующая зеленому шарику, а Тип 1 – красному. После выполнения настройки модуля параметры необходимо сохранить в модуль аналогично случаю с распознанием одной области.
Помимо задач распознавания однотонных областей, зачастую возникает задача отслеживания разноцветных объектов, например, многоцветных меток — маркеров. Для этого модуль TrackingCam настраивается на распознавание составных объектов, которые описываются взаимными размерами, положением, ориентацией и прочими параметрами, входящих в них однотонных областей. При объединении однотонных областей в составные объекты разработчики модуля условно называют их узлами объекта, а векторы связывающих их параметров связями. Модуль TrackingCam способен отслеживать одновременно до 5 объектов, состоящих максимум из 3-х узлов.
Разберем процесс настройки модуля на примере распознавания одного объекта, состоящего из зеленой и красной областей. Для начала необходимо настроить модуль на распознование двух областей – зеленой и красной, как это описывалось ранее. После этих манипуляций в окне «Детектор блобов» будут отображаться сразу две разные области. Затем необходимо зайти во вкладку «Объекты», нажав кнопку «Количество паттернов», выбираем, сколько разноцветных областей будет детектироваться. Далее переключателем «Тип» указываем количество узлов, из которых состоит объект. В данном случае один объект состоит из 2-х цветных областей. После этого на схеме объекта окажутся подсвеченными 2 узла и образуется связь между ними. После настройки параметров искомого объекта переходим обратно во вкладку «Детектор» и затем в окне «Детектор блобов» можно будет увидеть обе ранее определенных области, объединенных в один объект. После выполнения настройки модуля параметры необходимо сохранить в модуль аналогичным ранее рассмотренному способу.

Внешний вид интерфейса TrackingCam при настройке на разноцветный объект
Текущую информацию о параметрах, как отдельных областей, так и целиком всего объекта можно посмотреть в терминале. Информация о параметрах областей, входящих в состав объекта, получается аналогично случаю распознавания отдельных областей – путем нажатия на интересующую область в окне «Детектор блобов». Работа модуля в режиме распознавания объектов может быть полезна при решении задач соревновательного направления – распознавания различных цветовых меток, следования по линиям, состоящим из разноцветных областей, классификация окружающих объектов.
Практическая часть
Работа с модулем технического зрения «TrackingCam»
Цель:
Освоить работу с модулем технического зрения TrackingCam, научиться подключать и настраивать его для распознавания объектов и запрограммировать реакцию микроконтроллера на визуальную информацию с камеры.
Оборудование:
модуль технического зрения TrackingCam;
блок-контроллер ЭДУино.
Для программы понадобятся библиотеки TrackingCamDxl.h и DxlMaster2.h, через которые контроллер обменивается данными с модулем машинного зрения по протоколу Dynamixel. Библиотеки TrackingCamDxl.h и библиотеку DxlMaster2.h можно скачать на сайте
Чтобы добавить библиотеку, скачаем zip-архив по указанной выше ссылке. Затем откроем Arduino IDE и перейдем в меню «Скетч» → «Подключить библиотеку» → «Добавить .ZIP библиотеку». Выберем скачанный zip-архив с библиотекой.

Добавление библиотек
После добавления у каждой библиотеки в меню «Примеры» появляются готовые программы. Чтобы открыть пример, пройдем по пути «Файл» → «Примеры» → «Имя библиотеки».

Пример из библиотеки
Модуль TrackingCam — устройство машинного зрения, которое распознает цветные объекты в поле зрения камеры. В нем есть CMOS-матрица и процессор, который в реальном времени обрабатывает изображение и определяет положение, размер и тип объектов заданных цветов. Модуль может одновременно отслеживать несколько объектов разных цветов, поэтому его используют в задачах навигации и распознавания целей.
Модуль технического зрения TrackingCam разработан и выпускается в России компанией ООО «Прикладная робототехника». На корпусе модуля расположены объектив, калибровочный винт, которым фиксируют объектив после настройки фокуса, светодиодный индикатор состояния модуля и разъемы Dynamixel для подключения к другим устройствам.
Модуль умеет распознавать как одноцветные объекты, так и составные, состоящие из нескольких цветовых областей. Составные объекты — это, например, цветные метки-маркеры из нескольких полос: робот может найти такую метку на стене и по ней определить, где он находится.
Как камера «видит»? В работе с датчиком цвета робот измерял цвет в одной точке: датчик выдавал три числа, красный, зеленый и синий. Изображение с камеры состоит из множества таких точек — пикселей, расположенных рядами, как клетки в тетради. Каждый пиксель тоже описывается тремя числами RGB. Например, в кадре размером 320 × 240 пикселей их 320 × 240 = 76 800, то есть камера за один кадр делает столько же измерений цвета, сколько датчик цвета сделал бы за 76 800 опросов.
Чтобы найти на кадре объект, программа модуля проверяет, какие пиксели подходят под заданный цвет, и объединяет соседние подходящие пиксели в одну область. Такую область называют блобом, от английского blob — «капля», «пятно». Для каждого блоба модуль вычисляет положение его центра, размер и тип, то есть к какому из заданных цветов он относится.
Обработка изображения требует много памяти и вычислений. У контроллера всего примерно 2 килобайта оперативной памяти, 2048 байт. Даже черно-белый кадр 320 × 240, где на каждый пиксель отводится всего один бит, занимает 76 800 / 8 = 9600 байт, почти в пять раз больше. Поэтому контроллер не обрабатывает изображение сам. Модуль TrackingCam работает автономно и не нагружает вычислениями основной контроллер робота: он сам анализирует каждый кадр и передает контроллеру только готовый результат, несколько чисел о найденных объектах.
Машинное зрение используют повсюду: камеры на конвейерах отбраковывают детали, смартфоны распознают лицо владельца, автомобили следят за дорожной разметкой, сканеры на кассе считывают штрихкоды.
Протокол Dynamixel служит для связи между микроконтроллером и модулем. Это последовательный протокол, который позволяет передавать команды и получать данные с высокой скоростью, до 1 Мбит/с. Библиотека TrackingCamDxl дает удобные функции для чтения данных о найденных объектах, а DxlMaster2 отвечает за саму передачу данных по протоколу.
Протокол Dynamixel как и в интерфейсе I2C из работы с LCD-дисплеем, несколько устройств подключаются к одной линии связи, и каждое имеет свой номер — идентификатор. Контроллер обращается к устройству по идентификатору, и отвечает только оно. Благодаря порту Dynamixel несколько камер можно объединить в последовательную сеть и так расширить область зрения робота. Устройства соединяются друг с другом цепочкой: от контроллера к первому, от первого ко второму и так далее.
В этой работе модуль настроен на распознавание объектов типа 0, то есть определенного цвета. Программа считывает, обнаружен ли такой объект, и по этому включает или выключает встроенный светодиод на выводе 13.
Настройка модуля на цвет. Какой цвет соответствует типу 0, задается не в программе для контроллера, а заранее, при настройке самого модуля. Для этого модуль подключают к компьютеру кабелем USB Mini-B и запускают приложение TrackingCam App. После нажатия кнопки Connect в окне появляется изображение с камеры. Если, удерживая клавишу Ctrl, щелкнуть мышью по объекту на изображении, модуль отделит его от фона и сам подберет набор цветовых оттенков для распознавания. Модуль способен распознавать до 10 одноцветных областей разных оттенков. Каждой настроенной области присваивается свой номер типа, начиная с 0.
Схема подключения:
Кабель DXL подключается одним концом к разъему на плате ЭДУино, а другим к разъему на модуле TrackingCam.

Схема подключения
В данной работе разработаем программу для работы с модулем TrackingCam, которая будет считывать информацию о наличии объекта заданного типа и управлять светодиодом в зависимости от его обнаружения.
Разработаем программу для работы с модулем TrackingCam, которая считывает, обнаружен ли объект заданного типа, и в зависимости от этого включает или выключает светодиод.
В начале программы подключаются библиотеки TrackingCamDxl.h и DxlMaster2.h. Библиотека TrackingCamDxl содержит функции для работы с модулем TrackingCam, а DxlMaster2 обеспечивает связь по протоколу Dynamixel. Создается объект tc2 для управления модулем с идентификатором 51 и протоколом 1. Объявляется переменная blob для хранения типа обнаруженного объекта.
В функции setup() вывод 13 настраивается на выход: к нему подключен встроенный светодиод платы ЭДУино. Затем запускается шина Dynamixel командой DxlMaster.begin(57600), где 57600 — скорость передачи данных.
В основном цикле loop() вызывается функция tc2.readBlobs(1), которая считывает данные о найденных объектах. Аргумент 1 — количество объектов, которые нужно прочитать. Тип первого обнаруженного объекта сохраняется в переменную blob. Если тип равен 0, светодиод на выводе 13 включается, иначе выключается. Пауза 120 мс делает работу стабильной и не дает светодиоду переключаться слишком часто.
Пример рабочей программы****:
Перед запуском настроим модуль на цвет: подключим его к компьютеру, откроем TrackingCam App и выберем объект, например цветной мячик или кубик. Сохраним параметры, затем отключим модуль от компьютера и соединим его с платой ЭДУино кабелем DXL.
Загрузим программу в контроллер. Поднесем объект к камере: светодиод L на плате должен загореться. Уберем объект из поля зрения — светодиод погаснет. Проверим, на каком расстоянии и под каким углом камера еще видит объект, и как на распознавание влияет освещение: включим и выключим свет в классе, поднесем объект к окну.
Затем проверим, как модуль отличает цвета. Покажем камере объект похожего, но другого оттенка, например оранжевый вместо красного, и понаблюдаем, горит ли светодиод.
cpp
#include "TrackingCamDxl.h" // библиотека для работы с модулем технического зрения TrackingCam
#include "DxlMaster2.h" // библиотека для связи по протоколу Dynamixel
TrackingCamDxl tc2(51, 1); // объект камеры: идентификатор 51, протокол 1
int blob = 0; // тип обнаруженного объекта
void setup() {
pinMode(13, OUTPUT); // пин 13 — на выход (встроенный светодиод L)
DxlMaster.begin(57600); // запускаем шину Dynamixel на скорости 57600 бод
}
void loop() {
tc2.readBlobs(1); // считываем данные об одном обнаруженном объекте
blob = (int)tc2.blob[0].type; // берем тип первого объекта
// Если объект нужного типа (0) обнаружен — включаем светодиод, иначе выключаем
if ((blob == 0)) {
digitalWrite(13, HIGH);
} else {
digitalWrite(13, LOW);
}
delay(120); // пауза 120 мс между опросами камеры
}Рабочая программа на графическом языке программирование:


Контрольные вопросы:
Что такое «блоб» в терминологии TrackingCam и какие параметры объекта модуль определяет?
Зачем модулю собственный процессор и CMOS-матрица — что он делает с изображением сам, до передачи данных контроллеру?
За что отвечает протокол Dynamixel и почему для камеры важна его высокая скорость передачи?
Что произойдет в программе, если объект нужного типа исчезнет из поля зрения камеры?
Модель «Платформа всенаправленного движения». Следование за объектом
Цель:
Освоить работу с модулем технического зрения TrackingCam для отслеживания объектов, научиться строить следование за цветным объектом по его координатам и площади и запрограммировать плавное управление скоростью и направлением движения робота.
Оборудование:
4 двигателя постоянного тока;
сервопривод большой;
сервопривод малый;
модуль технического зрения TrackingCam;
блок-контроллер ЭДУино;
аккумуляторный модуль PowerBank 9V 3A.

Модель робота «Платформа всенаправленного движения» с модулем TrackingCam
Робот собран на четырехколесной платформе из предыдущих работ. Спереди, над захватом, на желтой пластине закреплен модуль TrackingCam, объектив которого направлен вперед по ходу движения. Камера соединена с платой ЭДУино кабелем DXL. В прошлой работе модуль только сообщал, видит ли он объект. Теперь робот использует данные камеры, чтобы ехать за объектом.
Для программы понадобятся библиотеки TrackingCamDxl.h и DxlMaster2.h для работы с модулем машинного зрения, а также функции analogWrite**(**) и digitalWrite**(**) для управления моторами.
Модуль TrackingCam позволяет роботу «видеть» цветные объекты и определять их параметры: координаты центра (cx, cy), площадь (area) и тип (type). Эти данные передаются по протоколу Dynamixel, и по ним робот принимает решения о движении. Координаты cx и cy определяют положение объекта в поле зрения камеры (от 0 до 255), а площадь тем меньше, чем дальше объект: она обратно пропорциональна квадрату расстояния.
Положение объекта на кадре задается двумя координатами, как точка на координатной плоскости из курса математики 6 класса. Координата x показывает, насколько объект смещен по горизонтали, y — по вертикали. Отличие от привычной плоскости в том, что в компьютерной графике начало координат обычно находится в левом верхнем углу кадра, а ось y направлена вниз.
Центр объекта — это точка, которая делит найденное цветовое пятно пополам по ширине и по высоте. Площадь — количество пикселей, из которых пятно состоит.
Если кадр имеет ширину 320 пикселей, его середина по горизонтали — x = 160. Объект с координатой cx = 160 находится точно перед роботом. Если cx больше 160, объект правее, если меньше — левее.
Площадь и расстояние. Чем дальше объект, тем меньше он выглядит на изображении. Из подобия треугольников (геометрия 8 класса) следует, что если расстояние до объекта увеличится вдвое, его ширина и высота на кадре уменьшатся вдвое. Площадь при этом уменьшится в 2 × 2 = 4 раза.
Рассчитаем, что означают значения из программы. Целевая площадь 5000 пикселей, площадь «вдали» 500. Площади отличаются в 10 раз, значит, расстояния отличаются в √10 ≈ 3,2 раза. Например, если робот останавливается в 30 см от объекта, то на максимальной скорости он едет к объекту, который дальше примерно 30 × 3,2 ≈ 96 см.
Алгоритм следования строится на двух принципах.
Первый — регулировка скорости в зависимости от расстояния до объекта, которое определяется по площади: чем ближе объект, тем медленнее едет робот, а на целевом расстоянии останавливается.
Второй — корректировка направления: если объект смещен влево или вправо от центра кадра, робот поворачивает в ту же сторону, чтобы объект снова оказался по центру. Чтобы движение было плавным, скорость фильтруется.
В промежутке между «далеко» и «близко» скорость рассчитывается функцией map(), знакомой по работе с пьезодинамиком. Она переводит площадь из диапазона 500–5000 в скорость от 50 до 15. Чем ближе объект, тем меньше скорость. Это упрощенный вариант пропорциональной составляющей ПИД-регулятора из работы с балансирующим роботом: сила реакции зависит от того, насколько текущее значение отличается от нужного.
Так работает адаптивный круиз-контроль в современных автомобилях: камера или радар измеряют расстояние до машины впереди, и автомобиль сам замедляется, когда расстояние сокращается, и ускоряется, когда оно растет.
Камера иногда ошибается: на одном кадре пятно получается чуть больше, на другом чуть меньше. Если сразу менять скорость по каждому кадру, робот будет дергаться. Поэтому новая скорость вычисляется так:
новая скорость = старая скорость × 0,7 + рассчитанная скорость × 0,3
Проследим, как скорость растет с нуля, если рассчитанная скорость равна 50. Первый шаг: 0 × 0,7 + 50 × 0,3 = 15. Второй: 15 × 0,7 + 15 = 25,5. Третий: 25,5 × 0,7 + 15 ≈ 32,9. Четвертый: ≈ 38. Скорость приближается к 50 постепенно, без рывков. Такой же фильтр сглаживал скорость колес у балансирующего робота.
Робот поворачивает не при любом смещении объекта от центра, а только если объект вышел за пределы центральной полосы кадра. Внутри полосы робот едет прямо. Без такой полосы робот поворачивал бы то вправо, то влево при малейшем смещении, и ехал бы зигзагом. Такая полоса — это «мертвая зона», как в программе балансирующего робота.
Схема подключения:

Схема расположения электронных компонентов

Схема подключения
Разработаем программу для четырехколесной платформы с модулем TrackingCam. Робот следует за цветным объектом, держится от него на нужном расстоянии и удерживает объект в центре поля зрения камеры.
В начале программы подключаются библиотеки TrackingCamDxl.h и DxlMaster2.h для работы с модулем TrackingCam. Создается объект tc2 для управления модулем с идентификатором 51 и протоколом 1. Объявляются переменные для выводов четырех моторов, текущей скорости, максимальной и минимальной скоростей, целевой площади объекта (то есть расстояния до него) и параметров обнаруженного объекта.
Функция setup**(**) настраивает моторы и запускает шину Dynamixel для связи с камерой. В основном цикле loop**(**) программа считывает данные с модуля: тип, координаты и площадь объекта, после чего вызывает функцию following**(**), которая управляет движением.
Функция calculateSpeed**(**) вычисляет скорость по площади объекта. Если площадь меньше farArea (объект далеко), скорость максимальная. Если площадь достигла targetArea (объект близко), скорость равна нулю. При промежуточных значениях скорость вычисляется пропорционально с помощью функции map**(**).
Функция following**(**) выполняет основной алгоритм слежения. Если объект не обнаружен, моторы останавливаются. Для плавного изменения скорости используется фильтр: новая скорость вычисляется как взвешенное среднее предыдущей и рассчитанной. Затем скорость ограничивается диапазоном 0–255. Если объект смещен вправо (cx > 200), робот поворачивает направо. Если смещен влево (cx < 120), поворачивает налево. Если объект в центре (от 120 до 200), робот едет прямо. Пауза между повторами составляет 200 мс. Функции turnRight**(**) и turnLeft**(**) используют скорость, уменьшенную на 30 %, чтобы робот поворачивал плавно.
Пример рабочей программы****:
Перед запуском настроим модуль TrackingCam на цвет объекта в приложении TrackingCam App, как в предыдущей работе. Лучше взять объект яркого цвета, например оранжевый мяч.
Загрузим программу в контроллер, отключим USB-кабель, подключим аккумуляторный модуль и поставим робота на пол. Держа объект перед камерой примерно в метре, будем медленно отходить. Робот должен ехать следом, замедляться при приближении и останавливаться на некотором расстоянии от объекта. Сместим объект влево и вправо: робот должен поворачиваться за ним.
Измерим линейкой, на каком расстоянии от объекта робот останавливается. Изменим значение targetArea, например на 2500 и на 10000, и проверим, как меняется это расстояние. Сравним результат с расчетом: при уменьшении целевой площади в 2 раза расстояние должно увеличиться примерно в √2 ≈ 1,4 раза.
Чтобы видеть, какие числа передает камера, можно временно добавить в setup**(**) строку Serial.begin**(9600**);, а в loop**(**) вывод cx и area в монитор порта.
cpp
#include "TrackingCamDxl.h"
#include "DxlMaster2.h"
TrackingCamDxl tc2(51, 1);
int DIR_M1 = 0;
int SPEED_M1 = 0;
int DIR_M2 = 0;
int SPEED_M2 = 0;
int DIR_M3 = 0;
int SPEED_M3 = 0;
int DIR_M4 = 0;
int SPEED_M4 = 0;
float speed = 0;
int maxSpeed = 50; // максимальная скорость PWM
int minSpeed = 15; // минимальная скорость PWM
int targetArea = 5000; // желаемый размер объекта
int farArea = 500; // размер объекта вдали
unsigned long previousMillis = 0; // переменная для задержки по времени
int type = 0; // тип объекта
int cx = 0; // координата x объекта
int cy = 0; // координата y объекта
int area = 0; // площадь объекта
void setup() {
initMotors();
DxlMaster.begin(57600);
}
void loop() {
tc2.readBlobs(1);
type = (int)tc2.blob[0].type;
cx = (int)tc2.blob[0].cx;
cy = (int)tc2.blob[0].cy;
area = (int)tc2.blob[0].area;
following();
}
void initMotors() {
DIR_M1 = (int)4;
SPEED_M1 = (int)5;
DIR_M2 = (int)7;
SPEED_M2 = (int)6;
DIR_M3 = (int)8;
SPEED_M3 = (int)3;
DIR_M4 = (int)12;
SPEED_M4 = (int)11;
pinMode(DIR_M1, OUTPUT);
pinMode(DIR_M2, OUTPUT);
pinMode(DIR_M3, OUTPUT);
pinMode(DIR_M4, OUTPUT);
pinMode(SPEED_M1, OUTPUT);
pinMode(SPEED_M2, OUTPUT);
pinMode(SPEED_M3, OUTPUT);
pinMode(SPEED_M4, OUTPUT);
}
int calculateSpeed(int area)
{
int result;
// объект дальше дальней границы
if (area <= farArea)
{
result = maxSpeed;
}
// объект достиг нужного расстояния
else if (area >= targetArea)
{
result = 0;
}
// промежуточная скорость
else
{
result = map(area, farArea, targetArea, maxSpeed, minSpeed);
}
return result;
}
void following() {
if (type == 0)
{
// Фильтруем некорректные значения
if ((area < 200 || area > 100000) || cx > 320 || cy > 240) // максимальное разрешение камеры
{
// Игнорируем этот кадр
return;
}
if ((cx == 0 || cy == 0))
{
if (millis() - previousMillis > 1000)
{
stopMotors();
}
return;
}
float k_speed = 0.3;
speed = speed * (1 - k_speed) + calculateSpeed(area) * k_speed;
speed = constrain(speed, 0, 255);
if(speed < 5)
{
stopMotors();
return;
}
if (cx > 200 )//макс значение координаты Х камеры
{
turnRight(speed);
previousMillis = millis();
}
else if ((cx < 120 ) && (cx != 0))//мин значение координаты Х камеры
{
turnLeft(speed);
previousMillis = millis();
}
else if ((cx >=120) && (cx <= 200))
{
forward(speed);
previousMillis = millis();
}
}
else {
stopMotors();
}
while (millis() - previousMillis < 200)
{};
}
void forward(int sp) {
digitalWrite(DIR_M1, HIGH);
analogWrite(SPEED_M1, sp);
digitalWrite(DIR_M3, HIGH);
analogWrite(SPEED_M3, sp);
digitalWrite(DIR_M2, LOW);
analogWrite(SPEED_M2, sp);
digitalWrite(DIR_M4, LOW);
analogWrite(SPEED_M4, sp);
}
void stopMotors() {
digitalWrite(DIR_M1, LOW);
analogWrite(SPEED_M1, 0);
digitalWrite(DIR_M3, LOW);
analogWrite(SPEED_M3, 0);
digitalWrite(DIR_M2, LOW);
analogWrite(SPEED_M2, 0);
digitalWrite(DIR_M4, LOW);
analogWrite(SPEED_M4, 0);
}
void turnRight(int sp) {
int slow = sp * 0.7; // поворот медленнее движения вперед
digitalWrite(DIR_M1, HIGH);
analogWrite(SPEED_M1, slow);
digitalWrite(DIR_M3, HIGH);
analogWrite(SPEED_M3, slow);
digitalWrite(DIR_M2, HIGH);
analogWrite(SPEED_M2, slow);
digitalWrite(DIR_M4, HIGH);
analogWrite(SPEED_M4, slow);
}
void turnLeft(int sp) {
int slow = sp * 0.7; // поворот медленнее движения вперед
digitalWrite(DIR_M1, LOW);
analogWrite(SPEED_M1, slow);
digitalWrite(DIR_M3, LOW);
analogWrite(SPEED_M3, slow);
digitalWrite(DIR_M2, LOW);
analogWrite(SPEED_M2, slow);
digitalWrite(DIR_M4, LOW);
analogWrite(SPEED_M4, slow);
}Пример рабочей программы на графическом языке:











Контрольные вопросы:
Как по площади объекта (area) робот судит о расстоянии до него, и почему связь именно обратная?
На чем построена регулировка скорости в функции calculateSpeed() и какую роль здесь играет map()?
Как робот решает, поворачивать ему или ехать прямо, опираясь на координату cx?
Для чего скорость фильтруется (берется взвешенное среднее с предыдущим значением), и что будет с движением без этой фильтрации?
Модель «Платформа всенаправленного движения». Движение по линии с помощью модуля технического зрения
Цель:
Освоить использование модуля машинного зрения TrackingCam для распознавания линии, научиться строить следование по траектории на основе координаты объекта в кадре и запрограммировать автономное движение платформы по линии заданного цвета.
Оборудование:
4 двигателя постоянного тока;
модуль технического зрения TrackingCam;
блок-контроллер ЭДУино;
аккумуляторный модуль PowerBank 9V 3A.
Робот собран так же, как в предыдущей работе: платформа всенаправленного движения с модулем технического зрения TrackingCam в передней части. Чтобы камера видела линию, ее объектив должен быть направлен на пол перед роботом, поэтому модуль наклоняют вниз. В работе с датчиками линии робот видел только две точки под собой. Камера видит сразу целый участок трассы, в том числе тот, до которого робот еще не доехал.
Для программы понадобятся библиотеки TrackingCamDxl.h и DxlMaster2.h для работы с модулем машинного зрения, а также функции analogWrite**(**) и digitalWrite**(**) для управления моторами.
Два датчика линии давали программе всего четыре возможных сочетания: 0 и 0, 1 и 0, 0 и 1, 1 и 1. Робот узнавал только то, что линия ушла вправо или влево, но не насколько. Камера сообщает координату центра линии на кадре, cx, — число от 0 до 320. По нему видно не только, в какую сторону сместилась линия, но и на сколько пикселей.
У камеры есть еще одно преимущество: она смотрит вперед. Датчики линии замечают поворот, только когда робот уже заехал на него. Камера видит поворот заранее, и робот может начать поворачивать вовремя.
Алгоритм следования по линии построен на простой логике: робот анализирует координату cx и исправляет направление движения. Если линия находится слева, робот поворачивает налево. Если линия справа, поворачивает направо. Если линия в центре, едет прямо. Благодаря этому робот удерживается на траектории, все время подстраивая направление под положение линии. Дополнительно в программе отбрасываются некорректные данные, а при потере линии робот останавливается.
Центральная полоса кадра, в которой робот едет прямо, та же, что и в работе со следованием за объектом: от 120 до 200 пикселей. Ее ширина 80 пикселей, то есть четверть ширины кадра.
Разработаем программу с модулем TrackingCam, которая автоматически следует по линии заданного цвета и исправляет направление движения по данным камеры.
Чтобы платформа двигалась правильно, настроим область видимости в приложении «TrackingCam App».
Настроим камеру на черный цвет. Результат распознавания отобразится в окне «Детектор блобов».

Окно «Детектор блобов» до настройки видимости
Видно, что камера распознает не только линию, но и темное окружение поля. Чтобы платформа двигалась по линии, ограничим область видимости камеры, настроив горизонтальный и вертикальный диапазоны. Теперь камера не будет распознавать другие темные объекты, кроме линии на поле.

Окно «Детектор блобов» после настройки видимости
Для камеры «черный» означает просто «темный». Под это описание подходят не только линия, но и край поля, тень от робота, ножка стола, темная обувь человека рядом с трассой. Если такой объект окажется в кадре, робот может поехать к нему, а не по линии.
Поэтому обработку ограничивают частью кадра, в которой может оказаться только линия. Такую часть называют областью интереса. Обычно выбирают нижнюю часть кадра: она соответствует участку пола прямо перед роботом, где линия точно есть, а посторонних предметов меньше всего.
Из-за наклона камеры изображение пола получается в перспективе. Участок пола, попадающий в кадр, имеет форму трапеции: ближе к роботу он узкий, дальше — широкий, как дорога на рисунке, уходящая к горизонту. Параллельные края линии на изображении сходятся вдали. Чем выше в кадре находится точка, тем дальше она от робота. Поэтому по верхней части кадра робот может заранее заметить поворот, а по нижней точнее определить, где линия прямо сейчас.
Программа этой работы использует координату cx так же, как два датчика: линия либо в центре, либо слева, либо справа. Это то же релейное управление, что и в работе с датчиками линии, и робот тоже будет двигаться небольшим зигзагом. Координата cx позволяет сделать управление плавным: поворачивать тем сильнее, чем дальше линия от центра кадра. Это пропорциональное управление, похожее на регулировку скорости по площади объекта в предыдущей работе.
Пример рабочей программы****:
Подготовим трассу: черная линия из изоленты на белом поле с плавными поворотами. Поставим робота на трассу, подключим модуль к компьютеру и настроим в приложении цвет линии и область видимости так, чтобы в окне «Детектор блобов» оставалась только линия.
Загрузим программу в контроллер, отключим USB-кабель, подключим аккумуляторный модуль и поставим робота так, чтобы линия была перед ним по центру. Понаблюдаем, как робот проходит прямые участки и повороты. Сравним его движение с роботом из работы с датчиками линии: насколько заметен зигзаг и как робот проходит крутые повороты.
Изменим границы центральной полосы: вместо 120 и 200 поставим 140 и 180, затем 100 и 220. Понаблюдаем, как ширина полосы влияет на плавность движения и на то, удерживается ли робот на линии.
Поставим рядом с трассой темный предмет, например черный пенал, и проверим, не сбивается ли робот. Если сбивается, уточним область видимости в приложении.
cpp
#include "TrackingCamDxl.h"
#include "DxlMaster2.h"
TrackingCamDxl tc2(51, 1);
int DIR_M1 = 0;
int SPEED_M1 = 0;
int DIR_M2 = 0;
int SPEED_M2 = 0;
int DIR_M3 = 0;
int SPEED_M3 = 0;
int DIR_M4 = 0;
int SPEED_M4 = 0;
float speed = 0;
unsigned long previousMillis = 0; // переменная для задержки по времени
int type = 0; // тип объекта
int cx = 0; // координата x объекта
int cy = 0; // координата y объекта
void setup() {
initMotors();
carData();
DxlMaster.begin(57600);
}
void loop() {
tc2.readBlobs(1);
type = (int)tc2.blob[0].type;
cx = (int)tc2.blob[0].cx;
cy = (int)tc2.blob[0].cy;
following();
}
void initMotors() {
DIR_M1 = (int)4;
SPEED_M1 = (int)5;
DIR_M2 = (int)7;
SPEED_M2 = (int)6;
DIR_M3 = (int)8;
SPEED_M3 = (int)3;
DIR_M4 = (int)12;
SPEED_M4 = (int)11;
pinMode(DIR_M1, OUTPUT);
pinMode(DIR_M2, OUTPUT);
pinMode(DIR_M3, OUTPUT);
pinMode(DIR_M4, OUTPUT);
pinMode(SPEED_M1, OUTPUT);
pinMode(SPEED_M2, OUTPUT);
pinMode(SPEED_M3, OUTPUT);
pinMode(SPEED_M4, OUTPUT);
}
void carData() {
speed = (int)40;
}
void following() {
if (type == 0)
{
// Фильтруем некорректные значения
if (cx > 320 || cy > 240) // максимальное разрешение камеры
{
// Игнорируем этот кадр
return;
}
if ((cx == 0 || cy == 0))
{
if (millis() - previousMillis > 1000)
{
stopMotors();
}
return;
}
if (cx > 200 )
{
turnRight(speed);
previousMillis = millis();
}
else if ((cx < 120 ) && (cx != 0))//мин значение координаты Х камеры
{
turnLeft(speed);
previousMillis = millis();
}
else if ((cx >= 120) && (cx <= 200))
{
forward(speed);
previousMillis = millis();
}
}
else {
stopMotors();
}
while (millis() - previousMillis < 200)
{};
}
void forward(int sp) {
digitalWrite(DIR_M1, HIGH);
analogWrite(SPEED_M1, sp);
digitalWrite(DIR_M3, HIGH);
analogWrite(SPEED_M3, sp);
digitalWrite(DIR_M2, LOW);
analogWrite(SPEED_M2, sp);
digitalWrite(DIR_M4, LOW);
analogWrite(SPEED_M4, sp);
}
void stopMotors() {
digitalWrite(DIR_M1, LOW);
analogWrite(SPEED_M1, 0);
digitalWrite(DIR_M3, LOW);
analogWrite(SPEED_M3, 0);
digitalWrite(DIR_M2, LOW);
analogWrite(SPEED_M2, 0);
digitalWrite(DIR_M4, LOW);
analogWrite(SPEED_M4, 0);
}
void turnRight(int sp) {
int slow = sp * 0.7; // поворот медленнее движения вперед
digitalWrite(DIR_M1, HIGH);
analogWrite(SPEED_M1, slow);
digitalWrite(DIR_M3, HIGH);
analogWrite(SPEED_M3, slow);
digitalWrite(DIR_M2, HIGH);
analogWrite(SPEED_M2, slow);
digitalWrite(DIR_M4, HIGH);
analogWrite(SPEED_M4, slow);
}
void turnLeft(int sp) {
int slow = sp * 0.7; // поворот медленнее движения вперед
digitalWrite(DIR_M1, LOW);
analogWrite(SPEED_M1, slow);
digitalWrite(DIR_M3, LOW);
analogWrite(SPEED_M3, slow);
digitalWrite(DIR_M2, LOW);
analogWrite(SPEED_M2, slow);
digitalWrite(DIR_M4, LOW);
analogWrite(SPEED_M4, slow);
}Пример рабочей программы на графическом языке:









Контрольные вопросы:
Чем этот способ следования по линии отличается от работы с оптическими датчиками линии?
Зачем перед запуском настраивают область видимости камеры по горизонтали и вертикали — какую проблему это решает?
Как по координате СХ строится логика «влево / прямо / вправо»?
Как программа ведет себя при потере линии и почему такая обработка нужна?