http://habrahabr.ru/company/technoworks/blog/216437/

Большинство проектов, использующих коптеры, опираются на ручное дистанционное управление, полностью автономных систем пока нет. Но для индустриального использования это необходимо; человеческий фактор — причина большинства аварий. Ниже рассказ пойдёт про то, как мы делали свою систему стабилизации с помощью ПИД, позволяющую свести к минимуму участие человека в процессе работы дрона.

^{Один из тестовых полётов нашего коптера}

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

ПИД

Простейшая задача стабилизации — контролировать угол наклона, но из законов динамики вращательного движения следует, что непосредственно управлять можно лишь его второй производной. Самый легкий способ повлиять на нужную величину — использовать ПИД.


^{MEMS — в нашем случае это гироскоп, акселерометр и магнитометр; DMP — Digital Motion Processor; ESC — контроллер оборотов бесколлекторных моторов.}

На вход регулятора подаётся обобщённая координата (в нашем случае угол), на выходе мы получаем момент сил (вторая производная угла). Каждый ПИД-регулятор стабилизирует значение одной обобщённой координаты. Мы используем три ПИД с постоянными коэффициентами, по одному на каждый угол Таита-Брайана.

Матчасть

Определим невязку — разницу между требуемым и реальным значением некоторой величины:

 — требуемое значение величины (угол с джойстика),
 — текущее значение величины (угол с датчика).

Зададим момент сил для угла в текущий момент времени:

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

В чём смысл этой формулы? Напишем уравнение динамики, положив .


 — момент инерции.

Для простоты уберём интегральную составляющую (). После переобозначения коэффициентов получим уравнение затухающих колебаний относительно :


где


Т. е. чем больше пропорциональная составляющая, тем более «резкой» будет реакция на воздействие (больше амплитуда). Чем больше дифференциальная составляющая, тем быстрее будет происходить затухание (больше декремент).

Из модели затухающих колебаний получаем выражение для коэффициента затухания:



Из возможных решений уравнения нам подходит режим, близкий к критическому (граница апериодичности, ) — нет отрицательного «перелёта» графика, переходный процесс короткий. Как видно, критический режим задается всего одним соотношением на коэффициенты ПИД-регулятора.

Интегральная составляющая устраняет статическую ошибку. Пусть невязка невелика и постоянна, тогда дифференциальная составляющая нулевая, а пропорциональная может быть настолько мала, что для её учёта не хватит точности ESC. В этом случае коптер не будет стремиться к требуемому положению. Но если суммировать малые отклонения , то через какое-то конечное время суммарного отклонения будет достаточно, чтобы компенсировать статическую ошибку. Коэффициент определяет это время: чем больше значение коэффициента, тем раньше система начинает учитывать нарастающую погрешность. При этом слишком большая интегральная составляющая приводит к автоколебаниям.

Более подробный анализ уравнения ПИД-регулятора можно найти в других статьях: раз, два.

Первая авария

Слишком большая дифференциальная составляющая на практике приводит к автоколебаниям, чего не должно быть в теории. Почему? Уберём все составляющие, кроме дифференциальной, и решим уравнение:

 — экспонента, никаких колебаний!

Причина такого эффекта в большом времени реакции системы (и вообще из-за того, что оно ненулевое). Из-за сдвига фаз, вызванного задержкой системы, аргумент функции получает некоторый декремент:



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


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

Компромиссом является установка не слишком маленьких коэффициентов в совокупности с введением ограничения сверху на все три составляющие: пропорциональную, интегральную и дифференциальную.

Стоит сказать, что реальная коррекция в почти горизонтальном положении — около 1–2 попугаев процентов мощности моторов (полётная мощность около 60%).


Рассмотрим решение уравнения второго порядка (1), которое в одном из случаев является затухающей синусоидой.

На практике действительно получается что-то похожее (пример справа). Для демонстрации коэффициенты специально ухудшены для увеличения времени затухания. Оригинальную прошивку ESC пришлось заменить, т. к. она вносила существенную задержку, из-за которой математическая модель плохо описывала реальную систему.


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

Калибровка ПИД

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



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

