Модуль 02 · Математичні основи роїв БПЛА

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

Земна та зв'язана системи координат мультикоптера

Зміст лекції

Лекція йде знизу вгору: від опису одного апарата до опису зв'язків між апаратами.

Частина	Про що	Питання, на яке відповідає
1. Апарат у просторі	системи координат, розклад тяги, три представлення орієнтації	як задати положення корпусу і чим апарат керує
2. Керування й оцінювання	рівновага, ПІД по складових, частоти контурів каскаду, числення шляху, фільтр Калмана	звідки взяти стан і як утримати апарат у рівновазі
3. Граф рою	матриці $A$ і $D$ , лапласіан, $λ_{2}$ , консенсус	як описати не апарати, а зв'язки між ними

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

Результати навчання

Після лекції ви зможете:

пояснити, чому нахил і висота в мультикоптера пов'язані й навіщо потрібні дві системи координат;
назвати три представлення орієнтації та сказати, яке з них використовують у автопілоті й чому;
пояснити, що робить кожен доданок ПІД-регулятора, звідки береться стала похибка й чим вона лікується;
зняти з графіка три показники якості регулятора й перевести задану тягу в оберти гвинтів;
пояснити, навіщо контури керування працюють на різних частотах;
пояснити, навіщо потрібне числення шляху і чому його похибка зростає;
пояснити, як фільтр Калмана поєднує прогноз і вимірювання і що означає його підсилення;
побудувати матриці $A$ , $D$ і лапласіан невеликого графа та обчислити рядок добутку $L x$ ;
пояснити правило протоколу консенсусу й сказати, як топологія впливає на час узгодження.

Частина 1 · Апарат у просторі

Щоб керувати апаратом, треба вміти сказати, як він повернутий і куди спрямована його тяга.

Кути Ейлера між земною та зв'язаною системами координат

Дві системи координат і розклад тяги

Земна система (earth frame) нерухома: у ній має сенс «летіти на північ», «набрати висоту», «зависнути над точкою». Зв'язана система (body frame) прикріплена до корпусу: у ній природно виражати те, що апарат відчуває і чим керує, — тягу гвинтів, покази гіроскопа, моменти крену й тангажа. Орієнтація — це словник перекладу між ними.

Мультикоптер не має окремого двигуна руху вперед. Щоб летіти, він мусить нахилитися, а щоб не втратити висоту при нахилі — додати обертів:

F_{верт} = F \cos γ, F_{гориз} = F \sin γ

Розклад вектора тяги при нахилі апарата на 20 градусів

Тут $F$ — сумарна тяга гвинтів уздовж вертикальної осі body frame, $γ$ — кут нахилу корпусу. Для $F = 15$ Н і $γ = 20^{\circ}$ вертикальна складова 14,1 Н, горизонтальна 5,1 Н. Апарат масою 1,5 кг важить 14,7 Н, і саме стільки тяги потрібно у прямому висінні. Похилившись на 20°, він мусить давати вже 15,6 Н — на 6,4 % більше, — інакше почне просідати. У планарної схеми нахил і висота пов'язані завжди — це не два незалежні канали.

Три представлення орієнтації

Орієнтація — це відповідь на питання «як апарат повернутий». Записати цю відповідь можна трьома способами, і вони описують те саме положення корпусу.

Same orientation written as Euler angles rotation matrix and quaternion

Кути Ейлера — три числа, які бачить оператор: крен, тангаж, рискання. Наочно, і саме їх показує телеметрія.

Матриця повороту — таблиця 3×3. Її сила в тому, що вона не просто описує поворот, а виконує його: множимо на вектор і дістаємо той самий вектор в іншій системі координат.

Кватерніон — чотири числа, з яких відновлюються вісь обертання й кут повороту навколо неї. Компактно й без особливих положень.

Представлення	Чисел	Головна властивість	Де застосовують
Кути Ейлера	3	наочні, але мають особливе положення	інтерфейс оператора, телеметрія
Матриця повороту	9	прямо перетворює вектори між системами	перерахунок сил, комп'ютерний зір
Кватерніон	4	компактний, без особливих положень	внутрішній стан автопілота

Три кути на самому апараті

Кожен кут Ейлера — це поворот навколо своєї осі, і саме тому їх три.

Three Euler angles shown on a quadrotor from three views

Кут	Навколо чого повертає	Що робить апарат
Крен $ϕ$ (roll)	вісь, спрямована вперед	нахиляється на бік і летить убік
Тангаж $θ$ (pitch)	вісь, спрямована вправо	нахиляється вперед і летить уперед
Рискання $ψ$ (yaw)	вертикальна вісь	повертає ніс; у горизонтальному висінні це не зрушує апарат із місця

Перші два кути керують рухом, третій у висінні повертає лише корпус. (Якщо апарат уже нахилений, зміна курсу повертає й напрямок горизонтальної тяги — але в висінні цього ефекту немає.) Наскільки саме керують, каже проста формула: якщо апарат тримає висоту й нахилений на кут $θ$ , горизонтальне прискорення дорівнює

