Полет квадрокоптера в связке Engee, ArduPilot и Godot
Engee & ArduPilot: симуляция полета квадрокоптера с визуализацией в Godot
В нашем демонстрационном примере мы покажем, как можно связать вместе среду инженерных расчётов Engee с системой автопилота ArduPilot (SITL Simulator). С помощью наземной станции QGroundControl зададим миссию полета для квадрокоптера, в Engee по поступающим ШИМ-сигналам из ArduPilot будем рассчитывать кватернион, тягу винтов, моменты и передавать результат расчёта сенсоров коптера обратно в ArduPilot в режиме lock-step, визуализировать полет будем в движке Godot.
Что это дает
- Настоящий автопилот. Та же прошивка ArduPilot, что стоит на реальных физических дронах, без единой правки в коде.
- Свою физику. Рама, моторы, масса — параметры модели Engee, а не правки исходного кода на C++ с ручной пересборкой прошивки.
- Быстрый цикл. Изменили модель или коэффициенты — перезапустили — полетели. Показательно, воспроизводимо.
Для быстрого начала работы с ArduPilot воспользуемся подготовленной инфраструктурой (Docker-окружением) для работы с ArduPilot под Windows/Linux из примеров пакета поддержки Engee.Интеграции:
# Загружаем примеры стенда: демонстрационная модель, подготовленное Docker-окружение, параметры автопилота
engee.package.getdemos("Engee-Device-Manager")
1. Как устроен стенд
Всего три составные части:
- окружение с ArduPilot SITL — прошивка
arducopterи ретранслятор телеметрии MAVProxy; - модель Engee — квадрокоптер и блоки обмена Ardupilot Setup / RX / TX;
- QGroundControl (может быть любая другая, по желанию) — наземная станция, позволяет задавать миссию полета, видеть полёт на карте.
Прошивка собрана в режиме внешнего расчёта физики ArduPilot. В этом режиме ArduPilot делегирует расчёт физики во внешнюю среду, и работает по принципу: отдать ШИМ-команды моторам — получить данные сенсоров. Обмен идёт по протоколу UDP на порту 9002 строго по очереди, с периодом 20 мс (частота цикла прошивки — 50 Гц, параметр SCHED_LOOP_RATE). Такой режим называется lock-step: ни одна из сторон не убегает вперёд, и задержка физики видна автопилоту так же, как на реальном борту.
Вот, как проходит один цикл. Прошивка отправляет ШИМ-кадр. Модель переводит его в обороты винтов, считает тягу, моменты, сопротивление воздуха и шаг интегратора — и возвращает кадр сенсоров: углы, скорости, координаты, высоту. Прошивка сравнивает это с целью, поправляет моторы — и так 50 раз в секунду, весь полёт.
Рис. 1. Схема стенда2. Аппарат: физика в Engee
Аппарат — квадрокоптер массой 1 кг с вылетом моторов 1 м. Модель — четыре подсистемы:
- 6 степеней свободы (кватернион) — движение твёрдого тела;
- Force and Torque — тяга винтов, реактивные моменты, моменты от разноса моторов; обороты ограничены 300 рад/с;
- Внешние условия — гравитация и сопротивление воздуха;
- Control XY — простые ПД-регуляторы Engee.
Четвёртая подсистема — является заглушкой для данной демонстрации и не используется, но остается в качестве возможной опции: коптер умеет летать и без автопилота. Это способ проверить физику до подключения ArduPilot: аппарат управляем, висит, выходит в горизонт.
Параметры аппарата — масса, геометрия рамы, винты, аэродинамика — заданы переменными скрипта Quadcopter_Parameters.ngscript. Поправив параметры, вы получите другую конструкцию аппарата. Так стенд настраивается индивидуально под вашу раму.
.png)
Рис. 2. Верхний уровень модели.
Рис. 3. Подсистема Force and Torque
3. Автопилот
В демонстрационном окружении — стандартная прошивка ArduPilot, входящая в состав ArduPilot SITL. Все параметры автопилота задаются с помощью штатного способа — посредством файла параметров engee_physics.parm:
| Параметр | Значение | Что задаёт |
|---|---|---|
| SCHED_LOOP_RATE | 50 | частота цикла прошивки, Гц |
| SIM_RATE_HZ | 50 | частота запросов физики, Гц |
| GPS1_TYPE | 100 | GPS-симулятор SITL |
| FRAME_CLASS | 1 | квадрокоптер |
| ATC_RATE_R/P/Y_MAX | 60 | предел угловых скоростей, °/с |
| ATC_ANGLE_MAX | 10 | предел крена и тангажа, ° |
| WP_SPD | 1 | скорость по маршруту, м/с |
Профиль консервативный: крены до 10°, мягкие угловые скорости.
Здесь GPS-симулятор включён: координаты считаются физикой и приходят автопилоту честно. Это отличие от нашей статьи «Спуфинг GPS-приёмника ArduPilot по протоколу MAVLink» — там симулятор отключали и подсовывали автопилоту подменённые координаты.
4. Запуск: три шага
- Загрузите примеры и тестовое окружение для вашего ПК — ячейка кода в начале статьи. Потребуется так же установка подсистемы Engee.Интеграции.
- Запустите тестовое окружение. После установки тестового окружения из п.1.: Windows:
windows\build_and_run.bat. Linux:bash linux/build_and_run.sh. Первая сборка — около 30 минут, дальше секунды. Проверка:docker ps -a, контейнерap_physicsв статусеUp. - Запустите модель. Сначала скрипт
Quadcopter_Parameters.ngscript, затем симуляцияexample_engee_ardupilot_flight.
Сеть настраивать не нужно: Engee слушает UDP 9002 на localhost, тестовое окружение отдает данные на этот порт.
5. Что вы увидите
Тестовое окружение настроено, модель запущена. Откройте QGroundControl — на карте появится аппарат. Поставьте в планировщике миссии взлёт, пару точек, посадку — и смотрите, как квадрокоптер в симуляции проходит миссию со скоростью 1 м/с, удерживает высоту и садится по команде.
Результат работы стенда — на записи:
6. Зачем это нужно на практике
Настройка регуляторов. Поменяли поведение моторов или массу — перенастроили параметры регуляторов в файле — проверили в полете. Цикл занимает минуты, риск нулевой.
Логика миссий и failsafe. Обход точек, возврат в исходную позицию, реакция на отказы, симуляция до работы с реальным аппаратом.
Регрессия прошивок. Стенд поднимается одной командой, значит контрольную миссию можно прогонять автоматически на каждой сборке прошивки.
7. Куда развивать
Своя физика. Ветер, отказ мотора, несимметричная рама — обычные блоки Engee. Прошивку трогать не нужно.
Симуляция физического аппарата и явлений реального мира. Добавьте батарею, шумы для гироскопов и акселерометров — автопилот столкнётся с неидеальными сенсорами, как в жизни.
Атака на навигацию. Следующий шаг — статья «Спуфинг GPS-приёмника ArduPilot по протоколу MAVLink»: тот же стенд, но автопилоту отправляют ложные координаты для моделирования отказа GPS. Последовательность выглядит логичной — сначала учим аппарат летать, потом проверяем, насколько его физические компоненты устойчивы к обману.
Заключение
Обмен данными между Engee и ArduPilot в режиме lock-step (косимуляционный обмен) объединяет лучшее с двух сторон: физику — в наглядной модели, которая меняется вместе с изменением параметров регуляторов; автопилот — в виде кода настоящей прошивки автопилота, который нет необходимости менять. Стенд открыт и воспроизводим: загрузите примеры, поднимите тестовое окружение, запустите модель, и настройте полёт своей рамы ещё до того, как она будет собрана.