Хотя такой подход не самый эффективный (мы не знаем «срок годности» коэффициентов количественно и считаем их константами), на практике задача стабилизации коптера в полёте была нами решена. Правда, возникла проблема с управлением, но об этом позднее.

Грабли

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

С одной стороны, существует минимальная мощность, которую уменьшить нельзя, или моторы просто остановятся. С другой, уменьшение мощности может быть необходимо для правильной работы алгоритма. Если мощность (throttle) уменьшить слишком сильно, ПИД может «зашкаливать» в нижнюю сторону. Чтобы решить эту проблему, мы ограничиваем доступные пилоту мощности.


Другая опасность — влияние побочных вибраций от моторов на -составляющую. Они порождают шумы в регуляторе и приводят к возникновению автоколебаний. Мы ограничили угловую скорость снизу по модулю: программный фильтр обнуляет все значения угловой скорости меньше порогового. Но проблема исчезла не до конца, сейчас думаем над более подходящим решением.

Третий подводный камень в том, что применение интегрального члена ПИД может помешать при взлёте с наклонной поверхности. Составляющая накапливается ещё до взлёта, и уже в воздухе аппарат испытывает перекос в обратную сторону. Лишь после пары секунд, за которые коптер успевает набрать существенную линейную скорость, интегральная составляющая уменьшается до значений, приемлемых для полёта. Чтобы решить эту проблему, можно включать интегральную составляющую только после того, как тяга моторов достигнет взлётного значения (около 50% от максимума), при этом коптер уже оказывается оторванным от земли.

Программное обеспечение

На рисунке приведена упрощённая блок-схема программы, исполняемой на контроллере платы стабилизации. Главной частью является цикл. Если хотя бы одно действие в нём не выполняется вовремя, частота перестаёт быть постоянной, и стабилизация работает неверно.

В качестве динамического датчика мы использовали MPU-6050 из-за его вычислительных возможностей. Встроенный процессор (DMP) способен частично обрабатывать данные с датчиков, что позволяет разгрузить центральный контроллер. Но оказалось, что надёжных библиотек для работы с этим устройством под Arduino не существует. Решение jrowberg'а привело к проблемам при использовании на сильно загруженном микроконтроллере. Код в примере опирается на синхронность считывания данных. FIFO-буфер датчика, в который записываются посчитанные величины, переполняется в случае несвоевременного считывания. Поскольку всегда считывается первый элемент из FIFO, то при частичной заполненности появляется задержка между помещением новых данных в FIFO и их обработкой на Arduino. В свою очередь, эта задержка приводит к возникновению автоколебаний. При переполнении буфер приходится очищать: его размер 1024, что не делится на 42 — размер пакета. Поэтому, когда буфер переполняется, в начале FIFO находится часть какого-то постороннего пакета. Иными словами, начиная с определенного момента структура нарушается: начало FIFO не совпадает с началом пакета, и считать корректные данные невозможно.

Кроме этого, понадобилось изменить в библиотеке формулу для преобразования «кватернион ↦ углы» для того, чтобы результат был из полного диапазона углов. Подробности этой проблемы описаны на трекере проекта: https://github.com/it-workshop/Quadrocopter/issues/18.

Бортовая электроника

Основной компонент, который мы разрабатывали самостоятельно, — плата стабилизации. Изначально она была основана на платформе Arduino Uno, потом заменили на более мощную Due, что позволило увеличить частоту ПИД-регуляторов с 40Гц до 66.(6)Гц.

Пропеллеры коптера приводятся в движение компактными бесколлекторными двигателями в связке со стандартными контроллерами оборотов — ESC. Мы используем ESC с изменённой прошивкой.



Для питания всей системы используется литий-полимерный аккумулятор (3S). Из соображений безопасности мы решили сделать систему мониторинга напряжения на аккумуляторе. В штатном режиме использования аккумуляторов система ведёт себя достаточно стабильно. Однако на начальных этапах работы мы наблюдали эффекты, вызванные неоптимальным использованием батарей:

• Вздувшиеся аккумуляторы. Причина в перезаряде и длительном хранении разряженных аккумуляторов. Производители рекомендуют не разряжать силовые аккумуляторы ниже значения 3,3В на каждую банку батареи, что в нашем случае даёт минимальное допустимое напряжение в 9,9В.
• Выключение моторов при низком напряжении. Это особенность реакции большинства прошивок ESC на низкое напряжение, которая может привести к серьёзной аварии — в первый момент выключается только один мотор, остальные продолжают работать.

