При сборке квадрокоптеров и других БПЛА обычно используют готовую плату полетного контроллера, содержащую все необходимые датчики и периферию, и готовую полетную прошивку, например, Betaflight, ArduPilot или PX4. Полетный контроллер управляет моторами квадрокоптера и обеспечивает стабильный полет.

В 2019-м году я взялся за достаточно амбициозную задачу — создать квадрокоптер, не используя готовую плату полетного контроллера и готовую прошивку, то есть реализовать вообще все с нуля. А затем описать этот процесс в виде учебника по теории и практике разработки полетных контроллеров БПЛА. Спустя четыре года мне удалось разработать стабильно летающий квадрокоптер на базе микроконтроллера ESP32 и платформы Arduino и даже получить для этого проекта небольшое распространение.

Исходный код проекта и все материалы опубликованы на GitHub: github.com/okalachev/flix. В статье я разберу из чего собран мой дрон и как устроена его прошивка.

Описание проекта

Прошивка полетного контроллера квадрокоптера — это достаточно сложный софт, который может включать передовые алгоритмы из теории автоматического управления. Такие прошивки пишут целые исследовательские группы и компании. Например, исходный код прошивки PX4, даже без учета сабмодулей, содержит порядка 400000 строк кода на С++. Этот код сложен для понимания и не подходит для изучения базовых принципов управления полетом БПЛА.

Я создал прошивку с максимально простым стилем кодирования, чтобы в ней мог разобраться любой, кто слегка знаком с Arduino. И она уже даже была использована в качестве учебного пособия в курсе по программированию полетных контроллеров в университете МАИ.

Сейчас моя прошивка состоит из 17 файлов на C++, в каждом из которых в среднем около 60 строк кода. Прошивку очень легко скомпилировать и загрузить в ESP32 как при помощи Arduino IDE, так и из консоли. Инструкции по сборке находятся в репозитории проекта.

Также я собрал миниатюрный квадрокоптер, который включает все компоненты полетного контроллера, но отдельно друг от друга. Небольшое видео с демонстрацией дрона, который у меня в итоге получился:

Выбор железа

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

Микроконтроллер

Центральным компонентом полетного контроллера является микроконтроллер, и чаще всего это микроконтроллеры семейства STM32. Я же решил взять более нестандартный микроконтроллер ESP32 в виде платы ESP32 Mini. Этот китайский микроконтроллер располагает большими ресурсами, чем тот же STM32F4 и при этом стоит дешевле. Но его главная особенность — это встроенный Wi-Fi и Bluetooth. По Wi-Fi можно организовать коммуникацию между квадрокоптером и наземной станцией: получать телеметрию и отправлять команды, а также управлять им при помощи смартфона, что я и реализовал.

Датчики

Минимальный набор датчиков, необходимый для стабильного полета квадрокоптера — это гироскоп и акселерометр. Эти два датчика (а также иногда магнитометр) обычно объединяются в едином чипе, который называется Inertial Measurement Unit, сокращенно IMU. Для своего квадрокоптера я использовал IMU InvenSense MPU-9250. IMU подключается к микроконтроллеру по шине SPI.

RC-приемник

Для приема сигнала с пульта управления используется стандартный 2.4 ГГц RC-приемник, который связывается с пультом по радиопротоколу Futaba S-FHSS. Приемник подключается к микроконтроллеру через интерфейс Futaba SBUS, который фактически является обычным UART с инвертированным сигналом RX. К счастью, ESP32 поддерживает инверсию сигнала UART аппаратно, а кроме того имеет свободные UART-интерфейсы, так что с подключением приемника проблем не возникает.

Моторы

Я использовал коллекторные моторы типоразмера 8520 (8.5 мм на 20 мм), которые часто используются в миниатюрных квадрокоптерах. Они гораздо дешевле бесколлекторных моторов и просты в управлении — подключаются к микроконтроллеру через полевой транзистор (MOSFET) и управляются ШИМ-сигналом. Впрочем, в первой версии для простоты я все же использовал готовый модуль для управления моторами с AliExpress, но в следующих версиях я планирую от него отказаться и использовать обычный транзистор.

Рама

Для размещения всех компонентов я взял стандартную 100-миллиметровую карбоновую раму с AliExpress, подходящую для используемых моторов. Следующую версию я планирую собрать на собственной раме, напечатанной на 3D-принтере, которая лучше подойдет для размещения вышеописанных компонентов.

