Робот-автомобиль команды АВРОРА на “Робокросс-2013”

Привет, Хабр!
Становится традицией публиковать отчёты команд после выступления на соревнованиях “Робокросс”.
В прошлом сезоне были отчёты команд НАМТ и MobRob, а сейчас, мне хотелось бы рассказать о работе нашей команды.

Я расскажу про то, как мы делали робота, и почему мы его сделали именно так. Надеюсь, это будет интересно участникам других команд, или просто увлекающимся подобной тематикой.

Меня зовут Владимир, я аспирант, участник команды Автономные Роботы Радиоуниверсиета (АВРОРА) г. Рязань. Мои основные интересы — искусственный интеллект, направление деятельности в команде — разработка программного обеспечения. Команда представляет студенческое конструкторское бюро (СКБ) нашего универа.

Описание задания

Соревнования “Робокросс-2013” проходили на автополигоне “Березовая пойма” завода ГАЗ (Нижний Новгород) с 17 по 20 июля. Под заезд был выделен участок “спортивного” круга, длиной около 100м.
Суть задания: робот-автомобиль автономно движется за человеком на медленной скорости (около 5-7 км/ч), и запоминает маршрут. Затем разворачивается (в ручном или автономном режиме) и по команде оператора должен автономно вернуться в исходную точку. При этом, на обратном пути судьи устанавливают на дорогу несколько препятствий, которых при движении вперёд не было (пластиковые бочки).
Задание схоже с поведением мула — если его завести в горы и отпустить, то он найдёт дорогу и вернётся обратно в стадо. Задание было взято по аналогии с одним из заданий ELROB-2012 (европейские соревнования роботов-автомобилей).
При выполнении задания оценивалось время, затраченное на движение от старта до финиша, а также количество сбитых препятствий. Победителем мог стать только робот с полностью автономным управлением.
Несмотря на кажущуюся простоту, всего лишь несколько команд справились с задачей (успешно добрались до финиша).

Описание механики и электроники

Над роботом-автомобилем работает несколько человек. Примерно по три — над механикой, электроникой и программированием верхнего уровня. Чётких должностей нет — все должны уметь разбираться в нескольких областях. Над созданием робота команда трудилась 2 месяца, работая даже по воскресеньям, и уходя из лаборатории не раньше 21.00.

Механика и электроника представляют собой “мышцы” и “периферическую нервную систему” робота.

Механика

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

Педальный блок

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

Здесь синий энкодер на одном валу с мотором соединяется цепью с рулевым карданом (под капотом)

Цепь зажигания находится в двух режимах — когда машина заводится с ключа (как обычно), либо же подключена через реле и кнопки безопасности, чтобы можно было заводить автономно. За этот способ переключения однажды мы поплатились, забыв переключить режим — машину невозможно остановить в случае потери управления. Но всё обошлось.

Электроника

Для всех своих робототехнических проектов мы используем собственную электронику. И разрабатываем её, что называется from scratch. Вроде можно купить Arduino, NI RIO, и какой-нить motor controller, или что-нибудь в этом роде, получится и дешевле, и быстрее. Но здесь есть несколько но. Научиться настраивать что-то, или тем более улучшать, можно только зная как всё это работает. Что называется “чувствовать” систему. А сделать это, можно лишь попробовав повторить её самому, по шагам разбираясь, что, почему, и как. У нас уже был опыт разработки, а также в течение нескольких лет мы развивали это направление. И если в первых сезонах мы отставали от команд, использующих готовые решения, то теперь мы имеем опытных разработчиков схемотехники, отлаженные решения и можем полностью адаптировать их под любые нужды.

Плата управления моторами

Электроника и “низкий уровень” программного обеспечения представлены полностью нашим решением — платами управления двигателями. Прошивка каждой платы адаптирована под конкретную задачу — управление сцеплением/тормозом или рулём. В каждой “зашит” соответствующий алгоритм. Обратная связь по абсолютным и относительным энкодерам, а также концевым выключателям и оборотам двигателя автомобиля.

Платы управления закреплены на выдвижной стойке, и легко убираются под водительское сиденье Газели.

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

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