a = g \tan θ

Нахил 10° дає 1,7 м/с², 20° — 3,6 м/с², 30° — 5,7 м/с². Ось чому в гонкових польотах апарат нахиляють майже на 45°: там $a \approx g$ .

Зворотний бік тієї самої монети: апарат орієнтується, де низ, по акселерометру. Тут є тонкість, на якій часто плутаються: акселерометр міряє не прискорення й не силу тяжіння окремо, а те, з якою силою на нього діє опора. Нерухомий на столі — показує рівно $g$ угору; у вільному падінні — майже нуль.

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

Матриця повороту: той самий приклад із числами

Кути кажуть, як апарат повернутий. Матриця вміє більше: вона перераховує вектори з однієї системи в іншу. Візьмімо вже знайомий випадок — апарат нахилений на 20°, гвинти дають 15 Н. Щоб усе було видно, беремо двовимірний зріз: одна вертикальна площина, матриця 2×2. У просторі вона 3×3, але робить те саме.

Rotation matrix worked example with a tilted quadrotor

У власній системі апарата тяга — це одне число: «15 Н перпендикулярно до рами». Бортовий комп'ютер інакше її і не знає. Але штурманові потрібні інші числа: скільки з цих 15 Н тримає висоту, а скільки штовхає вбік. Саме це й рахує матриця:

верхній рядок: $0,940 \cdot 0 + 0,342 \cdot 15 = 5, 1$ Н убік;
нижній рядок: $- 0,342 \cdot 0 + 0,940 \cdot 15 = 14, 1$ Н угору.

Кожен рядок матриці відповідає одній осі землі й каже, скільки в неї дає кожна вісь апарата. Числа 0,940 і 0,342 — це просто $\cos 20 °$ і $\sin 20 °$ .

Де це працює на практиці: камера побачила перешкоду «за 3 метри попереду від себе». Щоб нанести її на карту, цей вектор спершу повертають матрицею в земні осі — а потім додають до нього власну позицію апарата. Матриця відповідає саме за поворот; зсув до позиції додають окремо. У просторі вона 3×3, вектор із трьох чисел, принцип не змінюється.

Кватерніон: вісь і кут замість трьох чисел

Кватерніон спирається на простий факт: будь-який поворот тіла — це поворот навколо однієї осі на певний кут. Записують це чотирма числами: перше — косинус половини кута, решта три — одинична вісь, помножена на синус половини кута.

q = (\cos \frac{α}{2}; n \sin \frac{α}{2})

Quaternion example for yaw rotations of a quadrotor

На прикладі розворот навколо вертикалі: вісь одна й та сама, змінюється лише кут. Вісь вертикальна, тому ненульовою лишається тільки остання компонента.

Розворот апарата	Кватерніон
0° — ніс на північ	$(1, 00; 0; 0; 0, 00)$
90° управо — ніс на схід	$(0, 71; 0; 0; 0, 71)$
180° — ніс на південь	$(0, 00; 0; 0; 1, 00)$

Вісь тут вертикальна й спрямована вниз, тому додатний кут — це розворот управо, як на рисунку.

Найдивніше тут — половина кута: розвернувся на 180°, а в записі 90°. Так влаштована формула, і до цього просто звикають.

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

Особливе положення кутів Ейлера

Чому ж не користуватися скрізь найзручнішими — кутами? Через одну ваду, яку не обійти.

Уявіть, що апарат задер ніс строго вертикально. Тепер поверніть його навколо вертикальної осі — це рискання. А тепер спробуйте крен. Виявляється, це той самий фізичний рух: вісь крену збіглася з віссю рискання. Три числа описують лише два незалежні повороти, і розкласти орієнтацію однозначно вже неможливо. Це gimbal lock.

У самому цьому положенні розклад перестає бути однозначним — тих самих трьох чисел можна дібрати нескінченно багато, — а поблизу нього він стає дедалі чутливішим: крихітна похибка гіроскопа дає стрибок кута на десятки градусів. Для знімального польоту це не проблема — там тангаж рідко виходить за 30°. Для акробатики чи різкого маневру — проблема щоразу.

Підсилення похибки поблизу карданного замка

Множник $1 / \cos θ$ входить у перерахунок кутових швидкостей корпусу у швидкості зміни кутів Ейлера — і показує, як росте чутливість поблизу особливого положення: при тангажі 45° він дорівнює 1,4, при 80° — 5,8, при 89° — 57.

Тому в автопілоті працює розподіл ролей: рахують кватерніоном, перетворюють матрицею, показують оператору кути.

Це знадобиться в Модулі 06: у вектор спостереження навченої політики подають матрицю повороту, а не кути. Причина інша, ніж gimbal lock: кут стрибає з $+ 180 °$ на $- 180 °$ при плавному повороті, і мережі важко вчитися на вході з таким розривом.

Частина 2 · Керування й оцінювання стану

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

Стійка і нестійка рівновага на прикладі кульки

Стійкість рівноваги і чому мультикоптер нестійкий

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

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

