Локализация по Aruco маркерам

В прошлом посте мы рассказали, как проехать из точки А в точку Б, не задев при этом ничего. Но, чтобы что-то объехать, нужно понять, где находимся мы сами, и где находятся динамические препятствия (наши противники и не только).

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

Сегодня мы расскажем про то, как решали проблему определения положения роботов на поле Eurobot, как всё это сделать, запустить и настроить под себя.

Целевые условия работы системы локализации

Один из принципов Eurobot Open — автономность. Автономность от всего, что находится за игровым полем. Все вычисления, принятие решений и локализация должны производиться либо вна роботах, либо на специальных площадках на поле.

Размер поля 2х3 метра, а погрешность размеров и положения элементов на нём от поля к полю, как правило, в пределах 3-5 мм.

На картинке ниже — схема поля Eurobot 2019 с отмеченными зонами, на которые мы можем поставить что-то кроме роботов.

• 6 — Зона эксперимента (одно из дополнительных заданий)
• 10 — Зоны маяков
• 11 — Специальная башня для установки центрального устройства слежения (далее ЦУС). Находится над полем на высоте 1 метр
• Также на самих роботов можем установить маячки.

Для всех этих зон и габаритов самих роботов, есть жесткие ограничения по весу и размеру, из-за чего нельзя сделать один большой производительный супермодуль в котором бы всё уместилось. А еще есть ограничение в 3 минуты на подготовку к запуску. В течение этого времени один или два человека от команды должны вынести роботов и всё им сопутствующее, расставить, настроить и подготовиться к запуску. Очевидно, что чем меньше всякого барахла нужно выставлять и настраивать, тем меньше шансов ошибиться или быть оштрафованным еще до начала боя.

и ещё второй такой полный ящик

Анализируем варианты

С осознанием наших возможностей и желанием получить точность позиционирования хотя бы в несколько миллиметров давайте посмотрим, что вообще мы можем использовать для решения этой задачи:

• Колесная одометрия с помощью энкодеров, расположенных непосредственно на ведущих или дополнительных измерительных колесах позволяет считать пройденное роботом расстояние и определять его положение относительно начальной точки. Многие команды Eurobot используют ее как основной способ локализации. Это один из наиболее быстрых источников информации о перемещении для робота. Однако на практике, особенно при езде на омниколёсах (меньшая площадь контакта чем у классических колес, люфты роликов и бОльшие погрешности изготовления), одометрия подвержена проблеме накапливающейся ошибки, которую каким-то образом необходимо корректировать. Можно выравниваться о поверхность с заведомо известными координатами, а можно комплексировать с дополнительной системой. Также если использовать энкодеры на ведущих колесах, то существует проблема проскальзывания колеса при резких маневрах и при столкновениях робота. Измерительные колеса позволяют избавиться от этой проблемы, но их тяжело разместить в небольшом роботе.

  
  самодельное измерительное омни колесо на независимой подвеске и с магнитным энкодером, которые мы использовали в 2018 году

• Лидары обладают весомыми преимуществами: мы получаем почти круговой обзор и расстояния до препятствий. Из минусов стоит отметить высокую цену, сложности при попытке отличить небольшие далёкие объекты от мусора, относительно сложный алгоритм локализации. Дешевые лидары (такие как rplidar) имеют довольно низкую частоту обновления и угловое разрешение, а так же подвержены засветам от других лидаров, дальномеров и т. д. В роботах сложно найти место для лидара на уровне бортика для использования классических SLAM алгоритмов, а на предусмотренной правилами высоте для систем локализации мало статичных объектов для привязки. Однако, команда Skoltech’а уже несколько лет успешно катает с использованием hokuyo ust-10lx и в прошлом году заняла второе место на европейском финале соревнований Евробот во Франции.
• Инерциальные системы мы тоже рассматривали, но в нашем случае они плохо подошли. Акселерометр шумит, гироскоп плывет, про использование компаса внутри помещения и вовсе можно забыть. Даже применение фильтра Калмана не позволяет достичь желаемой точности. IMU получается эффективно использовать только для коррекции на очень коротком промежутке времени. Максимум полезной информации, что мы смогли получить с pixhawk2 — это угол при резком повороте (от столкновения, например).
• GPS не работает в помещениях, а точность без RTK — метры.
• Ультразвуковая локализация. Одну из реализаций можно найти тут: m.habr.com/ru/post/451408. Увы, точность для наших задач не очень высокая (+-2см), сильно влияют перекрестные помехи, либо, если использовать датчики с защитой от этого, получается низкая частота опроса. Одна из команд в России использует это решение с переменным успехом, также немецкая команда Turag довольно успешно катает на схожей технологии, разработанной внутри лабы.
• Визуальная одометрия не даёт информации о положении противников ввиду того, что на поле не очень много точек, за которые получится надежно зацепиться.
• Стереокамеры tof имеют узкий угол, не позволяющий ни накрыть поле при расположении на центральном устройстве слежения, ни получить круговой или хотя бы приемлемый обзор при установке на робота.
• Фидуциарные маркеры, которые мы выбрали и которые рассмотрим более подробно.

