Как сделать автопилот своими руками

Самодельный автопилот на одноплатном компьютере (SBC) Tinker board и Arduino DUE

Идея постройки автопилота появилась примерно 2 года назад. Хотелось создать полностью автономный аппарат способный добраться из точки А в точку Б с возможностью ухода от столкновений и облёта преград, способного преодолевать зоны глушения или отсутствия спутникового сигнала. Также хотелось иметь удобное и простое управление с помощью мышки как это реализовано в играх (стратегии) управляя движением ЛА с помощью точек. Начинать всё пришлось с нуля, как и эту статью поэтому если есть ошибки напишите об этом в комментариях. Начну по порядку.

Аппаратное обеспечение

Изначально я не знал какое железо лучше использовать для этого проекта, но в итоге пришёл к выводу, что оптимальным вариантом будет связка микроконтроллер (МК) + одноплатный компьютер. Где МК решает задачу стабилизации летательного аппарата (ЛА), его движению по заданному курсу и высоте, а одноплатный компьютер решает задачу навигации и движения по маршруту. Поскольку в планах был уход от столкновений компьютер должен был быть достаточно мощным чтобы обрабатывать информацию от датчиков обнаружения препятствий, компактным и не слишком дорогим на том момент под это описание наиболее подходил TinkerBoard, Raspbery тогда это была 3B+ и сильно уступала по характеристикам. В качестве МК хотелось иметь arduino совместимый контроллер т.к. на arduino имелась огромная база готовых скетчей и поэтому выбор пал на DUE 84 МГц, 32bit ARM Cortex-M3 т.к. он был наиболее мощным и должен был компенсировать прямоту моих рук)).

В качестве датчиков ориентации изначально я планировал использовать MPU 9250 с фильтром Маджевика, результаты его работы были отличными. Главным преимуществом этого варианта было то, что все расчёты, включая калибровки датчиков (акселерометра, гироскопа и магнитометра), находились на МК. Но возникла проблема, фильтр плохо компенсировал линейное ускорение, которое постоянно возникает при толчках или резкой смене курса. Выражается это в показаниях тангажа и крена, в момент ускорения они начинают уплывать, а проходя через пропорционально дифференциальный (ПД) регулятор и особенно дифференциальную часть, уплывание создавало проблемы. Поэтому пришлось использовать датчик с уже реализованным фильтром BNO 055.

BNO в отличии MPU 9250 имеет на борту встроенный МК Cortex M0 который сразу рассчитывает ориентацию в углах Эйлера, кватернине абсолютной ориентации и рассчитывает линейное ускорение хотя этот датчик тоже имеет ряд недостатков. Основной проблемой данного датчика является автокалибровка а точнее то что её нельзя выключить это такая «фитча» этого датчика и есть у этой калибровки неприятное свойство пропадать, иногда абсолютно внезапно даже просто находясь на одном месте без движения. Отражается это в основном на рысканье которое в данном датчике привязано к магнитометру и должно показывать направление на магнитный северный полюс (курс) но иногда оно показывает на 100 градусов в строну, а потом ещё покалибровавшись может вернутся обратно))). В прочем проблему курса ещё можно решить с помощью синхронизации с GPS. В остальном датчик работает отлично, тангаж и крен он всегда определяет правильно, и линейные ускорения не сильно сказываются на его работе, если, конечно, ускорение не превышает 2G, т.к. этот порог используется для измерения вектора гравитации и компенсации дрейфа гироскопов.

Остальной набор железа выглядит следующим образом: GPS Ublox Neo M8N с USB выходом,
барометр BMP 280, сонар HSCR 04 для получении данных о наличии земли и более точной вертикальной скорости, EEPROM 24c16 для хранения данных калибровки и настроек ПИД, GSM модуль Neoway M509E для отправки сообщений о координатах ЛА на случай аварии.

Функциональная схема показана на рисунке 1:

Рисунок 1 — Функциональная схема Автопилота.

Программное обеспечение

Для разработки ПО я использую QT вместе с IDE QT Creator т.к. он наиболее мне знаком, а также благодаря кроссплатформенности я могу запускать свои программы как на одноплатном ПК с Debian так, и десктопе с Windows, что очень удобно. Для разработки ПО микроконтроллера используется Arduino IDE. Для наглядности постараюсь представить все разделы на рисунке 2.

Рисунок 2. — Архитектура АП (BNO 080 добавлен на будущее).

1) Графический интерфейс управления — представляет из-себя спутниковую карту с помощью которой осуществляется управление ЛА. Сама программа отображения спутниковых снимков не моя, она была украдена мной тут (её автор тоже пытался сделать что-то подобное).

Управлять ЛА можно с помощью точек (маркеров) либо кнопок WADS. Для управления по точкам, необходимо проложить маршрут полёта зелёными маркерами они ставятся мышью (ПКМ), и нажать загрузить маршрут, либо использовать (красный) маркер мгновенного перемещения (ЛКМ) и тогда ЛА с текущей позиции полетит к этой точке, для его работы необходимо установить галку в чекбокс «Ручное» от случайных нажатий.