Контроллер “низкого уровня” (используем контроллеры LPC NXP с ARM7 архитектурой) управляет актуаторами, на основе команд, полученных от программы “высокого уровня” (i386 arch). Интерфейс связи — RS232, обычный COM-порт. Команда управления представляет собой набор параметров {угол поворота руля, скорость движения, тормоз, дополнительные реле}. Также контроллер присылает “верхнему уровню” текущие обороты двигателя и некоторые другие параметры, необходимые для принятия решений.

Описание архитектуры программы и алгоритмов

Наконец-то перешёл к части, над которой работал непосредственно.
Много лет назад, когда всё только начиналось, все универы закупали NI контроллеры для своих команд. У нас тогда средств на них не было, поэтому решено было делать “мозги” робота на i386 архитектуре, т.е. обычном ноуте или desktop пк. Зато я вряд ли бы получил опыт разработки под Linux и embedded-linux, а также известные библиотеки обработки изображений OpenCV и т.п. (не хочу обидеть NI, у нас тоже RIO появился недавно — любопытная штука).

Набор датчиков

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

• энкодеры положения руля и карданного вала (для позиционирования)
• защищённая от непогоды IP-камера (для обнаружения метки)
• линейный лазерный сканер SICK (для построения карты проходимости)
• плата ориентации (гироскопы+акселерометры) (для позиционирования)
• GPS приёмник (для позиционирования)

Датчики подключаются к ПК посредством RS232 и Ethernet.
Для обработки сенсорных данных в кузове Газели установлен обычный ПК. Питание компьютера осуществляется бензиновым генератором, закрепленным снаружи под кузовом (симметрично с бензобаком, на противоположной стороне).

Используемые библиотеки

В качестве операционной системы мы используем Ubuntu, среда разработки — QtCreator, язык — C++, библиотека для gui — Qt, получение и обработка изображений — OpenCV, обработка данных лазерного сканера — PCL, а также Boost для работы с файлами и сетью. В результате получилось кроссплатформенное решение. Однако всё же мы используем Ubuntu, так как система более предсказуема, в фоне случайно не начинает работать антивирус или индексация файлов, тормозя все остальные процессы… Да и общая производительность несколько выше.

Следование за меткой

Задание соревнований подразумевает выполнение 2х элементов — следование за меткой и следование по маршруту. Поэтому в программе было предусмотрено два основных режима работы — под каждый элемент задания соответственно.
В режиме следования за меткой, общий алгоритм программы предельно простой — необходимо двигаться за меткой и запоминать точки маршрута до тех пор, пока метку не уберёт оператор. Положение руля во время движения пропорционально положению метки относительно центра кадра.
В случае потери метки робот подаёт звуковой сигнал и через некоторое время (0.5сек) начинает тормозить. Это сделано в целях безопасности, да и по логике получается неплохо — когда надо чтобы автомобиль остановился — метка убирается (переворачивается тыльной стороной).
Метка представляет собой некоторый знак, расположенный на шесте (также для безопасности). Мы использовали розовый круг с чёрным контуром, так как данный цвет редко встречается в природе.

Проверка алгоритма движения за меткой

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

Демонстрация работы алгоритма поиска метки

Координаты маршрута описываются списком {GPS точка, ориентация (углом поворота), радиус (при попадании в который данная точка считается достигнутой)}. При окончании движения осуществляется реверс точек и маршрут сохраняется.

Возвращение в точку старта

В точке остановки оператор убирает метку — и робот останавливается. Проблема в том — что он должен как-то развернуться. В правилах соревнований не было ограничений на способ разворота робота, поэтому мы выбрали ручной способ — когда оператор сам разворачивает автомобиль. Алгоритм планирования траектории, гипотетически мог спланировать разворот, но мы решили потратить время на отладку более необходимых функций на тот момент.
Мы сразу отказались от “простых алгоритмов” типа “Если бочка слева — руль вправо, если цель справа — руль вправо” и т.п. Я называю это “быдло-методом”. Очевидно, что при появлении второй бочки, или дополнительных условий, быдло-методы являются не состоятельными.
Алгоритм автономного движения можно представить как последовательное решение следующих вопросов:

Где я?

