Starline Hackathon 2020

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

Если вы не знаете что такое ROS советую к изучению ресурсVoltbto.com, вот напримерстатья по работе с сервисами в ROS.

В конце марта 2020 года компания Starline во второй раз в своей истории провелахакатон.

По сравнению спредыдущим хакатоном(youtube), проведённым в октябре 2018 года, в этот раз сильно повысился уровень организации: был чёткий регламент, система оценки, равные условия для всех участников и сложное квалификационное задание - такое, каким и должно быть.

Организаторы в этот раз взяли высокую планку по сложности соревнований: настолько, что из 25 поданных заявок квалификацию прошли только 5. Для решения квалификационного задания было необходимо настроить роботу всимулятореgazeboдля проезда из одной точки в другую. Подробнее об этомздесь.

Такой уровень не может не радовать: наконец в России появилось ещё одно хорошее соревнование по робототехнике для специалистов, а не для школьников. Кстати по этой теме, отнести к таким соревнованиям в России на данный момент я могу лишь несколько (мой личный рейтинг):

1. StarlineHackathon

2. COEXHackathon

3. РобофестAutonet 18+

4. UpGreatЗимний город+ очень крутаястатья на vc.ru

5. Робокросстык-1,тык-2

6. РобофинистРТК Экстремал Pro (информация естьздесь)

7. КрокКонкурс летающих роботов[больше не проводит]

Все эти соревнования отличаются друг от друга достаточно сильно, но в них мне как специалисту было [бы] интересно участвовать.

Теперь расскажу немного про сам хакатон. Хотя по факту он уже таким не являлся, за исключением, возможно, жеребьевки участников без команды. В нём присутствовали все атрибуты соревнования: есть отборочные, заранее известное и чётко прописанное задание (а соответственно и подготовка).

Я@urpylkaс моим другом Артуром@goldarteрешили участвовать вдвоём (в предыдущий раз я занял третье место, а он первое).

Дано

Каждой команде был выделен робот Turtlebot E2 следующего содержания:

1. Платформа Kobuki

2. RGBD-камера Orbbec Astra

3. Rplidar A2

4. Компьютер Intel NUC [BOXNUC7I7BNH]

5. Дополнительная камера Logitech HD Pro C920

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

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

Организаторами изначально был построен целый город (7x7 клеток): из квадратных черных матов было сделано дорожное полотно с разметкой, а из коробок разного размера с прикрепленными иллюстрациями были оформленны дома. У каждого дома на полу малярной лентой была отмечена его позиция, чтобы если кто-то сместил дом, знал куда его вернуть на место. Это важно для возможности ориентации робота с помощью построенной карты.

В городе имелись препятствия (про расположение которых мы не знали заранее):

1. Светофоры

2. Знаки Стоп

3. Блоки Дорожные работы

4. Другие участники движения в виде статичных моделей машин

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

Задание и решение

По заданию нужно было проехать из точки старта в неё же (при этом вернуться в точку старта нужно с другой стороны) с учетом всех препятствий и сделать это как можно больше раз за отведенное время. При этом нарушения накладывают временные штрафы.

Мы разделили решение на несколько частей и занялись ими по отдельности:

1. Стейт машина для управления роботом

2. Определение дорожной разметки и способ не пересекать сплошную

3. Машинное зрение для распознавания знаков Стоп и светофоров

4. Ориентация и перемещение робота по городу с использованием алгоритмов, доступных в ROS

В начале решения задач по техническому зрению, мы изначально понимали какие есть алгоритмы и как они работают. Хорошей кодовой базой и документацией к ней являютсямануалы для TurtleBot3 на robotis.com, плюс в их репозитории с соревнованийautorace:turtlebot3autorace,https://github.com/ROBOTIS-GIT/turtlebot3autorace2020. В этих репозиториях есть хорошие примеры:

• Автоматическая парковка по разметке(правда разметка там упрощенная: всего одна дорожная полоса, плюс левая линия окращена в желтый цвет);

