Библиотека PID регулятора для Arduino v3.3 | AlexGyver

Базовые понятия

Квадрокоптеры бывают разные, но всех их объединяют четыре несущих винта:

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

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

Мы будем летать на раме квадрокоптера формы «X», потому что она мне больше нравится внешне. У каждой конструкции свои плюсы и свое предназначение. Кроме квадрокоптеров есть и другие мультикоптеры. Даже если не считать экзотические варианты, все равно их видов — целая куча!

Разберемся, как наш квадрокоптер устроен внутри, и чем же должен заниматься полетный контроллер, который мы планируем программировать.

Углы тангажа, крена и рыскания (pitch, roll, yaw) — углы, которыми принято определять и задавать ориентацию квадрокоптера в пространстве.
Иногда слово «угол» опускают и просто говорят: тангаж, крен, рыскание. Но согласно Википедии это не совсем точно. Полет квадрокоптера в необходимом направлении достигается изменением этих трех углов. Например, чтобы полететь вперед квадрокоптер должен наклониться за счет того, что задние моторы закрутятся чуть сильнее передних:

Газ квадрокоптера — среднее арифметическое между скоростями вращения всех моторов. Чем больше газ, тем больше суммарная тяга моторов, тем сильнее они тащат квадрокоптер вверх (НЕ ВПЕРЕД!!! «Тапок в пол» здесь означает наискорейший подъем). Обычно измеряется в процентах: 0% — моторы остановлены, 100% — вращаются с максимальной скоростью. Газ висения — минимальный уровень газа, который необходим, чтобы квадрокоптер не терял высоту.

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

Режимов полета существует много. Используется и GPS, и барометр, и дальномер. Но мы хотим реализовать базовый — режим стабилизации (stab, stabilize, летать в «стабе»), в котором квадрокоптер держит те углы, которые ему задаются с пульта не зависимо от внешних факторов. В этом режиме при отсутствии ветра квадрокоптер может висеть почти на месте. Ветер же придется компенсировать пилоту.

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

Скоростью вращения моторов управляет полетный контроллер (контроллер, ). Обычно это небольшая плата или коробочка с множеством входов и выходов. Существует огромное количество различных контроллеров с разным набором возможностей, разными прошивками, разными задачами. Вот лишь некоторые:

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

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

Благодаря им полетный контроллер узнает текущие углы тангажа, крена и рыскания. Эти датчики бывают встроенными в полетный контроллер, а бывают внешними. Процесс вычисления трех углов по показаниям датчиков — тема для отдельной статьи. Но нам этого здесь знать не надо: за нас все сделает MPU-6050.

Моторы на мультикоптерах потребляют большие токи, поэтому полетный контроллер управляет ими не напрямую, а через специальные аппаратные драйвера, называемые регуляторами скорости (ESC, ре́гуль, е́ска).

«Протокол» общения между регулятором и мотором нам не так важен, как «протокол» общения между полетным контроллером и регулятором, ведь нам предстоит из контроллера программно управлять регулятором. Бывают регуляторы, управляемые по i2c, но наиболее распространенные управляются сигналом прямоугольной формы с минимумом 0 вольт и максимумом 3-5 вольт (его называют ШИМ или PWM, а некоторые утверждают, что правильнее — PPM. Подробнее, например, здесь).

«Протокол» — это громко сказано: чтобы дать команду мотору вращаться с максимальной скоростью контроллер должен отправлять импульсы длительностью 2 миллисекунды, перемежающиеся логическим нулем длительностью 10 — 20 миллисекунд. Длительности импульса в 1 миллисекунду соответствует остановка мотора, 1.

При всей кажущейся простоте, здесь кроется засада: полетные контроллеры бывают разные с разными настройками, регуляторы бывают разные, и минимум (1 мс) и максимум (2 мс) — не универсальны. В зависимости от множества факторов диапазон 1-2 мс может на деле оказаться 1.1 — 1.9 мс.

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

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

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

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

Итак: во время калибровки радио полетный контроллер запоминает диапазоны приемника по всем каналам управления; во время калибровки регуляторов диапазон полетного контроллера заносится во все регуляторы.

Помимо программы для полетного контроллера необходима еще одна программа: интерфейс настройки полетного контроллера. Чаще всего им является программа для PC, которая соединяется с полетным контроллером по USB и позволяет пользователю настраивать и проверять полетную программу, например: запускать калибровку радио, настраивать параметры стабилизации, проверять работу датчиков, задавать маршрут полета на карте, определять поведение мультикоптера при потере сигнала и многое другое. Мы свой интерфейс настройки будем писать на C и Qt в виде консольной утилиты. Вот она, если заглянуть в будущее:

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

Поэтому полетный контроллер должен обеспечивать хоть какую-то безопасность: механизм armed/disarmed. Состояние квадрокоптера «disarmed» означает, что моторы отключены и даже команда полного газа с пульта не имеет никакого эффекта, хотя питание подано.

Состояние «armed» квадрокоптера означает, что команды с пульта выполняются полетным контроллером. В этом состоянии квадрокоптеры взлетают, летают и садятся. Квадрокоптер включается и должен сразу попасть в состояние disarmed на тот случай, если невнимальельный пилот включает его, когда стик газа на пульте находится не в нуле.