Для того чтобы принять решение куда робот должен ехать, какую поправку внести в траекторию, робот должен знать, где он находится, т.е. отвечать на вопрос “где я?”. Для этой цели нельзя использовать только данные с GPS приёмника, так как ошибка GPS составляет десятки метров, не говоря уже о движении вблизи зданий или в тоннеле. Более того, если вы встанете в поле с GPS приёмником и снимете координаты, то через некоторое, придя в то же место, координаты будут значительно отличаться. Также проблемой является низкий темп выдачи координат — от 1 до 5 раз в секунду, что недостаточно для управления автомобиля даже при скорости движении в 5км/ч — система просто будет не успевать реагировать на ситуацию, и робот будет двигаться как пьяный водитель, а то и произойдёт перерегулирование.

Выходом из данной ситуации является использование нескольких датчиков разной природы, основанные на разных физических явлениях. В нашем роботе, кроме GPS мы используем колёсные энкодеры — определение линейной и угловой скорости автомобиля на основе данных о том, насколько прокрутился карданный вал, и как был ориентирован руль. В качестве дополнительного канала также используется плата определения ориентации на основе гироскопов, акселерометров и магнитного компаса.
Каждый из датчиков обладает различным темпом выдачи информации (например, энкодеры — 70Гц, плата ориентации — 10 Гц). Также для них характерна различная природа ошибок. Из-за неточности определения размеров колёс и длины колёсной базы автомобиля, навигации, основанной только на энкодерах, свойственно накопление ошибки со временем. Гироскопы подвержены тепловому дрейфу и вибрации корпуса автомобиля. Компас сильно реагирует на крупные металлические объекты (мосты, арки и т.п.), соответственно использование каждого датчика в отдельности также невозможно.
Для объединения показаний различных датчиков применяются различные техники, мы выбрали использование разновидности нелинейного фильтра Калмана — Ансцентный фильтр Калмана(Unscented Kalman Filter). На пальцах принцип работы можно объяснить следующим образом — на основе модели движения автомобиля (он не может телепортироваться или двигаться боком) предсказываются показания каждого датчика. Затем, предсказанное значение сравнивается с фактическим (измеренным) и каждому датчику, в соответствии с вероятностью того, что он показывает правду, ставится свой вес (чем ближе к предсказанному, тем больше доверие). На основе полученных весов формируется оценка текущего положения и ориентации автомобиля.

Использование D-GPS, конечно, облегчило бы задачу, но это требует абонентской платы — нельзя купить и забыть (расходники оплачиваются очень проблемно).
Тем не менее, полученная оценка положения является приемлемой для управления — нет выраженных скачков координат, следовательно, регулятор движения по траектории ведёт себя предсказуемо.

Куда ехать?

Когда положение робота известно, то можно определить, где относительно робота находится целевая точка. Проблема на данном этапе заключается в том, что, как было отмечено раньше, GPS подвержен медленному дрейфу — координаты “плавают” в течение дня. Это можно объяснить неоднородностями атмосферы, тучи, облака — всё это оказывает влияние на время прохождения сигнала.
Проявляется это следующим образом.

Дрейф GPS

Во время того, как робот ждёт в точке разворота, текущее его положение начинает “уплывать” в сторону. Т.е. если мы запустим его двигаться быдло-методом только по GPS, то он будет двигаться на некотором смещении относительно первоначальной траектории, при этом датчики будут показывать что движемся мы нормально — они ведь не знают, что все координаты сдвинулись. Это приведёт к тому, что робот будет ехать по обочине (ну или по ёлкам, или бетонным блокам). Так, конечно, никуда не годится.

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

На рисунке — реальная карта проходимости, во время тестовых заездов на стадионе.

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

Принцип работа лазерного сканера схож с лазерным дальномером, только более сложная оптическая система непрерывно вращается, обеспечивая угол обзора 180 градусов с угловым разрешением 0.5 градуса и максимальной дальности 80 метров с точностью измерения 2см. Этих параметров было достаточно для данных соревнований, а установка его на определённой высоте позволяло видеть все возможные препятствия на трассе и контур дороги.
Сканер возвращает массив дальностей, который затем фильтруется от единичных выпадов (от травы или просто шумы), оставляя препятствия угловой шириной не менее 2 градусов. Затем массив отрисовывается на карте проходимости в полярных координатах относительно центра. Перед каждым обновлением карта проходимости заливается непроходимым цветом, а отрисовываются на ней только проходимые участки. Это избавляет алгоритм выбора целевой точки от соблазна выбрать точку за областью видимости сканера (в тени от бетонных блоков или забора). Если бы это произошло — то алгоритм планирования траектории не смог бы её построить, так как целевая точка не достижима.
Также важно то, что лазерный сканер жёстко прикреплён к кузову. Как следствие — карта проходимости строится в локальных координатах, т.е. мы не можем ошибиться с расположением дороги в пространстве.

