ChSTU repository

 
 
 3 
ЗМІСТ 
 
ВСТУП  
РОЗДІЛ 1. Аналіз методів і ключових технологій автономного польоту 
квадрокоптера  
1.1. Система управління польотом квадрокоптера 
1.1.1. Математична модель квадрокоптера 
1.1.2. Алгоритми управління польотом квадрокоптера 
1.1.3.  Моделювання польоту квадрокоптера по заданому маршруту 
1.2. Основні способи вирішення завдання планування маршруту 
1.2.1.  Планування глобального маршруту на основі мурашиного алгоритму 
1.2.2.  Стратегія відстеження заданого маршруту 
1.2.3.  Методи обльоту перешкод для БПЛА 
1.3. Інтегрована навігаційна система БПЛА 
1.3.1. Застосування безплатформеної інерційно-супутникових навігаційних 
систем для БПЛА 
1.3.2. Візуальна навігаційна система на основі комп'ютерного зору і 
алгоритму EKF-SLAM 
1.3.3. Основні особливості інтегрованих БІНС із застосуванням 
барометричного висотоміра і радіовисотомір  
Висновки до розділу 1 
РОЗДІЛ 2. Розробка системи управління польотом квадрокоптера з 
можливістю обліт перешкод у складному середовищі  
2.1. Загальна структура автономної системи управління польотом 
квадрокоптера 
2.2. Алгоритм планування глобального маршруту на основі хмарно-
точкової карти і поліпшеного мурашиного алгоритму 
2.2.1. Хмарно-точкова пошукова карта маршруту  
2.2.2. Розробка поліпшеного мурашиного алгоритму для глобального 
планування 
 
 
 4 
2.3. Відстеження спланованого глобального маршруту на основі 
поліпшеного методу руху по лінії візування 
2.4. Розробка алгоритму обльоту перешкод на основі управління поворотом 
вектора швидкості політ 
2.4.1. Передбачення стану руху перешкод 
2.4.2. Стратегія обльоту нерухомих перешкод 
2.4.3. Стратегія обльоту рухомих перешкод 
2.5. Розробка алгоритму підвищення стабільності польоту на основі 
багаторежимної системи стабілізації 
Висновки до розділу 2 
РОЗДІЛ 3. Розробка математичної моделі автономної системи керування 
польотом квадрокоптера 
3.1 Макетування візуальної навігаційної системи 
Висновки до розділу 3  
РОЗДІЛ 4. Розроблення автоматичної системи керування 
квадрокоптером 
4.1 Датчики і їх основні характеристики  
4.2 Розробка інтерфейсу навігаційної інформації 
4.3 Моделювання автономного польоту квадрокоптера в складному 
середовищі 
Висновки до розділу 4  
ВИСНОВКИ  
СПИСОК ВИКОРИСТАНИХ ДЖЕРЕЛ 
ДОДАТОК А Акт впровадження 
ДОДАТОК Б Тези публікації. 
ДОДАТОК В Презентація кваліфікованої роботи 
  
 
 
 5 
ВСТУП 
 
Актуальність роботи. Квадрокоптер – це безпілотний літальний апарат 
(БПЛА), обладнаний чотирма двигунами з повітряними гвинтами (пропелерами), 
які забезпечують його тягу. На сьогодні подібні пристрої широко та різноманітно 
використовуються, проте їхнє застосування переважно обмежене ручним 
дистанційним керуванням оператора з пульта та виконанням простих маршрутів 
польоту. Це обумовлено переважно низькою автономністю квадрокоптерів, 
оскільки автоматичне облітання різних перешкод в складних умовах та автономна 
навігація без сигналу супутникової навігаційної системи (СНС) представляють 
труднощі. 
Актуальною є задача розробки автономної системи управління польотом, що 
дозволяє здійснювати політ квадрокоптера  спланованим маршрутом з можливістю 
обльоту перешкод і автономною навігацією. 
Робоче середовище польоту квадрокоптера стає все більш складним у зв'язку 
з розширенням сфери застосування цих літальних апаратів. Воно може бути 
наповнене різними перешкодами (рухомими і нерухомими). В ньому може бути 
відсутнім сигнал СНС. Більшість алгоритмів планування маршруту і систем 
управління, описаних в літературі, досить добре опрацьовані для застосування в 
детермінованому статичному відомому середовищі, але в складному невідомому 
середовищі з різними перешкодами і впливами атмосфери не забезпечують 
достатню працездатність. Існуючі для квадрокоптера комплексні навігаційні 
системи (КНС) не можуть забезпечити достовірність навігаційної інформації в разі 
відсутності сигналу СНС. По всьому світу ведуться інтенсивні дослідження по 
технології навігації квадрокоптера з використанням візуальної навігаційної 
системи (ВНС) на основі одночасної локалізації і картографування (SLAM), за 
допомогою комп'ютерного зору, орієнтирів, карти місцевості та ін. Використання 
ВНС дозволяє досягнути високої точності для квадрокоптера, особливо в 
невідомому динамічному середовищі без сигналу СНС, але дане використання 
обмежене для квадрокоптера із-за низької швидкості обчислення алгоритмів 
 
 
 6 
обробки зображень і SLAM. 
Таким чином, рішення і дослідження зазначених проблем вельми актуальне і 
має важливе практичне значення. 
Мета роботи і завдання дослідження. Мета дослідження даної роботи 
полягає в розробці автоматичної системи управління польотом квадрокоптера, що 
дозволяє забезпечити автономний політ за спланованим маршрутом з можливістю 
обльоту перешкод в складному середовищі та автономною навігацією. 
Для досягнення мети роботи формулюються наступні завдання: 
1. Розробка швидкого алгоритму планування просторового глобального 
маршруту до певного середовища з нерухомими перешкодами; 
2. Розробка алгоритму відстеження спланованого маршруту в режимі 
реального часу; 
3. Розробка швидкого алгоритму обльоту перешкод в складному середовищі 
для квадрокоптера. 
4. Розробка системи траєкторного і кутового управління польотом 
квадрокоптера по заданому маршруту; 
5. Створення деталізованої математичної моделі квадрокоптера з 
урахуванням впливу гіроскопічних ефектів гвинтів і моторів, а також врахування 
впливу вітру і ефекту екрану поверхні землі. 
Методи дослідження. При вирішенні завдань, що розглядаються в роботи, 
були використані методи математичного аналізу і моделювання, синтезу лінійних 
(ПІД) і нелінійних (бекстеппінг) регуляторів, управління поворотом вектора 
швидкості, тести хі-квадрат за залишковою помилкою і станом, одночасної 
локалізації і картографування, призначення ваги для злиття даних, лінійної і 
нелінійної калмановської фільтрації. У процесі математичного моделювання 
застосовувалися обчислювальні системи: програмний комплекс "Універсальний 
механізм" (УМ), інструмент 3D-моделювання Solidworks, C ++ (мова 
програмування), середовище моделювання MATLAB. 
  
 
 
 7 
РОЗДІЛ 1 
 АНАЛІЗ МЕТОДІВ І КЛЮЧОВИХ ТЕХНОЛОГІЙ АВТОНОМНОГО 
ПОЛЬОТУ КВАДРОКОПТЕРА 
 
 
У загальному сенсі, завдання управління автономним польотом квадрокоптера 
можна сформулювати наступним чином: необхідно досягти заданої цільової 
точки, рухаючись з початкової точки, і при цьому уникати зіткнень з 
перешкодами. Для досягнення цієї мети потрібно розробити ефективний 
маршрут польоту, щоб уникнути перешкод, і створити систему управління 
траєкторією та кутовим положенням, що забезпечить перехід квадрокоптера від 
початкового положення до цільової точки. 
- Незважаючи на видиму простоту завдання, існують численні труднощі, 
обмеження та умови. До найважливіших з них відносяться: 
- - Необхідність забезпечення можливості польоту квадрокоптера по заданому 
маршруту з утриманням стабільності його кутового положення. 
- - Обхід статичних та уникання динамічних перешкод, що вимагає побудови 
або постійного оновлення карти навколишнього середовища в режимі реального 
часу. 
- - Визначення місцезнаходження квадрокоптера в просторі в реальному 
часі незалежно від наявності чи відсутності сигналів з системи глобального 
позиціонування, для реалізації алгоритмів автономного управління польотом.. 
Отже, головною трудністю в автономному польоті квадрокоптера є 
складність автоматичного обліту різноманітних перешкод і навігації в складному 
динамічному середовищі при відсутності сигналу системи глобального 
позиціонування (СНС). Хоча в останні роки вітчизняні та закордонні вчені провели 
безліч досліджень по ключовим технологіям для автономного управління польотом 
БПЛА, але закінченою системи, реалізованою для автономного польоту 
квадрокоптера в зазначених вище умовах, поки не з'явилося. 
У зв'язку з цим у даному розділі було проведено огляд наявних методів 
 
 
 8 
розв'язання поставлених завдань, а також проведений аналіз можливостей і 
обмежень цих методів щодо вирішення завдання автономного польоту 
квадрокоптера. Для полегшення сприйняття, сформульовані вище завдання було 
об'єднано в три групи. 
 
 
1.1. Система управління польотом квадрокоптера 
 
У численних публікаціях обговорюються і пропонуються різні підходи до 
створення алгоритмів і систем управління польотом квадрокоптера, зокрема 
використання лінійних квадратичних регуляторів, успішно вирішують завдання 
стабілізації при зависанні, але не працюють при істотних нелінійних і перехресних 
зв'язках, використання змінного режиму управління, простого і надійного, але 
вимагає адаптації логіки перемикань до режимів польоту. У роботі наданий 
алгоритм відстеження траєкторії квадрокоптера, що базується на ПД-регуляторі та 
методі "бекстеппінг" з використанням коригуючого фільтру. В роботах  
пропонується нелінійний контролер на основі нейронної мережі, а в  - алгоритм 
«реального часу» планування траєкторії і запобігання зіткнень. В роботі  
розглядається адаптивний алгоритм відстеження положення для вертикального 
зльоту і посадки при обмежених зовнішніх збурень. Для створення системи 
управління можна використовувати методи, які застосовуються для літальних 
апаратів та інших типів техніки. У даній роботі не пропонуються нові підходи; 
замість цього досліджуються можливості використання простих алгоритмів 
регулювання для досягнення автономного польоту квадрокоптера вздовж заданого 
маршруту. Ці алгоритми базуються на традиційних принципах, зокрема на 
використанні ПІД-регуляторів і методу, відомого в літературі як "бекстеппінг", 
який ґрунтується на принципах функцій Ляпунова. Для проведення такого 
дослідження потрібна більш деталізована математична модель руху. 
 
 
 
 9 
1.1.1. Математична модель квадрокоптера 
У даному дослідженні була розроблена більш докладна математична модель 
квадрокоптера з урахуванням впливу ефекту екрану землі, впливу вітру, 
гіроскопічних моментів двигунів та гвинтів і інших факторів. Аналіз аеродинаміки 
гвинта за умов впливу вітру та фізична модель квадрокоптера у режимі зависання 
представлені на зображенні. 1.1. 
 
Рис. 1.1. (а) - Аеродинамічний аналіз гвинта при дії вітрового обурення; 
(б) - Фізична модель квадрокоптера в режимі зависання 
 
На рис. 1.1 для i-го гвинта: wbi  - вектор вітру в зв'язаній системі координат,  
wdi  - індукована швидкість, wmi - результуюча індукуюча швидкість гвинта, Pti  - 
сила тяги гвинта, Pwi  - сила тяги при дії вітру. 
Сила тяги і індукована швидкість гвинта можуть бути записані таким чином 
: 
                             (1.1) 
де С pi  - коефіцієнт сили тяги,   - щільність повітря, Si  - площа диска 
обертання гвинта; S pi  - проєкція площі гвинта. 
Результуюча індукційна швидкість гвинта: 
 
 
 10 
                                                  (1.2) 
За даними літератури , сила тяги i-ого гвинта при дії вітрового обурення: 
                                  (1.3) 
Під час польоту близько до поверхні землі виявляється екранний ефект, який 
проявляється у зростанні тяги на крилі, що обертаються, зумовлений взаємодією з 
поверхнею. Це призводить до того, що на обертовий диск збільшується сила тяги. 
Таким чином, квадрокоптеру потрібно менше енергії для утримання у стані 
польоту. 
Відношення сили тяги без урахування екранного ефекту до сили тяги з 
урахуванням екранного ефекту знаходиться в зазначеному взаємозв'язку: 
                                     (1.4) 
де h - висота польоту; r - радіус диска обертання i-го гвинта; V- швидкість 
польоту в нормальній земній системі координат. 
Результуюча сила тяги квадрокоптера: 
                             (1.5) 
Сила опору повітря може бути отримана з використанням рівняння 
динамічного тиску : 
                                 (1.6) 
де [Sx ,S ,S ]T T
y z  - площа квадрокоптера, піддається впливу вітру; [wx , wy , wz ] - 
швидкість вітру; [Cx ,C T
y ,Cz ]  - коефіцієнт опору. 
 
 
 11 
Для спрощення процесу розрахунку площі впливу і коефіцієнта опору моделі 
гвинтів квадрокоптера представимо у вигляді тонких напівпрозорих дисків, а 
модель корпусу квадрокоптера - у вигляді комбінації сфери і чотирьох циліндрів і 
побудуємо ці моделі в програмному середовищі Solidworks, як показано на рис. 1.2: 
 
Рис. 1.2. Спрощена модель квадрокоптера 
 
Площа квадрокоптера, що піддається дії вітру, можна записати в такий 
спосіб: 
                                    (1.7) 
де PS  - коефіцієнт, який висловлює напівпроникний характер обертового 
гвинта;   - кут нахилу (поєднання тангажу і крену кутів); S0  - площа корпусу 
квадрокоптера, піддається впливу вітру. 
Відповідно до формули числа Рейнольдса: 
                                                           (1.8) 
де v0 - швидкість обтікання; d - характерний розмір квадрокоптера (для 
сфери, циліндра і диска є діаметром);  - динамічна в'язкість повітря. 
Коефіцієнти опору повітря Cx ,Cy ,Cz  не є постійними у всьому діапазоні 
умов обтікання. На рис. 1.3 показана залежність коефіцієнта опору повітря від 
 
 
 12 
числа Рейнольдса .  
 
Рис. 1.3 Зв'язок між коефіцієнтом опору повітря та числом Рейнольдса 
 
Сила опору повітря і сила ваги: 
                        (1.9) 