Все параметры маркеров вводятся в соответствующие поля на форме, удалить маркеры можно двойным щелчком средней кнопки мыши, при этом они по-прежнему останутся в памяти ЛА, для удаления из памяти необходимо использовать кнопку удалить маршрут. По достижению точки, как и в стратегиях ЛА, будет вокруг неё вращаться. Управление кнопками WADS напрямую управляет рулями с помощью ПД регуляторов. При нажатии каждой кнопки на вход регулятора поступает значение, например, при нажатии S тангаж 30 а при отпускании 0. При нажатии W -30 и т.д. Включается WADS с помощью чекбоксов: «ручное», «кнопки». Данный режим помогает проверить работоспособность всех рулей пред стартом. Работает графический интерфейс на ноутбуке, команды управления от графического интерфейса с помощью TCP соккета предаются в ядро. Графический интерфейс управления представлен на рисунке 3:

Рисунок 3 — Графический интерфейс управления.

2) Ядро автопилота это та часть ПО которая вычисляется на одноплатном компьютере TinkerBoard. Ядро отвечает за навигацию и движение по маршруту. Для этого к компьютеру подключен датчик GPS. С его помощью можно получить текущее положение ЛА (широту и долготу) и сравнить это положение с тем, что имеется в маршруте полёта. В результате этой операции получается азимут на цель, который отправляется на микроконтроллер вместе с остальными параметрами полёта. В дальнейшем ядро можно оснастить своим IMU датчиком, чтобы реализовать ИНС. Например, можно использовать BNO 080 проинтегрировать, ускорение и получить скорость, а проинтегрировав скорость получить расстояние. Расстояние, полученное от ИНС, необходимо будет перевести в систему координат GPS (широту и долготу) для её использования в расчёте азимута.

Такую ИНС можно использовать в связке с GPS датчиком на случай временной потери связи со спутником, чтобы ЛА не пропустил «поворот» на точку. В момент работы от GPS ИНС будет постоянно корректироваться его показаниями и заполнять промежутки между периодами обновления GPS датчика. Таким же образом должен вносить поправки алгоритм машинного зрения или SLAM изменяя высоту точки и создавая смещения рассчитанного азимута. После окончания расчёта маршрута ядро отправляет по UART данные: азимут, высоту, угол атаки, тип точки, а также необходимо ли выполнять вращение вокруг этой точки.

3) Команды ядра выполняет микроконтроллер, основная задача МК следовать по заданному курсу на заданной высоте. Для этого на МК установлен IMU датчик BNO 055, барометр bmp 280 и сонар. Для движения по курсу используется азимут полученный от ядра он, сравнивается с текущим курсом и полученное рассогласование передаётся в ПД регуляторы управления рысканьем и креном. Управление тангажом осуществляется 2мя ПД регуляторами: 1-й определяет рассогласование текущей и заданной высоты, которое поступает на вход 2-го регулятора, при этом выход регулятора высоты ограничен текущим углом атаки, чтобы контролировать её набор. В случае, если в графическом интерфейсе тип точки выбран взлёт или посадка, для определения высоты используется сонар. Его показания комплексируются с данными барометра, чтобы наиболее точно определить расстояние до земли и вертикальную скорость. Помимо основных функций МК также собирает телеметрию о работе IMU датчиков, текущем направлении и высоте, передаёт их в ядро, где эти данные дополняются данными от GPS и поступают в графический интерфейс.

Заключение

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

Вообще ПД регуляторы мне кажутся не стабильными и хочется заменить их чем-то более надёжным, тем более они уже устарели. Также необходимо заменить расчёты с углами Эйлера на расчёты в кватернионах, т.к. последние более стабильно себя ведут при развороте ЛА на углы больше 120 градусов и полётах во время ветра.

Источник

Автопилот своими силами. Часть 1 — набираем обучающие данные

Привет, Хабр. Это пост-отчет-тьюториал про беспилотные автомобили — как (начать) делать свой без расходов на оборудование. Весь код доступен на github, и помимо прочего вы научитесь легко генерить такие класные картинки:

Вкратце

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

Что-зачем-почему более подробно

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

Автопилот преобразует данные сенсоров в управляющие воздействия — поворот руля и требуемое ускорение/замедление. В системе с лазерными дальномерами, как у Google, это может выглядеть так:

Простейший же вариант сенсора — видеокамера, «смотрящая» через лобовое стекло. С ним и будем работать, ведь камера на телефоне уже есть у каждого.

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

Хороших архитектур нейросетей хватает в открытом доступе, а вот с данными ситуация более печальная: во-первых данных просто мало, во-вторых почти все выборки — из США, а у нас на дорогах много от тех мест отличий.

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

The rocket engine is the models and the fuel is the data.
Andrew Ng

Во-вторых, процесс сбора данных недёшев, особенно если действовать «в лоб». Хороший пример — Udacity. Они специально подобрали модель автомобиля, где рулевое управление и газ/тормоз завязаны на цифровую шину, сделали интерфейс к шине и считывают оттуда данные напрямую. Плюс подхода — высокое качество данных. Минус — серьезная стоимость, отсекающая подавляющее большинство непрофессионалов. Ведь далеко не каждый даже современный авто пишет в CAN всю нужную нам информацию, да и с интерфейсом придется повозиться.

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