Фидуциарные маркеры

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

• ограничения из-за разрешения камеры;
• ненадежность детекции при использовании цвета как основного источника информации;
• необходимость определения ориентации маркера;
• слабоконтрастные метки хорошо работают только при хорошем освещении;
• требуется высокая вычислительная мощность оборудования (real time обработка поступающих кадров).

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

разные виды маркеров

На деле кроме определения положения маркера часто возникает потребность определения его уникальности, например, с целью детекции сразу нескольких объектов. Для этого можно менять цвета или форму, но тогда мы быстро столкнемся с падением надежности детектирования.
Данная проблема довольно распространенная и существует много стандартов маркеров. Самый известный из них — qr code, но он редко применяется для задач локализации из-за избыточности и необходимости высокого разрешения. Как правило, все маркеры двухцветные (чаще всего чб), простой формы (чаще всего квадрат) и каким-то образом кодируют идентификатор в себе. Наиболее известными маркерами для локализации являются Aruco, April Tag, ARToolKit.

Кому интересно подробнее с ними ознакомится — есть интересная статья, в которой также сравнивается и надежность их детекции.

В нашем случае имеем много положительных сторон использования маркеров:

• при наличии возможности установки статичной камеры с хорошим углом обзора и нанесения меток, это не требует труднонаходимого оборудования и потенциально не очень сложно в реализации;
• вынесенная камера может оценивать положение всех объектов, на которые мы можем поставить маркеры;
• расчеты ведутся не на бортовом устройстве робота, а вынесены на отдельный комп (но появляется проблема с надежностью и задержками при передаче данных);
• возможность решить ещё какую-нибудь задачку, связанную со зрением.

Проанализировав вышеперечисленные системы, мы остановились на детектировании маркеров. Это позволяло нам не только освободить наши odroid’ы на роботах, но и позволить им видеть всю ситуацию на поле с высоты птичьего полета.

Как это работает из коробки

“Надежность” детектирования маркеров — термин относительный. Для разных задач требуется разный уровень точности и стабильности. В том числе эти параметры зависят от видеосенсора, линз, условий освещения и т.д.

В нашем случае, при размере поля 2х3 метра и камере всего в метре над ним необходимо использовать сверхширокоугольную оптику с полем зрения не меньше 120-140 градусов для обеспечения видимости всего игрового пространства, что негативно влияет на детектирование (и его точность) небольших маркеров в дальнем углу игрового поля, а при произвольных условиях освещения делают эту задачу еще более затруднительной.
Как показала практика, даже при нормальных, вполне естественных, освещённых, условиях могут возникать ошибочные определения положения маркера.

Так на следующих изображениях показаны два последовательных кадра с результатами детектирования маркера: направление оси Z(синяя) изменено на 180 градусов.

www.youtube.com/watch?v=xzG2jQfxLlY

Доведение до соревновательного состояния

Меняем алгоритм