де m - маса квадрокоптера; g - прискорення сили тяжіння. 
Сила тяги в нормальній земній системі координат: 
                           (1.10) 
де R - матриця переходу;  ,,  - кути нишпорення, тангажу, крену. 
 
Рівняння динаміки руху центру мас в нормальній земній системі координат: 
                         (1.12) 
З урахуванням симетрії пристрою та припущення, що центр мас 
розташований у початку координат пов'язаної системи, рівняння динаміки 
кутового руху в зв'язаній системі координат можна виразити наступним чином: 
 
 
 13 
 
де wx , wy , wz  - проєкція вектора кутової швидкості апарату на зв'язану систему 
координат; M Rx , M Ry , M Rz  - проєкція результуючого моменту; I x , I y , I z  - 
осьові моменти інерції апарату; M qx , M qy , M qz  - моменти, що створюються 
гвинтами, M mx , M my i M px , M py  - гіроскопічні моменти двигунів і гвинтів, 
M wx , M wy , M wz  - моменти, створені із зусиль повітря, M gx , M gy , M gz   - моменти, які 
виникають внаслідок дії сили тяжіння та ефекту землі. Якщо не враховувати 
інерційність гвинтів під час зміни їхніх кутових швидкостей обертання, то вказані 
моменти можна виразити наступним чином: 
 
де l - відстань від центру мас до осі гвинта, Im i I p  - моменти інерції ротора 
і гвинта; mPi  - коефіцієнт моменту. 
Зміни кутів Ейлера визначаються через проєкцію кутової швидкості 
кінематичними рівняннями Ейлера: 
 
 
 14 
 
 
                        (1.18) 
 
 
Задачу управління траєкторією польоту квадрокоптера можна розглядати як 
послідовність завдань перельоту до наступної точки маршруту або руху вздовж 
заданих ділянок типових траєкторій, таких як пряма лінія чи коло. У такому 
випадку система автоматичного управління може бути створена як система з 
зворотнім зв'язком, що відстежує задану траєкторію. Зокрема, можна виділити 
канали управління висотою і рухом в горизонтальній площині. Стабілізація та 
управління в вертикальному напрямку здійснюється зміною загальної сили тяги. 
Горизонтальне переміщення апарата відбувається за допомогою 
горизонтальної проєкції загального вектора тяги, відхиленого від вертикалі. У 
даному випадку відхилення вектора тяги досягається за рахунок зміни кутів 
тангажа і крену при фіксованому положенні кута рискання. Зміна кутового 
положення досягається шляхом диференційованого управління швидкостями 
обертання гвинтів, що призводить до відповідних різниць у їх силах тяги і 
моментах. 
Підсистему, яка забезпечує необхідні значення кутових параметрів і висоти 
за рахунок зміни тяги гвинтів, можна назвати системою орієнтації та стабілізації. 
Підсистема, що відстежує задані траєкторії, може бути визначена як система 
траєкторного управління. Слід відзначити, що цей підхід не є найбільш 
ефективним, але надає найбільш зрозумілий розподіл управління між різними 
каналами. 
На рисунку 1.4 показана структура системи управління, де числами 
позначені: 1 - задана траєкторія; 2 - коригувальні пристрої (КУ) підсистеми 
траєкторного управління; 3 - перетворювач координат; 4 - регулятор підсистеми 
орієнтації і стабілізації; 5 - розподільник сигналів; 6 - обмежувач напруги; 7 - 
модель гвинтокрила групи; 8 - модель квадрокоптера.. 
 
 
 15 
 
Рис. 1.4. Схема системи управління квадрокоптера 
 
Пропонується використовувати один із відомих методів для розрахунку 
алгоритмів роботи підсистем стабілізації та траєкторного управління. Вибір 
рекомендується робити на користь методів, які, при порівнянні якості, вважаються 
найбільш простими. Таким чином, для кожної підсистеми проведено розрахунок 
коригуючих пристроїв у формі ПІД-регуляторів за допомогою методу 
"бекстеппінг" (Backstepping), який вже відомий в літературі. Після цього, шляхом 
порівняння отриманих результатів вибрано найбільш відповідний регулятор для 
кожної підсистеми. 
Для уникнення дублювання викладу, розрахунок регуляторів для кожної 
підсистеми продемонстровано на прикладі лише одного методу: "бекстеппінг" в 
алгоритмі стабілізації кутового положення та ПІД-регулятори в алгоритмі 
відстеження траєкторій. 
 
1.1.2. Алгоритми управління польотом квадрокоптера 
Проєкція горизонтальної сили мають вигляд: 
 
Звідси можна визначити кути крену і тангажу, при яких створюються 
необхідні впливи при відомій сумарній тязі: 
 
 
 16 
 
Необхідно звернути увагу, що такий підхід до траєкторних управлінь 
пред'являє дуже високі вимоги до швидкодії та точності підсистеми орієнтації і 
стабілізації, що викликає необхідність в більш глибокому дослідженні, що 
виходить за рамки цієї роботи. Тут будемо вважати результат задовільним, якщо 
він підтверджується моделюванням. 
Керуючі впливи U xd i U yd , а також U zd U1  для каналу управління висотою 
можна отримати, розглядаючи підсистему траєкторного управління як систему 
регулювання, яка відслідковує необхідні координати центру мас, зокрема - як 
вихідні сигнали ПІД-регулятора за відхиленнями координат центру мас від 
необхідних: 
 
Входами підсистеми орієнтації і стабілізації кутового положення являються 
заданими підсистемами траекторного управління сигнали  d ,  d , d , а виходами - 
параметри кутового руху об'єкта. Керуючий алгоритм цієї підсистеми повинен 
формувати керуючі сигнали, що забезпечують створення необхідних моментів 
M Rx , M Ry , M Rz  при умові, що загальна сила тяги гвинтів відповідає необхідній для 
вертикального руху. Якщо знехтувати динамікою і обмеженнями двигунів, 
гіроскопічними моментами і аеродинамічними моментами не несучої частини 
апарату, то керуючі сигнали повинні бути з точністю до коефіцієнта рівними 
моментам M Rx , M Ry , M Rz . 
Метод "бекстеппінг" включає в себе представлення складної системи в формі 
 
 
 17 
ланцюжка вкладених підсистем. Для кожної підсистеми формуються допоміжні 
сигнали, і створюються функції Ляпунова, залежні від цих сигналів. Спосіб 
досягнення стійкості системи в цілому полягає в виконанні критеріїв стійкості по 
Ляпунову за послідовним вибором цих сигналів для кожної підсистеми. Процедура 
"бекстеппінг" отримала свою назву від покрокового обходу інтеграторів 
зворотніми зв'язками. У випадках, коли розглядається кутовий рух літального 
апарату при малих кутах тангажу і крену, коли похідні кутів можна вважати 
рівними відповідним кутовим швидкостям, процедура може стати регулярною і 
досить простою. Для цього конкретного випадку кутового руху літального апарату 
можна наблизити до трьох підсистем.: 
 
Слідуючи приведеному алгоритму, введемо для підсистеми S1  допоміжний 
керуючий сигнал z1   d   і відповідну функцію Ляпунова V1(z1)  z2
1 / 2 , похідна 
  
якої V1(z1)  z1 z1  z1( d wx ) . 

Другий допоміжний керуючий сигнал сформуємо у вигляді z2  wx  d  k1z1  
з відповідною функцією Ляпунова V2 (z1, z2 )  (z2 2
1  z2 ) / 2 , похідна якої 
    
V2 (z1, z2 )  z1( d wx )  z2 (wx d  k1 z1) . 
 
Приймаючи для системи стабілізації yd   d  0 , отримаємо 
 
V (z , z )  z w  (k 2 1)z z  k z2  k z2
2 1 2 2 x 1 1 2 1 1 2 2 . 
 
Щоб підсистема була стійкою, тобто щоб V2 (z1, z2 )  0 , причому V2 (z1, z2 )  0  
 
 
 18 
тільки коли z1  z2  0 , приймемо: 
                   (1.23) 
Тоді керуючий сигнал матиме вигляд: 
 
 
2 
Функція Ляпунова V3 (z1) V2 (z1, z2 )  z1  (wx  d  k1z1)2
 / 2  0 , а її 
 
 
похідна 3 (z1) V2 (z1, z2 )  k1z
2 2 2
1  k2 z2  k1z1  k2 (wx  d  k1z1)2  0 , то є 
замкнута підсистема – стійка. 
Таким чином можна отримати інші керуючі сигнали: 
 
де k3  0; k4  0; k5  0; k6  0.  
 
1.1.3. Моделювання польоту квадрокоптера по заданому маршруту 
Продуктивність алгоритму була перевірена шляхом моделювання польоту по 
заданих траєкторіях. На рисунку 1.5 представлені результати моделювання в 
процесі відстеження траєкторії, яка включає в себе етапи: AB - зльот; BC - 
рівномірний прямолінійний рух; CD - рівномірний прискорений прямолінійний 
рух; DE - рівномірний рух навколо точки (5, 10, 15) по горизонтальному колу 
радіусом 5 м; EF - рівномірний прямолінійний рух; FG - рівномірний рух навколо 
точки (-5, 10, 10) по горизонтальному колу радіусом 10 м.; GH - посадка. Загальні 
результати задовільні, але присутня можливість покращення, зокрема у 
виправленні перерегулювання по висоті та ушвидшенні процесів. 
 
 
 19 
 
Рис. 1.5. Відстеження траєкторії 
 
Значення коефіцієнтів ПІД-регуляторів для систем траєкторного і кутового 
керування по відповідним похідним, обраними методом Циглера-Ніколса, 
приведені в Таблиці 1. 
Таблиця 1.1 
Значення коефіцієнтів ПІД-регуляторів 
 
Можна досягти необхідної якості перехідних процесів, вибираючи відповідні 
значення коефіцієнтів "бекстеппінг" - регуляторів в рівняннях (1.24) і (1.25). 
Відповідні значення цих коефіцієнтів подані в таблиці 1.2. 
 
 
 
 
 20 
Таблиця 1.2. 
Значення коефіцієнтів «бекстеппінг» - регуяторів 
 
На рис. 1.6 і рис. 1.7 показані зміни координат x, y, z: 
Час, с Час, с 
Час, с Час, с 
Час, с Час, с  
Рис. 1.6. Зміни координат                              Рис. 1.7. Зміна координат 
ПІД-регулятора                                     «бекстеппінг» регулятора 
  
Результати моделювання свідчать про те, що зміна координат центру мас 
квадрокоптера майже однакова для обох регуляторів. Однак ПІД-регулятор має 
більш просту структуру і менше сигналів зворотного зв'язку. Таким чином, для 
керування траєкторією польоту квадрокоптера обрані ПІД-регулятори. 
На рис. 1.8 і рис. 1.9 показані зміни кутового положення ( ,, ) : 
Час, с Час, с 
Час, с Час, с 
Час, с Час, с  
Рис. 1.8. Зміни кутового положення          Рис 1.9. Зміни кутового положення 
ПІД-регулятора                                  «бекстеппінг» регулятора 
 
 
 21 
Висновки моделювання вказують на те, що варіації кутового положення 
регулятора методу "бекстеппінг" виявляються помітно меншими, ніж відповідні 
зміни для ПІД-регулятора, і графік стає більш плавним. Тому для керуванням 
кутовим положенням польоту квадрокоптера обрані «бекстеппінг» регулятори. 
 
 
1.2. Основні способи вирішення завдання планування маршруту 
 
Планування маршруту (ПМ), що виключає зіткнення з перешкодами, є одним 
з основних питань при роботі рухаючих роботів - як наземних, так і повітряних. 
Рішення завдання ПМ необхідно розглядати з трьох сторін: глобальне ПМ, 
відстеження заданого маршруту і обліт перешкод (ОП). 
 
1.2.1. Планування глобального маршруту на основі мурашиного 
алгоритму 
Алгоритм планування глобального маршруту повинен розглядати методи 
обходу відомих перешкод. Це здійснюється за допомогою оптимізованого 
пошукового алгоритму планування маршруту. В даний час існує багато методів, 
пропонованих для вирішення завдання планування глобального маршруту. Для 
більшості відомих методів, таких як клітинне розбиття, дорожня карта, потенційне 
поле, перешкоджене вирішення проблем планування в складних умовах і адаптації, 
не вирішено питання стійкості. 
Мурашиний алгоритм (МА), що володіє паралельним і позитивним 
зворотним зв'язком, в останні роки широко використовується для планування 
маршруту в статичному та динамічному середовищі. У цих роботах для пошуку 
маршруту використовують сітки карти. 
Щоб зрозуміти, як функціонує алгоритм, потрібно розглянути поведінку 
мурашки, що рухається від мурашника в пошуках їжі. Спочатку мураха рухається 
у випадковому напрямку. Якщо мураха знаходить їжу, то вона повертається до 
мурашника, позначаючи пройдений шлях спеціальною хімічною речовиною - 
 
 
 22 
феромонами. Наступна мураха з більшою ймовірністю піде по стежці, вже 
поміченою феромонами. За допомогою феромона відбувається зв'язок між 
мурахами і пошук оптимального маршруту при відсутності централізованого 
управління і знань про всю мережу, а феромонні стежки є як би «колективною 
пам'яттю» мурашника. Однак феромон, як хімічна речовина, випаровується з 
плином часу, тому мурахи змушені освоювати нові маршрути і вибирати серед них 
оптимальний.  
m
В традиційному МА ймовірність pij  переходу мурахи m з позиції i в позицію 
j визначається формулою: 
               (1.26) 
де  ij  - кількість феромону на дузі ij; а - параметр впливу на  ij ;  jg  - 
величина, зворотна довжині дуги jg;   - параметр, що впливає на ij ; Sm  - безліч 
позицій, пройдених мурахою m; Dm  - розмір безлічі Sm . 
По завершенні чергової ітерації алгоритму відбувається випаровування 
феромону: 
                        (1.27) 
де m - номер мурашки з популяції; t - номер ітерації;  0,1  - швидкість 
випаровування феромону;  ij (t)  - приріст феромону; M - величина популяції 
мурах. 
Оновлення феромона: 
                         (1.28) 
де Q- запас феромону; Lm - довжина пройденого шляху. 
Швидкість обчислення традиційного МА є повільною, через що подібний 
 
 
 23 
алгоритм важко використовувати для планування глобального маршруту 
квадрокоптера в режимі реального часу в тривимірному середовищі. 
 
