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

Пара годин з життя математика-програміста або читаємо вікіпедію

Для початку в якості епіграфа цитую rocknrollnerd:
— Здрастуйте, мене звуть %username%, і потай розкриваю суми з сигма-нотації на листочку, щоб зрозуміти, що там відбувається.
— Привіт, %username%!


Отже, як я і говорив в своїй минулій статті, у мене є студенти, які панічно бояться математики, але в якості хобі колупаються паяльником і зараз хочуть зібрати візок-сігвей. Зібрати-то зібрали, а от тримати рівновагу вона не хоче. Вони думали використовувати ПИД-регуляторта ось тільки не зуміли підібрати коефіцієнти, щоб воно добре працювало. Прийшли до мене за порадою. А я ні бум-бум взагалі в теорії управління, ніколи і близько не підходив. Але зате коли на хабре я бачив статті, яка говорила про те, що лінійно-квадратичний регулятор допоміг автору, а під не допоміг.

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

Раз вже пішов ексгібіціонізм про мою роботу, то от вам моє робоче місце (кликабельно):




використані джерела

Отже, перше, що я роблю, йду в вікіпедія. Російська вікіпедія жорстока і нещадна, тому читаємо тільки англійський текст. Він теж огидний, але все ж не настільки. Отже, читаємо. З усіх вступних цеглин тексту тільки одна фраза мене зацікавила:
The theory of optimal control is concerned with operating a dynamic system at minimum cost. The case where the system dynamics are described by a set of linear differential equations and the cost is described by a quadratic function is called the LQ problem.


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

Finite-horizon, continuous-time LQR,
[серія жахливих формул]


Пропускаємо, ну його в болото таке читати, крім того, явно буде огидно руками вважати неперервні функції.

Infinite-horizon, continuous-time LQR,
[серія ще більш жахливих формул]


Час від часу не легше, ще й невласні інтеграли пішли, пропускаємо, раптом далі що цікавого знайдемо.

Finite-horizon, discrete-time LQR


О, це я люблю. У нас дискретна система, дивимося її стан через певні проміжки (якісь проміжки в першому прочитанні завжди дорівнюють одній секунді) часу. Похідні з рівняння пішли, т. к. вони тепер можуть бути наближені як (x_{k+1}-x_{l})/1 секунду. Тепер непогано було б зрозуміти, що таке x_k.

Одновимірний приклад

Фейнман писав, як саме він читав усі рівняння, що йому доводилося:
Actually, there was a certain amount of genuine quality to my guesses. I had a scheme, which I still use today when somebody is explaining
something that i'm trying to understand: I keep making up examples. For instance, the mathematicians would come in with a terrific theorem, and they're all excited. As they're telling me the conditions of the theorem, I construct something which fits all the conditions. You know, you have a set (one ball) — disjoint (two balls). Then the balls turn colors, grow hairs, or whatever, in my head as they put more conditions on. Finally they state the theorem, which is some dumb thing about the ball which isn't true for my hairy green ball thing, so I say, «False!»


Ну а ми що, крутіше Фейнмана? Особисто я ні. Тому давайте так. Є автомобіль, який їде з певною початковою швидкістю. Завдання його розігнати до якоїсь фінальної швидкості, при цьому єдине, на що ми можемо впливати, це на педаль газу, сиріч на прискорення автомобіля.

Давайте уявимо, автомобіль ідеальний і рухається по такому закону:


x_k — це швидкість автомобіля в секунду k, u_k — це положення педалі газу, що ми захочемо, її можна інтерпретувати як прискорення в секунду k. Отже, ми стартуємо з якоїсь швидкості x_0, і потім проводимо ось таке чисельне інтегрування (під які слова пішли). Відмітьте, що я не складаю м/с і м/с^2, u_k множиться на одну секунду інтервалу між вимірами. За замовчуванням у мене всі коефіцієнти або нуль, або один.

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

Прихований текст
#include < iostream>
#include "OpenNL_psm.h"