Для того, чтобы точно определить местоположение робота на поле, необходимо знать угловое положение камеры, общее преобразование системы координат и положение камеры относительно положения поля. Чтобы сделать это преобразование мы разработали свой алгоритм. Так как человек вешает ЦУС вручную, плюс поля немного отличаются друг от друга, то получить всегда идентичное положение камеры, конечно же, это не получается. Угловое смещение камеры относительно поля сказывается очень негативно на детектирование положения роботов. Принципиальное решение сводится к определению нескольких статических маркеров, которые мы можем установить перед матчем для калибровки.

Библиотека Aruco в opencv предоставляет возможность определения 6dof позиции маркера относительно камеры. Вначале мы пытались её использовать, но получалось очень неустойчиво и неточно. В нашем случае, когда положение камеры известно, фиксировано, а роботы (и связанные с ними маркеры) перемещаются на плоскости не проходящей через точку крепления камеры, рационально ограничиться задачей о детектировании 3dof позиции (z- const, х-у необходимо определить + вращение вокруг одной оси z) через триангуляцию точки на плоскости по видимому углу.

Ожидаемо, что снижение размерности и увеличение базы триангуляции (размер маркера против расстояния от плоскости маркеров до камеры) решаемой задачи уменьшает уровень неопределенности и зашумленности результатов детектирования.
Для этого вместо стандартной функции

cv::aruco::estimatePoseSingleMarkers(markerCorners, 0.05, cameraMatrix, distCoeffs, rvecs, tvecs);

которая по найденным углам маркеров определяет 6dof позу маркера, мы написали свой эстиматор позиции, работающий по следующим принципам:

• заранее известна 3д позиция камеры;
• заранее известна высота маркера;
• в результате детекции известны пиксели (v,u) 4х углов маркера;
• в результате калибровки камеры определена ‘матрица камеры’ размерностью 4х4;
• зная ‘матрицу камеры’, положение и ориентацию камеры, положение пикселей углов маркера, можно получить проекцию положения углов в 3д пространстве на плоскость z на которой находятся вершины данного маркера (при этом высота маркера определяется по его номеру);
• делая предыдущий шаг для всех 4х углов маркера и усредняя полученные данные, можно получить 3д координаты вершин маркера в системе координат камеры;
• применив трансформацию из СК камеры в СК игрового поля, получаются координаты маркера в СК поля.

Решаем вопросы освещения и производительности

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

Для снижения влияния освещенности окружающей среды на стабильность детектирования маркеров был разработан специальный “дизайн”, в котором использованы инвертированные аруко маркеры. Темная часть заменена на световозвращающую плёнку, а непосредственно на камеру устанавливается активная светодиодная подсветка. Обработка картинки выполняется в негативе, при это контрастность фона получается значительно ниже контрастности собственных маркеров.

Повышение контрастности маркера позволяет эффективно выделять регионы интереса на кадре и искать маркеры только в них, а не по всему кадру, что сильно поднимает производительность.

Сравнение различия контрастности стандартного маркера и маркера со светоотражающей поверхностью

Подбираем железо

Так как условия освещения не определены, а скорости роботов могут быть довольно большими, мы с самого начала искали камеру с глобальным затвором, а не с Rolling shutter’ом. Отличие в том, что глобальный собирает информацию о яркости пикселей одновременно, а rolling строку за строкой, из-за чего могут быть искажения геометрической формы движущихся объектов.
Наиболее доступной является камера oCam-1mgn, которую мы и использовали 2 года. Однако, она черно-белая и имеет разрешение всего 1Mpx, что находится на нижней границе приемлемого разрешения (количество пикселей изображения, на 1 “пиксель” маркера).
Изначально детектор считался на таком же одноплатнике, что и в самих роботах — odroid xu4, и скорость работы получалась около 20 fps, однако желание навесить ещё дополнительную функциональность на ЦУС и понимание, что нужна более совершенная камера, привели к замене ПК на более бодрый Intel Nuc, а камеру… камеру мы всё еще подбираем к новому сезону.

Запускаем проект

Далее предполагается, что у вас уже есть настроенный ROS Kinetic или Melodic.