1.2.2. Стратегія відстеження заданого маршруту 
Безпосереднє відстеження спланованого маршруту алгоритмами, 
розглянутими в 1.1.2, виявляється практично неможливим, так як маршрут заданий 
у вигляді послідовності дискретних точок, розташування яких в загальному 
випадку не є достатньо рівномірним. Звичайно, можна апроксимувати маршрут 
гладкими кривими, але це призведе до ускладнення обчислювального алгоритму, 
роботу якого неможливо буде забезпечити в режимі реального часу. Крім того, 
якщо кривизна заданого маршруту і значення швидкості польоту великі, то через 
обмеженість наявних перевантажень квадрокоптер не зможе відстежити задані 
точки маршруту і уникнути зіткнення навіть із заздалегідь відомими перешкодами. 
Тому виникає проміжна задача визначення поточних (для кожного моменту часу) 
цільових точок, 
Завдання пропонується вирішувати на основі методу «L1»  (метод 
відстеження лінії візування в системах наведення), який є одним з популярних 
методів для відстеження маршруту завдяки простій реалізації та високій швидкості 
обчислень. 
Схема визначення потрібного прискорення цим методом наведено на 
рис.1.10, де: V - вектор швидкості квадрокоптера; L1  - вектор від центру мас 
квадрокоптера до опорної точки;  - кут попередження між векторами V та L1 ; d 
- найкоротша відстань від центру мас квадрокоптера до траєкторії; ac  - вектор 
нормального прискорення для управління польотом квадрокоптера; D - опорна 
точка на запланованому маршруті; O - центр допоміжної окружності; R - радіус 
допоміжної окружності; C - точка центру мас квадрокоптера. Координати центру 
мас квадрокоптера і вектор швидкості його польоту покладаються відомими. 
 
 
 24 
 
Рис. 1.10. Схема управління польотом квадрокоптера при проходженні 
по запланованому маршруту 
 
Якщо відстань між положенням центру мас квадрокоптера і положенням 
опорної точки на запланованій траєкторії рівне L1  (як показано на рис. 1.10), то 
нормальне прискорення польоту квадрокоптера визначається наступним чином: 
                             (1.29) 
З формули (1.29) видно, що ключовим для визначення нормального 
прискорення управління є визначення вектора L1 ,тобто вибір опорної точки. 
В реальності запланований маршрут складається з ряду дискретних точок, 
тому важко використовувати якісь математичні формули для опису цього 
маршруту і обчислення координат точки D і кута  . Про недоліки використання 
методу апроксимацій для отримання гладкого маршруту та обчислення координат 
точки D і кута   сказано вище. У нашій роботі для вирішення подібної проблеми 
пропонується вибирати опорні точки серед точок маршруту - в цьому випадку не 
треба окремо обчислювати координати точок D. 
Заданий маршрут може бути складений з великої кількості точок, тому наш 
метод (див. 2.3) може значно зменшити обсяг обчислення алгоритму, що дозволить 
легше реалізувати алгоритм в режимі реального часу. 
 
 
 
 25 
1.2.3. Методи обльоту перешкод для БПЛА 
БПЛА повинен мати можливість обльоту нерухомих, але раніше не 
конкретних, а також - рухомих перешкод, польоту серед рухомих людей і 
транспортних засобів, причому робити це з урахуванням похибок в його сенсорних 
системах. Задача стає значно більш складною у випадку, коли необхідно складати 
маршрут в реальному часі, при виявленні динамічних або невідомих заздалегідь 
перешкод сенсорною системою, і системі планування доводиться локально 
змінювати маршрут відповідно.. 
Проблема планування маршруту з обходом перешкод глибоко вивчена в 
літературі, було розроблено кілька методик. У статичному середовищі існує багато 
методів, пропонованих для вирішення завдання планування маршруту, таких як 
клітинне розбиття, імовірнісна дорожня карта, потенційне поле, графік видимості, 
швидко-досліджуване випадкове дерево  і інші. Однак, їх застосування ускладнено 
в складних умовах, що змінюються, не вирішено питання стійкості. Останнім часом 
широко використовуються методи, засновані на біологічному інтелекті, такі як 
генетичний алгоритм, МА, оптимізація рою частинок  та ін. 
Розширення проблеми планування маршруту в динамічному середовищі теж 
було ретельно досліджено. Нечітка логіка досить широко використовується для 
обходу рухомих перешкод при управлінні напрямком і швидкістю руху робота. 
Щоб поліпшити результативність нечіткої логіки при плануванні маршруту в 
динамічному середовищі, її часто використовують разом з іншими алгоритмами. 
Нейронна мережа теж може застосовуватися для уникнення зіткнення з рухомими 
перешкодами при безперервності проб і навчанні в локальному навколишньому 
середовищі. Китайські дослідники представили метод з використанням рухомого 
вікна для уникнення зіткнень в режимі реального часу в невідомому середовищі. 
Доведено ефективність цих методів для вирішення завдань планування 
маршруту та обходу перешкод мобільних роботів у двомірному статичному 
середовищі, але алгоритми є занадто складними для ВП БПЛА в тривимірному 
динамічному середовищі. 
У нашій роботі був запропонований новий простий алгоритм ОП для 
 
 
 26 
квадрокоптера на основі управління поворотом вектора швидкості польоту. Цей 
алгоритм буде представлений в розділі 2. 
 
 
1.3. Інтегрована навігаційна система БПЛА 
 
Розглянемо безпілотний літальний апарат (БПЛА) легкого класу, що виконує 
політ в автономному режимі або при дистанційному пілотуванні в умовах 
недостатності наданому оператору інформацію про середовище. У цих випадках 
інформація з одного джерела не може забезпечити необхідну точність навігації 
БПЛА. Тому КНС на основі злиття декількох джерел навігаційної інформації, що 
дозволяє компенсувати недоліки кожного з джерел, безсумнівно, необхідна для 
БПЛА. КНС зазвичай поєднує два або більше двох типів самостійних НС. Таким 
чином, КНС може в повній мірі використовувати переваги взаємодоповнюваності 
різних НС і забезпечувати достовірну навігаційну інформацію. 
 
1.3.1. Застосування безплатформенних інерціально-супутникових 
навігаційних систем для БПЛА 
Безплатформенні інерціально-супутникові навігаційні системи (БІНС / СНС)  
являють собою перспективний клас сучасних інтегрованих НС для БПЛА. БІНС / 
СНС - це синтез двох самостійних систем - безплатформенної інерціальної 
навігаційної системи (БІНС) і супутникової навігаційної системи (СНС) дозволяє 
поєднати переваги та компенсувати недоліки, що характерні для кожної з окремих 
систем. 
Безперервна динамічна передача повного навігаційного рішення, такого як 
координати, швидкість, прискорення і кутова орієнтація, є однією з переваг БІНС. 
Також до переваг відносяться можливість високочастотної видачі інформації та 
незалежність від зовнішніх джерел даних. Однак важливим недоліком є 
накопичення помилок у визначенні навігаційних параметрів з часом. Точність 
вихідної інформації також залежить від точності чутливих елементів. 
 
 
 27 
Використання дорогих прецизійних гіроскопів і акселерометрів як чутливих 
елементів ускладнює і подорожчує систему БІНС. Для підвищення довгострокової 
точності необхідно періодично коригувати дані БІНС за показаннями зовнішніх 
приладів. Це стало передумовою до розробки алгоритмів КНС. 
Одним з найбільш перспективних засобів корекції для БІНС є СНС. Цей клас 
НС добре сполучається з іншими бортовими системами, відносно дешевий і 
швидко розвивається. 
Для спільної обробки інформації від обраних НС, може бути застосована 
слабо зв'язанна схема на основі фільтра Калмана, що передбачає вироблення 
незалежних рішень в БІНС і СНС. Зазначений фільтр на основі даних від СНС-
приймача формує оцінку вектора стану, що включає помилки БІНС і 
інструментальні похибки її датчиків, і автоматично встановлює оптимальні 
настройки БІНС. Структура такої системи показана на Рис. 1.11. 
 
БІНС 
 
 
Рис. 1.11. Слабосвязанна схема БІНС / СНС 
 
На рис. 1.11: X  X БІНС  - навігаційні параметри БПЛА з помилками; 
X  XCHC  - навігаційні параметри СНС з помилками;   - вектор оцінок 
інструментальних похибок гіроскопів;   - вектор оцінок інструментальних 
похибок акселерометрів. 
При використанні обробки інформації з'являється можливість створення 
комплексних безплатформенної інерціально-супутникових систем, які об'єднують 
 
 
 28 
гідності БІНС і СНС. Але у СНС є недоліки, які виражаються в спотворенні і 
затримки визначення сигналу. Також, за певних умов, приймач може втрачати 
сигнал через велику щільність хмар та інтерференцію з наземними джерелами 
радіосигналів. При цьому частота прийому знаходиться в дециметровому діапазоні 
радіохвиль. Крім того, погіршенню якості прийому сигналу сприяє розташування 
всередині будівель з залізобетону, в підвалах, тунелях або квартирах. Крім того, 
СНС має значні помилки за інформацією про висоту. СНС може визначити 
величину і напрям швидкості рухомих об'єктів тільки в горизонтальній площині, 
але не може визначити швидкість в вертикальному напрямку. 
З огляду на всі перераховані вище неточності СНС, в КНС, пропонованої в 
даній роботи, були спільно використані ВНС, барометричний висотомір і 
радіовисотомір для забезпечення виправлення помилок БІНС. 
 
1.3.2. Візуальна навігаційна система на основі комп'ютерного зору і 
алгоритму EKF-SLAM 
Одним з ключових питань у вирішенні завдання автономного польоту БПЛА 
є автономна навігація в середовищі відсутності сигналу СНС. Серед можливих 
підходів до вирішення цієї проблеми в останнє десятиліття популярна ВНС на 
основі алгоритму одночасної локалізації і створення карти (картографування) з 
розширеним фільтром Калмана, що дає можливість прогнозування координат і 
швидкості квадрокоптера в невідомому середовищі з одночасною оцінкою 
положення орієнтирів і створенням повної карти. 
Принципове рішення задачі одночасної локалізації і створення карти SLAM 
запропоновано досить давно, і до теперішнього часу ця концепція досягла певного 
рівня, достатнього для практичної реалізації в області робототехніки при вирішенні 
задач автономного руху. Розроблено різні поліпшення алгоритму SLAM, в тому 
числі - з використанням калмановської фільтрації. Зараз алгоритми SLAM з 
використанням розширеного фільтра Калмана (EKF - Extended Kalman Filter) 
мають сталу абревіатуру EKF-SLAM - Extended Kalman Filter for Simultaneous 
Localization And Mapping, яка використовується і в цій статті. Істотним недоліком 
 
 
 29 
існуючих алгоритмів EKF-SLAM є значне зростання обсягу необхідних обчислень 
зі збільшенням числа спостережуваних орієнтирів. Прагнення скоротити обсяг 
обчислень призводить до зниження точності позиціювання і можливості отримання 
суперечливої інформації при складанні карти. Перш за все необхідно отримати і 
обчислити координати орієнтирів довкілля. 
В даний час існує багато алгоритмів виявлення характерних точок. При 
порівнянні результатів моделювання в даній роботі вибираємо алгоритм SUSAN, 
запропонований Смітом і Бреді (Smith & Brady, 1997), який є більш швидким і 
стійким в разі розмиття і нерівномірної яскравості фону зображень. 
Після такої обробки залишається можливість існування неправильних пар 
відповідних характерних точок у фундаментальній матриці. Тому необхідна 
додаткова обробка за допомогою алгоритму RANSAC. В алгоритмі RANSAC 
реалізована сама загальна схема стійкої оцінки за допомогою вибору випадкових 
підмножин даних. 
Координати характерних точок можуть бути отримані за допомогою рівнянь: 
 
 
 (1.30) 
 
 
f
де zci  - проєкція координат точок на вісь z системи координати камери; 
[u ,v ,1]T
i i  - піксельні координати характерних точок i на зображенні; 
[x f , x f , x f
w w w ,1]T  - координати цих характерних точок в зв'язаній системі 
координат, що збігається на початку руху з системою координат камери; K3x3 - 
матриця внутрішніх параметрів камери; [R3x3 | L3x1]3x4  - матриця перетворення 
координат двох послідовних зображень (зовнішніх параметрів) камери.  
Перед отриманням координат характерних точок необхідне калібрування 
внутрішніх параметрів камери. Параметри камери отримані з використанням 
інструментарію системи Matlab для калібрування камер (Camera Calibration 
 
 
 30 
Toolbox for Matlab, Bouquet): 
 
Матриця перетворення зображень, що враховує параметри камери, може 
бути записана у вигляді: E  K 'T FK . 
У нашій роботі використано монокулярний комп'ютерний зір, тому K '  K  
Відповідно до використовуваного алгоритмом матриці обертання (переходу) 
і переміщення камери можуть бути отримані з виразу: 
 
 
 
 (1.31) 
 
 
 
де R - матриця обертання; l - матриця переміщення зображень; L  1 - 
матриця переміщення камери;   - масштабний коефіцієнт; матриця 
0 1 0 
W  1 0 0  ;U і V  - сингулярне розкладання матриці E UDV T , алгоритм якого 
0 0 1 
представлений в ; матриця D  diag(k 1 , k2 ,0), k1  k2 . 
З чотирьох варіантів вираження (1.31) правильний варіант вибирається по 
умові знаходження точок перед камерою, тобто проєкція координат точок на вісь z 
повинна бути більше нуля. 
Таким чином, з (1.30) за допомогою (1.31) можуть бути отримані чотири 
лінійних рівняння: 
 
 
 31 
 
Система рівнянь є перевизначеною, тому для вирішення використаний метод 
найменших квадратів. В результаті можуть бути отримані тривимірні координати 
характерних точок. 
Для перевірки працездатності алгоритму було проведено натурний 
експеримент в приміщенні (коридорі) (рис. 1.12). 
 
(а)                                            (б)                                          (в)  
Рис.1.12. Координати характерних точок: (а) виділення і відповідність 
точок, (б) двомірні координати точок, (в) тривимірні координати точок 
 
Порівняння положення характерних точок на зображеннях і значення їх 
двомірних і тривимірних координат показує, що отримані координати характерних 
точок приблизно відображають їх реальний стан. Це може свідчити про 
правильність методу. Час реакції дорівнює 0,138629 с, що доводить швидкодію 
методу. 
На рис. 1.13 показані системи координат, які були використані в даній роботі: 
OB X BYBZB  - базова нерухома система координат; OV ,k XV ,kYV ,k ZV ,k  - пов'язана 
система координат БПЛА в момент k; OW ,k XW ,kYW ,k ZW ,k  - система координат 
камери в момент k; OC XC YC ZC  - система координат камери для першого кадру 
1,k 1,k 1,k 1,k
зображення в зображення в момент k; OC XC YC Z  - система координат камери 
2,k 2,k 2,k C2,k
для другого кадру зображення в момент k. 
 
 
 32 
 