Для наших целей ESC было решено перепрограммировать. Благодаря использованию прошивки tgy (от SimonK) мы добились уменьшения задержки системы на пути от центрального контроллера до двигателей. В результате компоненты ПИД и угловая скорость стали более синусоидальными, а поведение всей системы приблизилось к поведению математической модели.

Для измерения динамических параметров используются следующие датчики:

• 6-осевой акселерометр-гироскоп InvenSense MPU-6050
• 3-осевой компас Honeywell HMC5883L

Телеметрия

Дистанционное управление реализовано в двух режимах (для обеспечения более гибкого процесса разработки):

1. С помощью модулей xBee Pro в конфигурации «коптер ↔ ПК».
2. С помощью выделенной радиочастоты (2.4ГГц) в конфигурации «пульт ДУ ↦ коптер».

Помимо управления через пульт ДУ происходит пересылка критических данных между коптером и ПК в режиме реального времени, для чего используются xBee Pro и приложение собственной разработки (см. скриншот). На компьютере можно видеть значение углов и угловой скорости, напряжение на аккумуляторе, мощность двигателей. Возможно настраивать коэффициенты ПИД в режиме реального времени. Программа сохраняет все поступающие данные и поддерживает последующее воспроизведение записанных файлов, что помогает при отладке. В случае ЧП мы можем остановить моторы с компьютера.



Данные, пересылаемые между коптером и ПК:

• ПК ↦ Коптер: канал управления (ПК/пульт ДУ), мощность моторов, настройка для включения/выключения стабилизации, коэффициенты ПИД и ограничения;
• Коптер ↦ ПК: углы, угловая скорость, компоненты , , , данные с джойстика (мощность + 3 угла), мощности моторов, напряжение на аккумуляторе.

Благодаря датчику от InvenSense, начальная обработка данных с датчиков происходит на встроенном процессоре (DMP). Мы разгружаем плату стабилизации, которая может использовать в качестве вычислителя даже маломощный AVR-микроконтроллер.

Воспроизводимость результатов

Чтобы создать такое устройство, нужно собрать аналогичную механическую конструкцию, эквивалентную электронную схему и использовать наше ПО.

Код проекта: github.com/it-workshop/Quadrocopter/tree/MPU-6050-Compass (распространяется на условиях лицензии MIT).

Направления для развития

• Процесс тестирования системы стабилизации можно упростить, используя более совершенный стенд. Верёвочный вариант — скорее одноразовое решение, более подходящим был бы жёсткий карданов подвес с тремя степенями свободы.
• Существуют методики автоматизации подбора коэффициентов ПИД-регулятора. Например, основанные на двоичном поиске (метод ветвей и границ). В нашем проекте коэффициенты подбирались вручную.
• Приложение для ПК, используемое для мониторинга и управления, было бы удобнее использовать на планшетном компьютере. В планах портировать приложение на Android или IOS.

Итоги

Главное достижение — отличная команда энтузиастов, способных работать над сложными робототехническими проектами. Мы верим, что всё дело в творческом подходе, возможности для самореализации, а также бесценном практическом опыте, которого всегда не хватает.

Мы создали новый проект системы стабилизации для мультикоптеров. Сейчас мы можем пилотировать квадрокоптер на открытом пространстве. Такие внешние факторы, как ветер, дождь и снег компенсируются автоматически благодаря ПИД-регулятору.

В настоящий момент мы усовершенствуем то, что сделали, и разрабатываем новые функции автоматизации.

Полёт коптера с нашей системой стабилизации

Мы — студенты МФТИ (в своём большинстве), которые в свободное время занимаются проектом на мастерской TechnoWorks. Кроме коптера у нас живут и другие проекты: железные и программные. О них мы расскажем как-нибудь потом. А еще у нас можно придумать и реализовать свою идею (а мы поможем найти людей).

Если есть желание присоединиться к нашей команде, свяжитесь с нами! Мастерская активно расширяется, для новых участников у нас полно творческой и технической работы. И печенек.

technoworks.ru/