Звідси наслідок для всього курсу: навчена політика не керує моторами напряму. Під нею лишається внутрішній контур стабілізації, а політика задає йому завдання вищого рівня. Конкретний набір залежить від задачі: у найпростішій постановці політика видає одне число — нормовану тягу, а кути тримає бортовий регулятор; у Модулі 06 розглядаємо ширший інтерфейс із тягою й кутовими швидкостями.

Скільки часу апарат прибирає відхилення

Сказати «контур стійкий» замало. Замовника цікавить інше: скільки секунд апарат прибирає відхилення. Це і є робоча мова налаштування.

Експоненційне згасання відхилення для двох темпів згасання

На графіку два контури з однаковим початковим відхиленням. Швидкий прибирає 95 % за 1,5 с, повільний — за 6,0 с, тобто вчетверо довше. Обидва «стійкі»: обидва повертаються, питання лише в часі.

Фраза	Що вона означає
«контур стійкий»	мале збурення не викликає великого відхилення
«відхилення гаситься за 1,5 с»	залишилося 5 % від початкового через 1,5 с
«контур млявий»	повертається, але може не вкластися у вимоги задачі

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

ПІД: що робить кожна складова

Практично всі внутрішні контури мультикоптера — це один і той самий регулятор із трьох доданків. Уся його робота починається з помилки $e$ — різниці між заданим і поточним значенням.

u = k_{p} e + k_{i} \int e d t + k_{d} \dot{e}

П — пропорційний: реагує на те, що є зараз. Чим далі апарат від цілі, тим сильніше штовхаємо. Відхилення 0,5 м при $k_{p} = 6$ дає 3 Н. Логічно й майже працює — але сам по собі цей доданок апарат не зупиняє: підлетівши до цілі, апарат має швидкість і проскакує її, потім повертається, знову проскакує. Виходить гойдалка.

Д — диференційний: реагує на швидкість зміни. Це гальмо. Він дивиться не на саму помилку, а на те, як швидко вона зменшується, і гасить рух заздалегідь. Саме він перетворює гойдалку на плавний підхід до цілі.

Тут є практична тонкість, яку роблять майже в кожній реалізації: замість похідної помилки беруть мінус похідну самої висоти, тобто $k_{d} (- v_{z})$ . Причина проста. Коли оператор різко змінює завдання, помилка стрибає сходинкою, а похідна сходинки — миттєвий сплеск: апарат смикнеться. Швидкість же $v_{z}$ змінюється плавно, бо це фізична величина. У сталому режимі обидва варіанти збігаються, а на стрибку завдання другий поводиться пристойно.

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

Contribution of each PID term under a constant disturbance

На графіку той самий апарат зі сталим збуренням 0,6 Н і трьома наборами коефіцієнтів: сам П коливається без згасання, П+Д зупиняється — але на 0,10 м нижче цілі, і лише П+І+Д доходить до заданої висоти.

Дві застороги до графіка. Модель тут спрощена: вагу вже скомпенсовано доданком $m g$ , тому $u$ — це добавка до тяги висіння, а повна тяга дорівнює $m g + u$ ; опору повітря не враховано, через що чистий П і не згасає. І коефіцієнти підібрані так, щоб показати роль кожної складової, а не як приклад доброго налаштування: у варіанта П+І+Д перерегулювання близько 28 %, що за критерієм наступної картки не пройшло б.

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

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

e_{ст} = \frac{d}{k_{p}}

Стала похибка від вітру з інтегральною складовою та без неї

На цьому рисунку збурення горизонтальне — вітер зносить апарат убік; на попередньому воно тягнуло вниз. Механізм той самий, тому й число те саме: для $d = 0, 6$ Н і $k_{p} = 6$ Н/м похибка дорівнює 0,10 м. Збільшити $k_{p}$ втричі — похибка впаде до 3 см, але контур стане нервовим і почне підсилювати шум датчика. Прибирає похибку саме інтегральна складова: вона накопичує ці 10 см, доки не додасть рівно ті 0,6 Н, яких бракувало.

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

Якщо перебрати з	Що побачите
$k_{p}$	коливання, апарат «нервує»
$k_{d}$	смикання від шуму датчика
$k_{i}$	повільне розгойдування, апарат довго не заспокоюється

Як міряють якість регулятора

ПІД налаштовують не на око: три показники знімають із графіка висоти $z (t)$ і саме їх наводять у звіті. Позначимо задану висоту $z^{*}$ , початкову $z_{0}$ , розмах $Δ = | z^{*} - z_{0} |$ .

Показник	Як рахують	Прийнятно
Перерегулювання $σ$	$(z_{max} - z^{*}) / Δ \cdot 100 %$	$σ \leq 10 %$
Час перехідного процесу $t_{п}$	момент, після якого крива не виходить за коридор $\| z - z^{*} \| \leq 0, 05 Δ$	що менший, то краще
Стала похибка $e_{ст}$	середнє $\| z - z^{*} \|$ на останніх 2 с	якнайменша