Итак, у нас есть карта проходимости, текущее положение робота и текущая точка маршрута, куда необходимо попасть. Здесь в дело вступает алгоритм выбора целевой точки. Он объединяет эти данные для получения валидной (корректной) целевой точки, в которую автомобиль может гарантированно доехать.
Принцип его работы достаточно прост — рандомизированный выбор множества точек на карте проходимости, рядом с идеальной целью. Затем выбирается ближайшая к цели точка. Учитываются также геометрические размеры автомобиля — вокруг точки должно быть свободное пространство, чтобы движение в выбранной области было безопасным. Если исходная цель выходит за размеры карты проходимости — то точка по радиусу сдвигается на расстояние Rmax (в нашем случае размеры карты 40*40 метров, Rmax = 15м) так, чтобы она лежала на карте, и уже затем выполняется поиск.

Схематическое изображение карты проходимости. Белый квадрат символизирует непроходимую область

Как ехать?

На данный момент задача управления сводится к тому, что нам нужно попасть из центра карты проходимости в целевую точку. И снова проблема — классические алгоритмы поиска кратчайшего пути (A* и т.п.) здесь не работают. Почему?

Во-первых, пространство, в котором движется робот-автомобиль — непрерывно. Координаты (x,y) — вещественные числа. Классические алгоритмы рассчитаны на дискретное пространство состояний. Здесь же множество переходов бесконечно, и перебрать их не получится.

Во-вторых, автомобиль — неголономная система. Это означает, что автомобиль не может мгновенно изменить своё положение или ориентацию в пространстве. Более того, его следующее положение достаточно “жёстко” связано с предыдущим — ограничение на производную скорости, невозможность мгновенно остановиться или сменить направление движения. Поэтому даже если построить кратчайший путь с помощью дискретного алгоритма (допустим, округлив координаты до целых), то проехать по такой траектории не будет представляться возможным.

Из всего этого следует, что получить методом перебора кратчайшую траекторию не представляется возможным. В данном случае можно говорить только о траектории “близкой к оптимальной”. Такая траектория не является кратчайшей. Для нас является более важным то, чтобы робот в конечной точке траектории имел требуемую ориентацию — если робот приедет в промежуточную вершину перпендикулярно дороге — это кончится плохо. (Хотя на втором плане всё-таки важно, чтобы робот петлял как можно меньше).

В одну и ту же точку можно приехать по-разному

Для поиска пути в непрерывном пространстве состояний применяются различные алгоритмы. Одними из наиболее популярных являются разновидности Rapidly exploring Random Tree (RRT). При работе RRT перебор всех возможных состояний не осуществляется. Вместо этого всё пространство (в общем случае) состояний покрывается равномерно распределёнными вершинами, которые в дальнейшем используются как опорные для построения дерева решения.
Однако в чистом виде, для планирования траектории автомобиля RRT не подходит, так как он, опять же, не учитывает кинематические ограничения. Поэтому в нашей работе мы использовали алгоритм, предложенный Kuwata, Y и другими [1].
Общая идея остаётся такой же, как и в RRT. На пути следования автомобиля случайным образом выбираются N промежуточных состояний.

Чёрными кружками показаны промежуточные состояния. Красный круг — цель и ориентация автомобиля в ней

Затем строится дерево траекторий из исходной вершины. Процесс проверки достижимости соседних точек модифицирован с учётом модели движения автомобиля.


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

Когда лимит итераций исчерпан (алгоритм должен успевать работать в реальном времени — машина же движется!), либо количество ветвей достигших цели превышает порог M, то осуществляется выбор кратчайшей траектории из множества доступных.
Полученная траектория представляет собой набор траекторий Дьюбинса (Dubins path) — набор отрезков и дуг некоторого минимального радиуса поворота автомобиля. Данный вид траекторий был разработан специально для представления движения автомобиля и является кратчайшим путём для движения из точки (x,y,theta) в (x’,y’,theta’) с учётом начальной и конечной ориентации.

Пример путей Дьюбинса