• Распознание объектов;

• Реализация стейт-машины.

State Machine

Идея решения изначально была следующая: Делаем стэйт-машину с двумя состояниями:GOTO_0,GOTO_1. Они являются состояниями следования в точки0и1на карте соотвественно.

В момент, когда робот находится в точке0, на глобальную карту планировщика наносится препятствие, отмеченное красной линией; в момент, когда робот покидает точку0, на глобальной карте удаляется препятствие, отмеченное красной линией, и добавляется другое препятствие, отмеченное зелёной линией. Это нужно для того, чтобы глобальный планировщик не строил маршрутов, ведущих на точку старта по тому же пути, по которому из неё выехал робот.

По завершению (достижению точки) состояния циклично меняются. Так достигается то, что робот ездит кругами.

В состоянияхGOTO_0,GOTO_1стэйт-машины проверяется, не был ли замечен красный свет светофора или знак Стоп. Если таковые были замечены, делаем остановку до окончания красного света или на одну секунду в случае знака.

Также впоследствии были добавлены состоянияINITиIDLE. ВINITмы включаем платформу, инициализируем объекты вспомогательных классов и переходим в состояниеIDLE. Также перейти в состояниеIDLEможно изGOTO_0иGOTO_1. В качестве фреймворка для state-machine было реализованособстсвенное решение, построенное на базестэйт-машины Karn Saheb. В реализацию классаStateMachineдобавлено полеSтипа пустого классаObjectStorage. Это позволяет организовать удобную работу с одними данными из разных состояний черезsetattr(Class, Attr, Value)и через его сокращенный вариантClass.Attr = Value.

Детекция разметки

В начале думали использовать, что вродеlane-detection:

1. github/awesome-lane-detection

2. github поиск lane-detection

3. hackster.io curved-lane-detection

Также завели конфигурированиеbirdviewс помощьюdynamic_reconfigure.

Затем посмотрели в сторону использования нейронных сетей для детектирования попутной полосы цветомcolor-segmentation. Нам понравилось решениеdheera/ros-semantic-segmentation. По размеченной цветом картинке можно высчитать координаты области и передавать эти данные в локальный планировщик.

На тему семантической сегментации изображений естьстатьяна хабре.

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

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

Затем нам нужно было как-то правильно выбирать полосу и не ехать по встречке. Мы решили динамически наносить на глобальную карту препятствия, таким образом, что если робот въезжает в радиусR1центра некоторого перекрестка, то во всех запрещенных встречных направлениях перекрестка строятся препятствия, до тех пор пока робот не отдалится от центра данного перекрестка на радиусR2.

Распознание объектов

В рамках ограниченного времени хакатона, было решено заранее поискать готовые инструменты для выделения нужных объектов на изображении. Одним из инструментов оказался ROS пакетfind_object_2d: он позволяет на лету сконфигурировать детектор фич и дескриптор для классификации объектов по заранее имеющимся картинкам, а также предоставляет список определённых объектов и их координат с матрицами гомографии в топик/objectsиobjectsStamped. Помимо выделения объектов нужно было, например, правильно распознать цвет светофора, поэтому помимо готовых решений пришлось применять решения из библиотекиopencv. Итоговым решением задачи по определению знака стоп и цвета светофора стала нодаdetect_objects_node.py. Кстати вотнеплохой кодиз проекта по управлению машинойCarND-Capstoneв котором мы черпнули много полезного.

Распознавание знака стоп

Распознавание знака стоп было реализовано с помощью пакетаfind_object_2d. В качестве детектора фич и дескриптора был выбран ORB с увеличенным количеством итераций афинных преобразований для более робастного определения знака под разными углами. Все параметры распознавания доступныздесь. Для довольно уверенного распознавания знака с разных ракурсов хватило вот этой картинки:

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

Для удобной интеграции распознавания знака с машиной состояний был реализован сервисdetected_stop, который возвращает информацию о том, распознан знак или нет.