Саме ці три числа й наводять, коли звітують про якість налаштування регулятора.

⚠️ Одна пастка при вимірюванні. Керування обмежене: $u$ не може бути більшим за одиницю. На зльоті з нуля регулятор майже напевно впирається в цю межу — і доки він там сидить, апарат розганяється не так, як «хоче» регулятор, а як дозволяє максимальна тяга. Перерегулювання, зняте з такого розгону, характеризує не налаштування, а межу тяги. Тому показники знімають із помірного стрибка завдання, де насичення не настає.

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

T = 4 k_{F} n^{2} ⟹ n = \sqrt{\frac{T}{4 k_{F}}}, u = \frac{T}{T_{max}} \in [0; 1]

Для Crazyflie 2.1 у моделі CF2X: $k_{F} = 3, 16 \cdot 10^{- 10}$ Н/(об/хв) $^{2}$ , сумарна максимальна тяга $T_{max} = 0,596$ Н. Висіння апарата масою 27 г вимагає $T = m g = 0,265$ Н, тобто $u \approx 0, 44$ і близько 14 500 об/хв на гвинт.

Скільки лишається в запасі

Перед будь-яким груповим експериментом корисно за хвилину порахувати, скільки тяги лишається понад вагу. Тяга одного гвинта у висінні дорівнює $F_{i} = m g / N_{гв}$ , де $N_{гв}$ — кількість гвинтів.

Crazyflie 2.1: у висінні мотори видають близько 44 % максимуму, тобто повна тяга більша за вагу у 2,25 раза. Решта — це все, що є на розгін, набір висоти й боротьбу з вітром одночасно.

Що з цього випливає: запас витрачається не лише на маневр. Частина його потрібна, щоб апарат лишався керованим після відмови гвинта, — і там уже важить не сама тяга, а те, як гвинти розставлені. Числа за геометріями й розбір відмов — у Модулі 11.

Чому контури працюють на різних частотах

Досі ми говорили про регулятор так, ніби він один. Насправді їх кілька — і ось чому.

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

Cascade control rates over one policy step

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

Три контури керування, від зовнішнього до внутрішнього:

Контур	Частота в цьому прикладі	Що він видає
Зовнішній: позиція та швидкість (тут — політика)	50 Гц	завдання для контуру кутів
Середній: кути крену й тангажа	250 Гц	потрібну швидкість обертання
Внутрішній: кутові швидкості	500 Гц	команди на мотори через розподіл керування

Окремо від них стоїть крок симулятора, 1000 Гц — це не контур керування, а чисельне інтегрування рівнянь руху.

Правило, яке пов'язує сусідні контури: внутрішній має бути помітно швидшим за зовнішній — на практиці в кілька разів, часто беруть орієнтир близько п'яти. Сенс не в самому числі, а у вимозі: нижній контур мусить встигнути відпрацювати завдання, перш ніж верхній видасть наступне. Якщо правило порушити, верхній контур бачить невиконане завдання, просить сильніше, нижній не встигає — і замість керування виходить розгойдування. Саме тому в ряду 50 → 250 → 500 Гц відношення й не однакові: важить не коефіцієнт, а запас за швидкодією.

Який саме інтерфейс у політики — залежить від задачі. У найпростішій постановці вона видає одне число, нормовану тягу, а кути тримає бортовий регулятор. У Модулі 06 інтерфейс ширший — тяга й кутові швидкості. Показані тут 50/250/500 Гц — це числа конкретного прикладу, а не стандарт: у різних автопілотах вони різні.

Числення шляху: навігація без жодного орієнтира

Апарат має знати, де він. Найочевидніше джерело — GPS, але в приміщенні чи тунелі сигналу немає, а серед високої забудови він буває неточним. Що робити тоді?

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

Замість лага й компаса в апарата інерціальний вимірювальний блок (IMU): акселерометр міряє прискорення, гіроскоп — кутову швидкість. Далі чиста арифметика:

1прискорення

2швидкість

3положення

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

Чому позиція спливає сама собою

У числення шляху є вада, і вона вбудована в сам метод: помилки не забуваються, вони накопичуються.

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

Дрейф числення шляху за IMU росте квадратично

Час без зовнішньої корекції	Похибка положення
10 с	2,5 м
30 с	22,5 м
60 с	90 м

Числа наведено для скромного сталого зміщення акселерометра 0,05 м/с² — саме для цієї моделі зростання квадратичне. Закономірність важливіша за самі числа: удвічі довше без корекції — вчетверо гірша позиція. Саме тому апарат не може довго літати «наосліп» і мусить час від часу звірятися із зовнішнім джерелом — GPS, камерою, дальноміром.

Фільтр Калмана: ідея на побутовому прикладі

Дрейф із попередньої картки лікується зовнішнім джерелом — GPS, камерою, барометром. Але зовнішнє джерело теж бреше: барометр реагує на протяг, GPS стрибає біля будівель. Маємо два неточні джерела. Кому вірити?

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

Саме це й робить фільтр Калмана — три кроки, які повторюються 100+ разів на секунду:

1Прогноз за моделлю

2Нове вимірювання

3Корекція за надійністю

4Уточнена оцінка

Головне, що варто винести: фільтр працює не з числами, а з парами «число + наскільки йому вірити». Тому він і знає, кого слухати більше.

Один такт фільтра з числами

Оцінюємо висоту. Модель руху дає прогноз 10,00 м, і ми знаємо її типову похибку — стандартне відхилення 0,30 м. Барометр показує 11,20 м із похибкою 0,80 м. Розбіжність 1,20 м: комусь із двох треба поступитися.

Один крок корекції фільтра Калмана як три розподіли

Фільтр рахує підсилення $K$ — частку розбіжності, на яку варто зрушити оцінку:

\hat{x} = {\hat{x}}^{-} + K (z - {\hat{x}}^{-}), K = \frac{0, 09}{0, 09 + 0, 64} = 0,123

Крок	Обчислення	Результат
Розбіжність прогнозу й вимірювання	$11, 20 - 10, 00$	1,20 м
Наскільки рушити оцінку	$0,123 \cdot 1, 20$	0,15 м
Нова оцінка	$10, 00 + 0, 15$	10,15 м

Читається так: «барометру вірю приблизно на 12 %». Звідки саме 12 %? Стандартне відхилення прогнозу майже втричі менше, а у ваги входять квадрати цих чисел: $0, 30^{2} = 0, 09$ проти $0, 80^{2} = 0, 64$ , тобто прогноз важить усемеро більше. Похибки двох джерел у прикладі вважаємо незалежними.

$K$ близьке до нуля — «вірю моделі, датчик шумний»; близьке до одиниці — «вірю датчику, модель груба». Якби барометр мав похибку 0,10 м, підсилення стало б 0,9, і оцінка пішла б майже до його показів.

Частина 3 · Граф рою

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

Чотири топології рою та їхня алгебраїчна зв'язність

Навіщо роєві граф

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

Топологія	Хто з ким зв'язаний	Чим платимо
Ланцюг	кожен, крім крайніх, бачить двох сусідів	новина проходить рій по черзі
Кільце	замкнений ланцюг	одне зайве ребро, зате два шляхи
Зірка	усі через один центр	центр — єдина точка відмови
Повний	кожен із кожним	багато каналів і радіотрафіку

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

Дві таблиці, якими описують рій

Щоб рахувати щось про рій, топологію треба записати числами. Для цього досить двох таблиць. Візьмемо ланцюг із п'яти апаратів: 1—2—3—4—5.

Матриця суміжності $A$ відповідає на питання «хто кого чує»: на перетині рядка $i$ та стовпця $j$ стоїть одиниця, якщо зв'язок є, і нуль, якщо немає. Вона симетрична: якщо 2 чує 3, то й 3 чує 2.

Матриця степенів вершин $D$ — кількість сусідів кожного апарата, виписана по діагоналі. Це просто сума одиниць у відповідному рядку $A$ : у ланцюгу крайні апарати мають по одному сусідові, середні — по два.

Їхня різниця і є лапласіан:

L = D - A

Degree matrix minus adjacency matrix equals the graph Laplacian

На діагоналі $L$ — число сусідів, поза нею — мінус одиниця там, де є зв'язок. Сума кожного рядка дорівнює нулю, і це не випадковість, а те, завдяки чому лапласіан узагалі працює.

Що робить лапласіан: множимо на живі числа

Сама по собі матриця нічого не рахує. Уся суть з'являється, коли ми множимо її на вектор станів рою. Нехай п'ять апаратів висять на висотах $x = (12, 4, 9, 2, 7)$ метрів.

Laplacian matrix multiplied by the vector of altitudes

Візьмемо третій рядок $L$ — це $(0, - 1, 2, - 1, 0)$ — і помножимо поелементно на висоти:

(L x)_{3} = - 1 \cdot 4 + 2 \cdot 9 - 1 \cdot 2 = 12

Те саме число можна отримати інакше, і ось це важливо:

(L x)_{3} = \underset{вище за сусіда 2}{\underset{⏟}{(9 - 4)}} + \underset{вище за сусіда 4}{\underset{⏟}{(9 - 2)}} = 5 + 7 = 12

Рядок лапласіана рахує, наскільки апарат відрізняється від своїх сусідів — і більше нічого. Апарат 3 висить на 12 метрів вище за них сумарно, тому число додатне й велике; апарат 4 нижчий за обох своїх сусідів, і в нього виходить −12.

Перевірка на здоровий глузд: якщо всі висять на однаковій висоті, кожна дужка дорівнює нулю, і весь добуток $L x$ нульовий. Лапласіан рухає лише те, що розсинхронізоване — саме через нульову суму рядків.

