Статья в формате «Редакция выступила. Что сделано?». Год назад мы рассказывали, как и почему используем стандарт UAVCAN и единую шину данных для наших проектов с беспилотниками. Одной из фишек, изложенных на уровне концепции, была true HITL-симуляция. Что ж, пора от концепции перейти к представлению проекта. Мы разработали новый вариант HITL-симуляции, когда автопилот может даже не знать, работает ли он с реальными датчиками на шине или же в симулируемом окружении. Набор программных модулей, работающих в «боевом» режиме и в режиме симуляции (почти) идентичен, в отличие от альтернативного MAVLINK-HITL подхода.
Stand on the shoulders of giants
Этот проект — наш вклад в экосистему вокруг PX4 и UAVCAN, которая нам очень помогает в работе. Он опубликован на github и доступен всем желающим: https://github.com/InnopolisAero/innopolis_vtol_dynamics. Будем рады адоптерам и контрибьюторам.
Вокруг
Про вопросы надежности и ответственность при разработке систем управления для беспилотников, особенно летающих, сказано много, в том числе здесь и тут. Часть этих рассуждений (например, идея применения для разработки встраиваемого ПО подходов из области web-разработки) очень дискуссионная, если не сказать холиварная. Но то, что нужна единая шина данных и связанная с этим возможность создания распределенных систем управления, думаю, уже не должно вызывать сомнений.
Я всё же опишу некоторые моменты, которые хоть напрямую не относятся к теме заголовка, но составляют существенную доли мотивации этого проекта. Надежность по-прежнему является барьером для широкого коммерческого применения беспилотников за границами видимости оператора (BVLOS — Beyond Visual Line of Sight), для доставки последней мили, например. Посмотрим на лидеров:
Даже по внешнему виду аппаратов понятно, что разработчики думают о повышении надежности за счет избыточности (redundancy) органов управления. Но надежность на исполнительных механизмах не останавливается: в идеале нужно резервировать и исполнительные механизмы и все основные сенсоры и контроллеры.Такого в популярных opensource автопилотах нет и все разработчики или делают это долго и дорого сами или не делают. Что мешает сделать автопилот аэротакси на PX4? За такое предложение в некоторых местах могут и побить. Но, на мой взгляд, как раз таки развивая в PX4 тему с распределёнными бортовыми системами, можно этого добиться. И тогда (потенциально) можно сделать автопилот аэротакси по цене сравнимой с автопилотом дрона (или хотя бы того же порядка).
Хорошо, допустим, мы сделали распределенную систему с резервированием на UAVCAN шине. Как это все отлаживать? Для тестов нам нужно не меньше 50 дронов в качестве расходников. Еще одно преимущество архитектуры: чтобы уменьшить количество неудачных лётных тестов, мы можем сначала их (испытания) симулировать. Причем мы можем симулировать наши датчики и исполнительные механизмы на шине практически неотличимо от настоящих устройств, просто подключив симулятор к той же шине. Если бы мы то же самое хотели сделать для архитектуры на картинке слева, то нам пришлось бы эмулировать 3-5 различных интерфейсов и еще больше их портов и протоколов.
Проблема
Как делается HITL-симуляция в PX4 (и Ardupilot) сейчас? Несколько различных интерфейсов, портов и протоколов никто (ну или почти никто) не симулирует. А используют один интерфейс: UART и MAVLINK в нём. Интро по тому, как это сейчас делается в PX4, тут. Вот только программная конфигурация автопилота, работающего в режиме такой симуляции, существенно отличается от лётной конфигурации: работает разный набор драйверов и программных модулей. Такая симуляции неполная. К тому же, она не отвечает тенденциям перехода к единой бортовой шине данных.
Решение
В целом, из пространного введения уже понятно, что нужно делать. Оставалось, собственно, сделать. Симулятор состоит из следующих компонентов:
матчасть,
UavDynamics— симулятор динамики БПЛА (на примере Innopolis VTOL: динамика квадрокоптера и самолета с симуляцией аэродинамики);коммуникатор,
Communicators— пакет обмена сообщениями с полетным стеком PX4 в режимах HITL через UAVCAN и SITL через MAVLink;красота,
inno_sim_interface— мост для взаимодействия с трехмерным визуализатором InnoSimulator через ROS.
Имманентными компонентами симулятора обычно являются модули моделирования динамики (физики) и трёхмерного рендеринга. На рисунке выше они названы UavDynamics и Innopolis Simulator. Про нашу реализацию динамики я расскажу дополнительно, но в целом, это может быть и на базе Gazebo. Для реализации подключения к шине UAVCAN был разработан пакет Communicators. Он содержит как реализацию симуляции по MAVLINK для совместимости, так и UAVCAN-шлюз uavcan_communicator. Для работы с CAN-шиной с персонального компьютера (на котором запускается симуляция) нужно примерно такое устройство:
И в аппаратной части этого достаточно, чтоб автопилот считал, что к нему подключены реальные датчики.
Подопытный
На примере всегда понятнее, поэтому вот он: симулятор был протестирован на Innopolis VTOL, который может быть классифицирован как quadplane (самолёто-квадрокоптер). Он оснащен автопилотом PX4 CUAV v5+, который осуществляет управление тремя управляющими поверхностями (элеронами с обеих сторон крыла, рулями высоты и направления) посредством сервоприводов (естественно, с UAVCAN), а также 4 коптерными BLDC-двигателями с помощью электронных регуляторов скорости (ESC) с интерфейсом UAVCAN и маршевым самолетным двигателем: BLDC с ESC через UAVCAN либо ДВС (управление заслонкой сервой). Он также содержит следующий набор датчиков на базе UAVCAN: датчик воздушной скорости, барометр, приёмник GPS и компас, лазерный высотомер, датчик уровня топлива, датчики системы электропитания, обратная связь от ESC и ДВС (обороты). В симуляторе мы стремились создать наиболее близкую виртуальную модель данного БПЛА как по составу так и по динамике. Также в качестве альтернативного примера поддерживается модель динамики коптера на базе FlightGoggles.
Масса, кг | 7.0 |
Площадь крыла, м2 | 0.37 |
Длина, м | 1.5 |
Крейсерская скорость, м/с | 16 |
Максимальная воздушная скорость, м/с | 40 |
Динамика
Что касается динамики, то в общем случае для ROS есть неплохой инструмент — Gazebo. Это такой конструктор моделей, в нём в качестве базовых блоков есть и двигатели и твердое тело и аэродинамика. Но, если хочется большей кастомизации или лучшей графики, то он не годится.
К примеру, аэродинамика в Gazebo моделируется двумя линейными характеристиками. Мы в этом вопросе немного больше заморачивались для создания более физичной модели именно нашего аппарата (см. ниже) и нам нужны более сложные нелинейные зависимости. К тому же, у нас есть Innopolis Simulator с крутой графикой.
Так что от Gazebo мы отказались и сделали свою симуляцию динамики.
В общем случае в основе математического описания летательного аппарата лежит модель твёрдого тела, которая состоит из дифференциальных уравнений кинематики и динамики. Уравнения кинематики описывают, как изменяются координаты при заданных скоростях.
Динамика отвечает за изменение скоростей под воздействием сил и моментов. Это немного сложнее законов Ньютона и уравнения поступательного движения материальной точки из-за того, что учитывается вращение, но ассоциации годные.
Индивидуальность моделей разных аппаратов заключаются в параметрах (массо-инерционные характеристики) и силах, которые действуют на БПЛА:
управляющие воздействия от органов управления;
аэродинамика;
сила тяжести и плавучесть (если хочется экзотики).
Аэродинамика
Аэродинамическая сила представляет собой сумму подъемной силы, силы сопротивления и силы бокового скольжения, которые могут быть рассчитаны с помощью следующих уравнений соответственно:
Аэродинамический момент делится на 2 составляющие: первая зависит от модуля воздушной скорости и угла атаки, вторая — от воздушной скорости и положения управляющих поверхностей:
Аэродинамические коэффициенты мы получали посредством CFD (computational fluid dynamics). Результатом CFD-исследования являются массивы данных, описывающие основные аэродинамические характеристики рассматриваемого самолета вертикального взлета и посадки с учетом его отклонения по углу атаки и скольжения и отклонения рулевых поверхностей в рассматриваемом диапазоне скоростей. Также исследовались градиенты распределения полей давления, скоростей и турбулентных потоков в окрестности аппарата.
Актуаторы
Тягу движителя можно моделировать как функцию, которая получает скорость двигателя и возвращает силу и момент на основе табличной зависимости (статические характеристик). Эта таблица может быть получена посредством CFD-анализа или экспериментально на примерно таком стенде:
Что касается динамики движителя, можем считать, что передаточная функция регулятора двигателя BLDC и ESC может быть описана как
Модели сенсоров
Для моделирования датчика воздушной скорости и барометра используется модель стандартной атмосферы ISA (InternationalStandard Atmosphere). Акселерометр и гироскоп моделируются в соответствии с выражениями:
UavDynamics (inno_vtol)
inno_vtol — это отдельный узел ROS, который реализует описанную выше математику. Он осуществляет связь через топики, и не имеет значения, какой тип связи со стеком полётного контроллера вы используете. uav_dynamics публикует и подписывается на следующие топики:
№ | имя топика | тип сообщения |
подписка (к inno_vtol) | ||
1 | /uav/actuators | sensor_msgs/Joy |
публикация (от inno_vtol) | ||
1 | /uav/static_temperature | uavcan_msgs/StaticTemperature |
2 | /uav/static_pressure | uavcan_msgs/StaticPressure |
3 | /uav/raw_air_data | uavcan_msgs/RawAirData |
4 | /uav/gps_position | uavcan_msgs/Fix |
5 | /uav/imu | sensor_msgs/Imu |
6 | /uav/mag | sensor_msgs/MagneticField |
7 | /uav/esc_status | uavcan_msgs/EscStatus |
8 | /uav/ice_status | uavcan_msgs/IceReciprocatingStatus |
9 | /uav/fuel_tank_status | uavcan_msgs/IceFuelTankStatus |
10 | /uav/battery_status | sensor_msgs/BatteryState |
Как видите, у нас есть обратная связь от регуляторов подъемных двигателей и ДВС.
uavcan_communicator
uavcan_communicator — это мост между UAVCAN и ROS. Он обеспечивает минимально необходимый набор датчиков для UAVCAN-HITL-симуляции, а также может быть использован и для других целей. В таблицах ниже представлены поддерживаемые преобразования для общего понимания, об обмене какими данными идёт речь:
№ | Имя | Сообщение ROS | Сообщение UAVCAN |
UAVCAN ➔ ROS (Simulator) | |||
1 | Actuators | ||
2 | Arm | ||
3 | AhrsSolution | ||
4 | EscStatus | uavcan_msgs::EscStatus | |
5 | CircuitStatus | uavcan_msgs::CircuitStatus | |
ROS (Simulator) ➔ UAVCAN | |||
1 | BaroStaticTemperature | uavcan_msgs/StaticTemperature | |
2 | BaroStaticPressure | uavcan_msgs/StaticPressure | |
3 | DiffPressure | uavcan_msgs/RawAirData | |
4 | Gps | uavcan_msgs/Fix | |
5 | Imu | ||
6 | Magnetometer | ||
(Отвлеченно в сторону)
Да-да, большая часть кода в робототехнике — про преобразование форматов и пересылку сообщений. Вот есть PX4 с собственными uORB внутри — это раз. Всю переферию мы, благо, свели в UAVCAN, но тут тоже свои форматы — это два. Дальше мы цепляем бортовой компьютер (или симулятор, про который идёт речь) c ROS, тут свои сообщения — это три. Ну и когда мы общаемся с наземным пунктом управления принятым стандартом является MAVLINK, тоже свои отдельные форматы сообщений — уже четыре. Наземный пункт управления написан на Qt, там сигналы и слоты. Если используется Gazebo, у него внутри тоже свои топики. Пять, шесть (и это только навскидку) — ну вы поняли, то ещё «перекладывание джсонов».
Хватает ли пропускной способности CAN-шины?
Для начала необходимо проанализировать ограничение скорости. В нашем случае мы используем CAN-сниффер на базе SLCAN для обмена данными между PX4 на CAN-порту со скоростью 1000 Кбит/с и ПК через последовательный порт с той же скоростью. Это и является ограничением. Посмотрим, сколько данных нужно передавать.
Каждый CAN-кадр содержит 12 байт полезных данных (4 байта ID и 8 байт данных). Хотя базовый не расширенный CAN-кадр имеет в худшем случае длину 134 бита, SLCAN всегда преобразует эти данные в 27 байт, где каждый байт имеет дополнительные стартовые и стоповые биты, поэтому мы можем считать его узким местом нашего потока данных. Учитывая вышесказанное, положим, что производительность SLCAN составляет 100 000 байт/с или 3703 CAN-кадра в секунду. В таблице ниже вы можете увидеть все датчики, участвующие в коммуникации, сколько CAN-кадров им требуется для передачи сообщения, их частоту и процент от максимального объема передаваемых данных.
Данные | Требуемое число CAN-фреймов | Частота | Доля, % |
NodeStatus | 1 | 4 | 0,108 |
RawCommand | 2 | 200 | 10,8 |
StaticTemperature | 1 | 20 | 0,54 |
StaticPressure | 1 | 20 | 0,54 |
MagneticFieldStrength | 1 | 33 | 0,89 |
RawAirData | 3 | 20 | 1,62 |
Fix | 4 | 5 | 0,81 |
RawIMU | 7 | 200 | 38 |
Итого | 22 | 53,3 |
UAVCAN IMU
Хотя автопилот PX4 имеет широкий набор реализованных UAVCAN v0 датчиков, для закрытия необходимого для моделирования минимального набора датчиков UAVCAN нам потребуются дополнительные модули гироскопа и акселерометра. Поскольку PX4 разделяет датчики и аппаратную логику, достаточно легко создать наследников от UavcanSensorBridgeBase, которые подписываются на выбранный стандартный тип данных uavcan.equipment.ahrs.RawIMU и обновляет состояние PX4Accelerometer и PX4Gyroscope. Соответствующий принятый PR в PX4: https://github.com/PX4/PX4-Autopilot/pull/17026.
Итак, из таблицы видно, что в настоящий момент датчики (без IMU) потребляют только 15,3% потока, поэтому мы можем легко использовать IMU с частотой не менее 200 Гц, и он будет потреблять дополнительные 38% потока. Другой вопрос заключается в загрузке CPU, когда мы используем UAVCAN IMU вместо I²C/SPI. Наши тесты на CUAV v5+ в режиме ожидания с активированными датчиками показали, что UAVCAN IMU с частотой 200 Гц потребляет меньше CPU, чем стандартный внутренний IMU на шинах I²C/SPI: 43,4% против 64,8%. В то же время следует отметить, что внутренние IMU представлены 3 комбинированными гироскопами и акселерометрами (icm20602, icm20689 и bmi055), которые работают на частоте 400 Гц. Производительность можно было бы значительно увеличить, улучшив CAN-адаптер или даже используя CAN-FD вместо стандартного CAN, но поскольку мы пока не используем UAVCAN IMU в реальном полете, а для моделирования достаточно частоты 200 Гц, этот вопрос оставляем за скобками.
inno_sim_interface
Для трёхмерной реалистичной визуализации, симуляции камер и лидаров используем интерфейс подключения к Innopolis simulator: inno_sim_interface.
Как тестануть?
Проект довольно нишевый, предназначен для разработчиков, работающих с PX4 и использующих (или планирующих использовать) UAVCAN. В связи с этим важно любое ваше участие. Скачивайте, пробуйте, пишите Issues и комментарии, присылайте PR-ы. Далее краткая инструкция, какие есть варианты запуска, и что для этого нужно, со ссылками на readme.
Вариант True HITL
Нужно из железа:
PX4 — мы используем и рекомендуем CUAV V5+;
USB-UAVCAN — у нас есть свои, обращайтесь, если нужно. Подойдут, думаю, и от OlliW и Zubax и даже Kvazer;
компьютер с Ubuntu (мы тестировали на 18.04, на 20-й тоже должно работать) и ROS (melodic — проверено или noetic — должно завестись). Для запуска 3d-визуализатора есть доп. требования по производительности компьютера;
Клоним и билдим по инструкции https://github.com/InnopolisAero/innopolis_vtol_dynamics#2-installation-and-building;
Прошиваем PX4 (нашим Firmware);
Запускаем https://github.com/InnopolisAero/innopolis_vtol_dynamics#3-usage-example.
Вариант SITL — просто пощупать
Тут гораздо проще — вам не нужно ничего кроме компьютера, см. инструкцию и пропускайте всё, что касается HITL, а что касается SITL, соответсвенно, запускайте. Так получится потестить динамику и 3d-визуализацию.
Profit
Уже на этапе тестирования до того как мы опубликовали проект у нас появился кейс отлавливания бага в прошивке PX4. @PonomarevDA заметил при работе с новой прошивкой, что после нескольких минут использования UAVCAN-датчика дифференциального давления (симулируемого или реального), его фильтрованное значение всегда становится нестабильным. innopolis_vtol_dynamics стал удобным инструментом чтобы тестить фиксы.
Credits
Команда RaccoonLab благодарит вас за внимание!
Наш инстаграмчик: https://www.instagram.com/raccoon.lab/, велком. Мы ценим ваши лайки и подписки.
О том, что у нас еще есть по теме (в том числе девайсы): http://raccoonlab.org/uavcan.
Авторы проекта:
Дмитрий Пономарёв @PonomarevDA — большая часть реализации за ним и плюсы ему, плиз;
Константин Бурдинов @sainquake и Айрат Биляев — электроника;
Андрей Маевский — аэродинамика;
Илья Тарасов — моделирование;
Дмитирий Девитт @GigaFlopsis — лётные тесты;
Роман Федоренко — втянул всех в эту движуху и написал статейку.
Благодарности:
Сергею Копылову и Top dev team за виртуальный Иннополис и за то, что там есть наши БПЛА;
Павлу Кириенко @Spym за создание UAVCAN, развитие сообщества и моральную поддержку наших проектов.