Первое, что необходимо сделать — скачать и установить OpenCV 3, Eigen3 и проект https://bitbucket.org/eurobot1718/ocam_usb_cam (для камеры Ocam), либо использовать иной проект, похожий по структуре публикации данных в топики.

Клонируем репозиторий с проектом арукодетектора: https://gitlab.com/eurobot2019/aruco_detector и gui для него https://github.com/alexpostnikov/aruco-gui
После установки проектов необходимо откалибровать камеру и получить матрицы дисперсии и матрицу искажения камеры, а затем все обернуть в yaml файл “camera_params.yml”. Простой пример — это использование шахматной доски. Ссылка на реализацию на С++, где надо лишь собрать и указать параметры доски и камеры, а также название выходного файла.

Основные настройки находятся в файле aruco_detector_params.yml:

• len_of_cube_markers и width_marker длина и ширина маркеров в метрах;
• markers_height — сопоставление маркера и его высоты;
• markers — массив, который описывает принадлежность конкретного маркера к конкретному роботу. В нем указываются маркеры по часовой стрелке. А в случае противников, так как их направление нам не важно, порядок маркеров в config и не важен. Необходимо просто указать код маркера. Также у нас присутствуют маркеры ещё и на верхней крышке, их номера выделены отдельно;
• static_markers_ids — маркеры, которые ищутся в процессе калибровки;
• markers_position — словарь со смещением и поворотом каждого маркера относительно центра робота;
• brightness_threshold можно не трогать — это используется в первичной автоматической калибровке света;
• camera_position_yellow и аналогичный параметр служат настройками по умолчанию в случае, если калибровка не прошла.

Матрица преобразования из системы координат камеры в систему координат поля находится в файле transform_from_cam_to_map.xml

После всей настройки наконец-то надо запускать. Пути к файлу с параметрами, сторона по умолчанию и путь к видеоустройству задаются в aruco_detector_debug.launch, который мы и запустим. Но для полноценного запуска лучше использовать баш скрипт aruco.bash, который кроме детектора запускает gui(127.0.0.1:5000). Здесь мы можем выбрать сторону, запустить калибровку и контролировать работу системы. Во втором окне видны зоны интереса и найденные маркеры.

Теперь в топиках: "/enemy_robot1/aruco", "/enemy_robot2/aruco", "/big_robot/aruco", "/small_robot/aruco" будет публиковаться положение найденных роботов.

Ура! Мы смогли получить координаты роботов. Вот только, как можно заметить, они неправильные. Всё потому, что перед использованием надо откалибровать положение камеры относительно поля. Для этого необходимо

• установить кубики в углы поля, при этом важно чтобы они были обращены сторонами прописанными в конфиге
• отрегулировать наклон камеры, чтобы в кадр попадало всё требуемое пространство
• отрегулировать яркость подсветки до надежной детекции маркеров
• в gui выбрать нужную сторону и нажать “calibrate”
• после этого состояние с “search static cubes” должно измениться на “search cubes”

Изготавливаем маркеры и ЦУС

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

высота и габариты маяков на роботе

Поэтому в качестве основных маркеров для наших роботов используются параллелепипеды 10х10х8см оклеенные с 4х сторон aruco, а также дополнительные маркеры на верхней стороне наших роботов для локализации в сложной зоне, находящейся под камерой.

Так как на крышку противника мы ничего не можем наклеить, для расширения зоны видимости мы решили сделать маркеры в виде усеченной пирамиды. И как видно — маркер стало видно сильно лучше.

Основу для кубапирамиды мы печатаем на 3d принтере, для того чтобы он был достаточно прочный и имел удобное и надежное крепление.

Модели для печати куба, пирамиды и ответной части крепления находятся тут

Данный куб обклеивается светоотражайкой (мы используем белую AVERY
DENNISON V6700B шириной 50мм, она хоть и дороже, но работает в разы лучше, чем дешевая китайская), и поверх неё клеятся сами маркеры. Самый простой способ это сделать — вырезать маркеры на плоттере, хотя можно и самим вырезать из бумаги или пленки за пару-тройку умиротворяющих часов (комплект — это 16 маркеров).