Питание

Для питания квадрокоптера используется однобаночный литий-полимерный аккумулятор с номинальным напряжением 3.7 В. Я подобрал аккумулятор с высокой токоотдачей, чтобы тока хватило и на моторы, и на микроконтроллер с Wi-Fi, IMU и RC-приемник. ESP32 питается от 3.3 В, но в платах ESP32 Mini и GY-91 есть встроенные LDO-регуляторы напряжения, которые позволили запитать их напрямую от аккумулятора.

Сборка

Вооружившись паяльником, мотком силиконовых проводов и двухсторонним скотчем, я произвел сборку своего дрона. Процесс занял около недели и выглядел примерно так:

На фотографиях видно микроконтроллер, модуль IMU, RC-приемник и моторы. Все это соединено проводами и закреплено на раме.

Полный список использовавшихся в текущей версии компонентов приведен в соответствущем списке в репозитории проекта. Там же есть ссылка на полную схему подключения компонентов.

После сборки я приступил к самому сложному этапу — разработке и отладке полетной прошивки. И этот этап затянулся на годы. Но в итоге я все-таки пришел к успеху. Ниже я рассмотрю общий алгоритм управления квадрокоптером, и как я его реализовал.

Алгоритм управления квадрокоптером

Квадрокоптер это не самостабилизирующийся летательный аппарат, и для его полета необходим достаточной сложный алгоритм управления. Цель этого алгоритма — поддерживать стабильный полет согласно заданным пилотом командам: например, удерживать заданную ориентацию, скорость или позицию. Для этого применяется алгоритм автоматического управления с обратной связью. Этот алгоритм состоит из подсистемы оценки состояния (estimation) и подсистемы регулирования (control).

Подсистема оценки состояния использует данные с датчиков для того, чтобы максимально точно рассчитать текущее состояние квадрокоптера: угловая скорость, ориентация, линейная скорость, позиция и другие параметры.

Подсистема регулирования принимает на вход текущее состояние и желаемое состояние, которое еще называется уставкой (setpoint). На основе разницы между текущим и желаемым состоянием алгоритм регулирования рассчитывает управляющее воздействие (control action) — сигналы на моторы. Цель управляющего воздействия — наиболее эффективно привести дрон в желаемое состояние. Этот алгоритм выполняется в бесконечном цикле с высокой частотой.

В моей прошивке для реализации этого цикла используется стандартная функция loop из Arduino, в которой последовательно производится чтение датчиков, оценка состояния, регулирование, а также выполняются сервисные функции. В начале итерации вызывается блокирующая функция readIMU, за счет чего частота главного цикла привязана к частоте работы датчика IMU, которая сконфигурирована на 1000 Гц. Главный цикл прошивки находится в файле flix.ino.

Подсистема оценки (файл estimate.ino)

Подсистема оценка реализует классический алгоритм оценки ориентации объекта с использованием гироскопа и акселерометра — комплементарный фильтр.

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

Выходами подсистемы оценки являются отфильтрованная угловая скорость по трем осям (переменная rates) и текущая ориентация квадрокоптера (переменная attitude). Ориентация представлена в виде кватерниона, что является стандартным способом представления ориентации в робототехнике.

Подсистема регулирования (файл control.ino)

Подсистема регулирования принимает на вход состояние дрона и положение стиков на пульте управления пилота. Задачей является расчет управляющих сигналов на моторы.

Стандартный алгоритм управления квадрокоптером включает несколько подуровней, которые еще называются контурами управления. Каждый контур отвечает за определенный аспект полета: позиция, скорость, ориентация, угловая скорость. Все вместе они образуют так называемый каскад, в котором вход каждого последующего контура является выходом предыдущего. Например, чтобы дрон полетел вперед — приобрел линейную скорость, ему необходимо наклониться вперед — поменять ориентацию, а чтобы поменять ориентацию, ему необходимо изменить угловую скорость.

Я реализовал два нижних уровня этого каскада: управление ориентацией и управление угловой скоростью. Это позволяет пилоту летать в режиме стабилизации ориентации. В этом режиме правый стик пульта определяет наклоны квадрокоптера, а левый — общий уровень газа и вращение вокруг вертикальной оси. В разных дронах этот режим может называться режимом удержания горизонта, «LEVEL», «ANGLE» или «STABILIZE». В моем дроне этот режим называется STAB.

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

