В первой части статьи, мы описывали конструкторские детали и используемую электронику в нашем SheevaBot, робота, собранного для участия в соревнованиях Евробот 2024. Перейдём к рассмотрению программной части.
Связь с низкоуровневыми микроконтроллерами
Первым шагом в программировании SheevaBot была установка связи между основным микрокомпьютером (RPi4) и микроконтроллерами, которые отвечают за управление различными компонентами робота. Было решено отказаться от использования Wi-Fi из-за риска потери сигнала, что могло привести к непредсказуемому поведению робота.
В начале разработки мы попробовали создать собственный API для передачи данных по SPI, используя протокол с передачей трех чисел (номер мотора, направление вращения и скорость/число оборотов). Этот подход работал с сервоприводами и шаговыми двигателями, но оказался неэффективным для электромоторов, особенно в условиях интеграции с ROS. В результате пришлось искать готовые варианты связи. Так мы пришли к microROS.
Итак, со связью определились UART + microROS. Осталось выбрать из всего многообразия вариантов относительно недорогой контроллер, доступный и при этом достаточно мощный для решения наших задач. Тут можно найти таблицы с «метой» контроллеров, где colcon.meta, colcon_lowmem.meta, colcon_verylowmem.meta характеризуют количество нод, сервисов и т.д., которые может выполнять принадлежащий к этому классу микроконтроллер.
Низкоуровневое управление
Для того чтобы управлять моторами, сервоприводами, различными датчиками и прочей электроникой в нашем проекте использовался микроконтроллер Raspberry Pi Pico на базе RP2040. Выбор пал на эту отладочную плату неспроста.
Во-первых, она недорогая и мощная, обладает оперативной памятью 264 КБ и Flash-памятью 2 МБ, и кроме того имеет на борту два ядра ARM Cortex-M0+ с тактовой частотой 133 МГц. Для сравнения в таблице приведены характеристики широко известной серии микроконтроллеров STM — STM32F411CEU6 (в народе известной как Black Pill).
Характеристика |
Raspberry Pi Pico |
STM32F411CEU6 |
Микроконтроллер |
RP2040 |
STM32F411CE |
Архитектура |
ARM Cortex-M0+ dual-core |
ARM Cortex-M4 |
Частота ядра |
133 МГц |
100 МГц |
Оперативная память (RAM) |
264 КБ |
128 КБ |
Встроенная память (Flash) |
2 МБ |
512 КБ |
GPIO |
26 |
16 |
SPI |
2 |
3 |
I2C |
2 |
2 |
UART |
2 |
3 |
Во-вторых, пакет microROS достаточно просто устанавливается на Pico, что в совокупности со всем вышеперечисленным послужило основой при выборе микроконтроллера.
Программирование Pico осуществлялось в VS Code при помощи Raspberry Pi Pico C/C++ SDK. Кстати, на сайте Raspberry представлена исчерпывающая документация по данному микроконтроллеру, а также руководство по быстрому старту в Pico SDK с настройкой всего необходимого.
Проект автономного робота получился достаточно требовательным к количеству ножек микроконтроллера, и на одной Pico не помещались все элементы (а именно — управляемый по шине I2C дисплей, 6 сервоприводов, 4 пары Nema17 с драйверами DRV8825, 2 концевых выключателя, 4 бесколлекторных электродвигателя и датчик MPU6050). Поэтому было принято решение увеличить количество микроконтроллеров до двух. При этом пришлось решать сразу две достаточно сложные и взаимоисключающие задачи — равномерно распределить нагрузку и удобно и компактно расположить эти микроконтроллеры на шилде. Итак, распределение микроконтроллеров выглядит следующим образом:
Pico1 – моторы, сервоприводы, MPU6050, концевые выключатели;
Pico2 – шаговые двигатели, дисплей.
Зачем нужен microROS?
Как было отмечено выше, одним из критериев выбора именно микроконтроллера Raspberry Pi Pico была возможность достаточно простой установки на него microROS. Так почему же нами был выбран именно microROS?
Что же такое microROS? В общих чертах, microROS — это «ROS2» для микроконтроллеров. Основное его преимущество заключается в том, что он предоставляет возможность быстрой интеграции микроконтроллера в среду ROS2 без необходимости вручную прописывать API для взаимодействия микроконтроллера и микрокомпьютера. Благодаря этому результат различных «сложных» операций (таких как обработка изображений, навигация и т.п.), выполняемых на микрокомпьютере, может быть передан на микроконтроллер, который, в свою очередь, запустит те или иные актуаторы и передаст обратно показания тех или иных датчиков.
Таким образом, благодаря microROS осуществляется формирование единой робототехнической системы, которая может воспринимать окружающий мир, анализировать его и изменять.
Управление периферией
Далее указаны ссылки и описание кода управления моторами, сервоприводами, шаговыми двигателями, дисплеем и MPU6050.
Скрытый текст
Управление шаговыми двигателями Nema 17 осуществлялось с помощью драйверов DRV8825. Код можно найти по пути pico2\external_lib\stepper.
Для минимального управления мотором на ножку STEP драйвера необходимо подавать ШИМ-сигнал (каждый импульс ШИМ — поворот на определённый градус), а также выбрать направление вращения (ножка DIR). В нашем проекте мы решили пойти дальше и контролировать состояние драйвера, считывая показания с ножки FAULT. Если произошёл перегрев драйвера, то на ней устанавливается состояние «0», и при последующей отправке данных на микроконтроллер по таймеру (с помощью функции timer_callback(...) в pico2.cpp) срабатывает функция stepper_overheat(), которая возвращает соответствующий код ошибки и (на всякий случай) выключает драйвер, отправляя сигнал на ножку EN. Сама функция управления вращением stepper() выбирает необходимый мотор, проверяет драйверы на перегрев (считывает показания с ножки FAULT), затем выбирает направление вращения и на определённое время устанавливает скважность ШИМ 50%, после чего сбрасывает её в 0. Тем самым мотор поворачивается на заданный угол.
С целью минимизации нагрева драйверов было принято решение после окончания поворота кассетницы отключать их. В то же время для подъёма кассетницы такой вариант не подходит, так как её необходимо постоянно удерживать в фиксированном положении.
Для приема сообщений от микрокомпьютера в pico2.cpp была объявлена функция stepper_subscriber_callback(), а само сообщение представляло собой стандартный тип geometry_msgs/msg/Vector3, содержащий данные о номере мотора, направлении вращения и угле поворота (как раз три координаты).
Управление сервоприводами MG90S было организовано достаточно просто. Код можно найти по пути pico1\external_lib\servo. Угол поворота данного сервопривода настраивается заполнением ШИМ при фиксированной частоте 50 Гц, подробнее об этом можно почитать здесь. За всё это отвечает функция servo(), которая выбирает необходимый сервопривод и настраивает в соответствии с полученным углом ширину импульса в пределах 500–2500 мкс (0–180 градусов).
Для приема сообщений от микрокомпьютера в pico2.cpp была объявлена функция servo_subscriber_callback(), а само сообщение представляло собой стандартный тип geometry_msgs/msg/Vector3, содержащий данные о номере сервопривода и угле поворота (две координаты, третья не используется).
Управление мотор-редукторами JGB37-3525 предполагает использование ШИМ и несколько пинов для выбора направления и считывания скорости вращения. Код можно найти по пути pico1\external_lib\motors. Итак, для корректной работы мотора необходим ШИМ - сигнал частотой около 20 кГц, скорость же зависит от скважности этого сигнала.
Для управления скоростью и направлением вращения служит функция motorN_controller(), (N — номер мотора), принимающая всего один аргумент — speed, который может быть положительным и отрицательным. В соответствии со знаком выбирается направление вращения, а модуль задаёт скорость.
Для приема сообщений от микрокомпьютера в pico1.cpp была объявлена функция twist_subscriber_callback(), а само сообщение представляло собой стандартный тип geometry_msgs/msg/Twist. При этом для полного контроля движения робота на плоскости достаточно использовать линейные скорости по осям x и y и угловую скорость вращения вокруг оси z. Эти данные поступают от микрокомпьютера, предварительно обрабатываются при помощи библиотеки kinematics, которая преобразует полученные скорости в обороты каждого из четырёх моторов и затем уже подаются в функцию motorN_controller().
Чтение показаний с MPU6050 осуществлялось при помощи библиотеки i2cdevlib, которая в своём составе содержала пример использования данного модуля для вычисления углов поворота вокруг осей x, y, z, а также линейных ускорений по осям x и y. Код можно найти по пути pico1/external_lib/imu. При этом сама библиотека и вспомогательные файлы находятся в папке MPU6050, а код для чтения данных — в imu.cpp.
Библиотека содержит достаточно сложную математику, весьма требовательную к ресурсам микроконтроллера. Для снижения нагрузки на него можно увеличивать значения параметра MPU6050_DMP_FIFO_RATE_DIVISOR в MPU6050_6Axis_MotionApps_V6_12.h. Подробнее об этом написано здесь.
Передача значений углов и линейных ускорений в микрокомпьютер осуществлялась по таймеру в функции timer_callback(rcl_timer_t *timer, int64_t last_call_time), а само сообщение представляло собой стандартный тип sensor_msgs__msg__Imu, в который передавались значения линейных ускорений по осям x и y, а также угол поворота относительно оси z. При включении MPU6050 проводилась калибровка, в результате которой устанавливалась некоторая стационарная система координат, относительно которой и измерялся угол.
К сожалению, особенности математической обработки показаний датчика делают невозможным избавиться от дрейфа, в результате чего изначально заданная «стационарная» система координат произвольно поворачивается, причем тем сильнее, чем более резко движется робот. Единственный способ избежать этого — периодически калиброваться, предварительно позиционируя робота так же, как он был выставлен изначально. Но это возможно сделать только посредством каких-то внешних систем, ведь сам датчик уже содержит ошибку. Отчасти разрешить эту проблему может помочь магнитометр, который устанавливается в более продвинутых вариантах данной схемы, но данное решение в помещениях, где существуют сильные магнитные поля, представляется весьма сомнительным.
Управление дисплеем предполагало использование внешней библиотеки. Код можно найти по пути pico2\external_lib\display. При этом сама библиотека и вспомогательные файлы находятся в папке SSD1306, а функции Print_string() и Print_char() предназначены для печати соответственно строки и символа в display.cpp.
Для приема сообщений от микрокомпьютера в pico2.cpp была объявлена функция display_subscriber_callback(), а само сообщение представляло собой стандартный тип std_msgs/msg/UInt8 для передачи печатаемого числа.
В целях сохранения исторической справедливости стоит отметить, что изначально использовался дисплей ST7735_TFT с большей диагональю и для управления им — более функциональная библиотека. Однако для подключения этого дисплея требовалось намного больше проводов, количество которых было ограничено токосъёмником Slip-ring, поэтому в итоге был выбран экран, управляемый по I2C.
Описание алгоритмов и ПО
Детальное описание алгоритма работы SheevaBot
Разработку алгоритма работы в глобальном плане можно разбить на несколько частей: движение робота, процесс захвата, работа с камерами, основной алгоритм.
Движение
На вход принимается радиус-вектор в системе координат центра игрового поля, где угол отсчитывается относительно робота (с датчика MPU6050).
На рисунке представлена локальная система координат робота, стрелками обозначено направление вращения, а пунктиром — вся система координат поделена на области. В зависимости от того, в какой диапазон входит угол радиус-вектора, туда и поворачивается робот. Тут стоит отметить, что датчик IMU MPU6050 выдавал значения либо от 0° до 180°, либо от -180° до 0°, поэтому пришлось адаптировать алгоритм.
Длина радиус-вектора формировалась в управляющем алгоритме, в котором вычислялось расстояние от положения робота до цели, но об этом далее.
Внешняя камера
О том, как детектировать ArUco-маркеры с помощью библиотеки OpenCV, написано большое количество материала и проектов на GitHub.
Для получения с камеры данные о расстоянии до определенных ArUco-маркеров необходимо знать параметры камеры (фокальное расстояние, координаты принципиальной точки и коэффициенты дисторсии). Благодаря хорошей калибровке можно выжать даже из самой дешевой камеры все возможное, однако все равно чем ближе к краям угла обзора, тем больше ошибка в предсказании расстояния на поле.
Сбор цветов
Поскольку процесс сжатия/разжатия манипулятора на сервоприводе, а также подъема/спуска Манипуляторного цветка это фактически бинарные команды, то мы обошлись одной нодой с большим числом условных операторов. При поступлении управляющей команды на захват цветка последовательно выполняют опускание Манипуляторного цветка, Захват сервоприводом, поднятие Манипуляторного цветка, поворот Кассетницы, по прибытию в пункт доставки цветков, задача становится еще проще, подать команду на Разжатие сервоприводами.
Основной алгоритм
Основной алгоритм взаимодействия робота с его подсистемами реализован через два топика ROS: один для чтения команд от основного алгоритма, другой для отправки ответов от нод. Этот подход позволил легко отлаживать работу системы, анализируя сообщения в топиках. Вся информация, отправляемая в топики, имеет стандартный формат: команды вида "бери цветок" или "двигайся по радиус-вектору {r, φ}", а ответы от нод имеют структуру "название системы/код ноды: ответ".
Особенности работы с RPi4 и microROS стали очевидны на этапе тестирования. Запуск ноды для подключения Pico по microROS требовал постоянной перезагрузки Pico, что усложняло работу. Для решения этой проблемы был использован один из пинов RPi4, подключённый к ножке перезагрузки Pico. Если от Pico не поступало сигналов, пин RPi4 сбрасывал напряжение, вызывая перезагрузку.
Стратегия поведения на поле
Перед началом движения робот с помощью камеры и 4 ArUco-маркеров определяет центр системы координат и координаты точек интереса на поле (места сканирования, цветы, стартовые базы). Далее робот направляется к месту сканирования, оптимально выбранному для поиска цветов.
На этапе сканирования робот вращается на месте, считывая информацию о каждом цветке: индекс, расстояние до него и угол поворота. Далее вычисляется расстояние до ближайшего цветка, и робот двигается по радиус-вектору, который строится на основе известных угла и расстояния. После приближения к цветку манипулятор опускается, захватывает цветок и поднимается. Этот цикл повторяется 6 раз, пока все "руки" робота не захватят цветы.
Когда все цветы собраны, робот направляется к базе для выгрузки, после чего алгоритм завершает свою работу.
Система антистолкновений
Для предотвращения столкновений на поле была реализована простая, но надёжная система антистолкновений. Она активирует экстренное торможение при достижении предельного расстояния между роботами. Если робот соперника отступает на безопасное расстояние, наш робот может продолжить движение. Однако в наших матчах это не происходило, и срабатывание системы приводило к автоматическому завершению матча по истечении времени.
Итоги и выводы
Несмотря на то, что финальные результаты не были высокими, наша команда смогла пройти допуск, победить в одном матче и, что самое важное, одержать победу над самими собой и получить награду «За волю к победе». Пройдя этот путь, мы поняли, что на этапе подготовки к соревнованиям предварительное прототипирование важно, правильно подобранная камера — очень важна, а запас несгораемых Raspberry Pi — бесценен. :)
На основе первого опыта участия, мы сможем построить более надёжного и многофункционального робота в новом сезоне. За процессом подготовки к Eurobot 2025 и к другим соревнованиям по робототехнике приглашаем в t.me/engistories (Контактное лицо: Вальковец Данила).
Над SheevaBot работали:
1. Ермаков Александр (капитан)
2. Двинских Павел (электронщик)
3. Караваев Кирилл (электронщик)
4. Коновалов Георгий (конструктор)
5. Любимцев Иван (программист)
6. Маньшин Тимур (программист)
7. Монастырный Максим (программист)
8. Осипов Кирилл (конструктор)
9. Трифонов Фёдор (технолог)
10. Шувалов Глеб (SMM).
ret77876
А с какой частотой происходил сбор и процессинг(получение окончательных данных о позиции робота) с MPU6050? Какие - то фильтры применяли? И какая по-итогу была точность локализации, если замеряли?
juneberry6 Автор
Опрос всех датчиков со стороны Raspberry Pi 4 происходил по таймеру c частотой 5 Гц, но сам imu может работать по прерываниям с частотой примерно 90 Гц, если верить статье https://alexgyver.ru/arduino-mpu6050/. Мы же выставили делитель MPU6050_DMP_FIFO_RATE_DIVISOR равный 0x03, что должно было немного уменьшить частоту опроса, но измерения ее не проводилось. По поводу фильтров - применялись средства встроенной библиотеки MPU6050_6Axis_MotionApps_V6_12.h.
juneberry6 Автор
Точность локализации - примерно 5—10 градусов. Причем, при резком изменении направлении вращения наблюдалось значительное смещение начального угла, под которым был выставлен робот при включении.