[Из песочницы] Самодельный автопилот на одноплтаном компьютере (SBC) Tinker board и Arduino DUE
Идея постройки автопилота появилась примерно 2 года назад. Хотелось создать полностью автономный аппарат способный добраться из точки А в точку Б с возможностью ухода от столкновений и облёта преград, способного преодолевать зоны глушения или отсутствия спутникового сигнала.
Также хотелось иметь удобное и простое управление с помощью мышки как это реализовано в играх (стратегии) управляя движением ЛА с помощью точек. Начинать всё пришлось с полного 0ля, как и эту статью поэтому если есть ошибки напишите об этом в комментариях. Начну по порядку.
Аппаратное обеспечение
Изначально я не знал какое железо лучше использовать для этого проекта, но в итоге пришёл к выводу, что оптимальным вариантом будет связка микроконтроллер (МК) + одноплатный компьютер. Где МК решает задачу стабилизации летательного аппарата (ЛА), его движению по заданному курсу и высоте, а одноплатный компьютер решает задачу навигации и движения по маршруту. Поскольку в планах был уход от столкновений компьютер должен был быть достаточно мощным чтобы обрабатывать информацию от датчиков обнаружения препятствий компактным и не слишком дорогим на том момент под это описание наиболее подходил 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, и поступают в графический интерфейс.
Заключение
На данный момент автопилот ещё находится на стадии полётных испытаний и полностью не настроен. В прочем я провёл только 2а запуска и пока не подобрал коэффициенты для регуляторов.
Вообще ПД регуляторы мне кажутся не стабильными и хочется заменить их чем-то более надёжным тем более они уже устарели. Также необходимо заменить расчёты с углами Эйлера на расчёты в кватернионах т.к. последние более стабильно себя ведут при развороте ЛА на большие углы поворота и полётах во время ветра.
Ссылка на исходники.