Управление угловой скоростью

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

Из диаграммы видно, что на квадрокоптер действуют сила гравитации и четыре силы тяги пропеллеров, которые должны ее компенсировать. Но если создать «перекос» по тяге между парами пропеллеров (передние — задние, левые — правые), то можно создать крутящий момент, который будет изменять угловую скорость квадрокоптера:

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

Управление угловой скоростью вокруг вертикальной оси (рыскание) реализуется по-другому — используется тот факт, что каждый из моторов помимо вращения пропеллера немного вращает также и сам квадрокоптер в противоположную сторону. Направление вращения у пропеллеров разное, и если одна диагональная пара пропеллеров вращается быстрее, чем другая, то квадрокоптер будет вращаться вокруг вертикальной оси. На диаграмме синие стрелки показывают крутящие моменты от моторов, которые действуют на квадрокоптер:

В итоге, если мы поместим требуемые крутящие моменты в вектор torqueTarget, а общую требуемую тягу в переменную thrustTarget, то управляющие сигналы на моторы можно рассчитать как простые суммы:

motors[MOTOR_FRONT_LEFT]  = thrustTarget + torqueTarget.y + torqueTarget.x - torqueTarget.z;
motors[MOTOR_FRONT_RIGHT] = thrustTarget + torqueTarget.y - torqueTarget.x + torqueTarget.z;
motors[MOTOR_REAR_LEFT]   = thrustTarget - torqueTarget.y + torqueTarget.x + torqueTarget.z;
motors[MOTOR_REAR_RIGHT]  = thrustTarget - torqueTarget.y - torqueTarget.x - torqueTarget.z;

Эта набор формул называется миксером (mixer) и фактически полностью определяет конфигурацию БПЛА. Например, для мультикоптера с шестью пропеллерами (гексакоптера) миксер будет другим, а основной алгоритм управления останется тем же. Миксер реализован в функции controlTorque.

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

...P-регулятор

В идеальном случае для расчета torqueTarget мы просто считаем ошибку регулирования — разницу между требуемой угловой скоростью и фактической и домножаем ее на некоторый произвольно выбранный коэффициент пропорциональности p, который определит «резкость» реакции квадрокоптера:

torqueTarget = p * (ratesTarget - rates);

Такой алгоритм управления называется пропорциональным регулятором или P-регулятором. Но на практике возникают две проблемы:

• Во-первых, моторы обладают инерцией, и они не могут мгновенно изменить скорость вращения. Из-за этого при управлении с помощью простого P-регулятора возникнут «перестрелы» и осцилляции.

• Во-вторых, все моторы немного отличаются друг от друга и расположены на раме не идеально. Это вызывает статическую ошибку управления, или проще говоря, постоянное нежелательное вращение квадрокоптера вокруг какой-то случайной оси.

Самый простой способ решения этих проблем — это PID-регулятор.

PID-регулятор (файл pid.h)

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

Дифференциальный коэффициент d умножается на производную ошибки регулирования и таким образом реализует упреждение в управлении системой. Это решает проблему инерционности системы.

Интегральный коэффициент i умножается на интеграл ошибки регулирования и решает проблему статической ошибки управления. Через пару секунд полета это значение «накапливается» и становится в точности равным статической ошибке, компенсируя ее.

Упрощенно, PID-регулятор на С++ выглядит так:

integral += error * dt;
control_action = p * error + i * integral + d * (error - prevError) / dt;

Более интуитивно понять суть его работы можно на специально созданной демо-странице.

В итоге, для управления угловой скоростью применяются три независимых PID-регулятора для каждой из осей: rollRatePID, pitchRatePID и yawRatePID. За работу с этими тремя регуляторами отвечает функция controlRates.

Управление ориентацией

Управление ориентацией принимает на вход требуемую ориентацию (которую определяет пилот), текущую ориентацию квадрокоптера и рассчитывает требуемую угловую скорость ratesTarget. Расчет ошибки регулирования здесь сложнее. Для ее расчета используется специальная кинематическая формула, реализованная в функции angularRatesBetweenVectors. Затем этот вектор ошибки регулирования также поступает на вход трех PID-регуляторов: rollPID, pitchPID и yawPID. Правда интегральный и дифференциальный компоненты в них не задействованы (I = D = 0), так как на практике для стабильного полета в них нет необходимости, так что фактически это простые P-регуляторы.

