28.10.2019 19:02
zjor 21 15700 Источник
Статья служит шпаргалкой для тех, кто хочет сделать свой обратный маятник. Здесь описаны проблемы, из-за которых я все переделывал несколько раз, приведен краткий обзор теории, необходимый для понимания, как стабилизировать систему.


Зачем мне это было нужно?


Коротко: хотел расширить свой ЧПУ станок, но что-то пошло не так…
Полная версия истории
Еще с детства хотелось иметь свой ЧПУ станок, т.к. занимался моделями самолетов, где нужно делать много маленьких повторяющихся деталей. Сначала купил готовый DIY наборчик, а потом решил увеличить. Поигрался два месяца, но все же станок маленький, рабочая область была лишь 18 на 10 см, в нем нет сенсоров позиционирования. Решил купить направляющую побольше, поставить концевые выключатели и установить каретку в середину шаговым мотором. Сделал за пол дня, но нельзя же по прямой к мечте идти — к большому ЧПУ, надо усложнить задачу и поставить маятник на каретку, тогда мне это показалось просто, но пришлось вспомнить институтские годы и ознакомиться с ТАУ.


Неудачные попытки


Проект занял почти два года проб и ошибок, перепроектирования, ожидания деталей и неполно прожитых выходных, чтобы желающие повторить сэкономили себе время и нервы, считаю нужным рассказать о неудачных решениях.
  • гироскоп (MPU6050) вместо энкодера — принципиально ничего против, но датчик должен располагаться на вращающемся стержне, это вносит непредсказуемое воздействие и невозможность прокрутить стержень несколько раз вокруг оси.
  • абсолютный энкодер — если это потенциометр, то шум в измерения вносит даже движение проводов (в основном из-за контактов в arduino), 10-ти битный АЦП — это все же мало; если это более дорогой датчик, то считывание происходит по последовательному интерфейсу, а это вносит задержку в систему особенно в совокупности с шаговым мотором.
  • нежесткость системы — в какой-то момент я брал алюминиевую трубку с грузом на конце, при колебаниях каретки, в ней самой начинались сильные колебания, и тут же было непонятно какую систему мы стабилизуем. Надо стремиться к тому, чтобы физическая система была максимально приближена тому, что смоделировано.
  • трение — этим явлением часто пренебрегают, я старался уменьшить его, используя большие колеса каретки и V-slot профили, в отличие от рельсов с ползунками с маленькими шариками, т.к. трение качения обратно пропорционально радиусу.
  • использование шагового двигателя — много времени потратил, пытаясь пойти этим путем, вводит в заблуждение упрощение формул (фактически мы сразу управляем ускорением основания маятника) и простота конструкции (можно забыть про трение в рельсе, энкодер мотора, если предположить, что шаги мотор не пропускает), но… Чтобы точно управлять скоростью, время между шагами должно исчисляться десятками микросекунд, значит о выводе состояния в консоль можно забыть. Без обратной связи нельзя быть уверенным, что мотор не пропустил шаги и скорость действительно такая, как система думает. Не утверждаю, что это тупиковое решение, если кому-то удастся стабилизировать маятник шаговым мотором, буду рад на это взглянуть.


Свободный маятник


Для полноты картины, смоделируем маятник на свободной каретке без трения.

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

$ \begin{cases} L\cdot\ddot\theta + g\cdot{sin(\theta)} - \ddot{x}\cdot{cos(th)} = 0 \\ (m + M)\cdot\ddot{x} + m\cdot\ddot{\theta}\cdot{L}\cdot{cos(\theta)} - m\cdot{L}\dot{\theta}^2\cdot{sin(\theta)} = 0 \end{cases} $


из которых можно найти, как меняется вектор состояния:

$ \begin{cases} \dot\theta = w\\ \dot{w} = \frac{g \cdot{sin(\theta)} + b\cdot{L}\cdot{w^2}\cdot{sin(\theta)}\cdot{cos(\theta)}}{L\cdot(1 + b\cdot{cos^2(\theta))}}\\ \dot{x} = v\\ \dot{v} = b\cdot\frac{L\cdot{w^2}\cdot{sin(\theta)} - g\cdot{sin(\theta)}\cdot{cos(\theta)}}{1 + b\cdot{cos^2(\theta)}} \end{cases} , b = \frac{m}{M + m} $


и смоделировать систему. Код здесь.


Почему система неустойчива?


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

$ \dot{\mathbf{x}} = A\mathbf{x}, \mathbf{x}(t) = e^{At}\mathbf{x}(0) $


Экспонента в степени матрицы выглядит понятней, если перейти в систему координат из собственных векторов, тогда матрица $А$ будет диагональной($D$), и экспонента будет иметь вид:

$ e^{Dt} = \begin{bmatrix} e^{\lambda_1t} & 0 & \dots & 0 \\ 0 & e^{\lambda_2t} & \dots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \dots & e^{\lambda_nt} \\ \end{bmatrix} $


Теперь видно, при наличии собственных значений ($\lambda_i$) с положительной действительной частью, соответствующая компонента вектора состояния будет стремиться в бесконечность, и система развалится. Вышесказанное касается непрерывных систем, более подробно про устойчивость рассказывается в этой видео-лекции.
Проверим так ли это, для обратного маятника. Линеаризуем нашу систему около положения равновесия при $\theta = 0, sin(\theta) \approx \theta, cos(\theta) \approx 1, w^2 \approx 0$:

$ \begin{bmatrix} \dot\theta \\ \dot\omega\\ \dot{x} \\ \dot{v} \end{bmatrix} = \begin{bmatrix} 0 & 1 & 0 & 0 \\ \frac{g}{L(1 + b)}{\theta} & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ -g\frac{b}{1 + b}\theta & 0 & 0 & 0 \end{bmatrix} \begin{bmatrix} \theta \\ \omega\\ x \\ v \end{bmatrix} $


Ненулевые собственные значения имеют вид $\pm\sqrt{\frac{g}{L(1 + b)}}$, таким образом мы убедились в неустойчивости.

Добавляем обратную связь


Теперь на каретку будет действовать сила $f$, одно из уравнений перепишется в виде: $(m + M)\cdot\ddot{x} + m\cdot\ddot{\theta}\cdot{L}\cdot{cos(\theta)} - m\cdot{L}\dot{\theta}^2\cdot{sin(\theta)} = f$, и линеаризованная система примет вид:

$ \begin{bmatrix} \dot\theta \\ \dot\omega\\ \dot{x} \\ \dot{v} \end{bmatrix} = \begin{bmatrix} 0 & 1 & 0 & 0 \\ \frac{g}{L(1 + b)}{\theta} & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ -g\frac{b}{1 + b}\theta & 0 & 0 & 0 \end{bmatrix} \begin{bmatrix} \theta \\ \omega\\ x \\ v \end{bmatrix} + \begin{bmatrix} 0 \\ \frac{1}{L}\frac{1}{2m + M} \\ 0 \\ \frac{1}{2m + M} \end{bmatrix} \cdot{f} $


Теперь система ($\dot{\mathbf{x}} = A\mathbf{x} + Bu$) стала управляемой, в этом можно убедиться, проверив, что ранг матрицы $\begin{bmatrix} B && AB && A^2B && A^3B \end{bmatrix}$ равен размерности вектора состояния, т.е. 4. Для удержания маятника в вертикальном положении я использовал линейно-квадратичный регулятор состояния, т.е. управление (u или f) есть произведение вектора состояния $[\theta, \dot{\theta}, x, \dot{x}]$ на вектор параметров, которые находятся один раз минимизацией квадратичного функционала. Код симуляции здесь.


Управление двигателем