Вот наш прошлогодний набор.

различные ”поколения” наших маркеров

Их же можно использовать в качестве статических маркеров для калибровки. Для этого размещаем их в дальних углах поля и нажимаем — calibrate.

Собираем центральное устройство слежения

Что понадобится:

Одноплатный компьютер. Данные чертежи сделаны под расположение Intel NUC, но можно подумать про размещение в корпусе, например, odroid xu4 или raspberry pi4.

oCam. Собственно камера, ради которой всё собирается. Мы поменяли штатный объектив на 2,5мм для получения широкого угла обзора
Дисплей (waveshare 7inch HDMI LCD (B)).С его помощью производится калибровка и ряд других операций.

LED лента. Используется для подсветки светоотражающей поверхности маркеров. Лучше использовать максимально яркую из доступных.

Аккумулятор 16.8v LiPo. Мы используем 4s5200мАч. Это те же аккумы, что и на роботе, так как удобно и хватает надолго.

Понижающий dc-dc преобразователь. Для питания светодиодной ленты. Nuc и роутер питаются напрямую от аккума, потому что вся обвязка питания уже есть на них самих.

Печатные детали. Это, в частности, крепление под камеру, дисплей и уголки для сборки корпуса: https://yadi.sk/d/swUJUwxTnTVYFw

Детали корпуса. В нашем случае, это был карбоновый лист 2мм. Но это не самый оптимальный вариант, так как карбон токопроводящий, сложен в обработке и не такой лёгкий, как, например, оргстекло – самый простой, пожалуй, вариант. Хотя и сильно жестче. Находятся по ссылке выше.

Роутер (Zyxel keenetic extra). Нам пришлось извлекать его из корпуса, чтобы уложиться по габаритам и по массе. Но в общем случае крайне не рекомендуем этого делать.
Много мелочёвки. Винты М3*8 ~40шт, стойки для печатных плат 3х10 ~12шт, провода, отвёртки, wago и т. п…

Сборка

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

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

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

Ставим одноплатный компьютер и роутер. В нашем случае, это NUC и Zyxel keenetic extra.

И получаем собранную систему слежения. Осталось только отрегулировать наклон камеры.

отладка локализации в 2018 году

Что в итоге

Вот мы и рассказали все секреты, позволяющие повторить нашу систему своими руками. Используя её, можно довольно быстро и без каких-либо переделок сделать свою систему локализации робота на поле Eurobot. А с минимальными переделками сделать довольно универсальную и надежную систему отслеживания чего-либо. Однако стоит учесть, что при использовании aruco 8х8, размере маркера 10х8см и камере в 1Mpx особо не получится увеличить радиус детектирования, однако, если у нас есть жесткие ограничения на размер и производительность, то в общем случае просто использовать более интересное оборудование.

В текущем году в правилах появилось упоминание про использование aruco 4х4, а также их не пересекающиеся диапазоны для каждой стороны поля. В ближайшее время мы планируем интегрировать это в наше решение, чтобы в дальнейшем не боятся за пересечение номеров маркеров. Еще маст хэв, но чего пока не сделали, это ручная калибровка на поле.
В следующей статье расскажем про нижний уровень роботов, как считается кинематика роботов, как управляем сервами, двигателями и шишки при работе с freertos.

Хочется сказать спасибо:

Алексею Постникову (от имени которого, наверное, и стоило это всё опубликовать) — за, в общем-то, весь код в этой статье и непосредственно помощь в написании статьи.
Егору Александрову — за все чертежи и модели, помощь в написании статьи и 34 запятые.
Тамаре Синельниковой — за помощь в написании статьи.

И всей остальной команде!

До начала соревнований осталось меньше 4х месяцев, но команда Sberbank Eurobot Team, всё так же открыта новым участникам. Наш telegram: https://t.me/SetUpSber

Предыдущие статьи:

Что общего между револьвером, шайбами и автономным роботом
Свой стек навигации. Лучше чем у ROS?

Автор: Егоров Дмитрий

Источник