Этот контур реализован в функции controlAttitude. Согласно каскаду, выход этого контура поступает в описанную выше функцию controlRates.

Результат

Объединив вышеописанные подсистемы, а также добавив к ним сервисные функции — чтение данных с датчиков (imu.ino), положения стиков на пульте управления (rc.ino) и, собственно, вывод управляющих сигналов на моторы (motors.ino), в теории мы и получаем стабильно летающий и управляющийся с пульта квадрокоптер. Ну и на практике, спустя много месяцев отладки, мы также его получаем.

Подытоживая, приведу ссылку на полную диаграмму алгоритма, которую я реализовал на языке D2: диаграмма управления полетом.

В репозитории проекта также есть краткий обзор архитектуры прошивки.

Почему разработка шла так долго

Главная проблема разработки прошивки для квадрокоптера — это крайне сложная отладка летающего аппарата. Ведь летящий дрон нельзя «поставить на паузу», чтобы проанализировать его состояние. Большую часть времени разработки дрон вообще не делал ничего похожего на стабильный полет. При попытке взлета он либо мгновенно переворачивался, либо взлетал, входил в дикие осцилляции и тут же врезался в ближайшую стену. Как правило, после нескольких часов такой отладки у меня опускались руки и я забрасывал проект на месяцы.

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

Основные причины, по которым квадрокоптер не летал или летал плохо:

1. Из-за неправильной конфигурации IMU реальная частота обновления данных с гироскопа и акселерометра составляла не 1000 Гц, а 50 Гц. Такой частоты критически недостаточно для стабилизации квадрокоптера.

2. Частота обновления ШИМ-сигнала на моторах также была низкой — 50 Гц. Увеличение частоты до 200 Гц сильно улучшило стабильность полета.

3. В изначальной версии сборки по ошибке были использованы моторы с некорректным напряжением питания — 6 В вместо 3.7 В. В результате чего когда алгоритм квадрокоптера был относительно отлажен, квадрокоптер был катастрофически слаб по тяге и мог летать только первые 20-30 секунд после полной зарядки аккумулятора.

4. Отсутствие фильтрации D-компонента PID-регулятора. D-компонент крайне чувствителен к шумам, поэтому его нужно фильтровать. В изначальной версии алгоритма такой фильтрации не было, что приводило к очень сильным осцилляциям при полете. После добавления low-pass фильтра полет стал стабильным.

5. Многие другие критичные ошибки и неточности в алгоритме управления, которые удалось отладить благодаря симуляции.

Дополнительные функции

Помимо собственно полета с пульта, мой дрон имеет ряд дополнительных функций. Так, он частично реализует коммуникацию по протоколу MAVLink — популярному протоколу для взаимодействия с БПЛА. Это позволяет подключаться к нему по Wi-Fi и просматривать телеметрию при помощи программы QGroundControl. Кроме того, с использованием мобильной версии этой программы можно управлять дроном со смартфона.

Также, как уже было упомянуто, у дрона есть полноценный симулятор, реализованный на основе среды для симуляции Gazebo. Симулятор запускает реальный код прошивки, имитируя работу датчиков и моторов, и частично имитируя API Arduino. Код симуляции и инструкции по его сборке также доступны в репозитории проекта и любой желающий может запустить его на своем компьютере (Ubuntu / macOS).

Впрочем, тема симуляции заслуживает отдельной статьи. Здесь лишь приведу только ссылку на пост в Telegram-канале, который рассказывает об архитектуре симулятора моего дрона.

Блог проекта и будущий учебник

Если вам интересен проект, вы можете подписаться на блог в Telegram, где публикуются все новости проекта, а также посты о разработке полетных контроллеров и БПЛА в целом. Такие, например, как этот пост, где подробно описывается, как на моем дроне был реализован алгоритм выполнения автоматического флипа:

Если статья «зайдет», я могу написать и другие статьи на тему разработки полетных контроллера БПЛА, благо по этой теме есть о чем рассказать. Впрочем, основной контент я планирую размещать в блоге проекта, а также в будущем учебнике по разработке полетных контроллеров, для которого я и создал этот дрон, хотя и для собственного самообразования, конечно, тоже.

Буду рад ответить на любые вопросы и комментарии. Спасибо за внимание!