int main() {
const int N = 60;
const double xN = 2.3;
const double x0 = .5;
const double hard_penalty = 100.;

nlNewContext();
nlSolverParameteri(NL_NB_VARIABLES, N*2);
nlSolverParameteri(NL_LEAST_SQUARES, NL_TRUE);
nlBegin(NL_SYSTEM);
nlBegin(NL_MATRIX);

nlBegin(NL_ROW); // x0 = x0
nlCoefficient(0, 1);
nlRightHandSide(x0);
nlScaleRow(hard_penalty);
nlEnd(NL_ROW);

nlBegin(NL_ROW); // xN = xN
nlCoefficient((N-1)*2, 1);
nlRightHandSide(xN);
nlScaleRow(hard_penalty);
nlEnd(NL_ROW);

nlBegin(NL_ROW); // uN = 0, for convenience, normally uN is not defined
nlCoefficient((N-1)*2+1, 1);
nlScaleRow(hard_penalty);
nlEnd(NL_ROW);

for (int i=0; i < N-1; i++) {
nlBegin(NL_ROW); // x{i+1} = xi + ui
nlCoefficient((i+1)*2 , -1);
nlCoefficient((i )*2 , 1);
nlCoefficient((i )*2+1, 1);
nlScaleRow(hard_penalty);
nlEnd(NL_ROW);
}

for (int i=0; i < N; i++) {
nlBegin(NL_ROW); // xi = xN, soft
nlCoefficient(i*2, 1);
nlRightHandSide(xN);
nlEnd(NL_ROW);
}

nlEnd(NL_MATRIX);
nlEnd(NL_SYSTEM);
nlSolve();

for (int i=0; i < N; i++) {
std::cout << nlGetVariable(i*2) << " " << nlGetVariable(i*2+1) << std::endl;
}

nlDeleteContext(nlGetCurrent());
return 0;
}


Отже, давайте розбиратися, що я роблю. Для початку я кажу N=60, на всі відводжу 60 секунд. Потім кажу, що фінальна швидкість повинна бути 2.3 метра в секунду, а початкова півметра на секунду, це виставлено від балди. Змінних у мене буде 60*2 — 60 значень швидкості і 60 значень прискорення (строго кажучи, прискорення повинно бути 59, але мені простіше сказати, що їх 60, а останнє повинно бути одно суворо нулю).

Разом, у мене 2*N змінних (N=60), парні змінні (я починаю рахувати з нуля, як і кожен нормальний програміст) — це швидкість, а непарні — це прискорення. Ставлю початкову та кінцеву швидкість ось цими рядками:
За фактом, я сказав, що хочу, щоб початкова швидкість дорівнювала x0 (.5m/s), а кінцева — xN (2.3 m/s).
nlBegin(NL_ROW); // x0 = x0
nlCoefficient(0, 1);
nlRightHandSide(x0);
nlScaleRow(hard_penalty);
nlEnd(NL_ROW);

nlBegin(NL_ROW); // xN = xN
nlCoefficient((N-1)*2, 1);
nlRightHandSide(xN);
nlScaleRow(hard_penalty);
nlEnd(NL_ROW);


Ми знаємо, що x{i+1} = xi + ui, тому давайте додамо N-1 таке рівняння в нашу систему:

for (int i=0; i < N-1; i++) {
nlBegin(NL_ROW); // x{i+1} = xi + ui
nlCoefficient((i+1)*2 , -1);
nlCoefficient((i )*2 , 1);
nlCoefficient((i )*2+1, 1);
nlScaleRow(hard_penalty);
nlEnd(NL_ROW);
}


Отже, ми додали всі жорсткі обмеження в систему, а що саме, власне, ми можемо хотіти оптимізувати? Давайте просто для початку скажемо, що ми хочемо, щоб x_i як можна швидше досяг фінальної швидкості xN:

for (int i=0; i < N; i++) {
nlBegin(NL_ROW); // xi = xN, soft
nlCoefficient(i*2, 1);
nlRightHandSide(xN);
nlEnd(NL_ROW);
}


Ну ось наша система готова, жорсткі правила переходу між станами задані, квадратична функція якості системи теж, давайте потрясем коробочку і подивимося, що найменші квадрати нам дадуть в якості рішення x_i, u_i (зелена лінія — це швидкість, червона — прискорення):



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

А давайте змінимоось комміт) цільову функцію, замість якнайшвидшого збіжності попросимо як можна менше прискорення:

for (int i=0; i < N; i++) {
nlBegin(NL_ROW); // ui = 0, soft
nlCoefficient(i*2+1, 1);
nlEnd(NL_ROW);
}




Мда, тепер машина прискорюється на два метра в секунду за цілу хвилину. Ок, давайте і швидку збіжність до фінальної швидкості, і маленьке прискорення спробуємоось комміт)?

