Розжовуємо лінійно-квадратичний регулятор для управління оберненим маятником

Преамбула
Продовжую докладний опис використання лінійно-квадратичного регулятора на прикладі управління оберненим маятником. До слова сказати, термін «ЛКР» дуже неточно відображає суть того, що відбувається, як мені вже підказали в коментарях, в українській школі теорії управління цей підхід називається «аналітичним конструюванням регуляторів», що істотно точніше.

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

Отже, це вже четверта стаття, для кращого розуміння того, що відбувається непогано б прочитати попередні три:


Ось фотографія системи (кликабельно):



Використовуються датчики
У мене тільки два джерела інформації про події в системі, це два інкрементальних енкодера, один (1000 імпульсів на оберт) показує положення каретки, другий (2000 імпульсів на оберт) дає положення самого маятника. В попередній статті я вважав імпульси за допомогою самої ж ардуины, але це досить неточно і слабо стійкий до зашумленими показаннями енкодерів. Крім того, це витрачає і без того слабкі обчислювальні ресурси атмеги. Зараз я поставив одну мікросхему HCTL-2032 (приблизно п'ять євро штука на алиэкспрессе), вона вміє рахувати імпульси двох енкодерів, надаючи для цього 32-бітні лічильники. Щоб заощадити ноги ардуины, між лічильником і самої ардуиной варто 74hc165, читає один байт у паралель і віддає цей байт послідовно.

Вимірюємо параметри двигуна ще раз
Порівняно з минулим статтею у мене трішки змінилися параметри системи (інший драйвер двигуна плюс поставив енкодер на каретку), тому вимірюю ще раз параметри системи. Отже, з невеликим кроком (приблизно в один вольт, від 24В до 24В) подаю постійна напруга на двигун і вимірюю зміна швидкості каретки. Швидкість каретки вважається як різниця між сусідніми положеннями каретки ділити на минулий час (про коректність такого підходу пізніше). Використовувався код можна побачити здесь.

По горизонталі-час в секундах, а по вертикалі швидкість руху каретки в метрах в секунду. Зашумлені криві — це результати реального виміру, гладкі — це моя математична модель системи. Зараз завдання порахувати саме модель для гладких кривих, які наближають реальність.



З'ясувалося, що я трохи перестарався з мотором, на 24В каретка прискорюється приблизно на 60 метрів в секунду за секунду і жорсткості системи не вистачає для розумного вимірювання чого б то не було. Тому обрізаю вимірювання на п'ятнадцяти вольтах, в початковій частині кривих можна бачити коливання у швидкості каретки.

Отже, для кожного вимірювання вважаю сталу швидкість (висота плато) і малюю графік, на якому по горизонталі подана напруга, а по вертикалі стала швидкість.



Якщо б у мене в системі не було б тертя, то цей графік був би лінійним, і моя модель будувалася б як v_{k+1} = a*v_k + b*u_k. І адже він практично линеен на своїй позитивній і негативній частинах одночасно, але ці дві прямі не проходять через нуль. Це (у тому числі) з-за тертя. Ми бачимо, що якщо при позитивному напрузі ми додамо, а при негативному віднімемо буквально частку вольта, то графік пройде через нуль і стане лінійним.

Якщо ми хочемо враховувати тертя в системі, то модель будується наступним чином (не забудьте прочитати попередню статтю, якщо незрозуміло, звідки береться ця формула):



Отже, маючи дані, нам потрібно знайти значення a, b і c. Фиттинг можна робити як завгодно, враховуючи, що він робиться тільки один раз на десктопі, можна просто зробити три вкладених циклу. Вот код фиттинг, який мені каже, що для дискретизації в 20мс між вимірами a=.4, b = .06, c=-.01. Тобто, напруга, необхідне для компенсації тертя у мене в системі, дорівнює одній десятій вольта. Криві, отримані таким наближенням, накладені на реальний вимір на картинці вище.

Контролюємо прикладене силу, а не напруга
Разом, на даний момент наша система може бути записана наступним чином:



На жаль, це не є лінійним диференціальним рівнянням виду x_{k+1} = A x_k + B u_k, яке потрібно для лінійно-квадратичного регулятора. Крім того, навіть якщо б не це, то мені потрібно додати ще і маятник на каретку, а я не знаю як вивести залежність положення і кутової швидкості маятника від напруги. Я б волів, щоб у мене в якості управління було б не напруга, а безпосередньо прискорення каретки:



Окей, припустимо, що мій регулятор використовує прискорення в якості управління. Але ж в реальності-то я повинен подати напругу на мотор. Як його отримати? Дуже просто:



Тобто, маючи поточну швидкість (стан системи), ми легко можемо порахувати напруження, яке потрібно прикласти, щоб отримати необхідне прискорення. Таким чином, ми вбили двох зайців разом: і позбулися нелінійності в регуляторі, але при цьому залишивши компенсацію тертя, і отримали інтуїтивно зрозумілий контроль. Мені куди як простіше уявляти собі прискорення, ніж напруга, яке дуже нелінійно впливає на швидкість.

Нескладна, але довга математика або додаємо маятник
Рівняння руху маятника
Отже, завдання додати маятник в нашу систему і записати відповідні лінійні рівняння для регулятора. Давайте наведемо повний список необхідних для цього фізичних величин:

  • m: маса маятника
  • M: маса каретки
  • f: сила, прикладена мотором до каретки через ремінь
  • 2l: довжина маятника (l — це відстань від петлі до центру мас маятника)
  • I: момент інерції маятника (до речі, а ваші шрифти дозволяють бачити різницю між I і l?)
  • θ(t): кут відхилення маятника від вертикалі, в положенні нестійкої рівноваги нульовий, збільшується за годинниковою стрілкою
  • x(t): положення каретки


Разом змінними моєї системи буде чотиривимірний вектор, що складається з положення каретки, її швидкості, кута відхилення маятника і його кутової швидкості. Завдання на зараз записати систему лінійних диференціальних рівнянь такого вигляду:



Якщо ми зуміємо її записати в безперервному вигляді, то потім просто дискретизуем і ми виграли бій. Зверніть увагу, що тут я використовую в якості управління прикладену силу, а не прискорення, а другий закон Ньютона нам покаже, як це зробити. Отже, нам потрібно знайти невідомі матриці A і B.

Запишемо другий закон Ньютона для каретки: прискорення каретки, помножене на масу, дорівнює сумі двох горизонтальних сил: перша сила — це наше управління, сила, прикладена через ремінь електромотором. Друга сила — це сила, з якою маятник, намагаючись впасти або піднятися, штовхає каретку вбік. Щоб її знайти, можна записати координати центру массс маятника як двовимірний вектор (x(t) + l sin(θ(t)), l cos(θ(t))). Якщо ми продиференціюємо двічі координату, то отримаємо прискорення, а сила — це прискорення на масу. Оскільки нас цікавить тільки горизонтальна складова, то має сенс брати тільки другу похідну від x(t) + l sin(θ(t). Загальна:



Це нелінійне рівняння, але воно може бути добре наближене лінійним навколо точки рівноваги. Якщо кут близький до нульового, то перший чудовий межа (ну або ряд Тейлора) кажуть, що синус кута приблизно дорівнює куту. Те ж саме для косинуса: для кутів, близьких до нульових, косинус майже дорівнює одиниці. Ну і точно так само знехтуємо ступенями, починаючи з квадрата (якщо у нас швидкість дорівнює .1 радиана в секунду, то квадрат швидкості дасть зовсім маленький вкла, тому нехтуємо їм сміливо):



Тоді попереднє рівняння може бути наближене лінійним:



Тепер запишемо другий закон Ньютона (версія для обертання) для маятника. Для початку, що таке другий закон Ньютона для обертання? Це сума моментів дорівнює кутовому прискоренню, помноженій на момент інерції. Момент інерції — це такий аналог маси для поступального руху. Нас цікавить проекція всіх сил на пряму, перпендикулярну до маятника (адже обертає тільки перпендикулярна складова, правда?) Ну і зрозуміло, ці сили треба помножити на плече, щоб отримати момент. Сил, діючих на маятник, три: гравітація і горизонтальна і вертикальна складові сили реакції опори, які ми можемо отримати як і в минулий раз, двічі взявши похідну по часу від координат центру мас маятника:



Наблизимо його лінійним рівнянням:



Запишемо рівняння (1) і (2) у систему:



Для зручності назвемо визначник матриці буквою D:



Звернемо матрицю:



Тепер запишемо наші диференціальні рівняння, за фактом, ми знайшли матриці A і B:



Дискретизируем лінійну систему

Отже, ми записали неперывное диференціальне рівняння:



Для постійного кроку T отримуємо наступне:



Якщо ми позначимо через E одиничну матрицю 4x4, то можна записати:



Момент інерції для стрижня, що обертається навколо кінця, можна подивитися тут, тому спрощуємо все що можна:



Це і є найголовніше рівняння руху маятника.

Лінійно-квадратичний регулятор
Код обчислення коефіцієнтів регулятора можна взяти тут. Раніше я використовував найменші квадрати для підрахунку, але це все ж таки досить громіздко, тому в новому коді матриця посилення вважається безпосередньо.

Отже, ЛКР нам говорить, що якщо взяти керуючу силу f_k = 24.95 x_k + 18.54 v_k + 70.44 θ_k + 14.96 ω_k, то відхиливши маятник на 12° від вертикалі, і повівши каретку на 20 сантиметрів від положення рановесия, то ми повинні отримати наступне поведінка системи:



x_k — положення каретки, v_k — її швидкість, θ_k — кут маятника, ω_k — його кутова швидкість. Напруга на графіку дано в десятки вольт, щоб зрівняти масштаби трьох графіків.

Забиваємо коефіцієнти ардуину
Ось такий у мене вийшов маятник, керуючий код можна подивитися здесь:



В принципі, результатом я цілком задоволений. Амплітуда відхилення каретки від бажаного становища в районі двох сантиметрів, що досить пристойно. Чому вона не стоїть як укопана на нульовій позиції, ловлячи відхилення маятника дуже маленькими рухами? Причин кілька. Найпростіша — люфт в редукторі мотора, який становить приблизно п'ять міліметрів положення каретки, що означає, що каретці досить дорого змінювати напрямок руху. Наступна, не менш важлива причина — це оцінка кутовий швидкості руху маятника (ну і швидкості руху каретки).

Як оцінити швидкість руху, якщо в наявності лише інкрементальний енкодер? Найпростіший спосіб — це взяти два останніх значення енкодера і поділити їх різницю на минулий час.Це відомо як кінцеві різниці і досить широко застосовується на практиці.

Давайте подивимося на наступний графік:



Я записав кут маятника протягом декількох секунд, він показаний червоним. Графік злегка зашумлений з-за дозволу енкодера, але загалом і в цілому досить гладкий. Швидкість нам доводиться знаходити синтетично з історії положення маятника. Якщо ми візьмемо безпосередньо різниця двох сусідніх положення і поділимо на 20 мілісекунд, то це нам дасть синій графік. Користь кінцевих різниць в тому, що вони дуже просто програмуються. Їх недолік в тому, що вони драматичним чином підсилюють будь шум вимірювань.

Якщо взяти таку оцінку швидкості, регулятор сходить з розуму:



Єдина різниця з попереднім відео — це оцінка швидкості, нічого більше.

По-хорошому, треба б наблизити червону криву яким-небудь многочленом, і вважати його похідну, це основна ідея фільтра Савицького-Голея. Щоб не ламати голову, я просто згладив оцінку швидкості, не взявши сусідні семпли, а різницю поточного семпла і вісім семплів тому, поділивши на (20мс*8). Такий підхід сильно згладжує оцінку швидкості, але його недолік в тому, що він вносить лад в систему. Подивіться на зелену криву: вона явно відстає від реального стану речей. Така затримка в оцінці і змушує мій маятник злегка захитався.

До слова сказати, якщо взяти фільтр Савицького-Голея в лоб, то він теж створить схожу затримку. Таким чином, я знайшов розумний для мене компроміс між досягненням мети і простотою артилерії. Боротися з зашумленими вимірами я планую зовсім іншими методами, це тема для наступних розмов. Також в подальші розмови буде входити автоматична розгойдування маятника, щоб він сам піднімався з нижнього стану у верхнє.

Stay tuned!
Джерело: Хабрахабр

0 коментарів

Тільки зареєстровані та авторизовані користувачі можуть залишати коментарі.