Рис. 1.13. Системи координат для моменту часу k 
 
По візуальній одометрії  можна обчислити координати характерних точок і 
центру мас БПЛА: 
 
f f T T
де Xb,k  xb,k , y f
b,k , z f v v v v
b,k  i xb,k  xb,k , yb,k , zb,k   - координати 
характерних точок ітцентру мас БПЛА в нерухомій системі координат; 
T
X f   f f f v v v v T
w,k xw,k , yw,k , zw,k  i xw,k  xw,k , yw,k , zw,k   - координати характерних 
точок і центру мас БПЛА в зв'язаній системі координат; 
B B
 RV і LV ,k  - матриці обертання і переміщення системи координат БПЛА на 
V V
нерухому систему координат; RW і LW  - матриці перетворення і матриця 
переміщення системи координат камери на систему координат БПЛА; Rk i Lk  - 
матриці обертання і переміщення системи координат першого кадру зображення на 
 
 
 33 
систему координат другого кадру зображення. 
У початковий момент система координат БПЛА збігається з нерухомою 
системою координат. У своїй роботі вважаємо, що рух бортової камери з 
V
достатньою точністю збігається з рухом БПЛА, тобто матрицю RW  приймаємо 
V
одинично., а LW  - нульовою. Використання тільки візуального алгоритму 
комп'ютерного зору не дозволяє точно отримати масштабний коефіцієнт   (як 
правило, використовується поєднання з інерціальним вимірювальним блоком), 
B
тому помилка матриці LV  збільшується з часом. Для вирішення цієї проблеми в 
даній роботі отримання координат БПЛА проводиться алгоритмом SLAM, а не 
візуальною одометрією, тобто координати характерних точок можна отримати з 
рівняння: 
                                                (1.34) 
Для алгоритму планування польоту БПЛА доцільно використовувати 
модель, що описує тільки кінематику траєкторного руху. При цьому можна 
прийняти, що на кожному кроці алгоритму швидкість польоту не змінюється, а 
зміна швидкості від кроку до кроку обмежені наявними прискореннями. 
Вектор стану моделі руху камери БПЛА і матрицю координат орієнтирів в 
нерухомій системі координат можна записати наступним чином: 
                        (1.35) 
V v v v v T
де b  vxb ,vyb ,vzb   - швидкість камери в нерухомій системі координат; 
X f ,i  x f ,i , y f ,i f ,i T
b  b b , zb   - координати і-ого орієнтиру в нерухомій системі 
координат, n – кількість спостерігаючих орієнтирів. 
Розширений вектор стану камери на БПЛА і орієнтирів карти в нерухомій 
системі координат: 
                                            (1.36) 
По принципу візуальної одометрії , рівняння стану руху камери на БПЛА і 
 
 
 34 
орієнтирів карти можна записати у вигляді: 
 
де   - гауссовські білі шуми з нульовим середнім. 
Вектор спостерігання: 
                                                    (1.38) 
Оцінка вектора стану в алгоритмі одночасної локалізації і картографування 
(SLAM) для розглянутої моделі , яка являється нелінійною, але з «гладкими» 
нелінійними функціями в правих частинах рівнянь станів і спостережень, може 
бути отримана за допомогою розширеного фільтра Калмана (Extended Kalman filter 
– EKF), в якому на кожному кроці проводиться лінеарізація за допомогою 
розкладання в ряд Тейлора з відкиданням членів ряду вище першого порядку. 
Якщо нелінійну модель БПЛА і орієнтирів карти записати у вигляді 
                                     (1.39) 
де uk  ak  - керуючі змінні;  k ,k  - гауссовські білі шуми з нульовим 
математичним очікуванням і коваріаціями Q і R відповідно, то застосування 
розширеного фільтра Калмана для алгоритму SLAM можна представити у вигляді 
наступномій послідовності дій: 
A. Прогноз («однокроковий») стану ковараційної матриці помилок з 
використанням локальної лінеарізації нелінійної системи: 
 
f 
 
 X k ,uk   f X k ,u 
де Fk 
k
 |  , Gk  |  матриці частних похідних першого 
 X Xk ,uk u X
k K ,uk
k
 
 
 35 
порядку (матриця Якобі) при розкладанні нелінійних функцій f (Xk ,uk )  в ряд 

Тейлора в околицях оцінки на k-му кроці (Xk ,uk ) : 
              (1.41) 
Як видно з приведеного вище аналізу, коваріаційна матриця може бути 
записана наступним чином: 
           (1.42) 
де Pvv  - коваріаційна матриця положення і орієнтації робота; Pmv , Pvm  - крос-
коваріаційна матриця робота і орієнтирів; Pmm  - коваріаційна матриця положення 
орієнтирів. 
Б. Визначення помилки прогнозу і коваріаційної матриці за спостереженнями 
на кроці k + 1: 
       (1.43) 
де Hk1  - матриця Якобі при розкладанні нелінійних функцій h(Xk )  в ряд 

Тейлора в околиці X k1|k . 
  
В. Корекція оцінок стану X k1  X k1|k  Kk1 yk1  і коваріаційної 
T 1
матриці Pk1   I  Kk1Hk1 Pk1|k , де Kk1  Pk1|k Hk1Sk1   
Г. Розширення вектора стану і коваріаційної матриці. 
 
 
 
Орієнтири, виявлені датчиками апарату на кожному кроці, включають 
 
 
 36 
орієнтири, вже існуючі на карті, а також нові орієнтири. Існуючі орієнтири 
були використані для оцінок стану на вищенаведених етапах. Нові орієнтири 
додаються в вектор стану системи через процес ініціалізації. Нехай на кроці k j-ий 
f , j
спостережуваний орієнтир у векторі спостереження з координатами xw,k  є новим. 
Його координати в нерухомій системі координат: 
                    (1.44) 
Новий вектор стану системи після розширення: 
 
Коваріаційна матриця нового вектора стану:  
 
f ,i
де gx,gz  - матриця Якобі g XV ,k , X w,k   для XV ,k i Zk . 
Визнано, що асоціація даних є однією з основних проблем для вирішення 
завдання SLAM. Під асоціацією даних розуміється встановлення відношення 
еквівалентності між спостерігаючими орієнтирами та орієнтирами у векторі стану. 
Навіть невелике число помилково асоційованих збігів може призводити до 
розбіжності алгоритму SLAM. Таким чином, алгоритм асоціації даних має 
важливий вплив на точність і надійність алгоритму SLAM. 
В останні роки було запропоновано багато способів асоціації даних для 
алгоритма SLAM. Найбільш часто використовуваним алгоритмом асоціації даних 
в них є алгоритм ICNN (Individual Compatibility Nearest Neighbor - Індивідуальної 
Сумісності Найближчого Сусіда). У цьому алгоритмі для асоціації кожного 
спостережуваного орієнтиру проводиться порівняння відстані до існуючих 
орієнтирів з наперед заданим на підставі експертних оцінок порогом. Якщо 
відстань між спостережуваним і вже існуючим на мапі орієнтирами менше певного 
порогу, то спостережуваний орієнтир поєднується з цим існуючим орієнтиром. 
 
 
 37 
Коли на карті є кілька сумісних зі спостережуваним орієнтирів, то в якості 
асоційованого вибирається орієнтир з мінімальною відстанню. 
З огляду на помилки вимірювань, як відстані між i-м спостережуваним 
орієнтиром і j-м орієнтиром на локальній карті доцільно використовувати відстань 
Махаланобіса: 
                                                   (1.47) 

де yij  yi  y j  - вектор різниці координат за результатами вимірювань; yi  -

вектор вимірювань координат i -го спостережуваного орієнтира; y j  - вектор 
координат j -го орієнтира в локальній карті; J - коваріаційна матриця помилок 
вимірювань. 
Якщо Dij Gate , то вважаємо і -й спостережуваний орієнтир, що збігається 
з j –м існуючим. Тут Gate - заданий поріг. 
Якщо більш ніж один орієнтир з присутніх на локальній карті задовольняє 
умовам Dij Gate  то в якості асоційованого орієнтира вибираємо орієнтир з 
мінімальною нормованою відстанню. 
Цей алгоритм характеризується високою швидкістю розрахунку і простотою 
в реалізації, але не враховує кореляцію між орієнтирами. Через це правильність і 
надійність асоціації недостатньо висока, що може призводити до розбіжності 
алгоритму SLAM. Алгоритми JCBB (Joint Compatibility Branch and Bound - 
Спільної Сумісності Гілок і Кордонів) і MHT (Multiple Hypothesis Tracking - 
Відстеження Кількох гіпотез) мають високу точність і надійність, але їх 
обчислювальна складність різко зростає зі збільшенням кількості орієнтирів, тому 
вони не можуть бути застосовані до навколишнього середовища з великою 
кількістю орієнтирів. Для вирішення завдання асоціації застосовувалися також 
алгоритми оптимізації, в тому числі - цілочисельного програмування, а також 
теорія графів . 
 
 
 38 
1.3.3. Основні особливості інтегрованих БІНС із застосуванням 
барометричного висотоміра і радіовисотоміра 
Для визначення висоти також можна використовувати GPS-приймач. 
Принцип дії заснований на одночасному вимірі відстані до декількох (як правило, 
від чотирьох до шести) мовчань супутників, що знаходяться на відомих і 
спеціально коректованих орбітах. Але точність позиціонування висоти дуже 
низька, іноді навіть помилка може досягати більше 1000 метрів, особливо коли всі 
доступні супутники в позиції, близькій до горизонту. У цих випадках за допомогою 
GPS неможливо отримати точну інформацію про висоту. Тому потрібно 
використовувати висотомір для корекції помилки передбаченої висотою БІНС в 
КНС в режимі реального часу. 
Висотою польоту називається відстань від поверхні землі до апарату, 
розрахована по вертикалі від деякого рівня, прийнятого за початок відліку. Для 
вимірювання висоти зазвичай використовують альтиметр або висотоміри. За 
принципом пристрою висотометри діляться на барометричні і радіотехнічні. 
Принцип барометричного висотоміра заснований на використанні 
закономірної зміни атмосферного тиску з висотою. Існує багато джерел помилок, 
які викликають відхилення показників висоти барометричного висотоміра від 
істинного. Основними з них є помилки шумів вимірювання і помилки моделі 
атмосфери . Принцип барометричного висотоміра є правильним, тільки якщо 
умови реальної атмосфери відповідають стандарту атмосфері . Якщо реальна 
атмосфера не відповідає стандартній атмосфері, то подібний метод неминуче 
виробляє помилку вимірювання, яку можна називати методичною або 
принциповою помилкою. Ця помилка не може бути усунена при фільтрації. 
Принцип дії радіовисотомірів  заснований на визначенні часу проходження 
радіосигналу від передавальної антени до поверхні, що відбиває і назад, до 
приймальної антени. До недоліків барометричного висотоміра можна віднести 
невірне вимірювання тиску на борту, велику похибку (до 10 м), вимір відносної 
висоти, а не абсолютної, і складність роботи з висотоміром. До плюсів подібного 
висотоміра відноситься економічність ресурсів для визначення висоти незалежно 
 
 
 39 
від рельєфу. 
Недоліками радіовисотоміра є його виражена спрямованість вимірювань і 
потреба у потужному джерелі випромінювання. Однак радіотехнічні засоби 
відзначаються високою точністю визначення навігаційних параметрів і 
можливістю вимірювання їх в будь-яких погодних умовах. 
Оскільки барометричний висотомір і радіовисотомір мають свої переваги і 
недоліки, для підвищення точності вимірювання висоти ми використовуємо ці два 
типи висотомірів узгоджено як допоміжний навігаційний інструмент для системи 
контролю навігації. 
 
Висновки до розділу 1 
 
Таким чином, в першому розділі роботи представлено аналіз методів і 
ключових технологій для досягнення автономного польоту БПЛА. 
1. Розроблено детальну модель математичного опису квадрокоптера, у якій 
враховано вплив факторів, таких як вітер, ефект екрану землі та гіроскопічні 
моменти від гвинтомоторних груп. Розроблені системи управління траєкторією та 
кутовим положенням квадрокоптера, базовані на ПІД-регуляторах і бекстеппінг-
регуляторах. 
2. Результати моделювання польоту квадрокоптера за заданим 
маршрутом підтверджують, що зміни координат центру мас квадрокоптера 
практично ідентичні для обох типів регуляторів. Водночас, діапазон зміни кутового 
положення при використанні бекстеппінг-регуляторів помітно менший, ніж у 
випадку ПІД-регуляторів, при цьому крива стає більш плавною. Таким чином, 
використання ПІД-регуляторів для траєкторного та бекстеппінг-регуляторів для 
кутового управління спрощує структуру системи управління і підвищує 
стабільність польоту квадрокоптера.. 
3. Аналіз алгоритмів планування глобального маршруту на основі МА, 
відстеження траєкторії на основі методу «L1» і ОП виявив, що традиційний МА 
важко використовувати для планування глобального просторового маршруту в 
 
 
 40 
режимі реального часу, стратегія вибору точки маршруту в якості опорної точки є 
дуже важливою для точного відстеження спланованого маршруту, існуючі 
алгоритми ОП важко застосовувати в тривимірному динамічному середовищі в 
режимі реального часу для обльоту різних перешкод. 
4. При розгляді навігаційної системи БПЛА встановлена можливість 
використання БІНС, СНС, ВНС, барометричного висотоміра і радіовисотомір в 
КНС для забезпечення точності навігаційної інформації квадрокоптера. Аналіз 
ВНС на основі алгоритму EKF-SLAM показує, що недоліком цього алгоритму є 
значне зростання обсягу необхідних обчислень зі збільшенням числа 
спостережуваних орієнтирів.
 
РОЗДІЛ 2 
РОЗРОБКА СИСТЕМИ УПРАВЛІННЯ ПОЛЬОТОМ 
КВАДРОКОПТЕРА З МОЖЛИВІСТЮ ОБЛЬОТУ ПЕРЕШКОД В 
СКЛАДНОМУ СЕРЕДОВИЩІ 
 
 
Існуючі системи кутового управління польотом квадрокоптера і алгоритми 
планування маршруту, відстеження маршруту та ОП, наведені в розділі 1, досить 
добре опрацьовані для застосування в детермінованому і статичному середовищі. 
Але для розширення області застосування робоче середовище польоту 
квадрокоптера повинно ставати все більш складним. 
 
 
2.1. Загальна структура автономної системи управління польотом 
квадрокоптера 
 