Для настройки некоторых параметров детектора был применён механизм динамической конфигурации параметровdynamic_reconfigure, который позволяет на лету (что особенно актуально в условиях ограниченного времени) подобрать нужные параметры через инструментrqt. Например вот так мы настраивали распознание красного света светофора:

Распознавание светофора и его цвета

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

Рабочий алгоритм по выделению цвета светофора выглядел следующим образом: вначале на изображении выделяются области определённой насыщенности в цветовом пространстве HSV, затем среди этих областей производится поиск областей, наиболее похожих на круг (методом Hough Circles) с определёнными границами по размеру. Если такие области есть - мы определили, что находимся у светофора с определённым цветом.

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

Особенным моментом стало то, что мы не могли использовать видимое изображение с камеры Astra в виду очень узкого угла обзора. А также не особо подходила камераLogitech HD Pro C920также из-за достаточно узкого угла обзора установленная камера внутри робота не позволяет увидеть знак с другого края дороги нужна более широкоугольная камера. Мы же более-менее решили эту проблему с выносом камеры за пределы робота.

Navigation Stack

Приведу схему ROS Navigation Stack (информация взята изпрезентации PhD PaulEdouard Sarlin) для общего понимания процессов.

• Map prior: что заранее известно о окружающей среде

• Kinect: камера глубины, возвращающая облако точек

• Odometry: позиция робота в лабиринте

• Map updater: обрабатывает данные для создания и обновления внутренней карты окружающей среды

• Global planner: вычисляет маршрут от начальной точки в цель, используюя карту

• Local planner: выполняет и корректирует с учетом обстановки процесс перемещения робота по построенному маршруту

Для управления роботом используетсясупер-пакетnavigation, он содержит в себеmove_base(утилитадля управления роботом через угловые скорости) и два планировщика: локальный и глобальный. Планировщик это программа, которая, опираясь на положение робота на карте окружающего пространства, может построить маршрут в заданную координату. Локальный планировщик работает с небольшим участком карты в ограниченном пространстве вокруг робота с учётом данных с датчиков робота об окружающих объектах. Глобальный планировщик строит общий маршрут, ориентируясь по всей карте, без учета возможного появления новых препятствий.

Полезным к изучению будетмануалпо настройке Navigation Stack на роботе. И туда же очень хорошаястатьяпо настройке ROS Navigation Stack.

Также вышелновый пакетrobot_navigation, как замена старому.

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

Вместо стандартного локального планировщика мы решили использоватьteb_local_planner (github), который написали ребята изTU Dortmund University. Несколько ссылочек c примерами:

• https://github.com/rst-tu-dortmund/teblocalplannertutorials

• http://wiki.ros.org/navigation/Tutorials/Navigation%20Tuning%20Guide#TheLocalPlanner

• http://wiki.ros.org/teblocalplanner/Tutorials

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

В планировщиках обычно используются простые по назначению слои:

• obstacle_layer динамические препятствия (примернастройки Obstacle inflation in costmap2d)

• inflation_layer утолщение препятствий

• static_layer статичная карта

Каждый слой должен быть определнного типа:

• costmap_2d::StaticLayer препятствия, построенные на базе заданной статической карты

• costmap_2d::ObstacleLayer препятствия добавляемые с помощью топика типаLaserScanилиPointCloud

• costmap_2d::InflationLayer добавляет вcostmapлинию некоторого радиуса и определнной плотности вокруг препятствий

• costmap_2d::VoxelLayer добавление 3D объектов

Помимо использования обычного набора слоев, можно добавить свой. Однако у меня после добавления слоя типаcostmap_2d::StaticLayer, содержащего сплошную разметку, получаемыйcostmapбыл с сильными дефектами.

Подробнее проcostmap_2dвы можете прочитать в официальной документацииздесь. Там вы можете найти примеры конфигураций (1,2,3), описание простых типов (staticmap,obstacles), описаниеflatиlayered.