Сложность при реализации данного алгоритма заключается в том, что сам метод является вычислительно затратным (по сути это перебор). Необходимо было серьёзно оптимизировать его работу. Траектория должна периодически обновляться, чтобы реагировать на изменение дорожной обстановки. Из-за наличия промежуточных вершин, манёвры автомобиля получались достаточно сложными (в хорошем смысле).
Траектория отправляется на исполнение контроллеру движения, который осуществляет управление рулём и скоростью. Алгоритм контроллера взят аналогичный предложенному в [1].

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

Планировщик движения для шоссе

Переключение режимов программы

Про способ переключения режимов программы мы заранее не подумали. Пришлось впопыхах крепить снаружи на борт «Гозели» (мы так называем робота) небольшой клавиатурный блок с цифрами — правильная комбинация цифр определяла то, что робот будет делать дальше:

Клавиатура для переключения режима программы (внутри баночки). Рядом памятка оператору — чтобы не забыл

Вообще, с выбором режима у нас получилось не очень удобно. Так как для того, чтобы развернуть Гозель необходимо было освободить педали, а для этого — программа должна их «отпустить». Как результат — приходилось вводить множество промежуточных команд на клавиатуре. В нервной обстановке в них легко запутаться. Любопытно также то, что разворот автомобиля занимал по времени больше, чем автономная езда в обоих направлениях.

Общая архитектура

На основе описанных выше алгоритмов можно составить следующую схему взаимодействия компонентов программы:

Отладка

Отладка — важнейший этап разработки любого программного продукта, особенно, когда дело касается ПО для управления роботом-автомобилем. Но и здесь есть свои сложности.
Цена ошибки — повреждённый автомобиль или объекты инфраструктуры. Не говоря об опасности для членов команды. Здесь баги обходятся дорого.
Также немаловажной проблемой является стоимость испытаний. Отлаживать работу всей системы желательно в безлюдном месте, вдалеке от объектов, которые можно повредить, если что-то пойдёт не так. Для этого надо машину туда доставить. А оплата услуг эвакуатора ничем не компенсируется.

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

Наш симулятор

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

Мотор перегрелся

Самая “верхняя” часть алгоритма работала хорошо, а вот “низкий” уровень не очень — пришлось перенастраивать коэффициенты регуляторов управления рулём и педалями(что логично). К концу дня мы добились приемлемых результатов. Вроде всё получилось.

Небольшое превью соревнований и финальный заезд

Заключение и выводы

На соревнованиях “Робокросс-2013” наша команда заняла первое место. Мы объехали все бочки и благополучно вернулись в зону старта.
Вывод, который напрашивается сам собой — робот-автомобиль — сложная система. Создание каждого компонента это вызов, который должны принять и преодолеть разработчики, особенно в условиях нашей страны. Естественно — создание подобного устройства невозможно в одиночку, поэтому мне хотелось бы выразить признательность своей команде.
Я не рассказывал в деталях проблемы при разработке электроники, но там их, было даже больше чем в программном обеспечении.

Вопрос что дальше? Развиваться. Система представляет достаточно целостный вид. Улучшение каждого из компонентов должно повышать качество работы всего робота. Добавление логики правил дорожного движения затронет только алгоритм выбора локальной цели. Также тенденцией в мировом автономном автопроме является минимизация стоимости используемых датчиков — дорогие лазерные сканеры стараются не использовать, а делать упор на более дешёвые системы технического зрения. Необходимо двигаться и в этом направлении.

У классического автомобиля поворотными колёсами являются передние — человеку так проще управлять. А если сделать все колёса поворотными? Это даст возможность совершать более сложные манёвры, начиная от параллельной парковки до перестройки из ряда в ряд без снижения скорости. Человек с таким управлением вряд ли справится, а вот робот может:

Робот «Маракайка». В разработке

Другие проекты и роботы лаборатории можно посмотреть здесь.

p.s. Целью данного материала является также поиск единомышленников — мы начинаем opensource проект ПО для роботов-транспортных средств. Если вы заинтересовались данной тематикой, у вас есть опыт разработки (или являетесь участником других команд) напишите мне — .

Источники

1. Yoshiaki Kuwata, Gaston A. Firore, et al, «Motion Planning for Urban Driving using RRT,» 2008 IEEE RSJ International Conference on Intelligent Robots and System, September, 2008.