Результати проведеного аналізу існуючих методів вирішення поставлених 
завдань, частково відбиті в попередньому розділі, дозволяють зробити висновок, 
що за допомогою представлених в літературі і автономний польот квадрокоптера 
виявляється вельми складним завданням у літературі та відомих розробках методів. 
Таким чином, в даній роботі виникла необхідність розробки алгоритмів, що 
дозволяють успішно вирішити поставлені завдання, і використовувати ці 
алгоритми в автономній системі управління польотом квадрокоптера, яка має 
загальну структуру, показану на рис. 2.1. 
 
 
Рис. 2.1. Структура системи автоматичного управління квадрокоптера 
 
На рис. 2.1 показана загальна схема структури автономної системи 
управління, в якій можна виділити дві основні частини. Перша частина - система 
автоматичного управління (САУ) польотом квадрокоптера з плануванням 
маршруту в складному середовищі. Друга частина - КНС. Кольоровими рамками 
на даній схемі обведені ті завдання, для вирішення яких в роботі запропоновані 
нові або поліпшені алгоритми. 
У цьому розділі ми концентруємо увагу на першій частині структури системи 
управління, яка включає алгоритм планування глобального маршруту на основі 
хмарно-точкової карти і поліпшеного мурашиного алгоритму, алгоритм 
відстеження маршруту на основі поліпшеного методу «L1» з адаптивним 
вибором опорної точки, новий простий алгоритм ОП на основі управління 
поворотом вектора швидкості польоту і багаторежимний алгоритм 
стабілізації польоту квадрокоптера. 
 
 
 
 
2.2. Алгоритм планування глобального маршруту на основі хмарно-
точкової карти і поліпшеного мурашиного алгоритму 
 
В цьому розділі пропонуємо покращений мурашиний алгоритм для 
вирішення завдань планування глобального просторового маршруту для 
квадрокоптера в тривимірному середовищі в режимі реального часу. 
 
2.2.1. Хмарно-точкова пошукова карта маршруту 
Перший крок планування глобального маршруту є створення карти 
середовища. Зараз існує багато способів подання інформації про середовище, 
наприклад: методом сіток; ймовірнісної дорожньої карти ; графіка видимості ; 
діаграми Вороного  та ін. Ці методи мають свої переваги і недоліки. 
В реальності форми перешкод є нерегулярними, через що важко точно 
визначити, скільки елементів сітки займають перешкоди. Крім того, коли на карті 
існує багато локальних мереж, матриця суміжності буде занадто великий, через що 
значно знижується швидкість алгоритму і неможлива робота в реальному часі. Для 
вирішення цих проблем в цій роботі пропонується новий спосіб представлення 
інформації про середовище на основі дискретної хмарно-точкової карти. 
Спочатку потрібно створити моделі середовища і моделі перешкод. 
Перешкоди можуть мати різні форми і розміри. Для спрощення розрахунку і 
моделювання пропонується в якості моделі будь-яких нерухомих перешкод брати 
кола (в тривимірному середовищі - циліндри, а для рухомих перешкод - сфери), що 
описують реальну форму відповідних перешкод. Такі моделі перешкод показані на 
рис. 2.2. 
 
Рис. 2.2. Моделі  перешкод 
 
 
Структура нерухомого (НПП) і рухомого (ПП) перешкоди визначається 
наступним чином: 
 
де Oobs , Robs ,hobs ,Vobs  - координати центру, радіус, висота і швидкість руху 
перешкоди. 
Хмарно-точкова карта розбивається на кілька кільцевих безпечних і 
небезпечних зон, відповідно до положення і розмірів перешкод, з рівномірним 
розташуванням точок сітки в небезпечних зонах, як показано на Рис. 2.3, де 
В1,В2 ,В3  - перешкоди; S1, S2 , S3  - безпечні зони; D1, D2  - небезпечні зони; O - 
позиція робота; G - цільова точка; P1 P2 P3G  - один з можливих 
маршрутів без зіткнень. 
 
Рис. 2.3. Хмарно-точкова карта 
Структура хмарно-точкової карти визначається наступним чином: 
 
де B - перешкоди; S - безпечні зони; D - небезпечні зони; P - точки пошуку 
можливих маршрутів. 
 
 
2.2.2. Розробка поліпшеного мурашиного алгоритму для глобального 
планування 
Для перешкод довільної форми, серед яких можуть виявитися неопуклі 
фігури, можливі локальні мінімуми, відповідні тупиковим шляхам. Якщо 
математичні моделі перешкод вибирати у вигляді випуклих функцій, то локальні 
мінімуми практично виключаються, але збільшується число можливих маршрутів, 
в результаті чого збільшується час виконання алгоритму. Крім того, матриці 
суміжності традиційного мурашиного алгоритму є занадто великими, в результаті 
чого значно знижується швидкість алгоритму. 
Покращення традиційного мурашиного алгоритму включають: 
- Побудова нової функції ймовірності переходу, що дозволяє зменшити 
розглядається число шляхів: 
 
де ij  - величина, зворотна величині кута jig; dij  - величина, зворотна 
довжині дуги ij;  ,  - параметр впливу. 
У новій функції ймовірності переходу були додані два приваблені фактори і 
їх параметри впливу, щоб швидше знайти найкоротший шлях, як показано на рис. 
2.4, де точка i - поточна точка; g - цільова точка; j, j1, j2  - точки з можливістю 
проходу. 
Для традиційного мурашиного алгоритму точки j, j1, j2  мають однакові 
ймовірності проходу. Якщо додамо привертаючий фактор у вигляді величини, 
зворотної величині кута  (1 2 )  в функцію ймовірності переходу, то 
ймовірність проходу від точки O на точку j і j1  стане більше, ніж можливість 
доступу від точки O на точку j2 . Якщо додамо привертаючий фактор, зворотний 
 
довжині дуги d (dij  dij1)  в функцію ймовірності переходу, то ймовірність проходу 
від точки O на точку j стане більше, ніж можливість доступу від точки O на точку 
j1 . Таким чином, можемо знайти точку з найвищою ймовірністю проходу. 
 
 
Рис. 2.4. Вибір маршрутів з різною ймовірністю  
 
- Усунення обмеження довжини пошуку на кожному кроці: від поточної 
точки можна перейти до будь-якої точки, яка не перебуває у забороненій області, 
як показано на Рис. 2.5 червоними лініями, а не тільки до сусідній в сітці. 
 
 
Рис. 2.5. Пошук маршруту. Синій маршрут - традиційного алгоритму; 
червоний маршрут - поліпшеного алгоритму 
 
Видно, що кількість прохідних точок пошуку значно знижується, що 
дозволить значно підвищити швидкість розрахунку алгоритму планування. 
- Зменшення розміру матриці суміжності: при обчисленні ймовірності 
переходу визначаємо наступну точку переходу і розраховуємо матриці суміжності 
 
тільки для прохідних точок. Якщо в карті пошуку існує M точок пошуку і серед них 
N точок знаходиться за межами перешкод, то розмір суміжності можна записати в 
такий спосіб: 
 
де QT  - розмір суміжності традиційного алгоритму; QY  - поліпшеного 
алгоритму; nij  - кількість прохідних точок j-ого мурашки в i-ой ітерації; k - 
кількість ітерацій; m-кількість мурах. 
Отже, QY QT , що дозволяє ефективно поліпшити швидкість обчислення 
алгоритму. 
Визначимо область спостереження перед перешкодою і заборонену область 
переходу для кожної j-ої точки: 
               (2.3) 
де obs-num - кількість перешкод; Pj  - точка маршруту, 
Pj _ scopei i Pj _ scopei  - області (сектора) спостереження і заборонені області i-го 
перешкоди. 
Позначимо буквою O - початкову точку, буквою G - цільову. 
Алгоритм уникнення перешкод в режимі реального часу полягає в 
наступному: 
- Якщо вектор OG не в секторі спостереження перед перешкодою Scope _ O, 
то не треба створювати хмарно-точкову карту, так як робот може рухатись 
безпосередньо від поточної позиції в цільову точку, як показано на Рис. 2.6 (а). 
- Якщо вектор PjG  входить в область Scope _ Pj , то робот рухається від 
поточної позиції в точку Pj1 , яка має найбільшу ймовірність переходу і не 
знаходиться в області Pj _ fob . 
 
- Якщо вектор PG входить в область Scope_P, то зупинити пошук маршруту, 
тобто робот може рухатись безпосередньо з поточної позиції в цільову точку, як 
показано на рис. 2.6 (б) для точки Р2. 
 
Рис. 2.6. Стратегія уникнення перешкод 
 
Працездатність алгоритму перевірялась моделюванням планування 
маршруту в середовищі Matlab. В області моделювання існують чотири перешкод 
з різними радіусами. G – цільова точка; О – початкова точка. Щоб проілюструвати 
переваги пропонованого покращеного мурашиного алгоритму (ПМА), зрівняємо 
результати планування маршруту з використанням традиційного мурашиного 
алгоритму (ТМА). Результати моделювання показані на рис. 2.7. 
На рис. 2.7 точки з зірками - точки пошуку маршруту; рисунки 2.7 (а) і 2.7 (в) 
- результати при відстані між колами «сітки» L = 2; (Б) і (г) - результати при відстані 
між колами «сітки» L = 1; товсті суцільні лінії - результати планування маршруту 
при числі ітерацій N = 20; товсті пунктирні лінії - результати планування маршруту 
при числі ітерацій N = 3. Кількісні результати наведені в таблиці 2.1. 
Порівняння результатів моделювання в Таблиці 3 показує, що 
запропонований алгоритм працездатний і працює швидше, ніж традиційний 
алгоритм; спланований маршрут оминає всі перешкоди; довжина маршруту і час 
обчислення залежать від числа ітерацій, кількості точок маршруту пошуку і числа 
кроків пошуку. 
 
 
Рис. 2.7. Результати моделювання планування маршруту… 
 
Таблиця 2.1 
Порівняння результатів моделювання 
  
Довжина Число кроків Час 
маршруту (м) пошуку обчислення (с) 
L =1; N = 3 14,023 12 3,215 
L =1; N = 20 12,265 11 3,852 
ТМА 
L =2; N = 3 13,576 5 2,758 
L =2; N = 20 12,598 5 3,035 
L =1; N = 3 13,892 4 1,254 
L =1; N = 20 10,770 2 1,264 
УМА 
L =2; N = 3 13,621 3 1,035 
L =2; N = 20 10,770 2 1,136 
 
 
2.3. Відстеження спланованого глобального маршруту на основі 
поліпшеного методу руху по лінії візування 
 
Відстань L1 (див. Рис. р.10) - критичний параметр в алгоритмі відстеження. 
Це відстань істотно впливає на результат відстеження. Якщо відстань велика, 
квадрокоптер летить по дузі з меншою кривизною і повільно наближається до 
заданої траєкторії. У процесі спостереження немає великих коливань, але 
неможливо добре відстежувати всю траєкторію. Якщо значення L1  маленьке, 
квадрокоптер летить по дузі з більшою кривизною і швидко наближається до 
заданої траєкторії, але відбуватимуться великі коливання. У цьому випадку теж 
неможливо добре відстежувати всю траєкторію. Процес в обох випадках показаний 
на Рис. 2.9. 
 
 
Рис 2.8. Схема впливу відстані L1  на результат відстеження  
 
Пропонується динамічно змінювати відстань L1  (тобто адаптивно вибирати 
точку маршруту як чергової опорної точки). 
На рис. 2.9 показаний принцип адаптивного вибору опорної точки. 
 
 
Рис. 2.9. Вибір опорної точки 
 
Процес вибору опорної точки можна розділити на два етапи: 
1) вибір найближчої точки маршруту D1  з поточною крапкою позиції 
квадрокоптера C, яка відповідає умові: L1  d;   / 2 ; 
2) визначення порядкового номера опорної точки N в безлічі точок 
маршруту: 
                                                           (2.4) 
де n - порядковий номер точки D1  в безлічі точок маршруту;   - функція 
вибору опорної точки, яку можна записати в такий спосіб: 
 
де p, q, r - показники впливу кута випередження, відстані, кута повороту 
маршруту (визначаються шляхом попереднього моделювання); m - кількість точок 
в цій області;   - кут повороту маршруту, як показано на рис. 2.10. 
 
 
Рис. 2.10. Кути між точками маршруту  
 
Структура алгоритму показана на рис. 2.11, де X - координати центру мас 

квадрокоптера; X  - швидкість польоту квадрокоптера; X d  - координати 
обчисленої локальної цільової точки. 
 Рис. 
2.11. Структура алгоритму відслідковування маршруту 
 
Працездатність алгоритму відстеження маршруту перевірялася 
моделюванням планування маршруту в середовищі Matlab. Результати 
моделювання алгоритму показані на рис. 2.12 і 2.13, чорна лінія - заданий маршрут; 
червона лінія - вибір кожної точки маршруту в якості опорної точки; синя лінія - 
адаптивний вибір точки маршруту в якості опорної точки; 
З результатів моделювання, показаних на рис. 2.12, можна помітити, що 
траєкторія руху квадрокоптера має меншу амплітуду з використанням нашого 
алгоритму і його працездатність більш очевидна в разі відстеження двомірної 
прямій лінії 
 
 
   
(А) Відстеження прямій лінії                              (Б) Зміна відстані до лінії 
 
               
В) Відстеження окружності                                 Г) Зміна відстані до кола 
 
Рис. 2.12. Результати відслідковування двовимірних маршрутів 
 
 
(а) Відстеження прямої лінії                            (б) Відстеження відстані до лінії 
 
 
 
(в) Відстеження спіральної лінії                  (г) Відстеження відстані до спіральної лінії 
 
Рис. 2.13. Результати відстеження тривимірних маршрутів 
 
З результатів, показаних на рис. 2.13, можна помітити, що пропонований 
алгоритм може бути використаний для відстеження не тільки двовимірного 
маршруту, а й тривимірного маршруту, і навіть - ще більш складної тривимірної 
спіральної лінії. Крім того, пропонований алгоритм точніше відстежує 
спланований маршрут. 
 
 
2.4. Розробка алгоритму обльоту перешкод на основі управління 
поворотом вектора швидкості польоту 
 