Также я нашел решениеSr4l/virtualobstacles, которое представляло из себя Costmap Plugin для добавления слоев типаcostmap_2d::MovingObjects. У проекта была не очень хорошая документация, поэтому я написалLars Kistnerи он дополнил репу и прислал еще дополнительные инструкции и интересноевидео, где он это использует. Однако я не успел завести это решение и решил использовать что-то более топорное. В дальнейшем наверное правильнее будет использовать его решение.

Для построения препятствий на карте я написалреализациюрисования линий с помощью простейшегоалгоритма Брезенхэма.

Для динамического слияния (наложения) карт я написалMaps Merger.

Итого у нас получилось пять карт:

1. /maps/map_amcl чистая карта, полученная с помощью алгоритмаgmapping(подробнее о построении картыздесь)

2. /maps/map_mb/maps/map_amcl+ нарисованные сплошные линии (передавалась в глобальный планировщик)

3. /maps/crossroads карта с динамически наносимыми на неё перекрестками

4. /maps/start_wall карта с динамически наносимыми на неё стенками у точки старта

5. /maps/map_merged- карта динамически строящаяся из/maps/map_mb,/maps/crossroads,/maps/start_wall(передавалась в локальный планировщик)

Статические карты поднимаются с помощью стандартнойутилитыmap_serverвходящей в стекnavigation.

После настроек стека навигации можно выполнить следующую команды для теста робота:

rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped '{header: {stamp: now, frame_id: "map"}, pose: {position: {x: 0.674, y: 0.119, z: 0.0}, orientation: {w: 1.0}}}'# PS Координаты и фреймы должны быть ваши (выбраны исходя из используемых карт)

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

Другие интересные источники по теме

• Статьяпо использованиюcostmap_2dдля квадракоптера на Pixhawk

• Статья Layered Costmaps for Context-Sensitive NavigationDavid V. Lu, Dave Hershberger, and William D. Smart

• Работа Fast Gaussian Process Occupancy Maps. In 2018 15th International Conference on Control, Automation, Robotics and Vision (ICARCV) (pp. 15021507). IEEE.Yuan, Y., Kuang, H. & Schwertfeger, S. (Файл gpmaps.py)

• Супер-пакетROSgeometry2для проведения преобразований. Примерыздесь 1издесь 2.

• Утилитаoccupancy_grid_utils

• Также ребята из того жеTU Dortmund Universityнаписалиcostmap_converterутилиту позволяющую вынимать примитивы изcostmap. Исходный код выложен наgithub, софт написан с использованием алгоритмаDBSCAN.

• Аналогдляcostmap_2d, ЭтобиблиотекаC++, прямо аналогичная библиотеке GridMap ETHZ ASL, но разработанная для использования с затратами, когда элемент данных представляет собой беззнаковый символ (в отличие от двойников grid_map).

Определение препятствий

Некоторые команды просто убирали несколько уровней платформы TurtleBot и использовали только лидар. Мы же решили разделить эти задачи и завести их на разные сенсоры.

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

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

Также для детекции более низких препятствий, нам пришлось сместить RGBD-камеру ниже.

Итог

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

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

У нас же была очень круто проведененная неделя в Питере в компании специалистов и друзей. Мы очень благодарны компанииStarline, в частности: Алексею Хованскому, Николаю Дема, Александру Никифорову, Кириллу Гореву, Веденину Даниилу и Маркеловой Виктории.

Бонусом укажу пару-тройку ссылочек на ресурсы ребят и компанию ораганизавторов:

• developer.starline.ru открытое API некоторых продуктов Starline.

• Портал РобоФинист, кстати его основным разработчик является всего один человек Кирилл Горев. А еще на этомпорталевы можете найти больше фоточек с этого замечательного мероприятия.

• YouTube канал Николая Дема. Коля крутой робототехник и на его ютубчике периодически появляются интересные видосы.

На этом всё, спасибо большое за прочтение!