Математика на пальцах: ардуино головного мозга или линейно-квадратичный регулятор для управления электродвигателем

Постановка задачи: как со школьными знаниями дойти до выводов университетского уровня

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

Как я уже говорил в предыдущих статьях, мои знакомые студенты хотят построить обратный маятник, но умаялись подбирать коэффициенты ПИД-регулятора, поэтому я неспешно смотрю, что такое линейно-квадратичный регулятор, ну а заодно и вам пересказываю то, что прочитал. Задача для этой статьи — показать, как воплотить в железе одномерный пример из статьи про линейно-квадратичный регулятор. Грубо говоря, я хочу написать написать управление для сервомотора: у меня есть текущее положение оси привода и текущая скорость её вращения, я хочу её остановить в заданном положении. Я попытался было прочитать схожую статью на эту тему, но, признаться, ничего в ней не понял, поэтому сел разбираться самостоятельно, предпочтительно на пальцах и без страшных слов типа дифференциальных уравнений Лагранжа-Эйлера.

Продолжая рабочий эксгибиционизм, знакомлю вас с 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!

Удачи вам и будьте любопытными!