for (int i=0; i < N; i++) {
nlBegin(NL_ROW); // ui = 0, soft
nlCoefficient(i*2+1, 1);
nlEnd(NL_ROW);

nlBegin(NL_ROW); // xi = xN, soft
nlCoefficient(i*2, 1);
nlRightHandSide(xN);
nlEnd(NL_ROW);
}


Ага, супер, тепер стає красиво:



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

Отже, що ми маємо на даний момент? Те, що маючи початковий стан системи + маючи кінцевий стан системи + кількість секунд, які ми можемо знайти ідеальне управління u_i. Це добре, тільки до сигвею слабо застосовується. Чорт, як же вони роблять? Так, читаємо наступний текст у вікіпедії:

with a performance index defined as



the optimal control sequence minimizing the performance index is given by



where [ААА, ЩО ТАМ ДАЛІ ТАКЕ?!]


Так. «J =» я не розумію що після одно, але це явно квадратична функція, що ми спробували. Типу, швидкої збіжності до мети плюс найменшими витратами, це ми пізніше побачимо ближче, зараз мені досить мого розуміння. u_k = -F x_k.
Оп-па. Вони кажуть, що для нашого одновимірного прикладу в будь-який момент часу оптимальне прискорення — це якась константа -F, помножена на поточну швидкість? Ну-ка, ну-ка. А адже і правда, зелений і червоний графіки підозріло один на одного схожі!

Давайте-но спробуємо написати справжні рівняння для нашого 1D прикладу.

Отже, у нас є функція якості управління:



Ми хочемо її мінімізувати, при цьому дотримуючись обмеження на зв'язок між сусідніми значеннями швидкості нашої машини:



Стоп-стоп-стоп, а якого дідька у вікіпедії варто x_k^T Q x_k? Адже це ж простий x_k^2 у нашому випадку, а у нас (x_k-x_N)^2?! Ялинки, адже вони припускають, що фінальне стан, в яке ми хочемо потрапити, це нульовий вектор!!! ЯКОГОСЬ [CENSORED] ПРО ЦЕ НА ВСІЙ СТОРІНЦІ ВІКІПЕДІЇ НІ СЛОВА?!

Окей, дихаємо глибоко, заспокоюємося. Тепер я все x_i у формулюванні J висловлю через u_i, щоб не мати обмежень. Тепер у мене змінними буде тільки вектор керування. Отже, ми хочемо мінімізувати функцію J, яка записується так:



Зашибісь. А тепер давайте розкривати дужки (див. епіграф):



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



Разом ми отримуємо, що оптимальне керування буде оптимально тільки тоді, коли u_0 має ось такий вираз:



Обратие увагу, що цей вираз входять і інші невідомі u_i, але. Але прикол в тому, що ми я не хочу, щоб машина прискорювалася всю хвилину-на-всього два метри в секунду. Хвилину я їй дав просто як свідомо достатній час. А можу і годину дати. Член, що залежить від u_i, якщо грубо, то це вся робота по прискоренню, від N вона не залежить. Тому якщо N досить велике, то оптимальний u_0 лінійно залежить тільки від того, наскільки x0 далекий від кінцевого положення!

тобто, управління має виглядати наступним чином: ми моделюємо систему, знаходимо магічний коефіцієнт, лінійно зв'язує u_i і x_i, записуємо його, а потім в нашому роботі просто робимо лінійний пропорційний регулятор, використовуючи знайдений магічний коефіцієнт!


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

2D приклад

Як інтуїції це прекрасно, а ось перший код, який я дійсно написав:
Прихований текст
#include < iostream>
#include < vector>
#include "OpenNL_psm.h"

