Arduino - [З пісочниці] Використання інерційної навігаційної системи (ІНП) з кількома датчиками на прикладі задачі стабілізації висоти квадрокоптера
Визначення висоти та стабілізація по ній
Визначення координат та політ за заданими точками
Прийом команд з пульта керування та видача керуючих сигналів на мотори
Що ми маємо у розпорядженні?
На даний момент у легкій доступності маємо стандартний набір сенсорів: досить хороші 3-ох осьові гіроскопи.
середні за якістю 3-х осьові акселерометри
середній за якістю 3-х осьовий магнетометр
середній чи поганий барометр
Такий набір у зборі з Arduino-подібним процесором або у вигляді окремої хустки можна знайти за суму 70-100 $ У кожного датчика свої можливості та слабкі сторони. Окремо жоден з них не може вирішити жодного і перерахованих вище завдань, тому системи ІНС завжди будуються з комбінації датчиків, і найцікавіше тут - це обчислювальні алгоритми, що дозволяють з'єднати сильні сторони кожного датчика для усунення їх недоліків. Перше завдання – стабілізація орієнтації – досить успішно вирішується гіроскопами. Гіроскопи дуже точно вимірюють кутову швидкість і після інтегрування можна отримати кути. Але вони мають проблему — свідчення спливають з часом. Для корекції цього дрифту застосовується акселерометр, який завжди (ну чи майже завжди у довгостроковій перспективі) знає, де земля. Але акселерометр нічого не відчує, якщо його крутити навколо осі Z, тому нам потрібний магнетометр, який завжди знає, де північ. Друге завдання - знаходження висоти - частково вирішується барометром. Якщо обнулити свідчення на землі, то при підйомі на кожен метр ми знаємо, наскільки зміняться його свідчення (звісно, якщо ми не літаємо 12 годин і не почала змінюватись погода). Але за умовою завдання барометр у нас поганий, і він видає висоту +-1м з диким шумом амплітудою приблизно в цих межах. Насправді мій датчик показує наступне (на 10-й секунді переміщений на 1метр): На допомогу барометру може прийти сонар, який міряє висоту з дуже високою точністю (навіть той, що я придбав за 5$, видає точність ± 3мм по заяву виробника). Але сонар здатний працювати лише невисоко над землею (2-10м), міряє довго (до 200мс), чутливий до якості поверхні, до кута нахилу, і може втрачати сигнал. Третє завдання -визначення координат — не вирішується зазначеними вище датчиками. Акселерометр в комбінації з гіроскопом може видати лінійні горизонтальні прискорення, але тут є дві проблеми: величезний (порівняно з тим, що будемо вимірювати) вектор 1G, що постійно діє, і відсутність прив'язок для корекції. Тож визначення координат залишається прерогативою GPS-сенсора, і високу точність тут розраховувати не доводиться. У всіх аматорських польотних контролерах завдання знаходження орієнтації вирішено добре і на ній зупинятись не буду. Завдання досить просте і розписане в інтернеті (один, два). У MultiWii використовується гарне рішення без складнощів типу кватерніонів та матриць DCM (не забуваємо, що вважати все це буде простенький 16-мегагерцевий процесор), на основі спрощень для малих кутів та комплементарного фільтра. Отже, орієнтацію апарату в просторі ми знаємо з високим ступенем точності. Тепер можна перейти до основної теми статті, тобто спробувати покращити результати, які видає барометр (або сонар), щоб їх можна було згодувати ПІД-регулятору. Для цього показання повинні надходити без затримок, бути точними у короткій перспективі та не сильно спливати з часом. Тема ПІД-регуляторів заслуговує на окреме пильне вивчення, оскільки він широко використовується в системах управління процесами. Я рекомендую спочатку ознайомитися з їх визначенням, щоб краще зрозуміти міркування, наведені нижче. Згладжуємо
Чим нам не підходять показання барометра у поточному вигляді? Ну по-перше, сильна зашумленность сигналу викликатиме зайві керуючі на мотори. Застосувавши фільтр низьких частот, ми зменшимо шум, але втратимо швидкість виміру. А це означає, що будь-які короткочасні обурення залишаться поза увагою, різкі обуреннявідпрацюються з великою затримкою, і найголовніше ми не отримаємо диференціальну складову (D) для ПІД-регулятора. А як випливає з теорії, регулятор без цієї складової схильний до слабозагасних осциляцій навколо цільової величини, що і спостерігається на практиці. Інтегруємо
Добре, залишимо барометр і візьмемо акселерометр. Начебто все просто - від значення по осі Z віднімемо константу 1G, отримаємо лінійне вертикальне прискорення. Двічі проінтегруємо його (фактично підсумуємо у вимірювальному циклі) і отримаємо швидкість та відносне усунення. Для ПІД-регулятора це ласі показники, з ними можна побудувати хорошу динамічну модель. Але й тут не все так добре, як би хотілося. Нахил апарата викликає зміну проекції вектора прискорення А на вісь Z. Вібрації від двигуна або зміна температури можуть викликати «зсув» чутливості, і наша константа 1G вже не буде відповідати дійсності. Але навіть у випадку ідеально нерухомого апарату і точно виставленої 1G будь-який сенсор видає шум. Адже навіть крихітна помилка протягом десятка секунд подвійного інтегрування зростає до розміру слона, і ми бачимо швидкість 10м/с і висоту 20м (хоча від землі ще навіть не відірвалися). Комплементарний фільтр
Якщо пояснювати по простому - цей фільтр застосовується до двох величин, що вимірюються різними датчиками, і коригує одну з них так, що вона повільно прагне другої. У вимірювальному циклі фільтр реалізується простою формулою: При цьому вплив величини A2 на A1 пропорційно різниці між ними і визначається коефіцієнтом k (що більший, тим слабший вплив). Якщо застосувати цей фільтр до висоті, знайденої акселерометром, і показанням барометра, вийде цікава штука: дрифт акселерометра постійно коригуватиметься барометром, апоказання барометра будуть згладжуватися (оскільки A2 цей фільтр працює як фільтр низьких частот). Але коригуватися буде лише другий інтеграл, а перший, як і раніше, спокійно «дрейфувати» в нескінченність, і в результаті через малий коефіцієнт k барометр просто не зможе вплинути на ситуацію. Чому цей фільтр чудово працює для пари гіроскоп + акселерометр? Тому що там ми коригуємо перший інтеграл, і він зрештою перестає «спливати», коли величина корекції за час одного циклу зрівняється з величиною помилки гіроскопа, що додається в цьому циклі при інтегруванні. ПІД-регулятор приходить на допомогу
Але і з пари барометр + акселерометр можна отримати щось корисне, якщо застосувати до них ПІД-регулятор (так, область їх застосування вкрай велика). Отже, у чому головне слабке місце нашого інтегратора прискорення? У мікро-помилці, яка може виникнути з різних причин, описаних вище, при відніманні константи 1G. Якщо записати шукане прискорення у вигляді: то, регулюючи величину bias, можна керувати і першим інтегралом (швидкістю), і другим (зміщенням). Отже, мету нашого ПІД-регулятора знайдено. Але ще треба знати помилку. Припустимо, що bias зафіксується після настання певної стабілізації властивостей системи (температурних, вібраційних і т. д.). Коли bias буде знайдено, показання акселерометра стануть дуже близькими до істини і можна застосувати комплементарний фільтр, схрестивши їх із барометром. Величина корекції цього фільтра і буде помилкою, від якої відштовхуватиметься ПІД-регулятор (він прагне звести помилку до 0 за рахунок цільової змінної регулювання). Далі знаходимо всі три складові ПІД-регулятора. Пропорційна (P) – це сама помилка. Інтегральна (I) - просто інтегруємо її. Диференціальна (D) - з теоріїтреба диференціювати помилку. Але в ній сидять жахливі шуми барометра та акселерометра. Таке диференціювати страшно, тому застосуємо хитрість — візьмемо за D-швидкість, що складає знайдену акселерометром, з негативним знаком. Так як D покликана ввести згасання в регулятор, швидкість тут цілком пригодиться - чим вона більша, тим більше її «погасити». Помножимо кожен із компонентів на свої коефіцієнти, складемо і отримаємо bias. Але тут застосуємо другу хитрість - не додаватимемо bias безпосередньо до прискорення, а додамо тільки I-частину. Саме вона описує величину постійно діючої помилки, що відповідає нашому припущенню про повільну зміну bias. P і D частини додамо до швидкості, помноживши на dT (оскільки запозичили їх з прискорення). Оскільки основне завдання регулятора — знайти постійну складову помилки, я вирішив налаштувати його досить «м'яко», щоб щонайменше впливати на короткочасні зміни. Але тут залишається широке поле для експериментів, і все визначатиметься поведінкою реальних датчиків. А як же гіроскоп?
Вище я згадав, що для визначення висоти нам знадобиться ще й гіроскоп. Дійсно, описаний вище алгоритм працюватиме, тільки якщо вектор A (у локальній системі) дивиться точно вздовж осі Z. Як тільки апарат нахилиться, відбудуться дві неприємні речі: проекція A на вісь Z зміниться і ПІД-регулятор почне заново повільно і болісно шукати bias . І друга — будь-яке горизонтальне прискорення почне давати ненульову проекцію на локальну вісь Z. При кутах нахилу 45° вже й не зрозумієш, де прискорення. Але ми знаємо точну орієнтацію локальної системи щодо глобальної, немає нічого складного відновити справедливість — просто спроектуємо локальний вектор A на локальний вектор G (спочатку знайденийакселерометром і дбайливо обертається гіроскопом), який завжди дивиться в землю. Операція ця проста і випливає із визначення векторного твору: Зробити це треба до віднімання 1G. Тепер можна подивитися на код та результати. #define ACC_BARO_CMPF 300.0f #define ACC_BARO_P 30.0f #define ACC_BARO_I (ACC_BARO_P * 0.001f) #define ACC_BARO_D (ACC_BARO_P * 0.0 - 1.0f/ACC_BARO_CMPF)/1000000.0f) #define ACC_SCALE 9.80665f / acc_1G / 10000.0f
err = (alt - BaroAlt)/ACC_BARO_CMPF; // P term of error errI + = err * ACC_BARO_I; // I term of error accZ = (accADC[0]*EstG.V.X + accADC[1]*EstG.V.Y + accADC[2]*EstG.V.Z) * InvSqrt(fsq(EstG.V.X ) + fsq(EstG.V.Y) + fsq(EstG.V.Z)) - errI - acc_1G; // Integrator - velocity, cm/sec vel+= (accZ - err*ACC_BARO_P - vel*ACC_BARO_D) * cycleTime * ACC_SCALE;
// Integrator - altitude, cm alt + = vel * cycleTime * VEL_SCALE;
// Apply ACC->BARO complementary filter alt-= err; errPrev = err;