Чтобы перевести коптер в состояние «armed» пилоту необхоимо сделать какой-то заранее оговоренный жест стиками пульта. Часто этим жестом является удержание левого стика в правом нижнем углу (газ = 0%, рыскание = 100%) втечении пары секунд. После этого полетный контроллер делает хотя бы минимальную самопроверку и при ее успешном прохождении “армится” (к полету готов!)

Математика стабилизации, пид-регуляторы (pid)

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

Вы приобретаете двухосевой подвес для камеры, ставите туда, например, GoPro, включаете и вместо стабилизации получаете конвульсии, вибрации и дергания, хотя все датчики откалиброваны и механические проблемы устранены. Причина — неверные параметры ПИД-регуляторов.

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

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

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

Будем рассматривать квадрокоптер в двумерном пространстве, где у него есть только один угол — угол крена, и два мотора: левый и правый.

В полетный контроллер непрерывно поступают команды с земли: «крен 30 градусов», «крен -10 градусов», «крен 0 градусов (держать горизонт)»; его задача — как можно быстрее и точнее их выполнять с помощью моторов с учетом: ветра, неравномерного распределения веса квадрокоптера, неравномерного износа моторов, инерции квадрокоптера и т.п.

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

На Adruino вполне можно одну итерацию цикла обработки и управления уместить в 10 миллисекунд. Это значит, что раз в 10 миллисекунд будут считываться показания углов квадрокоптера, и на их основе будут отправляться управляющие сигналы к моторам. Эти 10 миллисекунд называют периодом регулирования. Понятно, что чем он меньше, тем чаще и точнее происходит регулирование.

Уровень газа поступает из приемника в контроллер. Обозначим его . Напомню, что это среднее арифметическое между скоростями вращения всех моторов, выраженное в процентах от максимальной скорости вращения. Если и — скорости вращения левого и правого моторов, то:где — реакция квадрокоптера (усилие), которое создает момент вращения за счет того, что левый мотор вращается на быстрее, чем газ, а правый — на столько же медленнее. может принимать и отрицательные значения, тогда правый мотор закрутится быстрее. Если мы научимся вычислять эту величину на каждой итерации цикла обработки, значит мы сможем управлять квадрокоптером. Понятно, что как минимум должно зависеть от текущего угла крена () и желаемого угла крена (), который поступает с пульта управления.Представим ситуацию: поступает команда «держать горизонт» ( = 0), а квадрокоптер имеет крен влево: — разность (ошибка) между и , которую контроллер стремится минимизировать.

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

Здесь P — коэффициент пропорциональности. Чем он больше, тем сильнее будет реакция, тем резче квадрокоптер будет реагировать на отклонение от требуемого угла крена. Эта интуитивно понятная и простая формула описывает работу пропорционального регулятора.

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

где D — настраиваемый коэффициент: чем он больше, тем сильнее останавливающее усилие. Из школьного курса физики всплывают смутные воспоминания, что скорость изменения любой величины — производная этой величины по времени:

И вот пропорциональный регулятор превращается в пропорционально-дифференциальный (пропорциональное слагаемое и дифференциальное):

Ошибку вычислить легко, ведь на каждой итерации мы знаем и ; P и D — настраиваемые перед запуском параметры. Для вычисления производной (скорости изменения ) необходимо хранить предыдущее значение, знать текущее значение и знать время, которое прошло между измерениями (период регулирования). И вот она — физика шестого класса школы (скорость = расстояние / время): — период регулирования; — значение ошибки с предыдущей итерации цикла регуляции. Кстати, эта формула — простейший способ численного дифференцирования, и он нам здесь вполне подойдет.

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

Необходим механизм, который бы отслеживал такие отклонения и исправлял их. Характерной особенностью таких ошибок является то, что они прявляют себя со временем. На помощь приходит интегральное слагаемое. Оно хранит сумму всех ошибкок по всем итерациям цикла обработки. Как же это поможет? Если пропорционального слагаемого не достаточно, чтобы исправить маленькую ошибку, но она все равно есть — постепенно, со временем, набирает силы интегральное слагаемое, увеличивая реакцию и квадрокоптер принимает требуемый угол крена.Тут есть нюанс. Предположим равна 1 градусу, цикл регулирования — 0.1с. Тогда за одну секунду сумма ошибок примет значение 10 градусов. А если цикл обработки — 0.01с, то сумма наберет аж 100 градусов. Чтобы за одно и тоже время интегральное слагаемое набирало одно и тоже значение при разных периодах регулирования, полученную сумму будем умножать на сам период регулирования. Легко посчитать, что в обоих случаях из примера получается сумма в 1 градус. Вот оно — интегральное слагаемое (пока без настраиваемого коэффициента):Эта формула — не что иное, как численный интеграл по времени функции в интервале от нуля до текущего момента. Именно поэтому слагаемое называется интегральным:

где T — текущий момент времени.Пришло время записать окончательную формулу пропорционально-интергрально-дифференциального регулятора:

где — один из настраиваемых параметров, которых теперь трое: . Эта формула удобна в применении из программного кода, а вот формула, которая приводится в учебниках:

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