Із власних чисел лапласіана нас цікавить друге найменше, $λ_{2}$ , — алгебраїчна зв'язність. Нуль означає, що рій розпався на окремі групи; що більше $λ_{2}$ , то швидше рій доходить спільного рішення. Це не те саме, що «більше каналів»: далі ви побачите ланцюг і зірку з однаковими чотирма ребрами, у яких $λ_{2}$ відрізняється у 2,6 раза, а час збирання — у 2,4 раза. Додам одразу застороги, щоб не винести з лекції зайвого: це справедливо для показаного далі протоколу, а не для довільного закону керування.

Протокол консенсусу: правило одного апарата

Тепер найцікавіше: лапласіан — це не лише спосіб описати рій, а й готовий закон керування ним.

Звідки береться керування «через матрицю». Апаратові не потрібно знати весь лапласіан: щоб порахувати свій рядок, досить власної висоти й висот сусідів. Кожен виконує те саме локальне правило — питає в сусідів їхню висоту, складає різниці, зсувається на маленький крок; жоден не бачить рою цілком. Запис $u = - L x$ просто збирає всі $N$ таких правил в один рядок — і саме завдяки цьому про рій можна щось довести: чи зійдеться він узагалі, до якого значення і як швидко.

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

Правило для кожного апарата звучить так: «дивися на сусідів і рухайся в їхній бік». Вище за сусідів — знижуйся, нижче — піднімайся, пропорційно різниці.

u_{i} = \sum_{j \in N_{i}} (x_{j} - x_{i})

Це той самий вираз, що ми рахували на попередній картці, лише зі зворотним знаком: $u = - L x$ . Апарат 3 висів на 12 м вище за сусідів — отже, отримує команду знижуватися.

Алгоритм цілком. Кожен апарат, сто разів на секунду, робить три дії:

1Спитати висоти сусідів

2Скласти різниці

3Зсунутися на малий крок

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

x_{i} \leftarrow x_{i} + Δ t \cdot u_{i}

Крок $Δ t$ малий (у нас 0,01 с) — апарат не стрибає до сусіда, а посувається на сантиметри. Важлива деталь реалізації: всі нові висоти рахують з одного знімка старих. Якщо оновлювати їх по черзі, поспіль у тому самому масиві, вийде вже інший алгоритм. Ось ці самі обчислення для апарата 1 на першому такті:

Величина	Значення
Своя висота	12,00 м
Висота єдиного сусіда (апарат 2)	4,00 м
Команда $u_{1} = 4, 00 - 12, 00$	−8,00
Зсув за такт: $0, 01 \cdot (- 8, 00)$	−0,08 м
Нова висота	11,92 м

Повторюємо для всіх п'яти, потім ще раз, і ще — тисячу разів.

Як рій сходиться: чотири кадри

Тепер прокрутимо цей цикл і подивимося на рій у чотирьох точках часу. Стрілка над стовпчиком — команда, яку апарат отримав цього такту.

Один такт для всього рою — це одне множення на матрицю:

x \leftarrow x - Δ t L x = (I - Δ t L) x

Підставимо початкові висоти. Добуток дає $L x = (8; - 13; 12; - 12; 5)$ , і за такт 0,01 с кожен апарат зсувається на соту частину свого числа:

x_{1} = (12; 4; 9; 2; 7) - 0, 01 \cdot (8; - 13; 12; - 12; 5) = (11, 92; 4, 13; 8, 88; 2, 12; 6, 95)

Сума висот при цьому не змінилася — 34,00 м до такту й 34,00 м після, — а розкид упав:

Такт	Розкид, м	Сума висот, м
0	10,00	34,00
1	9,80	34,00
2	9,61	34,00
200	2,10	34,00
1000	0,098	34,00

Consensus on a chain shown at four time steps

Такт 0. Розкид величезний: від 2 до 12 м. Апарати 1 і 3 вищі за своїх сусідів — команда вниз; 2 і 4 нижчі — команда вгору. Апарат 5 висить на 7 м, а його єдиний сусід — на 2 м, тому він теж отримує команду знижуватися. Стрілки на рисунку показують напрямок команди, не її величину.

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

Такт 200 (2 секунди). Розкид упав із 10 м до 2 м. Видно, як «хвиля» пішла ланцюгом: апарат 1 просів найсильніше, бо був найдалі від усіх.

Такт 1000 (10 секунд). Висоти від 6,75 до 6,85 м: спрацював критерій зупинки — розкид став менший за 0,10 м, тобто у сто разів менший за початковий. Абсолютно рівними вони стануть лише в нескінченності.

І тепер головне, що справді дивує. Жоден апарат не знає середньої висоти рою. Апарат 1 бачить лише апарата 2. Немає ні центру, ні лідера, ні спільної трансляції. Проте рій сходиться точно до середнього початкових значень: $(12 + 4 + 9 + 2 + 7) / 5 = 6, 8$ м.

Чому саме до середнього? Кожен зв'язок працює в обидва боки однаково: скільки апарат 1 віддає апаратові 2, стільки той і отримує. Сума всіх висот за такт не змінюється — а отже, не змінюється й середнє. Рій може перерозподіляти висоту між собою, але не може створити чи втратити її сумарно. Сама по собі ця властивість не гарантує, що рій зійдеться: для цього потрібні ще зв'язний граф і достатньо малий крок, як у показаному досліді.