У процесі польоту по спланованому маршруту квадрокоптер повинен мати 
можливість автономного ОП. У даній роботі пропонується новий простий метод 
обльоту різних перешкод в динамічному середовищі в режимі реального часу для 
квадрокоптера на основі управління поворотом вектора швидкості польоту. 
Якщо в середовищі відсутні перешкоди, з якими можуть відбутися зіткнення, 
то квадрокоптер летить до цільової точки за спланованим маршрутом за 
допомогою алгоритму відстеження маршруту. Якщо в середовищі існують 
нерухомі перешкоди, з якими можливі зіткнення, то потрібно обчислити 
мінімальне нормальне прискорення, необхідне для ОП. Якщо в середовищі існують 
рухливі перешкоди з можливістю зіткнення з квадрокоптером, то потрібно 
передбачити позицію зіткнення і помістити віртуальні нерухомі перешкоди в 
якості об'єкта обльоту в даному місці. 
Потім необхідно помножити мінімальне прискорення на відповідний 
коефіцієнт безпеки обльоту, щоб підвищити безпеку польоту квадрокоптера. Ці 
коефіцієнти пов'язані з розмірами, векторами швидкості, відстанями до перешкод, 
швидкості руху перешкод і іншими факторами. Обмежимо швидкість польоту 
квадрокоптера, щоб нормальне прискорення було не вище ніж максимальне 
безпечне прискорення польоту. У нашій роботі пояснимо принцип пропонованого 
алгоритму ПРО в двовимірному середовищі. 
Блок-схема алгоритму показана на рис. 2..4. 
 
 
Рис. 2.14. Блок-схема алгоритму обходу перешкод  
 
На рис. 2.14: ai  - мінімальне нормальне прискорення для обльоту перешкоди 
i; ki  - коефіцієнт безпеки обльоту перешкоди i; ac  - нормальне прискорення для 
обльоту перешкоди або відстеження спланованого маршруту; amax  - максимальне 
безпечне прискорення польоту квадрокоптера; n - кількість перешкод з можливістю 
зіткнення. 
 
2.4.1. Передбачення стану руху перешкод 
На практиці система часто є нелінійно. і не може задовольнити умовам 
використання стандартного фільтра Калмана. Для нелінійних систем зазвичай 
використовується розширений фільтр Калмана (EKF). Але недоліки EKF очевидні: 
необхідна для нелінійної системи лінеаризація, зазвичай - за допомогою 
розкладання в ряд Тейлора, зберігає член першого порядку і ігнорує члени вищого 
порядку. Цей процес лінеаризації може привести до великих помилок для істотно 
нелінійної системи, навіть розбіжностей. Крім того, в процесі фільтрації необхідно 
обчислювати матрицю Якобі, але для нелінійної системи важко реалізувати це 
 
обчислення і при цьому значно зменшити швидкість обчислень процесу фільтрації. 
Щоб подолати ці недоліки, Julier і його колеги запропонували сигма-точковий 
фільтр Калмана (Uncented Kalman Filter - UKF), котрий являється іншим видом 
широко використаного нелінійного фільтра . 
Для ОП важливо ефективно описати перешкоди, які можуть мати різні форми 
і розміри. Для спрощення розрахунку беремо модель перешкод, яка описана в 
параграфі 1.2.1. Відносні відносини положення і кутів являють Визначальною 
інформацією для уникнення зіткнення з перешкодою, як показано на Рис. 2.15, 
являються відстаню і кутами відносного розташування об'єкта і перешкоди, де V - 
вектор швидкості квадрокоптера; C - центр мас квадрокоптера; дві пунктирні лінії 
представляють дотичні лінії від точки C до кола перешкоди; rb  - радіус кола; B - 
центр кола;   - кут між дотичними лініями;   - кут між вектором швидкості і 
вектором CB; 
E1, E2 - дотичні точки на колі; d - відстань між центром мас квадрокоптера і 
центром кола. 
 
Рис. 2.15. Модель перешкоди і відносне положення робота і перешкоди 
 
Вектор стану руху перешкод для UKF в момент k: 
 
                                        (2.6) 
де X b _ k  [xb1_ k , yb1_ k , xb2 _ k , yb2 _ k ,...xbi _ k , ybi _ k ,..., xbn _ k , ybn _ k ]  - координати 
перешкод; n – кількість перешкод з можливістю зіткнення; 
Vb _ k  [vbx1_ k ,vby1_ k ,vbx2 _ k ,vby 2 _ k , ...,vbxi _ k ,vbyi _ k ,...,vbxn _ k ,vbyn _ k ]  - 
швидкість перешкод. 
Вектор спостерігання для UKF в момент k: 
                                                (2.7) 
де Dk  [d1_ k , d2 _ k ,..., di _ k ,..., dn _ k ]  - відстані; 
k  [1_ k ,2 _ k ,..., i _ k ,...,n _ k ]  - кути між дотичними; 
k  [1_ k ,2_ k ,...,i _ k ,...,n _ k ]  - кути між вектором швидкості і напрямом на 
перешкоду (лінією візування перешкоди). 
Оцінки вектора стану здійснюється наступним чином: 
 
де [xc _ k , yc _ k ] i [vx ,vy ]  - координати і швидкість робота для k-го моменту. 
Нелінійну модель системи руху перешкод можна записати в наступному 
вигляді: 
 
де uk  - керуючий сигнал; wk ,vk  - шум процесу і шум спостереження з 0 
 
середнього значення і їх коваріації Q і R. 
Процес сігма-точкової фільтрації Калмана може бути розділений на чотири 
етапи : 
1. Ініціалізація: 
 
x
де n – величина шуму процесу; m – веичина шуму спостерігання; X 0 - вектор 
 
стану системи; X 0  - вектор стану шуму процесу; X 0  - вектор стану шуму 
спостерігання. 
2. Розрахунок сігма-точок: 
 
3. Оновлення процесу: 
 
4. Оновлення спостереження: 
 
 
 
 
2.4.2. Стратегія обльоту нерухомих перешкод 
Необхідно відрегулювати напрямок польоту квадрокоптера за допомогою 
нормального прискорення ac , необхідного для зіткнення. 
 
Рис. 2.16. Процес обльоту перешкоди 
 
На рис. 2.16 представлений процес обльоту нерухомих перешкод. В цьому 
 
випадку мінімальне нормальне прискорення, необхідне для обльоту перешкоди i, 
можна записати в такий спосіб: 
 
                  (2.14) 
 
де d AG  - відстань від поточної точки до цільової точки. 
Для включення процесу обльоту нерухомих перешкод, необхідні певні 
умови: - di  dr , dr  -  поріг відстані обльоту; 
- вектор швидкості робота VC  знаходиться в області спостереження scope _ 
i і всередині кута i . 
Рівняння нормального прискорення, необхідного для виключення зіткнення 
з перешкодою i, можна записати в такий спосіб: 
                                               (2.15) 
де ki  - коефіцієнт безпеки обльоту. 
В якості нормального прискорення для обльоту всіх перешкод вибирається 
найбільше з безлічі безпечних нормальних прискорень, тобто 
ac  max[ac1, a2 ,..., aci ,..., acn ] . 
 
2.4.3. Стратегія обльоту рухомих перешкод 
В невідомому середовищі квадрокоптера потрібно уникнути зіткнення не 
тільки з нерухомими перешкодами, але також і з рухомими перешкодами. 
Квадрокоптер, що розглядається в даній роботі, в основному отримує інформацію 
про перешкоди за допомогою декількох бортових датчиків відстані (див. рис. 2.17). 
Для обльоту рухомих перешкод спочатку потрібно отримувати інформацію про рух 
перешкод і передбачити розташування зіткнення в режимі реального часу за 
допомогою сигма-точкового фільтра Калмана, потім помістити віртуальну 
статичну перешкоду в даному місці в якості об'єкта обходу. 
 
 
Рис. 2.17. Модель передбаченого зіткнення квадрокоптера з рухомою 
перешкодою 
 
Подібна стратегія обльоту рухомої перешкоди має дві основні переваги: 
1) Зменшення непотрібних дій при обльоті перешкод, що робить політ більш 
стабільним, а швидкість обчислення алгоритму більш високою; 
2) Зниження впливу візуального сліпого поля на ефективність алгоритму 
обльоту рухомих перешкод. 
Працездатність запропонованого алгоритму ВП (Управління поворотом - 
УП) перевірялася порівнянням результатів моделювання існуючих популярних 
алгоритмів обльоту нерухомих перешкод (Нечітка логіка - НЛ; Генетичний 
алгоритм - ГА; Потенційне поле - ПП; Дорожня карта - ДК; А *) в двовимірному 
середовищі, як показано на рис. 2.18. 
 
 
Рис. 2.18. Обліт нерухомих перешкод 
В таблиці 2.2 приведені дані нерухомих перешкод. 
Таблиця 2.2. 
Дані нерухомих перешкод 
Перешкода I П Ш IV V VI 
Координати (4.0, 4.5) (1.2, 6.0) (9.0, 7.5) (6.1, 1.8) (9.0, 4.0) (2.0, 8.2) 
Радіус (м) 1.2 1.0 1.0 0.8 0.5 0.7 
 
Порівняння результатів моделювання наведено в таблиці 2.3. 
Таблиця 2.3 
Порівняння результатів моделювання 
Алгоритм НЛ ГОЛ ПП ДК А * УП 
Час обчислення (с) 0.7625 2.8273 0.3833 2.122 2.5506 0.4618 
Довжина маршруту (м) 18.807 16.533 5.8807 17.671 18.120 16.246 
Гладкий маршрут да да да немає немає да 
Зіткнення немає немає да немає немає немає 
Результати моделювання показують, що запропонований алгоритм має 
високу швидкість обчислення. Для перевірки працездатності запропонованого 
алгоритму обльоту рухомих перешкод в динамічному середовищі додано рухливу 
перешкоду на карті, результати моделювання показані на Рис. 2.20. Початкові 
координати рухомого перешкоди (3.8,9.0); швидкості (1, -1); прискорення 
 
0.1 x ,0.1  y  , де x , y   - гауссовський шум, як показані на рис. 2.19. 
 
 
Рис. 2.19. Зміна прискорення рухомих перешкод  
 
Рис. 2.20. Обліт рухомої перешкоди в динамічному середовищі: 
(а) – початковий момент; (б) – момент виявлення можливості 
зіткнення; (в) – момент після обльоту рухомого обмеження; (г) – момент 
прибуття в цільову точку 
 
Результати фільтрації станів рухомих перешкод показані на рис.2.21. 
Результати моделювання показують, що квадрокоптер може ефективно 
уникати зіткнення з нерухомими і рухливими перешкодами. Крім того, сигма-
точковий фільтр Калмана (UKF) може досить точно передбачити стан руху та 
радіус рухомої перешкоди. 
 
 
2.5 Розробка алгоритму підвищення стабільності польоту на основі 
багаторежимної системи стабілізації 
 
Атмосферні умови мають значний вплив на положення та безпеку польоту 
квадрокоптера в зовнішньому середовищі. Основним фактором, що впливає на 
положення квадрокоптера, є вітер, серед інших метеорологічних умов, таких як хмари, 
дощ, туман, лід, град, блискавки і т. д. Система кутового управління квадрокоптером 
призначена для компенсації цих збурень польоту та мінімізації їх негативного впливу. 
У спрощеній моделі вітру поблизу поверхні землі враховуються три компоненти: 
постійна швидкість вітру, турбулентність та поривчастий вітер. Це спрощення 
полегшує моделювання та математичну обробку. 
Конструкція квадрокоптера досить проста, з осі гвинтів і кути лопатей, що 
фіксуються та регулюються швидкістю обертання. Для вертикального руху 
змінюються швидкості обертання всіх гвинтів, для горизонтального руху квадрокоптер 
нахиляється, що також вимагає відповідних змін швидкостей обертання гвинтів. 
Компенсацію моментів опору забезпечує протилежний напрямок обертання пар 
гвинтів. Проте, нелінійність характеристик квадрокоптера, їх залежність від різних 
чинників та низька перешкодозахищеність ускладнюють створення системи 
управління польотом. Особливо це стає актуальним при втраті стійкості при зміні 
режимів і умов польоту, зокрема - при зміні впливу вітру. 
 
 
 
 
Рис. 2.21. Фільтрації станів руху рухомої перешкоди: (а) – зміна 
помилки координати рухомої перешкоди; (б) – зміна помилки швидкості 
рухомої перешкоди; (в) – зміна радіуса рухомої перешкоди 
 
Для вирішення даної задачі пропонується система стабілізації з кількома 
режимами, що включає набір регуляторів кутового положення, які автоматично 
вибираються в залежності від умов польоту, таких як висота, кутове положення, 
вплив вітру та режими зльоту-посадки. Також у системі передбачена підсистема 
аварійного повернення до вихідної точки. 
Ще одним важливим фактором, що впливає на польот квадрокоптера, є ефект 
екрану, особливо в разі польоту низько над поверхнею землі. Ці дії можуть 
призвести до збільшення кутів нахилу апарату в режимах стабілізації. Таким 
чином, у першому наближенні можна розглядати чотири можливі режими польоту 
з різними налаштуваннями регуляторів. У режимі зльоту або посадки 
рекомендується обирати регулятор зі зменшеним перерегулюванням для 
уникнення зіткнення з землею та зменшення "тремтіння" квадрокоптера через 
ефект екрану. У безвітревому середовищі або при слабкому вітрі (3,4-5,4 м/с) 
рекомендується вибирати регулятор з невеликими змінами крену і тангажу. У 
середовищі з помірним вітром (5,5-7,9 м/с) можна вибирати регулятор з малим 
часом реакції для забезпечення стійкості та маневреності. Коли вітер перевищує 
критичне значення, рекомендується вибирати регулятори з найменшим часом 
реакції, а підсистему траєкторного управління переводити в режим повернення до 
вихідної точки.. 
Безліч режимів польотуQ q1,q2 ,q3,q4, де q1 : z  zT ,  max , i max  - 
зліт і посадка; q2 : z  zT , vw  vT ,   max , і  max  - відсутність вітру або 
слабкий вітер; q3 : z  zT ,vT  vw  vmax ,    max , i  max  - помірний 
вітер; q4 : vw  vmax або   max або  max  - режим нештатного завершення 
польоту. 
Більшість відповідних переходів 
 w , w z v v  
