Михаил Дроздов

В первой части — выбор компонентов, особенности проектирования и изготовления корпуса, распайка электроники. Микроконтроллер, видеокамера, датчики, двигатели — уже соединены, питание подключено. Осталось вдохнуть жизнь в электронный организм, чтобы все его компоненты пробудились и начали согласованно взаимодействовать. Программная архитектура Ограниченные вычислительные ресурсы микроконтроллера не позволили реализовать всю многообразную логику на борту робота. Для выполнения сложных операций нужна машина помощнее — сервер. Микроконтроллер берет на себя базовые задачи, например: сбор данных, управление моторами, получение изображения с камеры, передачу всей накопленной информации в центр обработки. Сервер, в свою очередь, занимается сложными вычислениями — к примеру, обработкой изображений с помощью ИИ. Также он отвечает за управление роботом, отправляя команды. Размещение модели ИИ на микроконтроллере — задача невыполнимая из-за аппаратных ограничений. Кроме того, сервер может хранить примитивную карту местности, сформированную на основе информации от датчиков, что дополнительно улучшает навигацию. Централизованный подход значительно упрощает масштабирование и упрощает архитектуру системы. В случае увеличения нагрузки можно добавить дополнительные вычислительные ресурсы: установить мощные GPU для обработки данных или подключить дополнительные серверы для распределения задач. Клиентская часть Встроенное ПО — это душа робота. Разработка велась в популярной у энтузиастов среде Arduino Framework, но с активным использованием возможностей операционной системы реального времени FreeRTOS, которая лежит в основе ESP-IDF. Еще одно немаловажное достоинство Arduino — множество готовых библиотек и примеров. В классическом Arduino-скетче функции setup() и loop() отвечают за инициализацию периферии и непрерывный цикл обработки. В среде ESP32 Arduino Framework запускается как компонент ESP-IDF и по умолчанию выполняется в рамках одной FreeRTOS-задачи, что дает преимущества обеих платформ. Документация ESP-IDF рекомендует создавать отдельные задачи на большие функции. Для их синхронизации требуются ресурсы, так как семафоры «не бесплатны». При таком подходе важно помнить о «стороже», который будет следить за выполнением каждой задачи. Если одна из них заберет на себя слишком много процессорного времени, «сторожу» придется вмешаться, что вызовет ошибку. Асинхронный подход исключает подобные ситуации — внутри функций есть собственные таймеры, которые запускают выполнение кода. Таким образом, получаем две задачи, распределенные по ядрам. Ядро 0 выполняет задачу loopCore0 — захватом изображения и сетевым взаимодействием. Кадры с камеры OV7670, отправляются по HTTP, а обмен данных происходит по WebSocket. Ядро 1 занято задачей loopCore1 — навигацией и управлением. Она опрашивает датчики, фильтрует данные, вычисляет ошибки и корректирует движение. Показания сохраняются в общий для обеих задач объект. Архитектура клиентской части со стороны робота. Общий объект робота содержит показания датчиков, действительные и заданные углы поворота, различные флаги состояния. Для синхронизации доступа к нему можно применить бинарные семафоры. Так гарантируется защита разделяемых ресурсов от состояния гонки и корректность операций чтения и записи в многозадачной среде. Не забываем про датчик Холла, показания которого нужно постоянно обновлять. Чтобы не занимать процессорное время постоянным опросом, в прошивке предусмотрено аппаратное прерывание. В нашем случае при возникновении сигнала на цифровом пине ввода-вывода, подключенном к датчику Холла, соответствующий блок кода как бы «вклинивается» в основную программу. Получается быстрая реакция на важные события, после чего управление возвращается. Принцип работы прерывания. Источник. Серверная часть В основе серверного приложения лежит фреймворк FastAPI. Он обеспечивает прием входящих HTTP-запросов и установку WebSocket-сессий, выступая в роли центрального узла, через который робот обменивается командами и данными. При первом соединении WebSocket-менеджер определяет тип подключения (оператор или робот) и сохраняет объект соединения в соответствующий список. Из каждой новой порции текстовых сообщений от робота парсер с помощью регулярных выражений извлекает телеметрию, например: расстояния до препятствий спереди и сбоку, напряжение батареи, отметку времени, угол поворота, потребляемый ток. Информация охраняется в локальной СУБД SQLite и сразу пересылается всем подключенным веб-клиентам. Команды управления — запуск, приостановка, возобновление, различные движения по направлениям, а также полная остановка — транслируются в обе стороны. Так гарантируется синхронизация состояния между роботом и клиентским интерфейсом. При этом используется единственно допустимый набор значений в RobotStatus. Поступающие от клиента WebSocket-сообщения также перенаправляются на робота. Прием изображений по REST-эндпоинту преобразует сырые байты в кадры PIL, подготавливает их для модели YOLO и выполняет детекцию человека. Далее, в зависимости от результата, генерируется соответствующая команда — пауза или возобновления работы. Готовый кадр кодируется в base64 и сразу же рассылается всем клиентам по WebSocket. Общая схема обмена сообщениями, обработки телеметрии и взаимодействия с моделью детекции изображена на рисунке: Архитектура серверной части. Основные циклы и общие переменные Точка входа в программу — функция setup(). Внутри нее: инициализируется последовательный порт, активируется шина I2C, настраиваются все сенсоры, подключаются точка Wi-Fi, устанавливается соединение с удаленным сервером посредством WebSocket, инициализируется камера. В конце вызывается функция startMainLoops(), которая создает две независимые задачи FreeRTOS. Каждая из них закрепляется за своим ядром ESP32 — loopCore0 на Core 0 и loopCore1 на Core 1, после чего loop() остается пустым, и все управление переходит на эти два цикла: В теле задачи loopCore0 происходит сбор и отправка данных на сервер, а при включенной камере — захват кадра и отправка POST-запроса: Функция stepLoop0() измеряет время между итерациями и сохраняет значения в navo.dt_loop0, что позволяет синхронизировать обработку данных. Переменные t_loop0, t_loop1 в классе Navo имеют формат double и хранят время в секундах. Такой подход позволяет избежать быстрого переполнения, характерного для целочисленных типов. Например, если использовать стандартный тип unsigned long с миллисекундами, переполнение наступит уже через 49 дней непрерывной работы, тогда как double в секундах гарантирует стабильную работу в течение нескольких лет, прежде чем потеряется точность на долях секунды. Дополнительно реализована защита от переполнения и появления отрицательных значений, которые могут возникнуть при сбросе счетчика времени внутри микроконтроллера. Вместо прямого использования сохраненного времени, система всегда оперирует его разницей. На каждой итерации цикла рассчитывается дельта между настоящим моментом и предыдущей итерацией. Полученное значение проверяется и, если оказывается некорректным, принудительно обнуляется. Затем результат сохраняется в dt_loop0 или dt_loop1. Такой подход гарантирует, что все расчеты основываются только на интервалах, которые остаются корректными даже при случайных сбросах времени: На Core 1 запускается второй цикл, отвечающий за навигацию и управление двигателями: Ключевые переменные хранятся в классе Navo: distanceFront и distanceSide — отфильтрованные расстояния до стен, distanceHall — одометрический шаг по датчику Холла, yaw и targetYaw — текущий и целевой угол поворота, t_loop* и dt_loop* — метки и интервалы времени для каждого цикла, voltage и current — параметры питания, autoMode и correction — состояние автоматики и направление корректировки, wheels — объект управления моторами. Благодаря такому разделению на два независимых цикла и единому хранилищу состояния в Navo, система способна одновременно обрабатывать сенсоры, отправлять данные и управлять движением. При этом минимизируются блокировки и задержки, свойственные однопоточному исполнению. Работа с датчиками и управление двигателями В модуле Sensors.cpp обработчик прерывания hallSensorISR() проверяет направление колес и время с момента последнего срабатывания — так отбрасываются дребезг и ложные триггеры. Затем при движении вперед или назад он прибавляет или вычитает шаг колеса — значение в миллиметрах хранит глобальная переменная: Используя библиотеку INA219_WE, функция updateVoltageAndCurrent() считывает напряжение и ток с датчика INA219. В свою очередь, updateDistances() каждые 0,1 с измеряет расстояние спереди и сбоку с помощью библиотеки HC-SR04. Полученные данные пропускаются через одномерный фильтр Калмана, также известный как фильтр «альфа-бета», чтобы получить сглаженные и стабильные значения navo.DistanceFront и navo.DistanceSide. Интервал 0,1 с выбран не случайно. Если постоянно опрашивать дальномеры, у процессора не останется времени на гироскоп, и его данные станут недостоверными. Чтобы предотвратить перерасход ресурсов, таймер distanceTime накапливает время на каждом шаге navo.dt_loop1 и сбрасывает его после обработки: Расчет угла поворота navo.yaw выполняется в функции updateYaw() с помощью библиотеки MPU6050. Из исходного значения гироскопа вычитается текущее смещение для коррекции дрейфа. Затем результат интегрируется по времени dt_loop1 и нормируется, то есть приводится к диапазону от 0 до 360 градусов: Если робот останавливается более, чем на две секунды, функция calibrateGyroOnce() усредняет смещение гироскопа через простой фильтр низких частот, что позволяет компенсировать дрейф. Класс WheelControl управляет четырьмя GPIO-пинами двигателей и отвечает за начало движения и остановку, которые могут быть как плавными, так и мгновенными. После назначения выводов в методе attach() и их настройки в режим OUTPUT, метод smoothControl() циклически изменяет скважность ШИМ от нуля до заданного значения. Такой подход создает эффект постепенного разгона. На основе этого метода реализованы методы forward(), backward(), left(), right() и stop(). Например, forward() при плавном старте обнуляет противоположную пару пинов, устанавливает direction FORWARD и постепенно прибавляет мощность: Итак, система сенсоров, фильтров и управления двигателями взаимодействует через общий объект navo. Два независимых цикла Core 0 и Core 1 микропроцессора ESP32 непрерывно считывают и обрабатывают данные, формируют координирующие сигналы и таким образом контролируют движение робота. Управление камерой Инициализация и управление камерой OV7670 в проекте построена на наследовании от класса I2SCamera, отвечающего за прием параллельного потока пикселей по периферии I2S и синхронизацию по сигналам VSYNC, HREF и PCLK: При вызове OV7670::init() сначала настраивается внешний тактовый сигнал XCLK через функцию ClockEnable(pin, freq), а затем инициализируется прием по I2S и встраивается аппаратное прерывание по VSYNC: Затем система проверяет ответ камеры по шине I2C и настраивает ее для работы в режиме QVGA RGB565. Все эти действия объединены в методе QQVGARGB565(), который последовательно вызывает функции reset(), QQVGA(), frameControl() и saturation(). Каждая из них отвечает за свою серию записей в регистры камеры через интерфейс I2C: Базовый механизм приема кадров реализован в классе I2SCamera. При запуске настраиваются буферы DMA и подключаются прерывания I2S и VSYNC. После этого периферия I2S копирует необработанные данные в кольцевой буфер. В завершение обработчик прерывания I2S извлекает данные из байтового потока и упаковывает их в итоговый массив frame: Функция i2sInit() настраивает все выводы GPIO (VSYNC, HREF, PCLK и шину данных D0−D7) через матрицу сигналов ESP32. Она также включает параллельный режим I2S и конфигурирует делители тактового сигнала. Такая настройка обеспечивает прием пиксельных тактов на необходимой частоте: Класс DMABuffer выделяет в динамической памяти два кольцевых буфера (по 4 байта на пиксель), а его метод next() создает односвязный список дескрипторов для прямого доступа к памяти. Процесс получения изображения построен следующим образом. Сначала камера OV7670 настраивается через шину I2C на режим захвата 160×120 RGB565. Затем аппаратный интерфейс I2S принимает данные с камеры в фоновом режиме. При этом однократный вызов функции oneFrame() блокирует выполнение программы до тех пор, пока не будет получен полный кадр. Такой подход позволяет легко интегрировать захват изображения в основной рабочий цикл Core 0: Для настройки камеры OV7670 используются ключевые регистры. Запись в них происходит в методах reset(), QQVGA(), frameControl() и saturation(). Ниже приведено их назначение. REG_COM7 (0x12) — управляет глобальным сбросом и выбором цветового формата. Установка бита 7 (0b1000 0000) сбрасывает камеру, а значение 0b0000 0100 задает режим RGB. REG_CLKRC (0x11) — настраивает делитель тактовой частоты XCLK. Установка бита 7 разрешает двойной внешний такт, что позволяет снизить нагрузку на внешний генератор. REG_SCALING_XSC (0x70) и REG_SCALING_YSC (0x71) — задают коэффициенты масштабирования изображения по горизонтали и вертикали. REG_SCALING_DCWCTR (0x72) — определяют параметры подавления цветового шума при снижении разрешения. REG_SCALING_PCLK_DIV (0x73) — управляют частотой PCLK во время масштабирования. REG_COM8 (0x13) — включает и настраивает автоматическую регулировку экспозиции, усиления и баланса белого AEC/AGC/AWB. REG_COM11 (0x3B) — управляет режимами шумоподавления и автоматическими настройками для частоты сети (50/60 Гц) и ночного режима. REG_COM15 (0x40) — определяет формат выходных данных. Бит 4 (0x10) переключает RGB565, а биты 6−7 настраивают диапазон уровня черного. Регистры управления цветовой матрицей (0x4F−0x54, 0x58) — отвечают за насыщенность и баланс цветовых каналов. Запись в эти регистры через шину I2C позволяет гибко настраивать режимы работы камеры OV7670 — от базовых параметров захвата QVGA до тонкой подстройки цветового баланса и частоты кадров. Основное преимущество такого подхода в том, что все настройки можно выполнить один раз при инициализации. Это позволяет не занимать шину I2C во время основной работы. Обмен данными и взаимодействие с сервером Для связи робота с сервером используются два канала: WebSocket и HTTP POST. Первый, реализованный с помощью библиотеки ArduinoWebsockets, служит для двунаправленного обмена командами и телеметрией в реальном времени. Второй, использующий встроенный Arduino HTTP Client, предназначен для отправки изображений с камеры. При запуске функция connectToServer() создает объект WebsocketsClient и назначает для него две функции обратного вызова: onEventsCallback — логирует события, такие как открытие и закрытие соединения, а также ответы на ping-запросы. onMessageCallback — обрабатывает текстовые команды от сервера. Эта функция изменяет состояние робота navo.autoMode или вызывает методы navo.wheels. для управления двигателями: forward(), backward(), left(), right(), stop(). Телеметрию периодически отправляет функция pollAndSendData(), которая вызывается из основного цикла ядра 0. Пока WebSocket-клиент доступен, он опрашивается методом poll(). Если сработал датчик Холла или прошло больше секунды с последней отправки, система формирует строку с текущими значениями и отправляет ее на сервер. После передачи данных одометра переменная distanceHall и соответствующий флаг сбрасываются. Во время работы камеры после создания очередного кадра устанавливается HTTP-соединение с заданным URL. Через него в теле POST-запроса отправляются данные из массива camera.frame длиной camera.frameBytes. После получения ответа система логирует его код и текст: Автоматический режим Автоматический режим управляется конечным автоматом, состояние которого хранится в переменной navo.autoMode. Этот режим состоит из трех этапов: установка начальной точки, движение по траектории «змейка» и возврат в режим ожидания. Команды start, stop, suspend и resume, поступающие от WebSocket-сервера, изменяют состояние navo.autoMode на ENABLE, DISABLE, SUSPEND или ACTIVE соответственно. При переходе в состояние ENABLE запускается процедура инициализации автоматического движения — за нее отвечает функция setStartingPoint(). Она сохраняет текущий угол робота navo.yaw в переменной navo.targetYaw и задает первую цель — поворот на –90°. С помощью метода rotate() робот поворачивается до тех пор, пока разница между текущим и целевым углом не станет меньше 1°. Начинается движение вперед, пока передний дальномер не обнаружит препятствие на расстоянии менее 110 мм. Тогда робот останавливается, задает новый целевой угол +90° и снова выполняет поворот. Только после второго успешного цикла rotate() начальная точка считается установленной. Система сбрасывает одометр, переводит autoMode в состояние ACTIVE, и робот начинает движение по основному алгоритму «змейка». В состоянии ACTIVE на каждом шаге цикла Core 1 выполняются три подзадачи. Первая из них — коррекция по боковому датчику. Когда срабатывает датчик Холла HallState::HALL_TRIGGERED, система рассчитывает угол поправки, отталкиваясь от разницы между расстоянием до бокового препятствия и пройденным путем. Вторая подзадача — коррекция по углу. Если разница между текущим navo.yaw и целевым navo.targetYaw углами превышает 5°, система выбирает направление TO_LEFT или TO_RIGHT и вызывает команду корректировки через wheels.correction(). Движение прямо возобновляется, когда абсолютная разница между заданным и текущим углом становится меньше 0,5°: Третья подзадача — обнаружение углов и выполнение разворота. За это отвечает CornerDetector, который по трем последовательным измерениям с переднего и бокового дальномеров догадывается, что роботу пора повернуть. В момент обнаружения угла робот останавливает движение и задает угол поворота: +90° для внешних углов и –90° для внутренних. Затем для выполнения маневра вызывается метод rotate(). После завершения поворота детектор сбрасывается, и движение возобновляется: Функция autoMode() объединяет эти этапы и реагирует на команды из AUTO_ENABLE, AUTO_DISABLE и SUSPEND: Серверная часть Рассмотрим архитектуру сервера и протоколы взаимодействия с клиентом — прием телеметрии от робота и последующее хранение данных в базе. Ниже описаны алгоритмы постобработки, которые отвечают за фильтрацию шумов, определение углов и линейную аппроксимацию для построения карты помещения. С помощью веб-интерфейса оператор может в реальном времени наблюдать визуализированную траекторию движения, стены, а также отслеживать уровень заряда батареи и управлять роботом с помощью кнопок. Прием данных Обмен телеметрией с роботом организован по WebSocket-соединению. Эту задачу выполняет класс ConnectionManager из модуля connection_manager.py. Его первая функция — установление и классификация подключения. При новом соединении метод connect() принимает WebSocket и определяет тип подключения: робот или оператор. Для каждого робота — их может быть и несколько — создается объект robot, который хранит текущий WebSocket и внутреннее состояние. Вторая задача — разбор входящих сообщений. С каждой отправкой робот отправляет строку вида: vol:7.2,cur:150.0,df:250.0,ds:100.0,ang:45.0,t:12.345,dh:30.0 Метод parse_robot_message() выделяет из нее MAC адрес и телеметрию: Сама функция parse_robot_state() использует регулярное выражение для извлечения значений в RobotState: Уровень заряда батареи вычисляется отдельно по методу _get_battery_percent(), исходя из напряжения в поле vol: Третья задача — рассылка клиентам данных и управляющих команд роботу. После парсинга сервер отправляет всем подключенным клиентам обновленную строку с телеметрией и процентом заряда: Когда робот присылает сообщение «done», его статус изменяется на RobotStatus.stopped. При отключении робот удаляется из списка robots, а всем клиентам отправляется сигнал для очистки данных. Метод send_message_to_robot() отправляет роботу текстовые команды по WebSocket и обновляет его внутренний статус. При этом система контролирует логику переходов между состояниями — например, не позволяет повторно запустить уже работающего робота. Таким образом, ConnectionManager — это центральный компонент, который выполняет следующие функции: принимает и идентифицирует подключения; парсит и сохраняет телеметрию; рассчитывает уровень заряда батареи; транслирует данные всем клиентам; передает команды управления обратно роботам. Обработка информации В проекте все сырые записи телеметрии робота сохраняются в базу данных с помощью класса RobotDatabase. При получении каждой новой строки данных, она добавляется в таблицу states: Далее функция get_valid() выполняет фильтрацию, отбраковывая шумовые или пропущенные измерения. Общий путь, пройденный по одометру, накапливается в переменной total. Когда система обнаруживает стабильную группу точек, где distance_hall равно нулю, а показания фронтального и бокового дальномеров почти не меняются (с допуском tolerance 1.0), первая точка в этой группе помечается как угол: После отбора валидных точек они передаются в process_states(), где на основе угла state.angle определяется направление движения: Затем для каждой группы точек стены выполняется линейная аппроксимация: Точки пересечения смежных прямых вычисляются функцией intersection(). Отладочная визуализация, пример которой представлен на сгенерированном изображении ниже, строится в функции plot_map(), где черными линиями рисуются отрезки стен, а синими точками — траектория робота: Карта комнаты во время работы робота. Веб-интерфейс оператора. Заключение Проект превратился из технической задачи в исследовательский марафон. Пришлось детально разбираться во всем: от проектирования механики в САПР и тонкостей 3D-печати до разработки многоуровневой архитектуры питания, от низкоуровневого программирования на C++ под FreeRTOS до создания полноценного клиент-серверного приложения с использованием ИИ. Пожалуй, самым трудным вызовом стала система навигации. В любом реальном проекте приходится идти на компромиссы, и выбор ультразвуковых датчиков HC-SR04 стал одним из них. Вместо того, чтобы дать роботу нормальное «зрение», пришлось научить его «слышать» мир — ориентироваться в пространстве с помощью эхолокации, подобно летучей мыши. Ограничения аппаратной части спровоцировали и серию программных решений. Неточные и «шумные» данные потребовали усложнения алгоритмов фильтрации, непрерывную коррекцию курса и управления состоянием. В итоге робот получился не самым зорким, и недостаток визуального восприятия компенсирует программной смекалкой. Должен признать, что именно эта борьба с несовершенством «железа» дала самый ценный опыт. Напоминаю, что начало истории — в первой части. Надеюсь, что этот проект — не финал, а лишь начало, MVP. Размышляю о более мощной платформе на базе Raspberry Pi, оснащенной LiDAR, точными лазерными дальномерами и камерой высокого разрешения. Опыт, полученный при создании «слышащего» робота пригодится для следующего шага — машины с настоящим, высокоточным зрением. Надеюсь, моя история вдохновит и кого‑то из читателей на собственные эксперименты.

Михаил Дроздов

Материалы автора

Как сделать робота. Часть 2

Как сделать робота. Часть 1