Математика на пальцах: ардуино головного мозга или линейно-квадратичный регулятор для управлением электродвигателем
Эта статья предполагает, что вы прочли мои статьи (ну или и без того знаете) про методы наименьших квадратов и про линейно-квадратичный регулятор.
Как я уже говорил в предыдущих статьях, мои знакомые студенты хотят построить обратный маятник, но умаялись подбирать коэффициенты ПИД-регулятора, поэтому я неспешно смотрю, что такое линейно-квадратичный регулятор, ну, а заодно и вам пересказываю то, что прочитал. Задача для этой статьи — показать, как воплотить в железе одномерный пример из статьи про линейно-квадратичный регулятор. Грубо говоря, я хочу написать написать управление для сервомотора: у меня есть текущее положение оси привода и текущая скорость её вращения, я хочу её остановить в заданном положении. Я попытался было прочитать схожую статью на эту тему, но, признаться, ничего в ней не понял, поэтому сел разбираться самостоятельно, предпочтительно на пальцах и без страшных слов типа дифференциальных уравнений Лагранжа-Эйлера.
Продолжая рабочий эксгибиционизм, знакомлю вас с Bubble Bobble, который живёт у нас с коллегой в кабинете. Он рецензирует статьи для конференции SIGGRAPH.
Инструкция строго одна: будем доброжелательны друг к другу.
Я неспособен написать стройную последовательность формул без ошибок. Абсолютно все мои формулы неверны. Но лично я способен по неправильной формуле восстановить правильную, так как у меня каждый символ имеет свою семантику. Если вы манипулируете математикой как просто набором грамматических правил над некоторым алфавитом, нам с вами не по пути. Лично я не могу оперировать ни одним понятием, если у меня нет «картинки» в голове, некой интуиции о природе рассматриваемого феномена. Если вы видите у меня ошибку, скажите мне об этом, я буду только рад. Заранее спасибо!
Nothing clears up a case so much as stating it to another person.
*— Sherlock Holmes, Silver Blaze*
Или русский вариант:
Разговор 2-х преподавателей:
— Ну и группа мне в этом году попалась тупая!
— А что так?
— Представляешь себе, объясняю теорему — не понимают! Объясняю второй раз — не понимают!!! В третий раз объясняю. Сам уже понял. А они не понимают…
На хабрахабр я пришёл за тремя вещами:
1. Чтобы поделиться знаниями с людьми, знающими меньше меня.
2. Это требует структурирования информации, таким образом, я ещё лучше усваиваю то, что понял.
3. Чтобы получить ценные комментарии от людей, которые знают больше меня (к слову, большинство знаний, что я почерпнул для этой статьи, мне передал уважаемый Arastas).
Каждая моя статья — это плод размышлений как минимум нескольких дней, как минимум нескольких часов написания текста и возможно нескольких часов написания исходного кода, так как по возможности я стараюсь сопровождать свои статьи программами. Я плохо отношусь к тем, кто с места в карьер мне говорит, что я идиот. Спасибо, я это и сам знаю, новой информации в этом нет, а неуважение к моей работе есть. Я крайне приветствую любые комментарии, содержащие конструктивную критику. Если я сказал глупость, то напишите мне, где именно и почему. Это поможет мне поправить текст, увеличив мои знания и знания других читателей. Я также приветствую любые конструктивные вопросы и никогда не отношусь высокомерно к людям, которые знают меньше меня, именно поэтому требую симметричного отношения от тех, кто знает больше меня.
Я хочу увеличить количество людей с хорошим настроением и высокой кармой на хабре (к тому же, как мы знаем, её жёсткий дефицит). Поэтому я щедро раздаю плюсы комментариям и в карму, но при этом не стесняюсь выдавать и минусы, последнее касается только высокомерных и/или невежливых людей.
Посмотрите сюда, чтобы понять, чего именно я бы не хотел видеть в комментариях. Мне не нужны вопли про быдлохабр и высказывания о том, что я дебил. Да, лично я не умею рассчитать токоограничительный резистор для светодиода. Я умею ограничить сверху его номинал. Да, я приму сопротивление светодиода за ноль и получу безопасное значение для резистора. И да, я знаю, что светодиод бесплатно светиться не может. Когда будет нужно, я разберусь в деталях, а пока грубая прикидка сверху, которая гарантированно не спалит мою плату — это прекрасно. Если вы видите неточность или просто ошибку, поправьте её, всем будет польза.
В распоряжении имеется оптический энкодер (1000 пульсов на оборот), электродвигатель (750 rpm при 12В без нагрузки), драйвер L6201 и atmega 328 (arduino nano).
В зависимости от PWM сигнала, что я подаю на вход L6201, он выдаёт разное напряжение на электродвигатель. То есть, мой микроконтроллер умеет управлять только напряжением на клеммах электродвигателя.
Совсем недавно я уже говорил о том, что из себя представляют уравнения Максвелла. Давайте повторим: уравнений Максвелла четыре по количеству следующих законов:
1. Закон Гаусса, «на пальцах» это просто закон сохранения: энергия из ниоткуда не берётся и в никуда не уходит.
2. Закон Гаусса для магнитного поля — то же самое, только для магнитного, а не электрического поля.
3. Закон Фарадея: если мы двигаем магнитами, то они порождают электрическое поле.
4. Закон Ампера: если мы двигаем электрическим полем, то порождаем магнитное.
Векторные поля, являющиеся решением этих четырёх уравнений, называются электрическим полем и магнитным. В статье про магнитную левитацию я интересовался в основном законами Гаусса, в данной статье мне интересны законы Ампера и Фарадея. Как вообще работает электродвигатель постоянного тока? Мы через обмотки пропускаем ток, он создаёт магнитное поле (см. закон Ампера). Это и заставляет вращаться ротор нашего двигателя.
Давайте попробуем представить, как работает такой двигатель. Пренебрегаем всем чем только можно (индуктивность, трение и прочее) и вспоминаем только курс седьмого-восьмого класса школы, а именно — закон Ома. Итак, закон ома гласит, что напряжение — это произведение силы тока на сопротивление (U=IR). Сопротивление обмоток двигателя постоянно, поэтому, с точностью до постоянного множителя, напряжение на обмотке и ток через неё — это одно и то же. Далее, закон Ампера говорит, что сила, прикладываемая к ротору, пропорционалена пропускаемому току, а значит, и нашему напряжению. То есть, если я живу в моём придуманном мире, то подав постоянные 12В на обмотки двигателя, я создаю некую постоянную силу (момент).
Но наш двигатель вполне подчиняется второму закону Ньютона (F=ma). Таким образом, если у меня на обмотке двигателя постоянное напряжение, то это влечёт за собой постоянное ускорение вала двигателя (масса-то не меняется!). А вот этот вывод начинает уже совсем плохо пахнуть, ведь если у меня есть постоянное ускорение, то я так и скорость света преплюнуть могу…
Тут самая пора вспомнить о том, что есть вариант не подавать напряжение на двигатель, а крутить его вал, наоборот снимая напряжение с него (см. велогенератор). Это следствие закона Фарадея: если мы крутим магнитами, то это порождает электромагнитную индукцию. «Напряжение» (строго говоря, ЭДС) прямо пропорционально оборотам двигателя: чем быстрее мы его крутим, тем больше ЭДС (U = C * v, где U — это напряжение, C — некая константа для нашего двигателя, а v — это скорость вращения вала).
И ведь какая заковыка: если мы-таки подаём напряжение на двигатель, то ротор крутится (закон Ампера), но при этом сам генерирует ЭДС (закон Фарадея), которая борется с поданным напряжением, уменьшая его! Таким образом, закон Ома для двигателя будет выглядеть скорее как U — I*R — C*v = 0.
Закон Ампера нам гласит о том, что ток пропорционален силе, создаваемой магнитным полем. А второй закон Ньютона гласит о том, что сила пропорциональна ускорению (а ускорение — это производная скорости по времени!). Таким образом, закон Ома для двигателя можно записать как u (t) — const1 * v'(t) — const2 * v (t) = 0. В дискретном мире производную v'(t) можно представить как v'(t) = v (t+1)-v (t), таким образом, закон Ома можно записать как v (t+1) = a*v (t) + b*u (t), где a и b — это константы, зависящие от физических параметров двигателя и от временного шага.
Я подал на остановленный двигатель четыре разных постоянных напряжения (24В, 18В, 12В, 6В) и записал скорость передвижения каретки при помощи инкрементального энкодера. Вот так выглядели испытания:
Вот эта картинка даёт четыре разных графика (некрасивые, зубчатые) для четырёх напряжений:
А также она даёт теоретическую кривую (я нашёл a=0.97, b=0.218) для этих напряжений. Параметры a и b найдены при помощи крайне тупого кода фиттинга. Разумеется, в реальном мире трение ненулевое, поэтому моя теоретическая кривая не совпадает со всеми измерениями, но постольку, поскольку я предполагаю, что моя каретка будет чаще двигаться с напряжением в районе нуля, а не в районе максимума, то теоретическая кривая лучше аппроксимирует движение при малых напряжениях.
Внимание: тут предполагается, что вы хорошо прочли мою статью про линейно-квадратичный регулятор.
В ней наш пример моделировался следующей системой уравнений:
тут x_k — это положение каретки, v_k — её скорость, а u_k — её ускорение. Из предыдущей секции мы знаем, что ускорение очень нелинейно зависит от приложенного напряжения, и меня это слегка напрягает, так как микроконтроллер умеет напрямую управлять только напряжением. Но при этом эта зависимость (при нулевом трении) экспоненциальная, что очень хорошо входит в рамки линейной системы перехода! Таким образом, я моделирую систему следующим образом:
тут x_k — это положение каретки, v_k — её скорость, а u_k — это уже не ускорение каретки, а напряжение, которое я прикладываю к клеммам электромотора. Для конкретно моего двигателя я нашёл a = .97, b = .218.
Вот код, который находит коэффициенты зависимости переменной управления (напряжение на клеммах) от вектора состояний (положение и скорость каретки). У меня каретка может максимально отклоняться от нулевого состояния на 245 мм, поэтому я поставил начальную нулевую скорость и положение 245 мм. Задача состоит в том, чтобы остановить каретку в нулевом положении (центр рельса).
Строчки 17–53 задают жесткие условия нашей системы (начальные и конечные условия, линейный переход состояний), а строчки 55–70 — оптимизируемую функцию. Основная задача — сходимость положения и скорости к нулю (строчки 56 и 61). Я постепенно увеличивал ограничение на управление (строчка 66), покуда результат не вписался в физические параметры системы (при данных параметрах напряжение не выходит за 24В).
Вот так выглядят теоретические кривые положения, скорости и напряжения, которое необходимо для их достижения:
Таким образом, вышеозначенный код говорит, что напряжение, которое необходимо приложить, может быть рассчитано как:
где x_k — это текущая позиция каретки, а v_k — её скорость.
Итак, самая последняя формула и есть наиглавнейшая для этой статьи, вот мой код для непосредственно управления кареткой.
Давайте я его приведу её основную часть:
void loop() {
vi = xi-xi_1;
int ui = 255.*(-0.000973669*xi -0.0563218*vi)/24.;
xi_1 = xi;
set_speed(ui);
delay(2);
}
Подсчёт значений экодера (функция ISR) заслуживает отдельной небольшой статьи, это не суть важно на данный момент. Что важно — это функция loop (). Я считаю текущую скорость как разность положений энкодера сейчас и две миллисекунды назад; затем текущее напряжение ui подсчитывается по только что приведённой формуле. Всё! Вот и весь тот линейно-квадратичный регулятор.
Неплохо бы сравнить то, что мы натеоретизировали, с тем, что получается на практике. Вот эта картинка даёт сравнение:
Таким образом, реальное положение каретки слегка отличается от теоретического, я полагаю, что это из-за неучтённого трения: чем ближе к нулю, тем меньше подаваемое напряжение, и однажды оно уже не может бороться с трением. Борьба с этим феноменом будет темой одной из следующих статей. Stay tuned!
Удачи вам и будьте любопытными!