12 13, w14 , w21, w23, w24 , w31, w32 , w34 . де T , T , max, max, max  - 
поріг переходу режимів польоту. 
Діаграма перемикання режимів показана на рис. 2.22, 
 
 
 
Рис. 2.22. Діаграма перемикання режимів 
 
Розробка багаторежимної системи стабілізації, що містить набір регуляторів 
управління кутовим становищем, показана на Рис. 2.23. 
 
Рис. 2.23. Принцип роботи багаторежимної системи стабілізації  
 
Ефективність алгоритму перевірялася шляхом моделювання реакцій на 
послідовні впливи та польоту вздовж визначених траєкторій в атмосферному 
середовищі. Це середовище включало стійкий вітер, турбулентність і поривчастий 
вітер, що було відображено на рис. 2.24.. 
 
 
Рис. 2.24. Швидкість вітру. Сині лінії - швидкість вітру на напрям X; 
зелені лінії - швидкість вітру на напрям Y; червоні лінії - швидкість вітру на 
напрям Z 
 
Реакції каналу кута нахилу x і   на ступеневий вплив - на рис. 2.25. 
 
Рис. 2.25. Реакції каналу кута нахилу x і   на ступеневий вплив 
 
На Рис. 2.25 (б) представлены следующие регуляторы: БЕК1, отвечающий за 
управление в режимах взлета и посадки; БЕК2, предназначенный для регулировки 
 
полета в условиях отсутствия ветра или при слабом ветре; БЕК3, регулятор для 
полета в условиях умеренного ветра; БЕК4, предназначенный для управления в 
нештатных ситуациях, приводящих к досрочному завершению полета.. 
Значення коефіцієнтів регуляторів за відповідними змінним, обрані методом 
Циглера - Ніколса , наведені в таблиці 2.4: 
Таблиця 2.4. 
Значення коефіцієнтів ПІД-регуляторів 
 
В Таблиці 2.5 наведені значення коефіцієнтів для підсистеми кутового 
управління, розраховані методом «бекстеппінг». 
Таблиця 2.5 
Значення коефіцієнтів регулятора «бекстеппінг» 
 
£1 £2 £3 £4 £5 £6 
БЕК1 30 5 1,5 0,5 20 10 
БЕК2 20 3,5 1,2 0,3 18 8 
БЕК3 40 10 3,5 1,5 35 15 
БЕК4 50 15 4,5 2,5 45 20 
 
На рис. 2.26 показані результати моделювання польоту квадрокоптера по 
заданому маршруту, описаного в розділі 1 (див. рис. 1.12). 
 
 
Рис. 2.26. Траєкторія польоту квадрокоптера  
 
На рис. 2.27 відображено порівняння зміни кута нахилу при використанні 
регулятора БЕК1 та багатоканальної системи стабілізації в умовах відсутності вітру 
та при його наявності. Результати моделювання свідчать про те, що квадрокоптер 
проявляє найвищу стабільність за участю багатоканальної системи стабілізації, 
особливо в умовах сильного вітру та близькості до поверхні землі.. 
 
Рис. 2.27. Зміни кута нахилу у в разі без вітру і при впливі вітру 
 
На рис. 2.28 показано ставлення змін швидкості вітру і режимів польоту 
квадрокоптера. Очевидно, що режими польоту залежать від висоти політу та 
швидкості вітру.. 
 
 
Рис. 2.28. Зміни швидкості вітру і режимів польоту квадрокоптера 
 
Коли швидкість вітру вище помірної, ймовірність аварії значно зростає, отже, 
квадрокоптер повинен мати функцію автоматичного повернення на базу при 
складних метеоумовах. Для перевірки цієї функції системи кутового управління 
проведено моделювання, результати якого показані на рис. 2.29, де чорної лінією 
представлена траєкторія польоту перед перериванням місії. Коли швидкість вітру 
стає вище помірної, квадрокоптер починає автономно повертатися на базу, як 
показано на даному Рис. синьою лінією. На рис. 2.30 показано порівняння зміни 
кута нахилу   з використанням регулятора БЕК1 і багаторежимної системи 
стабілізації в середовищі з більш помірним вітром. 
 
Рис. 2.29. Траєкторія автоматичного повернення на базу 
 
 
Рис. 2.30. Зміни кута нахилу   в суворому середовищі 
 
Зміни  швидкості вітру і режимів польоту показані на рис. 2.31. 
 
Рис. 2.31. Зміни швидкості вітру і режимів польоту квадрокоптера в 
середовищі з більш помірним вітром 
 
 
Висновки до розділу 2 
 
У даному розділі були розроблені алгоритми для глобального планування 
маршруту, відстеження задуманого маршруту, оминання перешкод і 
багаторежимна система стабілізації для автономної системи управління польотом 
квадрокоптера в складному середовищі.. 
1. Розроблена загальна структура автономної системи польоту 
квадрокоптера, яку можна поділити на дві ключові складові: система 
автоматичного керування польотом квадрокоптера з можливістю уникнення 
 
перешкод в складних умовах навколишнього середовища та комплексна 
навігаційна система.. 
2. Досліджено алгоритм планування глобального маршруту на основі 
нового способу подання інформації про середовище у вигляді хмарно-точкової 
карти і поліпшеного мурашиного алгоритму. Цей покращений алгоритм має високу 
швидкість обчислень і можливість застосування для планування глобального 
просторового маршруту в режимі реального часу. 
3. Запропоновано алгоритм відстеження спланованого маршруту, який 
може бути застосований для відстеження складного тривимірного маршруту, на 
основі поліпшеного методу "L1" з адаптивним вибором точки маршруту в якості 
опорної точки відстеження. Результати моделювання відстеження показують, що 
запропонований алгоритм точніше відстежує спланований маршрут. 
4. Розроблено простий алгоритм ОП на основі управління поворотом 
вектора швидкості польоту квадрокоптера. Результати моделювання показують, 
що запропонований алгоритм має більш високу швидкість обчислень. Крім того, 
подібний алгоритм може бути застосований в динамічному середовищі для обльоту 
рухомих перешкод. 
5. Побудовано математичну модель вітру поблизу поверхні землі при 
постійному вітрі, турбулентності землі і поривчастим вітром. 
6. Досліджено метод підвищення стійкості польоту квадрокоптера у 
середовищі з атмосферним впливом. Цей метод ґрунтується на використанні 
багаторежимної системи стабілізації, яка включає набір бекстеппінг-регуляторів 
управління кутовим положенням. Ці регулятори автоматично обираються в 
залежності від умов польоту, таких як висота, кутове положення, вплив вітру та 
режими зльоту-посадки. Крім того, система включає підсистему аварійного 
повернення до вихідної точки..
 
РОЗДІЛ 3 
 РОЗРОБКА МАТЕМАТИЧНОЇ МОДЕЛІ АВТОНОМНОЇ СИСТЕМИ 
КЕРУВАННЯ ПОЛЬОТОМ КВАДРОКОПТЕРА 
 
 
Структура математичної моделі системи керування польотом квадрокоптера 
з можливістю обльоту перешкод і комплексною навігацією в складному 
середовищі, пропонована в даній роботі, показана на рис. 3.1. 
 
Рис. 3.1. Структура математичної моделі системи керування 
 
На рис. 3.1: x, y, z - координати квадрокоптера; xd , yd , zd  - координати 
заданого маршруту;  , ,  - кути крену, рискання, тангажу;  d , d ,d  - задані кути 
крену, рискання, тангажу; u1,u2 ,u3,u4  - керуючі сигнали від регуляторів; 
U1,U2 ,U3,U4  - керуючі сигнали двигунів; 1,2 ,3,4  - швидкість обертання 
винтів; wgx , wgy , wgz i wbx , wby , wbz  - швидкість вітру в нерухомій і зв’язаних 
системах координат; M m , M p , M q - моменти моторів й гвинтів; Pt1, Pt 2 , Pt3, Pt 4  - 
сили тяги гвинтівбез урахування екранного ефекту; Pg1, Pg 2 , Pg3, Pg 4  - сили тяги з 
урахуванням екранного ефекту. 
 
3.1 Макетування візуальної навігаційної системи 
 
Для ілюстрації працездатності пропонованої ВНС на основі покращеного 
алгоритму EKF-SLAM з адаптивним діапазоном спостереження і локальної 
асоціації даних, в даному розділі проведемо макетування ВНС в навколишньому 
середовищі. 
 
 
 
(а) попередній кадр зображення             (б) наступний кадр зображення 
 
(в) координати орієнтирів в                   (г) координати орієнтирів і траєкторія 
зв'язаній системі координат                     руху в нерухомій системі координат 
Рис. 3.2. Результати експерименту навігації тільки по монокулярним 
даним 583 і 584-го кадру зображення 
 
При макетуванні ВНС камера зробила 1605 фотографій, відстань руху склала 
приблизно 820 метрів. Рух починається з початку координат нерухомої системи 
координат, початковий радіус локальної карти 25 м, швидкість руху приблизно 0,3 
м/с, мінімальне число орієнтирів для надійної корекції прогнозованого вектора 
 
стану Num min = 8, максимальне число орієнтирів для запобігання надмірного 
надлишку корекції Num max = 50, максимальний радіус надійного спостереження 
Rmax  35 м , крок одноразової зміни радіуса локальної карти R  0,1 м . На Рис. 
3.2 (а) вказані характерні точки (зелені) і точки (червоні), відповідні характерним 
точкам попереднього 582 кадру, які являються орієнтирами на даному 583 кадрі. 
На Рис. 3.2 (б) показані характерні точки (зелені) і точки (червоні), відповідні 
характерним точкам 583 кадру, котрі являються орієнтирами на даному 584 кадрі. 
Рисунок 3.2 (в) показує обчислені координати орієнтирів із 583 і 584 кадрі 
зображення. Рисунок 3.2 (г) показує траєкторію руху камери із чисто монокулярних 
даних. 
 
 
(а) – ВНС на основі традиційного EKF-SLAM з постійним діапазоном 25 
метрів 
 
(б) - ВНС на основі покращеного EKF-SLAM 
Рис. 3.3. Зміна часу обчислення 
 
 
 
Для перевірки працездатності запропонованого алгоритму проведено 
порівняння часу обчислення ВНС на основі традиційного і покращеного 
алгоритмів EKFSLAM. 
Результати на рис. 3.3 показують, що час обчислення ВНС на основі EKF-
SLAM з постійним діапазоном збільшується зі збільшенням часу, а час обчислення 
ВНС на основі AR-EKF-SLAM з локальною асоціацією даних підтримується в 
невеликому прийнятому діапазоні. 
Для порівняння точності позиціювання традиційним і пропонованим 
алгоритмами, на Рис. 3.4 показані зміни помилки визначення положення камери в 
квадраті, котра має вигляд по координатах х в момент k: 
 

де xk  - реальні, a xk  - спрогнозовані координати квадрокоптера. 
 
Рис. 3.4. Помилка позиціонування по координаті х 
 
Як видно з рисунка, помилка між реальним і спрогнозованим положенням 
БПЛА алгоритму EKF-SLAM з постійним діапазоном спостереженням досягає 
значних величин, а для пропонованого алгоритму з адаптивним діапазоном 
спостереженням ця помилка мала і порівняна з помилкою традиційного алгоритму 
EKF-SLAM з великим діапазоном спостереження, хоча і забезпечується при 
набагато меншому часу обчислень. 
Для перевірки точності пропонованого метожу проведено порівняння 
 
траєкторії, розрахованої по даним монокулярної зьомки, з інформацією 
супутникової (GPS) навігації. Результати показані на рис. 3.5 і 3.6. 
Для перевірки точности запропонованого  методу проведено порівняння 
траєкторій, розрахованій по даним монокулярної зьомки, з інформацій 
супутникової (GPS) навігацій. Результат показано на рис. 3.5 и 3.6. 
 
 
 
(а) попередній кадр зображення                      (б) наступний кадр зображення 
 
(в) координати орієнтирів зв'язаній              (г) координати орієнтирів і траєкторія руху 
 системі координат                                         в нерухомій системі координат 
 
Рис. 3.5. Результати експерименту навігації по тільки монокулярним 
даним 1604 і 1605-го кадру зображення 
 
 
Рис. 3.6. Порівняння результату: (а) - розрахункова траєкторія руху на 
площині OXY; (Б) - справжня траєкторія руху на супутниковій карті 
 
Порівняння результату експерименту на Рис. 3.5 і 3.6 показує, що 
передбачена траєкторія руху з прийнятною точністю збігається з істинною 
траєкторією руху на супутниковій карті. Це підтверджує правильність алгоритму. 
 
 
Висновки до розділу 3 
В розділі була розроблена структура математичної моделі системи керування 
польотом квадрокоптера з можливістю обльоту перешкод і комплексною 
навігацією в складному середовищі. 
Проведено перевірку працездатності математичеої моделі 
Порівняно результату експерименту та математичної моделі, що передбачена 
траєкторія руху з прийнятною точністю збігається з істинною траєкторією руху на 
супутниковій карті  
  
 
РОЗДІЛ 4 
РОЗРОБЛЕННЯ АВТОМАТИЧНОЇ СИСТЕМИ КЕРУВАННЯ 
КВАДРОКОПТЕРОМ 
 
 
Перед застосуванням КНС в системі управління польотом квадрокоптера 
була проведена перевірка працездатності КНС за допомогою макетування. Щоб 
наблизити макетування до реальних умов, воно було реалізовано в невідомому 
зовнішньому середовищі. 
 
4.1 Датчики і їх основні характеристики 
Основні датчики і плата розробки програми, використані в даній роботі: 
інерційний вимірювальний блок BMI160 (включає тривісний гіроскоп і тривісний 
акселерометр); веб-камера SCB-1100N BA59-02898A для візуальної навігаційної 
системи; Bluetooth GPS-приймач, барометричний висотомір BMP085; 
радіовисотомір індикатор PN 104-0170-00; плата розробки програми 
ArduinoBoard101 (показано на Рис. 4.1). 
 
Рис. 4.1. Основні датчики і плата розробки програми 
Основні характеристики датчиків: 
 
