frontwise 3 сен 2021 в 19:10

Как обмануть автопилот PX4: настоящая HITL-симуляция на шине UAVCAN

10 мин

7.1K

Разработка робототехники*Программирование микроконтроллеров*Производство и разработка электроники*РобототехникаМультикоптеры

Статья в формате «Редакция выступила. Что сделано?». Год назад мы рассказывали, как и почему используем стандарт 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), для доставки последней мили, например. Посмотрим на лидеров:

Слева направо: Wing, Volodrone, Wingcopter

Даже по внешнему виду аппаратов понятно, что разработчики думают о повышении надежности за счет избыточности (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 мы отказались и сделали свою симуляцию динамики.

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

$\begin{equation} \begin{split} w_{new} = w_{old} + w' * dt\\ q_{new} = q_{old} + \dfrac{1}{2} * q_{old} * w_{new} * dt\\ v_{new} = v_{old} + v' * dt\\ x_{new} = x_{new} + v * dt\\ \end{split} \end{equation}$

Динамика отвечает за изменение скоростей под воздействием сил и моментов. Это немного сложнее законов Ньютона и уравнения поступательного движения материальной точки из-за того, что учитывается вращение, но ассоциации годные.

$\begin{equation} \begin{split} w' = I^{-1} * ((M_{aero} +{\Sigma}M_{motor i}) - w * (I*w))\\ v' = \dfrac{1}{m} * (F_{aero} +{\Sigma}F_{motor i}) + R*g\\ \end{split} \end{equation}$

Индивидуальность моделей разных аппаратов заключаются в параметрах (массо-инерционные характеристики) и силах, которые действуют на БПЛА:

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

Аэродинамика

Аэродинамическая сила представляет собой сумму подъемной силы, силы сопротивления и силы бокового скольжения, которые могут быть рассчитаны с помощью следующих уравнений соответственно:

$\begin{equation} \begin{split} F_{lift} = \dfrac{1}{2} * q * CL_{\alpha}\\ F_{drag} = \dfrac{1}{2} * q * CD_{\alpha}\\ F_{sideslip} = \dfrac{1}{2} * q * (CS_{\alpha} + CS_{\beta})\\ \end{split} \end{equation}$

Аэродинамический момент делится на 2 составляющие: первая зависит от модуля воздушной скорости и угла атаки, вторая — от воздушной скорости и положения управляющих поверхностей:

$\begin{equation} \begin{split} M_{x} = \dfrac{1}{2} * q * l * (C_{mx} + C_{mxa} * {\delta}_{a})\\ M_{y} = \dfrac{1}{2} * q * l * (C_{my} + C_{mye} * {\delta}_{e})\\ M_{z} = \dfrac{1}{2} * q * l * (C_{mz} + C_{mzr} * {\delta}_{r})\\ \end{split} \end{equation}$

Аэродинамические коэффициенты мы получали посредством CFD (computational fluid dynamics). Результатом CFD-исследования являются массивы данных, описывающие основные аэродинамические характеристики рассматриваемого самолета вертикального взлета и посадки с учетом его отклонения по углу атаки и скольжения и отклонения рулевых поверхностей в рассматриваемом диапазоне скоростей. Также исследовались градиенты распределения полей давления, скоростей и турбулентных потоков в окрестности аппарата.

Актуаторы

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

Что касается динамики движителя, можем считать, что передаточная функция регулятора двигателя BLDC и ESC может быть описана как

$\begin{equation} W_{bldc}(s) = \dfrac{K}{Ts + 1} \end{equation}$

Модели сенсоров

Для моделирования датчика воздушной скорости и барометра используется модель стандартной атмосферы ISA (InternationalStandard Atmosphere). Акселерометр и гироскоп моделируются в соответствии с выражениями:

$\begin{equation} \begin{split} a_{accel} = F_{specific} + b_{accel} + v_{accel}\\ \Omega_{gyro} = w_{body} + b_{gyro} + v_{gyro}\\ \end{split} \end{equation}$

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	sensor_msgs/Joy	esc::RawCommand
2	Arm	std_msgs::Bool	esc::RawCommand
3	AhrsSolution	sensor_msgs::Imu	ahrs::AhrsSolution
4	EscStatus	uavcan_msgs::EscStatus	esc::Status
5	CircuitStatus	uavcan_msgs::CircuitStatus	power::CircuitStatus
ROS (Simulator) ➔ UAVCAN
1	BaroStaticTemperature	uavcan_msgs/StaticTemperature	air_data::StaticTemperature
2	BaroStaticPressure	uavcan_msgs/StaticPressure	air_data::StaticPressure
3	DiffPressure	uavcan_msgs/RawAirData	air_data::RawAirData
4	Gps	uavcan_msgs/Fix	gnss::Fix
5	Imu	sensor_msgs/Imu	ahrs::RawIMU
6	Magnetometer	sensor_msgs/MagneticField	ahrs::MagneticFieldStrength

(Отвлеченно в сторону)

Да-да, большая часть кода в робототехнике — про преобразование форматов и пересылку сообщений. Вот есть 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, развитие сообщества и моральную поддержку наших проектов.

Теги:

Хабы:

Если эта публикация вас вдохновила и вы хотите поддержать автора — не стесняйтесь нажать на кнопку