В этой серии — «выжималка» угла поворота из видео. Все шаги легко повторить своими силами с помощью кода на github.

Задача

• Есть видео с камеры, жестко закрепленной к авто (т.е. камера не болтается).
• Требуется для каждого кадра узнать текущий угол поворота руля.

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

Решение

Решение можно собрать из общедоступных компонент, немного их доработав:

Восстанавливаем траекторию камеры

Первый шаг — восстановление траекториии камеры в трехмерном пространстве с помощью библиотеки SLAM по видео (simultaneous localization and mapping, одновременная локализация и построение карты). На выходе для каждого (почти, см. нюансы) кадра получаем 6 параметров положения: 3D смещение и 3 угла ориентации.

В коде за эту часть отвечает модуль optical_trajectories

• При записи видео не гонитесь за максимальным разрешением — дальше определенного порога оно только повредит. У меня хорошо работают настройки в окрестностях 720х480.
• Камеру нужно будет откалибровать (инструкции, теория — актуальны части 1 и 2) на тех же настройках, с которыми записывалось видео с заезда.
• Системе SLAM нужна «хорошая» последовательность кадров, за которую можно «зацепиться» как за точку отсчета, поэтому часть видео в начале, пока система не «зацепится» останется не аннотированным. Если на вашем видео локализация не работает совсем, вероятны либо проблемы с калибровкой (попробуйте откалибровать несколько раз и посмотрите на разброс результатов), либо проблемы с качеством видео (слишком высокое разрешание, слишком сильное сжатие и т.д.).
• Возможны срывы отслеживания SLAM системой, если между соседними кадрами потеряется слишком много ключевых точек например, стекло на мгновение залило всплеском из лужи). В этом случае система сбросится в исходное не локализованное состояние и будет локализовываться заново. Поэтому из одного видео можно получить несколько траекторий (не пересекающихся во времени). Системы координат в этих траекториях будут совершенно разными.
• Конкретная библиотека ORB_SLAM2, которой я воспользовался, дает не очень надежные результаты по поступательным перемещениям, поэтому их пока игнорируем, а вот вращения определяет неплохо, их оставляем.

Определяем плоскость дороги

Траектория камеры в трехмерном пространстве — это хорошо, но напрямую еще не дает ответа на конечный вопрос — поворачивать налево или направо, и насколько быстро. Ведь у системы SLAM нет понятий «плоскость дороги», «верх-низ», и т.д. Эту информацию тоже надо добывать из «сырой» 3D траектории.

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

Горизонтальную плоскость выделяем прекрасным методом главных компонент по всем 3D точкам траектории — убираем направление с наименьшим собственным числом, и оставшиеся два дадут оптимальную плоскость.

За логику выделения плоскости также отвечает модуль optical_trajectories

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

Чтобы не загрязнять данные большими погрешностями с таких траекторий, проверяем, что разброс по последнему главному компоненту значительно (в 100 раз) меньше, чем по предпоследнему. Не прошедшие траектории просто выкидываем.

Вычисляем угол поворота

Зная базисные векторы горизонтальной плоскости v₁ и v₂ (два главных компонента с наибольшими собственными значениями из предыдущей части), проецируем на горизонтальную плоскость оптическую ось камеры:

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

Угол поворота между соседними кадрами дает школьная тригонометрия (первый множитель — абсолютная величина поворота, второй — знак, определяющий направление налево/направо). Здесь под a_t понимаем вектор проекции a_horizontal в момент времени t:

Эта часть вычислений тоже делается модулем optical_trajectories . На выходе получаем JSON файл следующего формата:

• plane — базисные векторы горизонтальной плоскости.
• trajectory — список элементов, по одному на каждый успешно отслеженный системой SLAM кадр.
  • frame_id — номер кадра в исходном видео (начиная с 0).
  • planar_direction — проекция отпической оси на горизонтальную плоскость
  • pose — положение камеры в 3D пространстве
    • rotation — ориентация оптической оси в формате единичного кватерниона.
    • translation — смещение.
  • time_use — время с начала видео в микросекундах
  • turn_angle — горизонтальное вращение относительно предыдущего кадра в радианах.

Убираем шум

Мы почти у цели, но остается еще проблема. Посмотрим на получившийся (пока что) график угловой скорости:

Визуализируем на видео:

Видно, что в общем направление поворота определяется правильно, но очень много высокочастотного шума. Убираем его Гауссовским размытием, которое является низкочастотным фильтром.

Сглаживание в коде производится модулем smooth_heading_directions

Результат после фильтра:

Это уже можно «скормить» обучаемой модели и рассчитывать на адекватные результаты.

Визуализация

Для наглядности по данным из JSON файлов траекторий можно наложить виртуальный руль на исходное видео, как на демках выше, и проверить, правильно ли он крутится. Этим занимается модуль render_turning .

Также легко построить покадровый график. Например, в IPython ноутбуке с установленным matplotlib:

На этом пока всё. В следующей серии — определяем поступательную скорость, чтобы обучить еще и управление скоростью, а пока что приветствуются pull-request’ы.

Источник