- електричні характеристики акселерометра: діапазон вимірювань - ±2g; 
нульовий зсув - ±25mg; нульовий зсув температурного дрейфу - ±1.0mg/K; помилка 
вирівнювання - ± 0.5°; швидкість передачі вихідних даних -1600 Hz; 
- електричні характеристики гіроскопа: діапазон вимірювань - ± 2500 /с; 
0
нульовий зсув - ± 3 /с; нульовий зсув температурного дрейфу - ± 0,050  /с/К; 
вихідний шум - 0,070 / s / Hz ; швидкість передачі вихідних даних - 3200Hz; 
- електричні характеристики GPS: точність - 10m ; висота над рівнем моря 
- 18000m ; швидкість -  515m / c ; швидкість передачі вихідних даних - 10Hz; 
- внутрішні параметри веб-камери, отримані за допомогою інструменту 
«Калібрування» (набір інструментів для Matlab, написаний в 
Каліфорнійському політехнічному університеті доктором Bouquet): Фокусна 
відстань: fc =  ± ; Центральна точка:cc = ± ; Спотворення: kc =  ± ; 
- електричні характеристики барометричного висотоміра; діапазон 
вимірювань -300~1100hPa ( + 9000 ~ -500m ); вихідний шум - 0.01hPa ( 0.1m ); дозвіл 
вихідних даних  - 0.01hPa ( 0.1m ) ; точність абсолютного тиску - ±1hPa; точність 
відносного тиску - ±0.2hPa; 
- електричні характеристики радіовисотоміра: діапазон вимірювань - 
0
2500feet ( 762m ); точність висоти - ±2%~5% ; дозвіл виялення кута 0, 25  ; 
швидкість передачі даних - 4300 Hz; 
 
 
4.2 Розробка інтерфейсу навігаційної інформації 
 
В нашій роботі був розроблений інтерфейс користувача за допомогою 
інструментарію MATLAB GUI, який може бути застосований для спостереження 
за польотом БПЛА і робтою датчиків на наземній станції в режимі реального часу, 
як показано на рис. 4.2. 
 
 
Рис. 4.2. Інтерфейс інформації КНС 
 
На рис. 4.3 показаний склад приладу макета для відпрацювання навігаційної 
інформації 
 
Рис. 4.3 Склад приладу макета для обробки навігаційної інформації 
 
Проведена обробка декількох джерел навігаційної інформації і відображення 
результатів навігації в компьютері за допомогою MATLAB і інтегрованих 
 
навігаційних алгоритмів. 
Рисунок 4.2 показує інформацію КНС в зовнішньому середовищі в режимі 
навігації БІНС/GPS/SLAM/BA/RA. В області візуальної навігаційної системи 
SLAM показує результати навігації 487-го і 488-го кадрів зображення, характерні 
точки (зелені) двох зображень, орієнтири (червоні) двох зображень, тривимірні 
координати орієнтирів в системі координат камери і траєкторію руху в земній 
системі координат. В області КНС показує траєкторію руху на супутниковій карті 
(червона лінія), отриману з КНС. 
На рис. 4.4 показані виходи датчиків. 
Вихідна навігаційна інформація датчиків показує: 
1. Передбачення зміни широти і довготи візуальної навігаційної системи 
в основному співпадають зі зміненими даними супутникової навігаційної системи 
GPS, хоча є невелика різниця. Це ще раз доводить доцільність застосування 
візуальної навігаційної системи. 
2. В порівнянні з іншими показниками, крива змінена вимірювальної 
висоти на рівнем моря від СНС має значно більші зміни величини і помилки. 
3. Крім цього, СНС може визначити величину і напрямок швидкості 
рухомих об’єктів тільки в горизонтальній площині, але не може визначити 
швидкість в вертикальному напрямі – можливо лиш приблизно обчислити зміни 
швидкості в даному напрямку в залежності від зміни висоти, що дає більші 
помилки. 
4. Тому в цілях підвищення точності навігації необхідно використовувати 
допоміжні датчики і навігаційні системи – зокрема, як в даній роботі, візуальну 
навігаційну систему, барометричний висотомір і радіовисотомір. 
На рис. 4.5 показані помилки виходів датчиків, виходи фільтра Калмана і 
траєкторія руху на горизонтальній площині. 
 
 
 
 
 
 
Рис. 4.4. Виводи датчиків 
 
 
 
 
 
 
 
 
Рис. 4.5. Результати КНС 
 
Результати макетування показують правильність і працездатність 
пропонованої КНС. 
 
 
4.3 Моделювання автономного польоту квадрокоптера в складному 
середовищі 
 
У попередніх розділах було розглянуто теоретичні питання планування і 
управління полѐтом квадрокоптера. В результаті була отримана модель системи 
управління польотом квадрокоптера з врахуванням атмосферного впливу. 
Планування траєкторії розглядалося детально, в результаті був отриманий 
алгоритм генерування глобального маршруту на базі поліпшеного мурашиного 
алгоритму і хмарно-точкової карти пошуку, алгоритм відстеження спланованого 
маршруту на основі методу "L1" з адаптивним вибором опорної точки відстеження, 
алгоритм обльоту перешкод на основі повороту вектора швидкості руху. 
Даний розділ присвячений можливості впровадження алгоритму планування та 
системи керування політним процесом в режимі реального часу в складних умовах, з 
урахуванням атмосферних впливів та різноманітних перешкод. Для досягнення цієї мети 
використовується програмний пакет Matlab Simulink. Об'єктом управління обрано 
квадрокоптер, який відноситься до категорії мініатюрних літальних апаратів з діаметром 
конструкції приблизно 0.5 метра. Квадрокоптер складається з платформи, на якій 
жорстко закріплені чотири ротори, як показано на рисунку 4.6. 
 
 
(а) фізична модель                                  (б) модель в Solidworks 
Рис. 4.6. Модель квадрокоптера  
 
Основні характеристики квадрокоптера представлені в таблиці 4.1: 
Таблиця 4.1 
Характеристики квадрокоптера 
Маса (kg) 1.017 
Моменти інерції (kg*m ) diag (0.0217,0.022,0.0397) 
Відстань від центра мас до осі винта (m) 0.29 
Площа поверхні (m) 0.459 
Радіус винта (m) 0.18 
 
Пакет програми системи управлення 
Змоделюємо автономну систему управління з плануванням маршруту в 
програмному пакеті MATLAB. Модель системи буде складатися з послідовності 
окремих блоків, як показано на рис. 4.7. У кольорових блоках будуть 
реалізовуватися окремі алгоритми. 
 
 
Рис. 4.7. Загальна система керування с плануванням маршруту 
 
1. Блок "PID" осуществляет систему управления траекторией с 
использованием регуляторов PID. (див. рис. 4.8). 
 
 
Рис. 4.8. Система траєкторного управлення 
 
2. Блок «converter» реализує перетворення координат, відповідно до 
порівняння (див. Рис. 4.9). 
 
Рис. 4.9. Перетворювач координат 
 
3. Блок "мультимодового контролера" впроваджує систему 
управління кутом на основі багаторежимової системи, яка складається з чотирьох 
регуляторів зворотного зв'язку, кожен з яких автоматично обирається відповідно 
до умов польоту, таких як висота, кутове положення, вплив вітру та режими зльоту-
посадки (див. рис. 4.10).. 
  
 
 
Рис. 4.10. Система кутового керування на основі багаторежимної 
системи  
 
Внутрішня реалізація блока «БЕК1»представлена нижче (див. рис. 4.11) 
 
Рис 4.11. Внутрішня реалізація блока «БЕК1» 
 
4 Блок «Allocation forces» реалізує розподільник сигналів (див. Рис. 4.12). 
 
Рис 4.12. Розподільник сигналів 
 
У квадрокоптера есть четыре основных типа движения: восходящее 
перемещение, наклон, тангаж (перемещение вперёд и назад) и рыскание (поворот 
вокруг вертикальной оси). 
(1) вертикальне переміщення: еквівалентно збільшується (зменшується) тяга 
чотирьох роторів, і квадрокоптер переміщує вгору; 
(2) крен: тяги роторів 1 і 3 різні, і квадрокоптер рухається навколо осі х (див. 
рис. 1.1 (б)); 
(3) тангаж: тяги роторів 2 і 4 різні, и квадрокоптер рухається навколо осі y 
(див. рис. 1.1 (б)); 
(4) рискання: реактивні моменти чотирьох роторівT1 T3  T2 T4 , і 
квадрокоптер рухається навколо осі z (див. Рис. 1.1 (б)). 
Відношення тяги роторів і режимів руху квадрокоптерів представлені в 
таблиці 4.2. 
Таблиця 4.2. 
Відношення тяги роторів і режимів руху квадрокоптера 
 
P P2 P3 P4 
Вертикальне переміщення + + + + 
  
Крен + — 
  
Тангаж — + 
Рискання + — + — 
«+»- збільшує, «-» - зменшує 
 
5. Блок «MOTOR» реализує модель роторної групи (див. Рис. 4.13). 
В своїй роботі використовуємо двигун моделі X2212 KV980. KV980 – означає 
напругу один вольт, двигун збільшується до 980 обертів кожну хвилину, (980 / 60) 
• 2п = 102.6, в робочому діапазоні зв’язок напруги і швидкості обертання лінійної, 
тобто: w 102.6 u(rad )  
 
 
Рис. 4.13. Модель роторної групи 
 
6. Блок "Модель квадрокоптера" втілює у собі опис квадрокоптера. (див. Рис. 4.14). 
 
Рис. 4.14. Модель квадрокоптера 
 
7. Блок "Модель вітру" виконує моделювання атмосферних умов близько до 
поверхні землі, враховуючи постійний вітер, турбулентність і поривчастий вітер. 
(див. Рис. 4.15). 
 
Рис. 4.15. Модель вітру вблизу поверхні землі  
 
 
8. Блок «State flow» реалізує перетворювач режимів польоту за допомогою 
потоку стану (див. рис. 4.16). 
 
Рис. 4.16. Перетворювач режимів польоту 
 
9. Блок «Global path» реалізує алгоритм планування глобального 
маршруту на основі покращеного мурашиного алгоритму і хмарно-точкової карти 
пошуку за дпомогою блоку Matlab Simulink "MATLAB function"  
10. Блок «Tracking and avoidance» реалізує алгоритми відслідковування 
спланованого маршруту покращеного методу "L1" з адаптивним вибором опорної 
токи відслідквування і обльоту перешкод на основі повороту вектора польоту (див. 
Рис. 4.17). 
 
Рис. 4.17. Блок алгоритмів відстежування спланованого маршруту та 
обльоту перешкод 
 
 
Алгоритми реалізуються за допомогою блока Matlab Simulink "MATLAB 
function" . 
 
Результати моделювання з врахуванням помилок навігації і 
впливу вітру 
Аналіз застосування пропонованої автономної системи управління польотом 
квадрокоптера проведено в тривимірному складному середовищі, в якому існують 
відомі нерухомі перешкоди (жовті циліндри), невідомі нерухомі перешкоди (сині 
циліндри), і рухливі перешкоди (червона сфера). Результати моделювання 
показують, що в процесі польоту по спланованому маршруту квадрокоптер 
успішно уникає зіткнення з усіма перешкодами, як показано на рис. 4.18. 
           
(а) Політ квадрокоптера в тривимірному  
складному динамічному середовищі (б) Проєкція на двовимірну площину 
Рис. 4.18. Моделювання системи керування польотом квадрокоптера з 
плануванням маршруту 
 
Вихідною інформацією є початкова точка, цільова точка і відомі перешкоди. 
Траєкторія польоту квадрокоптера складається з трьох ділянок: A-B і C-D - політ 
квадрокоптера за запланованим маршрутом; B-C - політ квадрокоптера з облітом 
перешкод, як показано на рис. 4.19. 
 
        
(
а) Траєкторія польоту квадрокоптера (б) Проєкція траєкторії польоту на 
в тривимірному середовищі горизонтальній площині 
Рис. 4.19. Траєкторія польоту квадрокоптера 
 
На рис. 4.20 показано вплив вітру на рух квадрокоптера. 
 
 
 
Рис. 4.20. Вплив вітру на рух квадрокоптера 
 
 
Висновки по розділу 4 
 
В розділі 4 було виконано лінійне моделювання ВНС та КНС, а також 
симуляцію польоту квадрокоптера в тривимірному складному середовищі.. 
1. Розроблено макетування ВНС на основі комп'ютерного зору і AR-EKF-
SLAM з локальної асоціації даних, що дозволяє забезпечити навігаційну 
інформацію в разі відсутності сигналу СНС для квадрокоптера. 
2. Проведено макетування КНС в зовнішнього невідомого 
середовища, в результаті чого підтверджена працездатність КНС, а також отримані 
оцінки точності локалізації. 
3. Результати моделювання показують, що пропонована автономною 
системою управління польотом квадрокоптера має можливість планування 
глобального маршруту, відстеження спланованого маршруту і обльоту різних 
перешкод у складній тривимірному середовищі в режимі реального часу. 
 
  
 
ВИСНОВКИ  
 
У роботі на підставі теоретичних і експериментальних досліджень вирішено 
актуальне науково-технічне завдання розробки автономної системи управління 
польотом квадрокоптера з можливістю інтелектуального планування маршруту, 
відстеження спланованого маршруту і обльоту перешкод в складному середовищі 
з урахуванням атмосферного впливу і комплексної навігації з можливістю 
виявлення та ізоляції несправностей. Під час виконання дослідження отримано 
результати, які мають важливе значення як для науки, так і для практики: 
1. Розроблено повноцінну навігаційну систему, яка здатна визначати 
координати перешкод та коригувати покази бортової інерціальної навігаційної 
системи (БІНС) на основі інформації від систем глобального позиціонування 
(СНС), високоточних навігаційних систем (ВНС), барометричного та 
радіовисотоміра. Результати моделювання та прототипування навігаційної системи 
демонструють, що запропонована система поєднує високу точність навігації з 
можливістю виявлення та ізоляції несправностей. 
2. Розроблено систему траєкторного управління для квадрокоптера, 
яка базується на ПІД-регуляторах та багаторежимній системі кутового управління 
польотом. Ця система використовує бекстеппінг-регулятори, щоб керувати рухом 
квадрокоптера вздовж заданого маршруту та підвищити стабільність польоту, 
особливо в умовах вітру та близькості до поверхні землі. Підтвердження отриманих 
результатів здійснено за допомогою моделювання несправностей.. 
3. Розроблено швидкий алгоритм планування просторового 
глобального маршруту до певного середовища з нерухомими перешкодами на 
основі хмарно-точкової карти і поліпшеного мурашиного алгоритму. 
4. Розроблено простий алгоритм обльоту перешкод в складному 
середовищі і алгоритм відстеження маршруту для квадрокоптера на основі 
управління поворотом вектора швидкості. Результати моделювання показують, що 
запропоновані алгоритми дозволяють літати по запланованому маршруту в 
складному середовищі з обльотом нерухомих і рухомих перешкод.