Предисловие
В 2013 году с целью популяризации робототехники в России и создания среды программистов и инженеров, ориентированных на данную тематику компания КРОК (г.Москва) организовала конкурс «Летающие роботы». Наша команда «iKar» (3 человека из Барнаула и 1 из Москвы) участвовала в 2013 году (конкурс «Улететь и вернуться») и 2014 году («Догнать и перегнать Крок») не победила, но добилась неплохих результатов.
1. С чего все началось или условия конкурса
Будучи по профессии программистом 1С, нередко приходится пользоваться форумом forum.mista.ru. Один из моих друзей-коллег первым заметил объявление на тему «Кому лимон» и предложил участвовать.
Условия конкурса выглядели заманчиво: необходимо было построить или купить летающего робота и научить его перемещаться/ ориентироваться в помещении-полигоне, автоматически взлетать и садиться и распознавать посадочные маркеры. Срок на всю работу 1 год, а приз — 1 миллион рублей.
Имелся опыт построения вертолетов и квадрокоптеров, как для хобби, так и для профессионального применения в аэрофотосъемке. Было много вопросов по поводу различных настроек, ПИД коэффициентов, кода и алгоритмов полетных контроллеров. Используя мотивацию конкурса, можно было глубоко во всем разобраться.
С детства была мечта заниматься робототехникой, участие в конкурсе позволило сделать первые шаги в данном направлении. Вера в собственные силы, «правое» дело и «вкусный» приз, сделали свое дело, весь семейный бюджет плюс доступные кредитные средства, были направлены на постройку «бюджетного» робота-беспилотника и поездку в Москву на конкурс.
2. Выбор платформы летательного аппарата
В качестве летательного аппарата (ЛА) можно было использовать:
1) готового дрона, например, «AR.Drone», уже умеющего летать и множество плюшек, из минусов:
— небольшая производительность железа;
— низкая грузоподъемность.
2) Построить собственный ЛА нужной грузоподъемности и с необходимым временем полета.
Второй пункт не вызывал сомнения, гексакоптер (6-ти моторный коптер) с расчетной грузоподъемностью в 2 кг и расчетным временем 20 минут при нагрузке с камерой и необходимой электроникой был отличным вариантом. Для тестов было решено сделать небольшой квадрокоптер – «мальчик для битья».
Гексакоптер на базе рамы Tarot-680
Квадрокоптер для «битья»
Особое отношение уделялось вопросу безопасности — все коптеры оснащались защитой винтов. Таким образом, после столкновении со стеной или при падении все оставалось целым. Это экономило деньги и время на ремонт.
3. Схема электрооборудования дрона 2013 года
Для более быстрого решения поставленной задачи и снижения трудозатрат было решено строить дрона из готовых модулей, чем, например, делать все на базе одноплатного компьютера, который бы выполнял все вычисления. Так, например, можно купить полетный контроллер, который может удерживать ЛА в горизонте и стабилизировать полет, а управление осуществлять с помощью другого устройства (микроконтроллера, компьютера)- автопилота, который принимает данные с датчиков, и радиомодема (об этом далее), и передавать команды в полетный контроллер для движении вперед, назад и т.д. Автопилот также выполняет всю интеллектуальную работу (навигация и прочее).
Поскольку анализировать изображение (определять посадочные маркеры) достаточно ресурсоемкая задача, то вычисления должны производится либо на борту на достаточно мощном микрокомпьютере, либо анализ производить вне дрона на базовой станции (ноутбуке), а изображение с дрона передавать на базовую станцию по Wifi, либо через аналоговый видеопередатчик. Таким образом, по распознаванию маркеров получалась такая схема (с аналоговым видеопередатчиком):
1) изображение с видеокамеры с борта дрона передавалось через видеопередатчик;
2) принималось видеоприемником на базовой станции;
3) аналоговый видеосигнал поступал на ноутбук через видеозахват (VIDEO->USB);
4) на ноутбуке изображение распознавалось с помощью программы (OpenCV);
5) по радиомодему в автопилот передавались экранные координаты «увиденного» маркера (X,Y);
6) по полученным экранным координатам, автопилот определял расположение маркера посадочной площадки относительно дрона и принимал решение о посадке или навигации для посадки.
Схема электрооборудования дрона 2013 года
4. Выбираем полетный контроллер
Полетный контроллер был в наличии – MultiWii ALL-IN-ONE; был снят с имеющегося FPV-квадрокоптера:
По летным характеристикам данный полетный контроллер неплох при ручном управлении, но значительно уступает по качеству стабилизации профессиональным контроллерам с более мощным железом и математикой.
При использовании для робота со стандартной прошивкой настроить MultiWii не удалось, потому прошивка была скорректирована.
Использование профессиональных контроллеров с открытой прошивкой типа OpenPilot или PixHawk для проекта было бы более разумным, но на тот момент это было невозможно.
5. Выбираем датчики
Для ориентации в пространстве необходимо было выбрать датчики для дрона.
Список выбора:
Из доступных по бюджету датчиков и простоте использования были ультразвуковые сонары и ИК-датчики. Поскольку у бюджетных УЗ дальность измерения была больше (до 4,5 метров), чем у ИК (до 1,5 метров), то они и были выбраны в качестве основных датчиков для дрона.
Всего потребовалось 5 шт.: 1 сонар был направлен вниз, для удержания высоты, и 4 шт. по периметру для ориентирования от стен.
Пришлось хорошо изучить УЗ датчики, чтобы заставить работать их корректно. Для более глубокого и безопасного изучения был построен наземный робот-танк с установленным сонаром. Танк держал заданное расстояние до препятствия, например, листка бумаги, который можно было двигать, и робот двигался за ним сам. Данные логировались и затем не трудно было их проанализировать и определить нюансы.
Ультразвуковые сонары можно классифицировать по различным характеристикам, таким как эффективный угол (диаграмма направленности), дальность измерения расстояния и так далее. Причем качество измерения сильно зависит от различных характеристик датчиков. Например, широконаправленные сонары работают хорошо при больших углах отклонения от поверхности, до которой измеряется расстояние, в то время как узконаправленные теряются. Также широконаправленные «бьют» дальше (в зависимости от модели) и измеряют расстояние практически от любой поверхности (асфальт, ткань и т.д.). Но есть у них небольшой недостаток — результаты измерений незначительно «прыгают» в отличие от узконаправленных, соответственно требуется фильтровать полученные значения.
Любые сонары требовали «качественного» питания, иначе их показания «прыгали» и невозможно было по ним ориентироваться. Для качественного питания нужно использовать фильтры по питанию или иметь отдельный DC-DC, полные рекомендации можно прочитать здесь. После того, как танк стал не нужен, он превратился в игрушку. С установленной на борту камерой (сонар убрал) можно было по FPV кататься по квартире.
Подробнее о танке: forum.rcdesign.ru/blogs/45616/
6. Выбираем автопилот, а вернее — немного попаяем
Чтобы считывать данные с сонаров, принимать решения по навигации и пересылать данные в полетный контроллер, нужен был отдельный вычислитель — автопилот. Поскольку были знания контроллеров семейства Atmega, то за 2 вечера спаял плату опроса на Atmega162 (единственный контроллер в DIP корпусе и имеющий 2 UART порта, который можно было купить в местном радиомагазине), и который в последствии был заменен на Atmega128 (прошивка выросла и не умещалась уже в памяти контроллера).
Штырьки на плате – это разъемы для подключения сонаров. Главным достоинством использования данных контроллеров по сравнению с контроллерами на Arduino было то, что код можно было отлаживать (дебажить) отладчиком через JTAG! То есть в любой момент можно остановить программу и посмотреть все, что интересует – значения переменных, счетчиков и прочего, а так же в случае ошибки пошагово найти, где она возникла.
7. Учимся удерживать высоту
Для того, чтобы ускорить разработку/тестирование в короткие сроки, было решено испытания проводить в квартире вместо того, чтобы куда-то ездить и терять драгоценное время. Недолго думая был создан стенд для испытаний в комнате 12 кв. метров.
В потолке и в поле были вкручены крючки, между ними натягивалась прочная парапланерная стропа. В раму дрона была установлена вертикальная направляющая – карбоновая трубка. Сверху имелся ограничитель, не позволяющий коптеру упираться в потолок. В результате коптер мог перемещаться только вертикально, что и требовалось, чтобы безопасно для себя и коптера настроить ПИД-коэффициенты удержания высоты.
Подробнее про полигон можно посмотреть в ролике:
Было потрачено много времени на настройку ПИДов, но удалось хорошо разобраться с механизмом регулирования на личном опыте и практике. Также определился с моделью сонаров для удержания высоты, которые приемлемо работали – это широко-направленные DYP-ME 007 v1.0. Популярные HC-SR04 работали хуже — «зависали» на плохой поверхности.
Благодаря работе на стенде «вживую», удалось достичь неплохого результата, коптер отлично держал заданную высоту.
8. Удержание курса или немного о компасе
Одной из следующих проблем стало удержание курса (ось Yaw), то есть, чтобы коптер не крутился вокруг собственной оси. Решение использовать компас, который имелся на борту полетного контроллера, оказалось на первый взгляд простым, но впоследствии недопустимым, потому что компас отказался работать в Москве на полигоне организаторов (силовые линии и прочее). Однако это решение позволило продолжить тестирование у себя в квартире и перейти к следующим задачам — двигаться дальше.
В Москве после выяснения на тесте, что на компас ориентироваться нельзя, получилось отключить компас и написать свой алгоритм удержания по оси Yaw. Для этого ввел величину расчетного угла Yaw (кстати, на полетных контроллерах Open Pilot CCD3 этот угол также рассчитывается, поскольку компаса на борту вообще нет, а угол поворота можно увидеть в программе настройки). Значение угла рассчитывалось по данным с гироскопа (углового ускорения по оси Yaw), формулу подобрал эмпирическим путем. Далее замерил в неподвижном состоянии насколько эта величина убегает за 5 минут (был небольшой дрейф в одну сторону), и корректировал этот угол через равные промежутки времени, чтобы свести получившийся дрейф в ноль. На столе все работало прекрасно, на коптере при отсутствии сильных вибраций это тоже должно было работать.
9. Удержание расстояния от одной стены
Закрепил один сонар (узконаправленный HY-SRF-05) впереди коптера для автоматического удержания позиции по направлению вперед-назад (по оси Pitch). С пульта управления вручную управлял перемещением коптера вправо-влево (по оси Roll), и в случае неудачной настройки ПИД(PID) регулятора, корректировал смещение коптера вперед-назад, при отклонении стика пульта на 10% от центра, коптер переходил в ручное управление по оси вперед-назад(Pitch).
Изрядно помучившись с настройкой ПИД(PID) регулятора и перебором его коэффициентов, получил 3 коэффициента регулирования, при которых аппарат почти сносно «держался» за стенку:
П-позиции (P position), когда ошибка регулирования считается, как разность между нужной дистанцией от стены и текущей по данным с сонара.
Д – позиции (D position), скорость движения отк стене.
Д по скорости (D-velocity), ускорение движения отк стене.
И (I)- коэффициент не по позиции не по скорости не учитывался = 0.
Дальнейшие поиски истины уже перевел на код полетного контроллера. Как выяснилось в коде MultiWii, многие настройки в коде подобраны эмпирически. Руками летать можно, а вот для робота все не очень удачно складывалось. Решил переписать ПИД регулятор и сделать аналогично: P и D position + D velocity, поколдовал с фильтрами, которые позаимствовал из кода полетного контроллера ArduPilot и получил результат на «хорошо», но с профессиональными контроллерами конечно не сравнить.
Когда все объединил, получил неплохой результат для первого раза: удержание в диапазоне 1 метра по горизонтали на высоте 50 см. Пробовал высоту делать выше, диапазон болтанки тоже увеличивался.
10. Подключение остальных сонаров и настройка совместной работы сонаров
Поскольку звук имеет свойство отражаться, то чтобы отраженный сигнал с одного сонара не ловить другим, опрашиваю сонары последовательно. Максимальная частота опроса используемых сонаров 20 Гц, получается, если опрашивать сонары последовательно по очереди (а их 5 штук), то каждый будет опрашиваться с частотой 4 Гц. Благодаря хитрому алгоритму, вкратце, суть которого заключалась в том, чтобы опрашивать «неинтересные» сонары (например, задний при движении вперед) реже — получил 5 Гц (5 раз в секунду в принципе неплохо!)
По поводу стабильности были следующие мысли:
Очевидно, чем чаще опрашиваются сонары (выше частота опроса), тем точнее можно позиционировать коптер, но важна не только частота, но и скорость реакции системы (response time) или задержка между непосредственным измерением, например, расстояния до стены и воздействием на моторы с учетом измеренного расстояния.
Насколько же быстро реагирует текущая система на полученные значения, как быстро (за какое время) дрон среагирует на измерение — подаст на двигатели управляющий расчетный сигнал, после расчетов ПИД, пересылки данных с автопилота на полетный контроллер и считывания расстояния с датчика?
Легко представить такую ситуацию, например, если бы информация с видеокамеры поступала нам в глаза (через монитор или очки) с задержкой 1-2 секунды, то легко ли было бы двигаться, несмотря на то, что частота оставалась бы 25 кадров в секунду? Пришлось бы идти очень медленно, чтобы не упасть. Ну а если пытаться таким образом ходить по канату, например?
Вот ещё пример: сигнал с лунохода с Луны приходит с задержкой в 3 секунды, в то время, как с Марса это время измеряется минутами, и уже управлять им с земли просто невозможно (либо скорость движения марсохода была бы очень и очень медленной).
Рассчитаем время задержки(response time) для получившейся системы дрона:
ИТОГО задержка предварительно: 3..29 + 0 + 25 + 2 + 14 = 44..70 мсек.
Подробное описание:
1) После того, как на сонар подается сигнал для замера расстояния (trig), ультразвуковой модуль излучает ультразвуковой сигнал, проходит определенное время, пока сигнал-звук вернется после отражения от препятствия. Данное время фиксируется в датчике-сонаре и выводится через аналоговый или цифровой выход датчика.
2) Поскольку использовал цифровый выход (Echo) сонара, на котором сигнал (+) формируется по приему отраженного эха, он регистрируется сразу на контроллере автопилота (по прерыванию) — по сути передача измеренного расстояния происходит без задержек, в отличие, например, от варианта передачи по UART или I2C, когда по приему эха контроллер сонара должен был бы передать данные последовательно, а контроллер автопилота должен был бы принять их.
3) Значение, полученное контроллером автопилота, рассчитывается и фильтруется. Вычисленное расстояние до стены или пола “скармливается” ПИД-регулятору автопилота по удержанию от стены или пола. В результате получает новое значение управления коптером по крену (Roll), тангажу (Pitch) и «газу» (Throttle).
4) Полученные значения управления (Roll, Pitch, Throttle) передаются по UART-порту в полетный контроллер(MultiWii).
5) Полетный контроллер считывает данные в кольцевой буфер и через определенное время (50 раз в секунду) “скармливает” их ПИД регулятору (который, по сути, является системой стабилизации multi wii), после чего новые значение управляющих воздействий миксуются (в зависимости от схемы ЛА (квадрокоптер, гексакоптер и т.д.) и подаются на регуляторы двигателей.
Поскольку последовательный цикл опроса сонаров 20 Гц, то время, которое можно потратить на каждый сонар — не более 50 мсек. На определение расстояния сонаром уходит до 29 мсек, и соответственно остается 21 мсек на расчеты (фильтры, ПИД и алгоритмы выбора пути).
Данного времени (21 мсек) оказалось мало для медленного 8-битного контроллера, потому был придуман вариант, когда в 50 мсек интервал рассчитываются управляющие воздействия от предыдущего значение сонара, а в текущий цикл просто считывается значение текущего сонара. Данное решение увеличивало время реакции системы на 50 мсек, но позволяло ей существовать в такой конфигурации низкопроизводительного железа.
ИТОГО задержка (лаг) = 44..70 мсек + 50 мсек = 94..120 мсек, по сути данная задержка – это реакция текущей системы на измеренное расстояние.
Очевидно, что чем медленнее реакция, тем менее стабильно будет себя вести аппарат при любом ПИД-регуляторе и частоте опроса датчиков. То есть стабильность аппарата зависит не только от частоты опроса датчиков, но и от «реакции» системы ЛА на измеренное расстояние (значения лага или response time).
11. Возможная оптимизация
Как можно было уменьшить значение лага в текущей системе? Варианты оптимизации:
1) Использование более быстрого контроллера для автопилота, например АРМ (172 МГц 32 бит)
значение 25 мсек – в которые входит расчет расстояния и ПИД регулятора с передачей можно сократит до 2 мсек.(-23 мсек).
Благодаря этому, считывать и выполнять расчет можно было бы в одном цикле опроса, а это ещё (- 50 мсек)
ИТОГО задержка (лаг) при оптимизации = 94..120 мсек – 23 мсек – 50 мсек = 21..47 мсек
2) Ещё более выгодное решение, если полетный контроллер и автопилот – один быстрый контроллер или компьютер, получилась бы схема ещё быстрее, так как удалось бы сэкономить время на передаче информации с автопилота на полетный контроллер, а также увеличить частоту работы системы стабилизации полетного контроллера (в MultiWii эта частота составляет около 300 Гц)
12. Автономное висение (удержание расстояние от стен + удержание высоты)
После того, как удалось удерживать высоту по сонару и более-менее удерживать расстояние от одной из стен, можно было цеплять 3-й сонар на левую или правую сторону коптера, и учиться висеть в комнате в автоматическом режиме. Но коптер болтался по каждой оси и в конечном итоге раскручивался против часовой стрелки, набирая амплитуду. Минусом было также, что дрон «вяло» удерживал курс(ось Yaw), особенно сильно закручивало аппарат вблизи стены. Также двигаясь от одной стены до другой, аппарат терял немного высоту, далее тормозя перед стеной, он пытался набрать высоту, тем самым почти выстреливая в противоположную сторону. При установке больших значений PID по yaw, аппарат пытался максимально выравниваться в момент закрутки около стены, таким образом, работая почти на двух диагональных двигателях (особенность управления многороторными системами по оси yaw), терял управляемость (так как по сути крутилось всего 2 мотора из 4-х в квадрокоптере) и влетал в стену.
Много времени было потрачено на стабилизацию аппарата по курсу (ось Yaw), подбору PID-ов автопилота и полетного контроллера. Также пришлось придумать пару приемов поведения аппарата, чтобы исключить раскачку. В итоге получилось более или менее стабильное поведение:
13. Навигация по лабиринту
Для прохождения конкурсного полигона не было необходимости в использовании сложных алгоритмов и построении карт, подошел самый простой алгоритм обхода лабиринта «правило правой руки». Немного модифицировал его и сделал на конечном автомате. Вкратце суть заключается в нескольких простых правилах движения:
Для движения дрона вперед/назад/вправо/влево, автопилот подает соответствующие команды на полетный контроллер для наклона платформы в соответствующую сторону. Если стены в направлении движения не «видно» по сонару, то платформа наклоняется на 5 градусов (подбирал угол вручную). Для того чтобы дрон не разгонялся, через равные промежутки времени, он принимает горизонтальное положение и движется по инерции(без наклона в какую либо сторону). Дополнительно для грубого контроля скорости использовал GPS приемник, при фиксации скорости превышающей критическую (её тоже подбирал эмпирически) дрон тормозит, то есть отклоняется в противоположную сторону от направления движения. Для движения в пределах видимости датчика расстояния — сонара, просто изменяется расстояние удержания от стены.
14. Обнаружение маркеров точек посадки
На дрон была установлена камера, изображение передавалось через видеопередатчик на ноутбук. И далее, если маркер был определен (распознан), то на автопилот передавались данные по радиомодему об экранных координатах маркера. На основе полученных данных автопилот рассчитывал расстояние до «цели» относительно коптера и видимых стен (подробную описано в пункте 3). Затем пытался выдержать данное расстояние, и включалась программа посадки, заключающаяся в медленном снижении высоты висения.
В финальной версии робота удалось реализовать поворотный механизм для камеры (камера поворачивалась вверх и вниз по оси Pitch). Таким образом, был организован механизм сопровождения цели, и при подлете к кресту камера поворачивалась вниз, удерживая центр посадочной площадки в центре внимания камеры. Таким образом, время «сопровождения цели» было увеличено и, следовательно, точность посадки тоже.
Распознавание маркера креста в круге выполнялось с помощью Open CV, использовался алгоритм поиска пересечения прямых, этой работой занимался отдельный человек в команде, который подключился к работе на последних сроках проекта, поэтому его не удалось включить в список участников (по правилам организаторов конкурса).
15. Поездка в Москву
Пройдя все контрольные точки для участия в конкурсе, наша команда была допущена для участия в финале. На момент поездки не все было готово в новой платформе на базе гексакоптера, потому финальные приготовления было решено сделать в Москве на тестовом полигоне.
Полигон достраивали в процессе тестирования.
Соревнования были разбиты на 2 дня, некоторые фотографии можно посмотреть на сайте организаторов.
Перед стартом:
Место ожидания на парковке.
Первая попытка нашей команды не удалась, робот пролетел, но зацепил ограждение и коснулся пола, но взлетел и мужественно добрался обратно.
Во второй попытке с навигацией все прошло успешно, робот не касался стенок, но не приземлялся в посадочные маркеры, хотя на базовой станции на ноутбуке маркеры хорошо распознавались.
И после соревнований получился почти идеальный вариант пролета, с посадкой в оба конца.
Итоги
В работе над проектом летающего робота в нашей команде участвовало 4 человека. Один человек отвечал за распознавание, один был отличным специалистом в языке Си и хорошо разбирался в AVR контроллерах, консультировал в сложных моментах проекта. Я, как капитан команды, конструировал дронов, программировал и тестировал, организационными вопросами при поездке в Москву занималась моя жена. Также для тестирования робота требовались помещения – полигоны для испытаний. Спасибо друзьям, кто предоставлял свои дома, гаражи и офисы. Несмотря на, казалось бы, небольшой результат, работы было проделано очень много. 90% времени ушло на изучение чего-то нового, организационные моменты, тестирование и преодоление трудностей, без которых проект бы не двигался.
Конкурс 2013 года собрал много участников, в финале на тестах и во время соревнования можно было немного пообщаться с коллегами (все остальное время уходило на доработку и настройку дрона).
На соревнованиях немного испортили картину погодные условия и были проблемы с беспроводной связью (wifi) у большинства участников, в том числе, и у организаторов. Для многих, в том числе и для нас, на тестах стало сюрпризом, что компас на реальном полигоне не работает (силовые кабели и прочее).
В целом, для первого соревнования для «взрослых» получилось очень даже неплохо.
Список источников о конкурсе:
www.robots.croc.ru
habrahabr.ru/company/croc/blog/192704
habrahabr.ru/post/193304
Автор: DmSk
