Інерціальний вимірювальний блок

Інерціальний вимірювальний блок, що містить три однокомпонентні гіроскопічні датчики кутової швидкості, три однокомпонентні маятникові акселерометри, осі чутливості яких паралельні осям 3 осі підвісу рухомих елементів датчиків взаємно ортогональні та кожна з них паралельна відповідній осі зв'язаної з об'єктом ортогональної системи координат, осі підвісу рухомих елементів однокомпонентних маятникових акселерометрів паралельні відповідним осям підвісу рухомих елементів однокомпонентних гіроскопічних датчиків кутової швидкості, згідно з корисною моделлю введені дві слідкуючі головки, кожна з яких знаходиться у двохосьовому кардановому підвісі з датчиками кутів на осях підвісу, а виходи датчиків кутів підвісів слідкуючих головок з'єднані з обчислювальним пристроєм. Суть корисної моделі пояснюється кресленнями, де на фіг. 1 наведена структурно-кінематична схема інерціального вимірювального блока, на фіг. 2 наведена структурно-функціональна схема інерціального вимірювального блока, на фіг. 3 наведене взаємне положення зв'язаного з корпусом об'єкта тріедра OXYZ та навігаційного тріедра ОиХиYиZи, на фіг. 4 наведена структурнофункціональна схема алгоритму визначення кутів , який реалізується за допомогою обчис, , лювального пристрою. Інерціальний вимірювальний блок містить три однокомпонентні гіроскопічні датчики кутової швидкості 1, 2, 3, три однокомпонентні маятникові акселерометри 4, 5, 6 та дві слідкуючі головки 7, 8. Кожна з осей тріедрів OiXiYiZi (і = 1, 2, 3, 4, 5, 6, 7, 8), зв'язаних з кожним із однокомпонентних гіроскопічних датчиків кутової швидкості 1, 2, 3, однокомпонентних маятникових акселерометрів 4, 5, 6, слідкуючими головками 7, 8, розміщена паралельно відповідній осі зв'язаної з об'єктом ортогональної системи координат OXYZ 9. Кожний з однокомпонентних гіроскопічних датчиків кутової швидкості 1, 2, 3 містить рухомі елементи - це рами 10, 11, 12, які відхиляються на певні кути. У кожній рамі 10, 11, 12 розташовані ротори 13, 14, 15 відповідно. Також до складу однокомпонентних гіроскопічних датчиків кутової швидкості 1, 2, 3 входять датчики 16, 17, 18 кутів відхилення рухомих елементів і датчики моментів 19, 20, 21. Кожен з однокомпонентних маятникових акселерометрів 4, 5, 6 містить рухомі елементи це інерційні маси 22, 23, 24 які відхиляються на певні кути. Також до складу однокомпонентних маятникових акселерометрів 4, 5, 6 входять датчики 25, 26, 27 кутів відхилення рухомих елементів і датчики моментів 28, 29, 30. Кожна з слідкуючих головок 7, 8 містить рухомі елементи - це рами підвісу 31, 32, 33, 34, які відхиляються на певні кути за допомогою датчиків моментів 35, 36, 37, 38 відповідно. Датчики моментів 35, 36 є керованим перетворювачем 39, датчики моментів 37, 38 є керованим перетворювачем 40. У рамах 32, 34 розташовані електронно-оптичні датчики 41, 42, виходи яких з'єднані з перетворювачами 39, 40 відповідно. До складу слідкуючих головок 7, 8 входять по два датчика 43, 44 та 45, 46 кутового відхилення елементів карданового підвісу. За допомогою датчиків 16, 17, 18 визначаються кути відхилення рухомих елементів 10, 11, 12 однокомпонентних гіроскопічних датчиків кутової швидкості 1, 2, 3, за допомогою датчиків 25, 26, 27 визнача 54017 4 ються кута відхилення рухомих елементів 22, 23, 24 однокомпонентних маятникових акселерометрів 4, 5, 6. Виходи 47, 48, 49, 50, 51, 52, 53, 54, 55, 56 кожного з датчиків 16, 17, 18, 25, 26, 27, 43, 44, 45, 46 кутів відхилення рухомих елементів з'єднані з обчислювальним пристроєм 57, за допомогою якого реалізується базовий алгоритм. За допомогою блока-алгоритму 58, який реалізується обчислювальним пристроєм 57, визначають три складові вектора кутової швидкості X, Y, Z та три складові вектора уявного прискорення WX, WY, WZ об'єкта. Вихідні сигнали всіх датчиків 16, 17, 18, 25, 26, 27, 43, 44, 45, 46, що пропорційні кутам відхилення їх рухомих елементів, подаються на вхід обчислювального пристрою 57, в якому відповідно спеціальному блоку-алгоритму 58 визначаються величини проекцій вектора кутової швидкості корпусу об'єкта на осі зв'язаної з об'єктом ортогональної системи координат OXYZ, величини проекцій вектора уявного прискорення на осі зв'язаної з об'єктом ортогональної системи координат OXYZ, кути орієнтації корпусу об'єкта в просторі. Інформація про кутове положення корпусу об'єкта з блокаалгоритму 58 та алгоритму визначення кутового положення корпусу за допомогою слідкуючих головок 7, 8 передається до блока-алгоритму 59, який дає змогу робити корекцію параметрів кутового положення корпусу, які визначаються блокомалгоритмом 58, та компенсацію помилок кутового положення корпусу, які з'являються за рахунок дрейфу гіроскопічних датчиків кутової швидкості 1, 2, 3. Інформація про три складові вектора кутової швидкості X, Y, Z та три складові вектора уявного прискорення WX, WY, WZ об'єкта з блокуалгоритму 58 та параметри кутового положення (лінія 60) з блоку-алгоритму 59 передається до блоку-алгоритму 61, у якому у якому визначаються уточнені три складові вектора кутової швидкості ’X, ’Y, ’Z та три складові вектора уявного прискорення W’X, W’Y, W’Z об'єкта. Блок-алгоритм 61 має виходи 62, 63, 64, 65, 66, 67 відповідно до кожного з інерціальних параметрів. Для реалізації алгоритму визначення кутів , на основі використання слідкуючих головок , 7, 8 необхідна навігаційна інформація про величи__ ни векторів rиі , r , aи та відстань до нерухомих і об'єктів r Si в інерціальній системі координат (необхідно ввести зворотній зв'язок 68 на первинний обчислювач), яка визначається за допомогою обчислювача 69. У відповідності до аналітичної форми подання алгоритму визначення кутів можливо сформувати структурно, , , функціональну схему алгоритму визначення кутів , , , на основі використання двох слідкуючих головок 7 та 8 у складі інерціального вимірювального блока, який реалізується за допомогою обчислювача 57 (див. фіг. 4). Структурно 5 54017 функціональна схема алгоритму містить блоки-дії, що реалізують функції складання (70 - 77), зведення до квадрату (78 - 93), віднімання (94 - 104), множення (105 - 129), знаходження кореню (130 133), ділення (134 - 139), синусу (140 - 143), косинусу (144 - 147); і блоки, що реалізують змінні навігаційні параметри, а саме проекції вектора rи1 (148 - 150), вектора rи2 (151 - 153), вектора r __и (154 - 156), вектора a 1i (157 - 159) та модулі векторів r S1 (160) та r S 2 (161). Інерціальний вимірювальний блок працює наступним чином. Під час руху корпус об'єкта або зв'язана з ним ортогональна система координат OXYZ рухається в просторі, тобто можна представити вектор кутової швидкості тріедра OXYZ та вектор уявного прискорення точки О. Кути відхилення рухомих елементів 10, 11, 12 однокомпонентних гіроскопічних датчиків кутової швидкості 1, 2, 3 при цьому пропорційні проекціям вектора абсолютної кутової швидкості тріедра OXYZ на осі чутливості гіроскопічних датчиків кутової швидкості. Кути відхилення рухомих елементів 22, 23, 24 однокомпонентних маятникових акселерометрів 4, 5, 6 при цьому пропорційні проекціям вектора уявного прискорення точки О на осі чутливості маятникових акселерометрів. За допомогою слідкуючих головок визначаються кути орієнтації тріедра OXYZ зв'язаної з корпусом об'єкта системи координат відносно навігаційного тріедра ОиХиYиZи. Розглянемо випадок, коли точка О тріедра OXYZ та точка Ои тріедра ОиХиYиZи співпадають. Взаємне положення зв'язаного з корпусом об'єкта тріедра OXYZ відносно інерціального тріедру ОиХиYиZи задається кутами Ейлера-Крилова , , . Введемо системи координат O'1X'1Y'1Z'1 та O'2X'2Y'2Z'2, які зв'язані з кожною із слідкуючих головок 7 та 8 відповідно. В кожній з цих систем ко__1 a1 (1) 1 0 0 де і = 1, 2 - номер слідкуючої головки. В інерціальній системі координат ОиХиYиZи одиничний радіус-вектор орієнтації на нерухомий об'єкт Si має вигляд: ауи і аzиі , (і=1,2). Проекції ортів a i на осі інерціальної системи координат ОиХиYиZи мають вигляд: __ і иі a і, де Миі = Миі ( , (3) , , i, i ) МТи1 ( , ,) МТ1і ( , __ i ai, ) i, i (5) Матрицю М1і ( i, i ) отримаємо за допомогою двох елементарних обертів системи координат OXYZ, а саме Z Y' YZ ' 'i ' 'i Y' 'i Z' 'i 'i 'i Y'i Z'i. i i Тобто матрицю М1і ( можна записати ) i, i як М1і( ) = Мі’1 ( i, i ) .М1і’( i i) , (6) . де Мі’1 ( i ) М1і’( i ) - матриці, які відповідають кожному з елементарних обертів системи координат O'iX'iY'iZ'i відносно тріедра OXYZ. Матриця.М1і’( i ) має вигляд: cos sin 1i ' ( i ) sin cos i i 0 Матриця Мі’1 ( cos 0 sin i'i ( i ) 0 0 i i 0 i (7) 1 ) має вигляд: 0 1 0 i i sin 0 cos i (8) . i (6): cos 1i i, i - матриця переходу sin i sin cos cos i sin cos i i 0 cos i 0 0 1 sin 0 i i sin cos i cos i sin i cos 0 i sin i i sin i 1i i, i cos cos cos i sin sin i i Матриця Ми1 ( , i i , i, i ) i (9) i i i cos i . cos sin cos 0 0 i 0 sin sin 0 i 0 1 i Транспонована матриця МT1і ( гляд: (2) __ i __ и aі __и aі = Підставимо співвідношення (7), (8) у формулу __ i ai ахи і від інерціальної системи координат ОиХиYиZи до зв’язаної із і-тою слідкуючою головкою системи координат O'iX'iY'iZ'i. Матриці переходу Миі ( , , , i, i ) визначаються наступним чином: Миі ( , , , i, i ) = М1і ( i , i ) Ми1 ( , , ,) (4) де Ми1 ( , , ,) - матриця переходу від інерціальної системи координат ОиХиYиZи ДО зв'язаної з об'єктом системи координат OXYZ; М1і ( i, i ) матриці переходу від зв'язаної з об'єктом системи координат OXYZ до зв'язаної з і-тою слідкуючою головкою системи координат O'іX'іY'іZ'і. Враховуючи рівняння (3) та (4), запишемо __ 2 ординат задамо орти та a 2 , які співпадають з оптичними осями електронно-оптичних датчиків 41 та 42 на нерухомий об'єкт Si відповідно. Таким чином, __ и ai 6 має ви cos i sin sin i sin cos i i i ,) має такий вигляд: . (10) 7 и1 , , 54017 sin sin sin sin cos cos cos sin cos cos cos sin Транспонована матриця МТИ1( , 8 cos sin cos sin sin cos sin sin sin cos cos i i a11 a12 , , , i, (11) . ,) має вигляд: , cos cos sin sin cos sin cos sin cos sin cos sin sin sin cos sin и1 , , sin cos cos Враховуючи співвідношення (11) та (12), отримаємо матрицю: иі sin cos cos sin cos i , , , i, iи sin sin cos cos sin sin cos cos sin cos i a13 ai21 ai22 ai23 ai31 ai32 ai33 i (12) . (13) де i a11 i a12 i a13 ai21 ai22 ai23 ai31 ai32 ai33 cos cos cos cos cos cos sin sin cos cos cos sin cos sin sin cos i cos sin i cos i sin sin i cos sin cos cos sin sin sin cos sin cos cos sin sin cos cos i cos sin sin cos cos i i i i cos i sin cos i sin sin sin sin i cos i sin i cos sin cos cos i cos i cos sin cos sin sin cos sin i cos a1 __ и a2 , , , и1 cos sin i sin i sin cos и a1Y и a1Z __1 , 1 a 1; 1 __ 2 a1 a1 11 12 a1 13 i sin cos cos sin sin i; sin sin sin i cos sin cos sin sin cos cos i; cos sin i; cos i; sin sin i sin i sin sin cos cos cos i; sin i; a1 23 a1 33 2 2 a11 a12 2 a13 1 a2 21 2 a31 a2 23 2 a33 0 0 a2 22 2 a32 (16) Використовуючи рівняння (15) та (16), отримаємо: (14) 0; 0 cos i; aи Y 2 aи Z 2 и a1 и a1Y и a1Z (15) a1 11 a1 21 a1 31 aи 2 1 a1 21 a1 31 a1 22 a1 32 cos sin i aи 2 и2 , , , 2 , 2 a 2 ; Враховуючи співвідношення (1) та (2), можна записати: и a1 sin i cos i; Підставляючи отриману матрицю (13) у формулу (3) отримаємо систему матричних рівнянь: __ и sin i sin i cos sin sin cos cos i; sin sin sin cos 2 a11 (17) aи Y a2 2 21 2 aи Z a31 2 Підставимо значення елементів матриці (13) у систему рівнянь (17): и a1 и a1Y cos cos cos 1 cos 1 cos 1 sin 1 sin sin cos sin cos 1 cos 1 sin cos cos sin cos sin cos sin sin sin cos sin 1; sin cos cos 1 sin 1 sin sin sin cos cos sin 1; и a1Z sin cos 1 cos 1 cos cos cos 1 sin 1 sin cos sin 1; au cos cos cos 2 cos 2 sin sin cos sin cos 2 aи Y 2 cos 2 sin 2 sin sin cos sin cos 2 cos 2 cos cos cos sin sin sin (18) sin 2; sin cos cos 2 sin 2 sin sin sin cos cos sin 2; aи Z sin cos 2 cos 2 cos cos cos 2 sin 2 sin cos sin 2; 2 Перейдемо до реального випадку, коли поточчок О та Ои визначається радіус-вектором r (див. не положення точки О тріедру OXYZ не співпадає фіг. 3). з точкою Ои тріедра ОиХиYиZи. Взаємний стан тоВиходячи з фіг. 3, можна записати наступне 9 54017 10 векторне рівняння: r + r i = rиі , тa иi r іи , , , i, i aii r иi иi ; rиi aи , отримаємо рівняння: i rиi rиі - радіус-вектор, який визначає положення нерухомого об'єкта Sі в інерціальній системі координат ОиХиYиZи. Ці радіус-вектори є відомими: r ai'u ri де r i - радіус-вектор, який визначає положення нерухомого об'єкта Si у зв'язаній з корпусом об'єкта системі координат OXYZ ; rиi ri Враховуючи, що (19) rі (20) , , , i, іи i aи i aii rиi (21) r r r r . Запишемо рівняння (19) у такому вигляді: Де rиі r , (23) rиi r 2иi r 2иi r 2иi ; (24) r i = rиі - r ; ri rиi r rиi rиi ri rиi ri ri ri rиi 1i i, i aii rSi , (25) ; r Si - відстань від точки О тріедра OXYZ до (22) rui r rиi нерухомого об'єкта Si. Підставимо вирази (1), (10) у рівняння (25): rиi , cos ri cos де rиi - довжина вектора rиi ; cos2 ri - довжина вектора r i . i i cos i sin sin i cos2 i sin i cos i cos2 i i i cos i sin sin i sin cos i sin2 i sin i i i 1 0 0 r Si (26) r Si Підставивши (2), (17), (21), (24), (26) у (23) отримаємо систему шести рівнянь: 11 54017 cos cos cos sin 1 cos2 cos cos sin 1 cos2 1 1 sin cos2 1 sin 1 sin cos sin2 cos cos 1 sin cos 1 12 sin sin sin cos sin 1 r S1 1 r 2u1 r 2u1 r 2u1 a r u1 r sin cos 2 cos cos2 r 2u1 r 2u1 cos 1 cos 1 u1 sin 1 cos2 1 1 sin 1 cos2 sin sin2 1 sin 1 sin sin cos sin cos cos sin 1 r S1 1 r 2u1 r 2u1 r 2u1 a r u1 r sin cos cos2 1 ; r 2u1 r 2u1 cos 1 cos cos 2 1 u1 cos2 cos2 1 sin2 1 cos sin 1 sin 1 sin 1 cos sin 1 r S1 1 r 2u1 r 2u1 r 2u1 a r ; r 2u1 r 2u1 cos cos 2 cos u1 2 r cos u1 cos cos2 2 sin 2 cos2 2 2 cos2 sin 2 sin sin sin 2 cos sin2 2 sin cos cos cos sin sin cos sin sin 2 r S2 2 r 2u2 r 2u2 r 2u2 a r u2 r sin cos 2 cos cos2 ; r 2u2 r 2u2 cos 2 cos 2 u2 sin 2 cos2 2 2 2 sin cos2 sin 2 sin sin2 2 sin cos sin sin cos cos sin 2 r S2 2 r 2u2 r 2u2 r 2u2 a r u2 r sin cos2 cos 2 2 ; r 2u2 r 2u2 u2 cos 2 cos cos 2 cos2 2 cos2 2 sin2 cos 2 sin r 2u2 r 2u2 r 2u2 a r u2 r 2u2 r 2u2 r 2u2 2 ; Система (27) складається з шести рівнянь, але має три параметри (кути , , ), які необхідно визначити. Сформуємо систему трьох незалежних рівнянь з трьома невідомими з системи (27): sin 2 2 sin cos sin 2 r S2 (27) 13 54017 cos cos cos cos2 cos sin 1 sin 1 cos2 1 cos 1 cos2 1 sin 1 sin 1 sin cos sin2 14 cos cos 1 sin cos sin sin sin cos sin 1 r S1 1 r 2u1 r 2u1 r 2u1 r a u1 r sin cos 2 ; r 2u1 r 2u1 u1 cos 1 cos 1 cos cos2 sin 1 cos2 1 sin 1 cos2 1 sin 1 sin sin2 1 sin cos sin 1 sin cos cos sin 1 r S1 r 2u1 r 2u1 r 2u1 r a u1 r sin cos cos2 2 2 ; r 2u1 r 2u1 cos 2 cos cos 2 u1 cos2 cos2 2 2 cos sin2 2 sin sin 2 2 2 sin cos sin 2 r S2 r 2u2 r 2u2 r 2u2 r a u2 r 2 u2 r 2u2 r 2u2 ; (28) Подібна система рівнянь має однозначне вирішення, що можна довести, лінеаризовавши її. Отримаємо лінеаризовану систему рівнянь: cos2 cos a 1 cos cos 1 1 sin r u1 r 2 1 a 1 cos cos 1 r 2 r 2u1 r 2u1 u1 sin 1 r u1 1 sin a 2 cos cos 2 r 2 u2 r 2u2 r 2u2 cos2 1 sin2 1 sin 1 r S1 cos2 1 cos2 1 sin2 1 sin 1 r S1 r 2u1 r 2u1 r 2u1 (29) ; sin 2 r u2 1 1 cos2 cos 1 r 2u1 r 2u1 r 2u1 cos2 cos cos2 ; r 2u1 r 2u1 u1 sin 1 1 ; 2 sin 2 2 cos2 2 cos2 2 sin2 r 2u2 r 2u2 r 2u2 2 sin 2 r S2 15 54017 16 Розв'язання системи рівнянь (29) має вигляд: 7 1 2 1 2 2 3 1 2 3 9 2 3 3 2 4 6 8 5 3 3 6 1 cos 1 cos 2 cos 2 cos 3 cos 1 sin 4 cos 2 sin 5 sin 4 3 1 5 6 3 9 8 5 ; 3 7 8 ; 5 2 4 5 2 2 7 5 6 5 3 6 1 1 2 sin 1; 6 3 5 6 3 1 9 6 1 2 5 1 1; 2 1; 2; 2; a r 2u1 r 2u1 r 2u1 r u1 ; 7 cos 2 1 cos 2 a 1 cos 2 1 2 sin 2 sin 1 r S1 1 r 2u1 r 2u1 r 2u1 r u1 ; 8 cos 2 1 cos 2 1 де a cos 2 1 2 sin 2 sin 1 r S1 1 r 2u1 r 2u1 r 2u1 r u2 ; 9 cos 2 2 cos 2 Отримане розв'язання (30) лінеаризованої системи рівнянь (29) можна представити у вигляді структурно-функціональної схеми алгоритму, який реалізується за допомогою обчислювального пристрою 57 (див. фіг. 4). Таким чином, інерціальний вимірювальний 2 cos 2 2 2 sin 2 2 sin 2 r S2 блок дозволяє збільшити точність визначення інерціальних параметрів за рахунок корекції кутового положення корпусу або компенсації помилок визначення кутового положення корпусу, які з'являються за рахунок дрейфу гіроскопічних датчиків кутової швидкості. 17 54017 18 19 54017 20 21 Комп’ютерна верстка А. Крулевський 54017 Підписне 22 Тираж 26 прим. Міністерство освіти і науки України Державний департамент інтелектуальної власності, вул. Урицького, 45, м. Київ, МСП, 03680, Україна ДП “Український інститут промислової власності”, вул. Глазунова, 1, м. Київ – 42, 01601