Теперь нужно управлять мотором постоянного тока, он содержит много параметров, которых я не знаю, поэтому я принял его за «черный ящик», описываемый следующими уравнениями с учетом трения:

$ \begin{cases} \dot{x} = v \\ \dot{v} = -a\cdot{v} + b\cdot{U} + c\cdot{sign(v)} \end{cases} $


Про вывод уравнений и оценку параметров можно прочитать здесь. Ниже приведу мои графики разгона мотора с кареткой в зависимости от напряжения (в реальности на выходе контроллера ШИМ-сигнал) и подогнанные кривые.

Коэффициенты модели я также нашел перебором, код.
Таким образом, регулятор дает нам требуемое ускорение, а из 2-го уравнения, зная все константы, найдем напряжение.

Собираем реальное устройство


Теперь у нас есть все знания, чтобы собрать и стабилизировать маятник. Я использовал следующее железо:
  • Arduino Mega 2560 — не UNO, потому что для двух энкодеров нужно 4 пина для прерываний
  • Энкодер для маятника — OMRON E6B2-CWZ6C 2500 пульсов на оборот — дает нам угол, вычисляем угловую скорость, разрешение достаточно высокое, поэтому хватило конечных разностей без сглаживания и усреднения
  • Энкодер для мотора — LPD3806-600BM-G5-24C 600 пульсов на оборот — дает положение каретки, вычисляем скорость
  • DC-мотор на 12V с редуктором 5:1
  • Драйвер мотора 10Amp 5V-30V

Таким образом мы явно измеряем угол маятника, положение каретки, вычисляем угловую скорость маятника и скорость каретки — получаем полное состояние, параметры регулятора я нашел этим скриптом. К удивлению все достаточно быстро заработало как есть. Результатом я доволен, стоит и даже стакан держит!


Код для Arduino находится здесь
Что удалось усовершенствовать по сравнению с многими вариантами, которые можно найти на youtube — этот маятник тихий, потому что ШИМ настроен вне слухового диапазона, и используются пластиковые колеса.
Теперь эта задача выглядит, как лабораторная работа: измерить параметры мотора и найти коэффициенты регулятора, попутно разбираясь в происходящем.

Что дальше?


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

Ссылки




Спасибо за внимание!