int main() {
const int N = 60;
const double x0 = 3.1;
const double v0 = .5;
const double hard_penalty = 100.;
const double rho = 16.;

nlNewContext();
nlSolverParameteri(NL_NB_VARIABLES, N*3);
nlSolverParameteri(NL_LEAST_SQUARES, NL_TRUE);
nlBegin(NL_SYSTEM);
nlBegin(NL_MATRIX);

nlBegin(NL_ROW);
nlCoefficient(0, 1); // x0 = 3.1
nlRightHandSide(x0);
nlScaleRow(hard_penalty);
nlEnd(NL_ROW);

nlBegin(NL_ROW);
nlCoefficient(1, 1); // v0 = .5
nlRightHandSide(v0);
nlScaleRow(hard_penalty);
nlEnd(NL_ROW);

nlBegin(NL_ROW);
nlCoefficient((N-1)*3, 1); // xN = 0
nlScaleRow(hard_penalty);
nlEnd(NL_ROW);

nlBegin(NL_ROW);
nlCoefficient((N-1)*3+1, 1); // vN = 0
nlScaleRow(hard_penalty);
nlEnd(NL_ROW);

nlBegin(NL_ROW); // uN = 0, for convenience, normally uN is not defined
nlCoefficient((N-1)*3+2, 1);
nlScaleRow(hard_penalty);
nlEnd(NL_ROW);

for (int i=0; i < N-1; i++){
nlBegin(NL_ROW); // x{N+1} = xN + vN
nlCoefficient((i+1)*3 , -1);
nlCoefficient((i )*3 , 1);
nlCoefficient((i )*3+1, 1);
nlScaleRow(hard_penalty);
nlEnd(NL_ROW);

nlBegin(NL_ROW); // v{N+1} = vN + uN
nlCoefficient((i+1)*3+1, -1);
nlCoefficient((i )*3+1, 1);
nlCoefficient((i )*3+2, 1);
nlScaleRow(hard_penalty);
nlEnd(NL_ROW);
}

for (int i=0; i < N; i++) {
nlBegin(NL_ROW); // xi = 0, soft
nlCoefficient(i*3, 1);
nlEnd(NL_ROW);

nlBegin(NL_ROW); // vi = 0, soft
nlCoefficient(i*3+1, 1);
nlEnd(NL_ROW);

nlBegin(NL_ROW); // ui = 0, soft
nlCoefficient(i*3+2, 1);
nlScaleRow(rho);
nlEnd(NL_ROW);
}

nlEnd(NL_MATRIX);
nlEnd(NL_SYSTEM);
nlSolve();

std::vector<double> solution;
for (int i=0; i<3*N; i++) {
solution.push_back(nlGetVariable(i));
}

nlDeleteContext(nlGetCurrent());

for (int i=0; i < N; i++) {
for (int j=0; j < 3; j++) {
std::cout << solution[i*3+j] << " ";
}
std::cout << std::endl;
}

return 0;
}



Тут все те ж саме, тільки змінних системи тепер дві: не тільки швидкість, але і координата машини.
Отже, дана початкова позиція + початкова швидкість (x0, v0), мені потрібно досягти кінцевої позиції (0,0), зупинитися в початку координат.
Перехід з одного моменту в наступний здійснюється як x_{k+1} = x_k + v_k, а v_{k+1} = v_k + u_k.

Я досить прокоментував попередній код, це не повинен викликати труднощів. Ось результат роботи:



Червона лінія — координата, зелена — швидкість, а синя — це безпосередньо положення педалі газу». Виходячи з попереднього, передбачається, що синя крива — це зважена сума червоної і зеленої. Хм, так це? Давайте спробуємо порахувати!
Тобто, теоретично, нам потрібно знайти два числа a і b, такі, що u_i = a*x_i + b*v_i. А це є не що інше, як лінійна регресія, що ми робили в минулій статті! Ось код.

В ньому я спочатку вважаю криві, що на картинці вище, а потім шукаю такі a і b, що синя крива = a*червона + b*зелена.

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


Відхилення близько однієї сотої метра за секунду за секунду! Круто!!! Той комміт, що я навів, дає a=-0.0513868, b=-0.347324. Відхилення дійсно невелика, але ж це нормально, початкові дані-то я не міняв.

А тепер давайте кардинально змінимо початкове положення і швидкість машини, залишивши магічні числа a і b з попередніх обчислень.

Ось різниця між справжнім оптимальним рішенням і тим, що ми отримуємо наитупейшей процедурою, давайте-но я її ще раз наведу повністю:
double xi = x0;
double vi = v0;
for (int i=0; i < N; i++) {
double ui = xi*a + vi*b;
xi = xi + vi;
vi = vi + ui;
std::cout << (ui-solution[i*3+2]) << std::endl;
}




І ніяких диференціальні рівняння Риккати, які нам пропонувала вікіпедія. Можливо, що й про них мені доведеться почитати, але це буде потім, коли припече. А поки що прості квадратичні функції мене влаштовують більш ніж повністю.

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

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

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

Сподіваюся, мені вдалося. Ще раз, я зовсім не фахівець у теорії управління, я її в очі не бачив, якщо у вас є що доповнити і виправити, не соромтеся.

Enjoy!

Джерело: Хабрахабр

0 коментарів

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