A model and software for the inertial measurement unit

The module of stabilization of an inclination of the plane with three degrees of freedom, the software for data acquisition from MPU-6050 using the I2C protocol, and the program of realization of the digital proportional-integral-diff erential regulator with algorithm of subtomatic factor is develop...

Повний опис

Збережено в:

Бібліографічні деталі
Дата:	2022
Автори:	Bezpalko, S.O., Shymkovysh, V.M., Doroshenko, A.Yu.
Формат:	Стаття
Мова:	Ukrainian
Опубліковано:	Інститут програмних систем НАН України 2022
Теми:	gyroscopic system stabilization module inertial measuring unit 6 axis inertial measurement system controller pulse width modulation I2C UDC 004.93’1
Онлайн доступ:	https://pp.isofts.kiev.ua/index.php/ojs1/article/view/493
Теги:	Додати тег Немає тегів, Будьте першим, хто поставить тег для цього запису!
Назва журналу:	Problems in programming

Репозитарії

Problems in programming

id	pp_isofts_kiev_ua-article-493
record_format	ojs
resource_txt_mv	ppisoftskievua/82/e4c2aa27af82d927253836e34a3c9d82.pdf
spelling	pp_isofts_kiev_ua-article-4932023-01-15T19:45:45Z A model and software for the inertial measurement unit Модель та програмне забезпечення для інерційного вимірювального пристрою Bezpalko, S.O. Shymkovysh, V.M. Doroshenko, A.Yu. gyroscopic system; stabilization module; inertial measuring unit; 6 axis inertial measurement system; controller; pulse width modulation; I2C UDC 004.93’1 гіроскопічна система; модуль стабілізації; інерційний вимірювальний пристрій; регулятор; алгоритми автоматичного підбору коефіцієнтів УДК 004.93’1 The module of stabilization of an inclination of the plane with three degrees of freedom, the software for data acquisition from MPU-6050 using the I2C protocol, and the program of realization of the digital proportional-integral-diff erential regulator with algorithm of subtomatic factor is developed. A model of a gyroscopic device for testing the created solution was built. Considering time and resource constraints, the following solutions were chosen: PID controller for motor voltage control, PWM as a way to create a control signal, Kalman fi lter for processing output of the IMU, device and relay method in pairs with the Ziegler-Nichols method for the selection of the coeffi cients of the regulator. Test results or the following characteristics of the transition process: the time of the transition process - 0.44s, overregulation - 6.2%. Further improvement of the quality of the transient process is possible through the use of high- quality inertial measuring devices, high-precision electric motors withhigh torque.Prombles in programming 2022; 2: 03-12 Проблеми автоматичного регулювання в сучасних технічних системах вимагають вирішення та- ких задач, як дотримання високої точності регулювання і здатності працювати в реальному часі. В даній роботі ці задачі вирішуються на прикладі розробки моделі та програмного забезпечен- ня модуля стабілізації кута нахилу площини з трьома ступенями свободи, а також програмного забезпечення для отримання даних з акселерометра-гіроскопа MPU-6050 з використанням протоколу I2C. Розроблено програмну реалізацію цифрового пропорційно-інтегрально-диференціального регулятора з алгоритмом автоматичного підбору коефіцієнтів. Побудовано модель гіроскопічного пристрою та проведено тестування створеного рішення.Prombles in programming 2022; 2: 03-12 Інститут програмних систем НАН України 2022-09-26 Article Article application/pdf https://pp.isofts.kiev.ua/index.php/ojs1/article/view/493 10.15407/pp2022.02.003 PROBLEMS IN PROGRAMMING; No 2 (2022); 3-12 ПРОБЛЕМЫ ПРОГРАММИРОВАНИЯ; No 2 (2022); 3-12 ПРОБЛЕМИ ПРОГРАМУВАННЯ; No 2 (2022); 3-12 1727-4907 10.15407/pp2022.02 uk https://pp.isofts.kiev.ua/index.php/ojs1/article/view/493/491 Copyright (c) 2022 PROBLEMS IN PROGRAMMING
institution	Problems in programming
baseUrl_str	https://pp.isofts.kiev.ua/index.php/ojs1/oai
datestamp_date	2023-01-15T19:45:45Z
collection	OJS
language	Ukrainian
topic	gyroscopic system stabilization module inertial measuring unit 6 axis inertial measurement system controller pulse width modulation I2C UDC 004.93’1
spellingShingle	gyroscopic system stabilization module inertial measuring unit 6 axis inertial measurement system controller pulse width modulation I2C UDC 004.93’1 Bezpalko, S.O. Shymkovysh, V.M. Doroshenko, A.Yu. A model and software for the inertial measurement unit
topic_facet	gyroscopic system stabilization module inertial measuring unit 6 axis inertial measurement system controller pulse width modulation I2C UDC 004.93’1 гіроскопічна система модуль стабілізації інерційний вимірювальний пристрій регулятор алгоритми автоматичного підбору коефіцієнтів УДК 004.93’1
format	Article
author	Bezpalko, S.O. Shymkovysh, V.M. Doroshenko, A.Yu.
author_facet	Bezpalko, S.O. Shymkovysh, V.M. Doroshenko, A.Yu.
author_sort	Bezpalko, S.O.
title	A model and software for the inertial measurement unit
title_short	A model and software for the inertial measurement unit
title_full	A model and software for the inertial measurement unit
title_fullStr	A model and software for the inertial measurement unit
title_full_unstemmed	A model and software for the inertial measurement unit
title_sort	model and software for the inertial measurement unit
title_alt	Модель та програмне забезпечення для інерційного вимірювального пристрою
description	The module of stabilization of an inclination of the plane with three degrees of freedom, the software for data acquisition from MPU-6050 using the I2C protocol, and the program of realization of the digital proportional-integral-diff erential regulator with algorithm of subtomatic factor is developed. A model of a gyroscopic device for testing the created solution was built. Considering time and resource constraints, the following solutions were chosen: PID controller for motor voltage control, PWM as a way to create a control signal, Kalman fi lter for processing output of the IMU, device and relay method in pairs with the Ziegler-Nichols method for the selection of the coeffi cients of the regulator. Test results or the following characteristics of the transition process: the time of the transition process - 0.44s, overregulation - 6.2%. Further improvement of the quality of the transient process is possible through the use of high- quality inertial measuring devices, high-precision electric motors withhigh torque.Prombles in programming 2022; 2: 03-12
publisher	Інститут програмних систем НАН України
publishDate	2022
url	https://pp.isofts.kiev.ua/index.php/ojs1/article/view/493
work_keys_str_mv	AT bezpalkoso amodelandsoftwarefortheinertialmeasurementunit AT shymkovyshvm amodelandsoftwarefortheinertialmeasurementunit AT doroshenkoayu amodelandsoftwarefortheinertialmeasurementunit AT bezpalkoso modelʹtaprogramnezabezpečennâdlâínercíjnogovimírûvalʹnogopristroû AT shymkovyshvm modelʹtaprogramnezabezpečennâdlâínercíjnogovimírûvalʹnogopristroû AT doroshenkoayu modelʹtaprogramnezabezpečennâdlâínercíjnogovimírûvalʹnogopristroû AT bezpalkoso modelandsoftwarefortheinertialmeasurementunit AT shymkovyshvm modelandsoftwarefortheinertialmeasurementunit AT doroshenkoayu modelandsoftwarefortheinertialmeasurementunit
first_indexed	2025-07-17T09:46:21Z
last_indexed	2025-07-17T09:46:21Z
_version_	1838409704993193984
fulltext	3 Прикладні засоби програмування та програмне забезпечення Вступ Сучасний етап розвитку людської цивілізації передбачає застосування різ- них механізмів, які потребують можли- вості точно регулювати кут нахилу своїх елементів у різних площинах. Наприклад, від танків нового покоління вимагають- ся досконалі системи наведення гармат, а отже можливість стабілізації ствола. До даних систем постійно висуваються нові вимоги: такі як збільшення точності ста- білізації та швидкості, поліпшення зава- достійкості та ін. Для виконання всіх цих вимог за- водами-виробниками і розробниками по- стійно удосконалюються конструкції і ро- бочі процеси в стабілізаторах озброєння. На даний момент створено нове покоління високоточних датчиків та обчислюваль- них комплексів, яке об’єднує результати науково-технічного прогресу і практично- го досвіду. Такі системи керування кутом на- хилу необхідні не тільки у військовій сфері, а й мають безліч застосувань у комерції. Зокрема, вдало застосувують- ся для вирішення проблеми стабілізації камери або фотоапарату для фото- чи відеозйомки. Метод, який не вимагає будь-яких додаткових характеристик комплекту «корпус-об’єктив», полягає у стабілізації всього корпусу камери — на противагу використанню внутрішнього механізму стабілізації зображення. Цьо- го досягають завдяки гіроскопу, приєд- наного до корпусу камери. Це дозволяє зовнішньому гіроскопу стабілізувати камеру. Використовується в основному для фотографування із транспортних за- собів під час руху в тому випадку, коли об’єктиви чи камери із іншим типом ста- білізації зображення недоступні або ви- являються неефективними. Нині дуже великої популярності набули квадрокоптери, для яких контроль кута нахилу відносно землі є першочерго- вою задачею для якісного та контрольо- ваного польоту. Також подібні технології можуть полегшувати щоденне життя лю- дей із хворобою Паркінсона, а також за- стосовуватись у хірургії для полегшення роботи лікаря та уникнення механічних помилок. Якісні електронні датчики-гіроско- пи стали доступними в роздрібній торго- вельній мережі зовсім недавно. Електро- нні гіроскопи – це перетворювачі нахилу і кутової швидкості в електричний струм. Вони служать для контролю положення і кутових швидкостей із коротким часом відгуку. Відмінністю електронних гірос- копів від їхніх механічних побратимів є компактність і мала вага, відсутність зно- шування елементів, висока швидкість від- гуку, низька напруга живлення та малий струм живлення. Діапазон робочих темпе- ратур дозволяє здійснювати контроль па- УДК 004.93’1 HTTPS://DOI.ORG/10.15407/PP2022.02.003 С. О. Безпалько, В. М. Шимкович, А. Ю. Дорошенко МОДЕЛЬ ТА ПРОГРАМНЕ ЗАБЕЗПЕЧЕННЯ ДЛЯ ІНЕРЦІЙНОГО ВИМІРЮВАЛЬНОГО ПРИСТРОЮ Проблеми автоматичного регулювання в сучасних технічних системах вимагають вирішення та- ких задач, як дотримання високої точності регулювання і здатності працювати в реальному часі. В даній роботі ці задачі вирішуються на прикладі розробки моделі та програмного забезпечен- ня модуля стабілізації кута нахилу площини з трьома ступенями свободи, а також програмного забезпечення для отримання даних з акселерометра-гіроскопа MPU-6050 з використанням про- токолу I2C. Розроблено програмну реалізацію цифрового пропорційно-інтегрально-диференці- ального регулятора з алгоритмом автоматичного підбору коефіцієнтів. Побудовано модель гірос- копічного пристрою та проведено тестування створеного рішення. Ключові слова: гіроскопічна система, модуль стабілізації, інерційний вимірювальний пристрій, регулятор, алгоритми автоматичного підбору коефіцієнтів. © С.О. Безпалько, В.М. Шимкович, А.Ю. Дорошенко, 2022 ISSN 1727-4907. Проблеми програмування. 2022. № 2 4 Прикладні засоби програмування та програмне забезпечення раметрів у широкому спектрі кліматичних умов і географічних поясів. Під час автоматичного регулювання кута виникають наступні задачі: необхід- но отримати високу точність регулювання (< 18 мрад), система має працювати в ре- альному часі, тобто встигати здійснювати обрахунки в детермінований проміжок часу, необхідний для нормальної роботи об’єкту(час реакції < 0.001 секунди). На- приклад (це особливо важливо), якщо ви- рішується питання стабілізації польоту квадрокоптера, коли занадто довгий час реакції призведе до падіння. Також необ- хідно, щоб перерегулюваня було не біль- ше 15%, адже це може впливати на ста- більність системи. 1. Апаратна платформа та модель з інерційним вимірювальним пристроєм В якості інерційного вимірюваль- ного пристрою в даній роботі був ви- користаний модуль акселерометра і гі- роскопа MPU6050, що підключається до мікроконтролера через шину I2C. Збір даних із MPU6050 можливий двома спо- собами: збір сирих даних прямо з гірос- копічного датчика та акселерометра або використання вбудованого процесора руху, що називається виробником Digital Motion Processor (DMP)[1]. Для розроб- ки коду та тестування отриманих даних було створено схему на макетній платі, що містить Arduino Nano та MPU6050. Це необхідно для дослідження різних способів отримання даних, перевірки працеспроможності інерційної вимірю- вальної системи та проведення необхід- них налаштувань. Завдяки підключенню до персонального комп’ютера через уні- версальну послідовну шину з′являється можливість будувати графіки залежнос- ті положення від часу і, таким чином, ві- зуалізувати бачити різні проблеми робо- ти датчика. Після налаштувань запускається го- ловний цикл програми і починається про- цес зчитування даних. Оскільки гіроскоп надає дані у вигляді кутової швидкості в даний момент, а не власне значення кута, необхідно здійснити розрахунки. Рівнян- ня кутів на момент часу буде мати наступ- ний вигляд: (1) де , та - 3 кута Ейлера в даний мо- мент часу; , та – кути в попе- редній проміжок часу, , та – ку- това швидкість у відповідних площинах, – час між двома замірами; Отримання часу здійснюється за допомогою функції millis() та розрахову- ється кількість секунд із моменту мину- лого обрахунку. Дані датчика гіроскопа розміщені за адресами 0x43, 0x44 для осі x, 0x45, 0x46 для y та 0x47, 0x48 для z (ри- сунок 3). 2 байти для кожної з осей. Для отримання всіх даних у коді запрошуються 6 байт з адреси 0x43. Піс- ля цього вони записуються у відповідні змінні. Поділ на значення 131 необхід- не для переведення даних у градуси/с. Це значення відрізняється в залежності від виставленого діапазону на гіроско- пі. Стандартне значення діапазону - 250 град/c. Після цього вираховується ста- ла похибка датчика з заміряних кутових швидкостей та обраховуються кути за формулою (1). Щоб збільшити точність показни- ків, до розрахунку можна додати кути, розраховані за допомогою акселерометра. З роботи [4] відомо, що: де x, y – кути в радіанах, a, b, с – рух у трьох координатах, заміряний акселеро- метром; за цими формулами розрахову- ються кути, що переводяться в градуси. Відтак також віднімається статична по- хибка. Статична похибка обраховується експериментально, поклавши датчик на рівну поверхню та зробивши декілька сотень розрахунків і порахувавши їх се- реднє значення. Після обрахування кутів гіроскопу та акселерометра можна отримати кінце- вий результат, застосувавши «підсумову- 5 Прикладні засоби програмування та програмне забезпечення ючий фільтр», який підсумовує дані в пев- ній пропорції. В даному випадку беруться 96% даних із гіроскопа та 4% даних із ак- селерометра, далі отримані дані виводять- ся в послідовний порт для відображення їх на персональному комп’ютері. Проте, цей метод збору інформації не є ідеальним. Статична похибка досі присутня, хоча і є дуже малою. Під час руху відбуваються певні стрибки, що уне- можливлює отримання кута обертання навколо осі Z з акселерометра, бо вона вирівняна з вектором гравітаційної сили. Однак головна проблема в тому, що після руху датчика з′являється велика похибка, яку неможливо усунути. Це пов’язано з тим, що мікроконтролер не встигає обро- бити всі дані. Тому якщо після руху по- класти датчик у те ж положення, в якому він знаходився, буде добре видно, що дані спокою будуть зовсім іншими. Для усунення таких помилок за- стосовується процесор руху, вбудований в MPU6050 [5]. Отриманий пакет даних повертає положення сенсора у вигляді кватерніона, який одразу виводиться в послідовний порт для відображення да- них на комп’ютері. Виведення положен- ня через кватерніон є перевагою даної бібліотеки, адже проводити обрахунки з використанням кватерніонів набагато легше, ніж з кутами Ейлера. Кватерніони можна перевести в кути Ейлера за допо- могою формули: де , – кути Ейлера, - елементи ква- терніона [6]. Проте, позаяк функції arctan та arcsin в комп’ютерних мовах програму- вання імплементовані так, що працюють тільки в межах від −π/2 до π/2, це накла- дає обмеження на всі ступені свободи [17]. Аби мати можливість згенерувати кути для всіх можливих орієнтацій платформи необхідно використовувати функцію atan2 замість arctan. де α, β,γ – кути Ейлера, q - кватерніон;[6] де α, β,γ – кути Ейлера, qi – елементи ква- терніона. В [5] міститься програмна реалі- зація даної формули в структурі MPU6050. В даному випадку для розрахунку нахилу в осі Y та в осі X використовується вектор гравітації, який можна отримати з акселе- рометра. 2. Цифровий ПІД регулятор ПІД регулятор можна виразити ма- тематично через наступне рівняння: де – пропорційний коефіцієнт, – ін- тегральний коефіціент, – диференці- альний коефіцієнт, e(t) – помилка; це рів- няння можна розділити на 3 окремі час- тини. Де pOut – формула пропорційного впливу, iOut – інтегрального та dOut – ди- ференціального[8]. Після цього формулу можна переписати наступним чином: Підібравши коефіцієнти регулято- ра, можна отримати майже ідеальний пе- рехідний процес системи. Тому програмна 6 Прикладні засоби програмування та програмне забезпечення реалізація має мати наступні особливості: можливість легкої зміни коефіцієнтів, ви- хід регулятора має регулюватись у певних межах, а також має бути можливість зада- вання різної позиції. Нижче на рис.1 показано оголошен- ня класу, який реалізує логіку роботи про- порційно-інтегрально-диференціального регулятора. Програмний інтерфейс даного кла- су складається з трьох функцій та одно- го конструктора. В конструкторі визнача- ються межі регулювання, setK відповідає за встановлення коефіцієнтів регулятора. SetPos використовується для визначення необхідного положення, calcReg – функ- ція, що прораховує керуючий вплив, на вхід вона приймає значення положення в даний момент часу, а на виході повертає значення впливу. Рисунок 1. Програмна реалізація цифрового ПІД регулятора Конструктор класу задає стандарт- не значення коефіцієнтам та визначає межі виходу регулювання. Для правиль- ної роботи також необхідно визначити час початку роботи. Для цього викорис- товується функція millis, яка є частиною ядра Arduino. Щоб розрахувати керуючий вплив спочатку треба отримати попередні дані. Тому в calcReg, на рис. 1, спачатку здійснюється обрахунок часу, що минув. Для цього через функцію millis отриму- ється поточний час і від нього відніма- ється preTime. Після обрахування dT в змінну preTime записується оновлене зна- чення теперішнього часу. Для отримання значення помилки від заданого значення необхідного положення віднімається ре- альне значення, зняте з датчиків, яке ця функція отримує через змінну inp_. Після цієї підготовки починається розрахунок впливу для кожної частини. 3. Керування двигунами гіроскопічної системи ПІД регулятор не матиме ніякої ко- ристі за відсутності можливості керуван- ня електромоторами, які безпосередньо впливають на зворотній зв’язок систе- ми. Для керування обраними драйверами двигунів необхідно генерувати сигнал на виведених пінах мікроконтролера. Щоб мати можливість міняти момент обертан- ня електромоторів необхідно міняти на- пругу на виході. Для вирішення цієї задачі використовується широтно-імпульсна мо- дуляція (ШІМ) [9]. Змінюючи коефіцієнт заповнення можна отримати сигнал із різною серед- ньою напругою. Це добре видно на рис. 2, Рисунок 2. Моделювання синусоїдального сигналу в ШІМ[10] 7 Прикладні засоби програмування та програмне забезпечення де модулюється синусоїдальний сигнал. Щоб вирівняти вихідний сигнал можна за- стосувати фільтр низьких частот у вигля- ді додаткової ємності, проте, позаяк дана система дуже інертна, це не потрібно. Середню напругу сигналу можна отримати за формулою: де – середня напруга, – функція ШІМ сигналу з періодом . Оскільки – це імпульсна хвиля, можна ска- зати, що її значення буде рівне , в періоді, де час і , коли відповідно, де D – коефіці- єнт заповнення. Тому дане рівняння мож- на переписати так: У випадку, якщо = 0, то дане рівняння можна спростити навіть більше: Мікроконтролер має можливість генерувати ШІМ сигнал своїх вихідних портах. Оскільки двигуни можуть бути підключені по-різному в залежності від фізичної реалізації, програма має мати можливість легко присвоювати піни для кожного електромотора. Головною ме- тою цього модуля є можливість легко змінювати напругу на входах електро- двигунів. Для реалізації цього функціо- налу було створено 2 класи – GimbalMot та Motor. Клас Motor відповідає за керуван- ня двигуном. У ньому зберігаються піни, що відведені на підключення цього мото- ру, напрям обертання та швидкість. Інтер- фейс цього класу складається з функції та конструктора. В конструкторі ініціалізу- ються відведені піни, а функція setO від- повідає за встановлення вихідного сигна- лу на пінах мікроконтролера. Клас GimbalMot у свою чергу від- повідає за керування трьома двигунами гіроскопічної системи. Для цього вико- ристовуються три об’єкти вище згадано- го класу Motor. Для повороту в осі x, для обертання в осі y та для обертання в осі z. Для ініціалізації необхідно передати на вхід номера шести пінів, по 2 для кожно- го мотору. Є можливість керувати кожним мотором окремо через відповідні функції setVel[x/y/z]. Збільшення частоти ШІМ необ- хідне щоб прибрати шум з електромо- торів, адже нижчі частоти може почути людське вухо. Після налаштування ШІМ починається налаштування переданих пінів як виходів. Потім із даних виходів створюються по три об’єкти класу Motor для кожної осі. Оскільки в даному ви- падку зберігається посилання на об’єкт, а не сам об’єкт, в деструкторі необхідно здійснити очищення пам’яті. Тому клас GimbalMot містить деструктор для звіль- нення пам’яті. Через це відбувається ви- вільнення пам’яті для кожного з моторів. Таким чином з′являється можливість на- лаштування потрібного двигуна без зміни роботи інших. 4. Фільтр вхідних даних Оскільки датчики зазвичай не іде- альні, їхній вихідний сигнал буде під впливом різних шумів, тож необхідно мати фільтр для отримання точніших да- них. Для розв′язання даної проблеми було прийнято рішення використати фільтр Калмана. Для того, щоби використовува- ти фільтр Калмана для оцінювання вну- трішнього стану процесу, маючи лише послідовність зашумлених спостережень, необхідно змоделювати процес відповід- но до моделі фільтру Калмана. Це озна- чає задання наступних матриць: - мо- делі переходу станів; - моделі спосте- реження; - коваріації шуму процесу; - коваріації шуму спостереження; та іноді - моделі керування для кожного момен- ту часу, k[12]. де – модель переходу стану для попе- реднього стану , - модель впливів керування, що застосовується до вектора керування , – шум процесу, що має 8 Прикладні засоби програмування та програмне забезпечення нормальний розподіл з нульовим середнім значенням та коваріацією ; У момент вимірювання справж- нього стану отримується за наступною формулою: де є моделлю спостереження, що відо- бражає простір справжнього стану у спо- стережуваний простір, і є шумом спо- стереження, що, як вважається, є ґаусовим білим шумом з нульовим середнім значен- ням і з коваріацією . Фільтр Калмана є рекурсивним, а отже для роботи йому необхідний тіль- ки минулий стан та поточні дані. Це на- дає перевагу над пакетними фільтрами, які для своєї роботи потребують збір та збереження масиву даних. Щоб зберег- ти стан фільтру використовуються дві змінні: – оцінка стану в момент часу k при заданих спостереженнях до моменту часу k включно та – ко- варіаційна матриця помилок. Алгоритм фільтра Калмана можна розділити на дві частини – стадія передбачення та уточнення. Стадія передбачення використовує оцінку стану з попереднього моменту часу для отримання оцінки стану на поточний момент часу. Розрахунки в цій стадії включають розрахунок передбаченої оцінки стану та коваріацію передбаченої оцінки. У стадії уточнення необхідно роз- рахувати наступні значення: відхилен- ня вимірювання, коваріацію відхилення, оптимальний передавальний коефіцієнт Калмана, оновлену оцінку стану, коваріа- цію оновленої оцінки [12]. Розроблений модуль має наступні функції: getAngle – повертає відфільтро- ване значення кута (для цього йому необ- хідно передати значення кута зібране з датчика), кутову швидкість в цій площи- ні та пройдений час в секундах. Функція setAngle відповідає за встановлення по- чаткового кута нахилу, необхідного для майбутніх розрахунків. Завдяки функції getRate можна та- кож отримати відфільтроване значення кутової швидкості. Для тонкого калібру- вання фільру використовуються функції setQangle, setQbias та setRmeasure. Q_ angle - дисперсія шуму процесу для аксе- лерометра, Q_bias - дисперсія шуму про- цесу для зміщення гіроскопа, R_measure - дисперсія шуму вимірювання. У конструкторі класу Kalman здій- снюється початкова ініціалізація змін- них. Виставляються початкові значення для вищезгаданих Q_angle, Q_bias та R_measure, які в подальшому можна ко- регувати для кращої роботи. Також за- повнюється матриця коваріації помилок P[2][2]. Позаяк на даний момент відоме точне положення та швидкість у почат- ковий момент необхідно заповнити її ну- льовим значенням. Алгоритм отримання даних можна умовно поділити на сім кроків. Стадія пе- редбачення включає перші 2 кроки (рис. 3). Перший крок передбачає виконання дискретних рівнянь оновлення часу філь- тра Калмана, обраховується змінна rate та angle. Наступним кроком буде обрахуван- ня нових значень для матриці коваріації помилок P. Після цього починається ста- дія уточнення, в якій обчислення здійсню- ються за формулами, згаданими вище. Наступною буде стадія уточнення. Крок номер 3 - це обрахунок відхилення вимірювання. Крок 4 включає обрахуван- ня коваріації відхилення. Далі у кроці 5 обраховується коефіцієнт Калмана, що на виході являє собою вектор 2 на 1. Після цього необхідно оновити оцінку стану, що на виході дає шуканий кут. Останній крок - це розрахунок коваріації оновленої оцін- ки, що знову оновлює значення в матриці P для наступного циклу. В кінці функція повертає отриманий кут. 9 Прикладні засоби програмування та програмне забезпечення Тестування розробленого фільтра було проведено на генераторі шуму. Як видно з графіку на рис. 4, фільтр Калма- на чудово справляється з даним шумом (на графіку – unfi ltered) та легко отримує значення (на графіку – fi ltered), подібне до реального (на графіку – real). Однак, як бачимо на рис. 5, фільтр може додавати затримку сигналу, тому для отримання точних даних необхідно здійснювати точне калібрування і підбір значень змінних Q_angle, Q_bias та R_ measure. Оскільки обрано інерційний ви- мірювальний пристрій, датчик MPU6050 під час роботи із вбудованим цифровим процесором руху (DMP) сам фільтрує ви- хідні дані. 5. Тестування Розроблена керуюча програма мі- кроконтролера складається з чотирьох відокремлених частин: модуль зв’язку з інерційним вимі- рювальним пристроєм; модуль, що містить реалізацію ПІД регулятора; модуль керування електродвигуна- ми гіроскопічної системи; модуль фільтрування вхідних да- них. Для проведення тестування роз- робленої системи було зібрано робочий макет. Для спрощення створення макету прийнято рішення зробити для тестуван- ня систему з одним ступенем свободи. Рисунок 3. Фрагмент коду фільтра Калмана, крок 1-2[7] Рисунок 4. Фрагмент коду фільтра Калмана, крок 3-7[7] 10 Прикладні засоби програмування та програмне забезпечення Для обрахунку пропорційної, інте- гральної та диференціальної частин ре- гулятора було вирішено застосувати ал- горитм автоматичного підбору. Для цієї задачі обрано релейний метод у парі з методом Зіглера-Нікольса[12]. Метод Зі- глера-Нікольса передбачає використання таблиці для підбору коефіцієнтів. Тип регулятора Класичний ПІД 0.6 0.5 0.125 Інтеральне правило Песена 0.7 0.4 0.15 Невелике перерегулювання 0.33 0.5 0.33 Без перерегулювання 0.2 0.4 0.33 Підбір коефіцієнтів регулятора про- ходитиме за відносно простим алгорит- мом, що включає поєднання двох методів. Спершу необхідно виставити максималь- ний вихід регулятора, та очікувати отри- мання заданого цільового положення, піс- ля чого вимкнути вихідний сигнал. Коли появиться похибка, ще раз налаштувати вихід контролера. Повторити ці кроки де- кілька разів. Далі необхідно обрахувати ультимативний множник, використовую- чи значення амплітуди виходу регулятора, та обрахувати період коливань регулюван- ня. Отримавши ці дані можна обрахувати коефіцієнти з використанням метода Зі- глера-Нікольса. Таким чином, було отримано на- ступний результат: На даному графіку(рис. 5) зображе- но перехідний процес системи при надан- ні короткотривалого фізичного імпульсу на макет системи. З даного графіку можна визначити наступні характеристики пере- хідного процесу: час перехідного процесу - 0.44с, перерегулювання - 6.2%. Результати тестування моделі задо- вільняють поставлені технічні вимоги до її роботи. Рисунок 4. Результат роботи фільтру 11 Прикладні засоби програмування та програмне забезпечення Висновки Розробка модулю стабілізації кута нахилу площини з трьома ступенями свободи була виконана із урахуванням новітніх досягнень у сфері збору даних про положення об’єкта в просторі. Да- ний модуль був розроблений так, щоб легко замінювати частин модуля та ви- користовувати різні вузли, які відрізня- ються від описаних у роботі. Для того, щоб поміняти вузол, необхідно зміни- ти модуль у програмі мікроконтролера, відповідальний за нього. За умови до- тримання заданого інтерфейсу для змі- ни буде достаньо поміняти лише один файл. Дані характеристики уможливлю- ють гнучке застосування даного модуля в багатьох різних сферах. Перевагами даного модуля є про- стота реалізації, легкість підключення, гнучкість програми мікроконтролера та можливість використання дешевих та по- ширених компонентів. Використання но- вітніх інерційних вимірювальних систем дозволяє легко отримувати точні дані про положення регульованої площини. Щоб мати можливість виконувати роботу на- віть у випадку шуму показників був реа- лізований фільтр даних, що дозволяє пра- цювати із більш давніми моделями дат- чиків. Для налаштування регулятора було запропоновано використання популярних методик підбору коефіцієнтів ПІД, що значно спрощує запуск системи, яка буде використовувати даний модуль. У процесі роботи були розроблені структурна, принципова електрична схе- ма, схема алгоритму програми мікрокон- тролера, обрані та описані основні вузли й елементи. Для підтвердення дієвості даного модуля було створено розділ, присвяче- ний розробці фізичної моделі. В ньому була сформована структура моделі та про- демонстрована можливість стабілізації площини навіть у разі використання не- ідеальних компонентів. References 1. Nyberg, L., & Tjellander, M. (2017). Camera stabilization (Dissertation). p. 29 2. John Pardue (2005) C Programming for Mi- crocontrollers, Knoxville.: Smiley Micros. p. 300 3. MPU-6000 and MPU-6050 Register Map and Descriptions Revision 4.2 – URL: https://invensense.tdk.com/wp-content/up- loads/2015/02/MPU-6000-Register-Map1. pdf (дата звернення: 05.04.2021) 4. Welch, Greg & Bishop, Gary (2006) An Intro- duction to the Kalman Filter. Proc. Siggraph Course. 8. pp. 1-16 jrowberg/i2cdevlib The I2C Device Library – URL: https://github. com/jrowberg/i2cdevlib (дата звернення: 21.04.2021) 5. Hamilton, William Rowan (2000) On quater- nions, or on a new system of imaginaries in algebra. Philosophical Magazine. 1844. pp. 489-495 6. Pandanom/StabilizationModule Module for plane stabilization with three degrees of freedom – URL: https://github.com/Pandan- om/StabilizationModule (дата звернення: 25.05.2021). 7. Samotyy V., Telenyk S., Kravets P., Shym- kovych V., Posvistak T. (2018) A real time Рисунок 5. Перехідний процес при впливі на систему 12 Прикладні засоби програмування та програмне забезпечення control system for balancing a ball on a plat- form with FPGA parallel implementation. Technical Transactions. Vol. 5. pp. 109-118 8. Yurkevich, Valery (2009) PWM PI/PID/PIDF Control for Nonlinear Nonaffi ne Systems via Singular Perturbation. 2009 International Fo- rum on Strategic Technologies. pp. 185-190. 9. Barr, Michael (2001) Pulse Width Modu- lation. Embedded Systems Programming. September. pp. 103-104 10. ESP8266 Arduino Core’s documentation – URL: https://arduino-esp8266.readthedocs. io/en/latest/ (дата звернення: 07.05.2021) 11. J. B. Ziegler and N. B. Nichols. «Optimum settings for automatic controllers» ASME Transactions v64. 1942; pp. 759-768. Отримано: 24.05.2022 Про авторів: Безпалько Станіслав Олегович, магі- странт 1 року навчання Національного Технічного Універсистету України «КПІ імені Ігоря Сікорського». https://orcid. org/0000-0001-8550-1792 Шимкович Володимир Миколайович, кан- дидат технічних наук, доцент кафедри інформаційних систем та технологій На- ціонального технічного універсистету України «КПІ імені Ігоря Сікорського». Кількість наукових публікацій в україн- ських виданнях – понад 30. Кількість на- укових публікацій в зарубіжних виданнях – понад 10. Індекс Хірша – 4. https://orcid. org/0000-0003-4014-2786 Дорошенко Анатолій Юхимович, доктор фізико-математичних наук, професор, завідувач відділу теорії комп’ютерних обчислень, професор кафедри інформаційних систем та технологій Національного технічного універсистету України «КПІ імені Іго- ря Сікорського». Кількість наукових публікацій в українських виданнях – понад 190. Кількість наукових публіка- цій в зарубіжних виданнях – понад 80. Індекс Хірша – 6. http://orcid.org/0000- 0002-8435-1451 Місце роботи авторів: Національний технічний університет України «Київський політехнічний інсти- тут імені Ігоря Сікорського», проспект Перемоги 37 та Інститут програмних систем НАН України, 03187, м. Київ-187, проспект Академіка Глушкова, 40. Тел.: (044) 526 3559 E-mail: stas110922@gmail.com, v.shymkovych@kpi.ua, doroshenkoanatoliy2@gmail.com

A model and software for the inertial measurement unit

Репозитарії

Схожі ресурси