Комментарии (21)


  1. Arastas
    28.10.2019 22:18
    #20816368
    +1

    Добро пожаловать в мир ТАУ, тут интересно. :)


    1. zjor Автор
      28.10.2019 22:23
      #20816394

      я еще хотел сделать reinforcement learning, чтобы он сам вставать научился, может в будущем :)


      1. Undiabler
        29.10.2019 01:16
        #20817280
        +1

        Сразу об этом подумал. Работал с таким примером маятника Self-Contained Cartpole Swingup Task
        решаемым, предложенным в том же репозитории, методом эволюционирующих сетей. Учитывая что у вас физическое устройство уже собрано интересно натренировать модель и дать в управление реально устройство.


        1. zjor Автор
          29.10.2019 01:23
          #20817286

          класс! можно попробовать, спасибо за ссылку.


          1. tas
            29.10.2019 15:17
            #20820100

            Вспомнилась похожая тема: habr.com/ru/post/206980


  1. lingvo
    28.10.2019 23:14
    #20816694

    Ключевое слово тут — модель и моделирование. Такие задачки как раз удобно решать на модели в симуляции, а потом, когда все отлажено, генерить готовый код для прошивки. Только контроллер должен быть реал-таймовый, а с Ардуино могут быть проблемы в этом месте.


    1. IronHead
      29.10.2019 12:49
      #20819172

      А чем в вашем понятии реал-таймовый контроллер отличается от ардуино? Точнее чего нет в ардуино, что оно не может выполнять роль реал-тайм контроллера по вашей терминологии?


      1. lingvo
        29.10.2019 15:25
        #20820172

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


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


        Чтобы сделать ее реалтаймовой надо бы:


        • функция loop должна вызываться по прерыванию таймера или с определенной частотой и гарантированно выполняться за время до следующего прерывания.
        • период вызова должен быть выбран таким, чтобы удовлетворял критерию устойчивости и регулируемости системы (чем меньше, тем лучше, ограничение в производительности процессора и I/O)
        • все функции, которые могут привести к не детерменируемому времени исполнения функции, должны быть убраны из этого кода (например printы)
        • все второстепенные функции должны иметь меньший приоритет, чем вызов управляющей функции и не прерывать ее исполнение.
        • Если функция loop не выполняется за указанное время, ватчдог должен сбрасывать контроллер и приводить систему в устойчивое состояние — это отказ.

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


        Если парадигма программирования Arduino так не позволяет сделать — то вот вам и проблема.


        1. IronHead
          29.10.2019 15:31
          #20820202
          +1

          Отлично. Замечания по правилам написания ПО вы написали верно. А ардуино как контроллер (железяка) то чем не угодил так и не объяснили?


          1. lingvo
            29.10.2019 15:47
            #20820310
            -2

            Контроллер для меня — это железо + софт. Если там есть возможность забебехать такой таймер, есть ватчдог, а ввод/вывод сделан на детерминированных функциях/регистрах, то железом может быть и Ардуино.


  1. livsius
    29.10.2019 01:09
    #20817264

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


  1. tbl
    29.10.2019 01:30
    #20817304

    гироскутеры на этом же принципе работают?


    1. anprs
      29.10.2019 07:33
      #20817732

      Да


  1. eugenk
    29.10.2019 11:42
    #20818698

    Спасибо. Очень качественное изложение. И очень качественный гитхаб проекта. Скачал себе немного поиграться :)


  1. andi123
    29.10.2019 13:45
    #20819522

    кто хочет сделать свой обратный маятник.

    А можно узнать, зачем хотеть его делать? Честно, интересно.
    Я пока вижу два варианта:
    1. Игрушка (как на видео)
    2. «сегвей».
    Или есть еще что-то?


    1. tmin10
      29.10.2019 14:59
      #20819952

      Балансирующий робот, хотя можно в раздел игрушки записать


  1. DirectX
    29.10.2019 15:48
    #20820320

    Кстати по данной теме есть еще интересный пример — обратный маятник Капицы ru.wikipedia.org/wiki/%D0%9C%D0%B0%D1%8F%D1%82%D0%BD%D0%B8%D0%BA_%D0%9A%D0%B0%D0%BF%D0%B8%D1%86%D1%8B, когда равновесие в верхней точке становится устойчивым при приложении колебаний к точке подвеса в вертикальном направлении. Это как раз иллюстрирует пример из приведённого видео, когда за счёт некоторых доработок неустойчивую систему можно скорректировать youtu.be/h7nJ6ZL4Lf0?list=PLMrJAkhIeNNR20Mz-VpzgfQs5zrYi085m&t=464


  1. Alexey_mosc
    29.10.2019 17:13
    #20820852
    -1

    Это же физическая модель популярного бенчмарка для задачи обучения с подкреплением. CartPole. Там суть в том (если вы вдруг не интересовались), что, грубо говоря, по фотографии этой тележки (снимок с экрана / пиксели) НС дает команду влево/вправо/на месте. То есть все происходит внутри нейросетевой функции.


  1. SlavaT
    29.10.2019 17:48
    #20821130

    Я бы посмотрел в учебники типа "Автоматизированный электропривод типовых производственных механизмов и технологических комплексов", там и без обратной связи есть автоматизированные схемы управления и уравнения двигателей разнообразных.


  1. prilichny
    29.10.2019 17:48
    #20821132

    оно собиралось исключительно в учебных целях? чтобы теорию с практикой сопоставить? или для чего-то еще?


  1. teemour
    29.10.2019 21:01
    #20822082

    апладисменты