Код: консенсус на різних топологіях

python

import numpy as np

def laplacian(edges, n):
    A = np.zeros((n, n))
    for i, j in edges:
        A[i, j] = A[j, i] = 1.0
    return np.diag(A.sum(1)) - A

def consensus(L, x0, dt=0.01, tol=0.01):
    """Протокол u_i = sum_j (x_j - x_i). Повертає час збіжності 99 %."""
    x, t = x0.copy(), 0.0
    span0 = x.max() - x.min()
    while x.max() - x.min() > tol * span0:
        x = x - dt * (L @ x)
        t += dt
    return t, x.mean()

n = 5
x0 = np.array([12.0, 4.0, 9.0, 2.0, 7.0])       # початкові висоти, м
topo = {"ланцюг": [(0,1),(1,2),(2,3),(3,4)],
        "зірка":  [(0,1),(0,2),(0,3),(0,4)],
        "кільце": [(0,1),(1,2),(2,3),(3,4),(4,0)],
        "повний": [(i,j) for i in range(n) for j in range(i+1,n)]}
for name, E in topo.items():
    L = laplacian(E, n)
    lam2 = np.sort(np.linalg.eigvalsh(L))[1]
    t99, xf = consensus(L, x0)
    print(f"{name:7s} ребер={len(E):2d}  lambda_2={lam2:.3f}  "
          f"t99={t99:5.2f} c  спільна висота={xf:.2f} м")

Що робить код, рядок за рядком. laplacian() будує $L = D - A$ зі списку ребер: ставить одиниці в матрицю суміжності, підсумовує рядки для степенів і віднімає. consensus() крутить цикл, і весь протокол — це один рядок: x = x - dt * (L @ x). Множення L @ x дає кожному апаратові його розбіжність із сусідами, крок dt перетворює її на маленьке зміщення. Цикл зупиняється, коли розкид висот став у сто разів меншим за початковий, — це й є «збіжність 99 %».

Передбачте перед запуском: яка топологія зійдеться найшвидше і чи буде спільна висота однаковою для всіх чотирьох.

Результат коду: топологія проти часу

text

ланцюг  ребер= 4  lambda_2=0.382  t99= 9.96 c  спільна висота=6.80 м
зірка   ребер= 4  lambda_2=1.000  t99= 4.23 c  спільна висота=6.80 м
кільце  ребер= 5  lambda_2=1.382  t99= 2.82 c  спільна висота=6.80 м
повний  ребер=10  lambda_2=5.000  t99= 0.90 c  спільна висота=6.80 м

Збіжність консенсусу на топології ланцюга проти кільця

Придивіться до форми кривих. У ланцюгу крайні апарати довго тягнуться до середини: апарат 1 бачить лише апарата 2, той лише 1 і 3 — новина про те, що на іншому кінці рою висота зовсім інша, доходить не одразу, а переповзає ланкою за ланкою. У кільці той самий рій має два шляхи замість одного, і криві стягуються втричі з половиною швидше: 2,82 с проти 9,96 с.

Спільна висота однакова для всіх топологій — 6,80 м, тобто середнє початкових значень. Відрізняється лише час, і він падає разом зі зростанням $λ_{2}$ . Найдешевша інженерна дія в усій темі — замкнути ланцюг у кільце: одне додаткове ребро піднімає $λ_{2}$ з 0,382 до 1,382 і скорочує час збіжності з 9,96 с до 2,82 с.

Одна обов'язкова засторога: числа справедливі саме для цього протоколу з одиничними вагами. $λ_{2}$ не задає час збіжності для довільного контролера — це властивість показаного експерименту, а не універсальний закон. Зверніть увагу й на те, що зірка має ті самі чотири ребра, що й ланцюг, а збирається у 2,4 раза швидше (4,23 с проти 9,96 с): справа не в кількості каналів, а в тому, як вони розставлені.

Консенсус: топологія проти часу

П'ять апаратів на різних висотах домовляються про спільну. Закон один і той самий, змінюється лише те, хто кого чує.

0.382алгебраїчна зв'язність λ₂

0.00 смодельний час

2.70 мнайбільше відхилення

12.1 стеоретична межа t₉₉

спільна висота 6.80 м — середнє початкових, однакове для всіх топологій

Повний граф має вдвічі більше ребер за кільце, а виграє в часі приблизно вчетверо: платимо за кожне ребро каналом зв'язку, а отримуємо тільки приріст λ₂. Саме тому «додати зв'язків» — не завжди правильна інженерна дія.

Типові непорозуміння

П'ять тверджень, які звучать переконливо й усі хибні. Якщо ви ловите себе на будь-якому з них — поверніться до відповідної картки.

Звучить так	Насправді
«Автопілот — це зручність, апарат і сам полетить»	Без стабілізації збурення не гасяться: похилившись, апарат рушає вбік і відхиляється далі
«Є GPS — навіщо якийсь фільтр»	GPS шумить і в приміщенні зникає, інерціальні датчики спливають. Фільтр поєднує прогноз і вимірювання з урахуванням їхньої невизначеності
«Контур стійкий, отже добрий»	Стійкий контур може прибирати відхилення 6 секунд замість 1,5 — і не вкластися у вимоги задачі
«Кількість каналів повністю визначає швидкість»	Самого підрахунку ребер замало: зірка й ланцюг мають по чотири, а збираються з різницею у 2,4 раза. Важать і кількість, і розстановка, і ваги
«Кути Ейлера застаріли, всі рахують кватерніонами»	Зазвичай оператору показують кути, а всередині рахують інше. Це не суперечність, а різні представлення під різні задачі

Висновки: що з цього повернеться далі

Тема лекції	Де знадобиться
ПІД, три показники якості, тяга ↔ оберти, компенсація ваги, три контури каскаду та їхні частоти	практика: висіння й налаштування регулятора
Під політикою лишається контур стабілізації; вона задає завдання, а не сигнали моторів	Модуль 06: тяга й кутові швидкості
Матриця повороту замість кутів Ейлера	Модуль 06: вектор спостереження політики
Дрейф датчиків, фільтр Калмана	Модуль 04: чому спостереження не дорівнює станові
Лапласіан, $λ_{2}$ , консенсус	Модуль 03 (стрій), Модулі 10 і 11 (метрики зв'язності)

Усе інше в цій лекції — контекст. Виводи стійкості, алгебра поворотів і спектральна теорія графів потрібні тим, хто проєктує контури керування; для завдань курсу достатньо назвати інструмент і сказати, що він дає.

Перехід до Модуля 03

Модуль 02 дав мову: орієнтація описує апарат, лапласіан описує рій, а $λ_{2}$ зв'язує топологію зі швидкістю узгодження. Тепер цю мову треба перетворити на закони керування групою.

Модуль 03 · Керування роєм: строї та топології охопить: правила Boids — розділення, вирівнювання, злиття; потенціальні поля й обхід перешкод; класи формаційного керування за вимірюваною величиною; вплив затримок у каналі; метрики якості строю та його безпеки.

Якщо Модуль 02 відповідав на питання «як це описати», то Модуль 03 відповідає на питання «як цим керувати» — і де класичні закони впираються в межу.

Література та ресурси

Quan Quan. Introduction to Multicopter Design and Control. Springer, 2017. ISBN 978-981-10-3382-7. Розділ 5 (системи координат і орієнтація), розділ 9 (оцінювання стану), розділ 10 (стійкість і керованість), розділ 11 (низькорівневе керування).
Olfati-Saber R., Murray R. M. Consensus Problems in Networks of Agents with Switching Topology and Time-Delays. IEEE Transactions on Automatic Control, 2004, 49(9), 1520–1533. DOI 10.1109/TAC.2004.834113
Fiedler M. Algebraic Connectivity of Graphs. Czechoslovak Mathematical Journal, 1973, 23(2), 298–305.
Ren W., Beard R. W. Consensus Seeking in Multiagent Systems under Dynamically Changing Interaction Topologies. IEEE TAC, 2005, 50(5), 655–661 (умова орієнтованого остовного дерева).
Kalman R. E. A New Approach to Linear Filtering and Prediction Problems. Journal of Basic Engineering, 1960, 82(1), 35–45. DOI 10.1115/1.3662552
Groves P. D. Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems. 2nd ed. Artech House, 2013. Розділи 3, 5, 14.
Реалізації для практики: NumPy — лінійна алгебра · NetworkX — графи й лапласіан · FilterPy — фільтр Калмана

Модуль 02 · Математичні основи роїв БПЛА ​

Зміст лекції ​

Результати навчання ​

Частина 1 · Апарат у просторі ​

Дві системи координат і розклад тяги ​

Три представлення орієнтації ​

Три кути на самому апараті ​

Матриця повороту: той самий приклад із числами ​

Кватерніон: вісь і кут замість трьох чисел ​

Особливе положення кутів Ейлера ​

Частина 2 · Керування й оцінювання стану ​

Стійкість рівноваги і чому мультикоптер нестійкий ​

Скільки часу апарат прибирає відхилення ​

ПІД: що робить кожна складова ​

Стала похибка: звідки береться й чим лікується ​

Як міряють якість регулятора ​

Скільки лишається в запасі ​

Чому контури працюють на різних частотах ​

Числення шляху: навігація без жодного орієнтира ​

Чому позиція спливає сама собою ​

Фільтр Калмана: ідея на побутовому прикладі ​

Один такт фільтра з числами ​

Частина 3 · Граф рою ​

Навіщо роєві граф ​

Дві таблиці, якими описують рій ​

Що робить лапласіан: множимо на живі числа ​

Протокол консенсусу: правило одного апарата ​

Як рій сходиться: чотири кадри ​

Код: консенсус на різних топологіях ​

Результат коду: топологія проти часу ​

Типові непорозуміння ​

Висновки: що з цього повернеться далі ​

Перехід до Модуля 03 ​

Література та ресурси ​