РЕФЕРАТ - kpi.ua
TRANSCRIPT
4
РЕФЕРАТ
Магістерська дисертація складається зі вступу, п’яти розділів, висновків,
переліку 109 найменувань. Загальний об’єм дисертації становить 146 сторінок,
з них 113 сторінок основного тексту, 45 рисунків та 10 таблиць, переліку
посилань на 10 сторінках та 2 додатки на 17 сторінках.
Актуальність теми. Дослідження в напрямку безплатформних
інерціальних навігаційних систем (БІНС) спрямовані на вирішення однієї з
найважливіших задач – підвищення точності системи за рахунок
комплексування БІНС з іншими навігаційними система, які засновані на
неінерціальній природі. Прогрес у електроніці, оптиці, комп’ютерній техніці та
програмуванні дає можливість використовувати оптичні прилади та системи у
широкому спектрі приладів споживчого ринку. Проблемі дослідження корекції
БІНС від супутниковою навігаційної системи та радіонавігаційних систем
присвячено багато робіт, проте дане дослідження не втрачає своєї актуальності.
Через це необхідно проводити ефективну корекцію та досліджувати нові
допоміжні (вторинні) системи для корекції БІНС. Також потрібно пам’ятати,
що при комплексуванні БІНС з іншими системами необхідно розробити та
синтезувати регулятори (фільтри), які зменшують похибки вже комплексної
системи, як правило використовують фільтр Калмана. Тому для досягнення
бажаної точності БІНС важливу увагу необхідно приділяти дослідженню
вторинних систем, їх характеристикам та способам комплексування та
корегування, а також алгоритму роботи навігаційного комплексу у цілому.
Виходячи з цього, такий підхід до вирішення задачі підвищення точності БІНС
дозволить отримати максимально можливі точні характеристики, на основі
яких і буде складатися математична модель навігаційного комплексу та
алгоритм роботи і саме тому ця робота є актуальною науково – дослідною
роботою.
Зв’язок роботи з науковими програмами, планами, темами. Науково –
дослідна робота по темі дисертації проводилася за власної ініціативи на
підприємстві ПАТ НТК «ЕЛЕКТРОНПРИЛАД», Київ.
5
Мета дисертаційної роботи полягає в представлені методів та алгоритмів,
які дозволяють дослідити точностні характеристики БІНС та провести корекцію
БІНС.
Досягнення мети передбачає вирішення таких задач:
- Аналіз побудови алгоритмів та математичної моделі БІНС;
- Аналіз результатів моделювання математичних моделей та алгоритмів
роботи БІНС;
- Дослідження та аналіз похибок БІНС;
- Дослідження алгоритмів та огляд побудови математичної моделі оптико
– електронної системи орієнтації та навігації;
- Дослідження схеми комплексування БІНС та ОЕСОН та синтез фільтра
Калмана, для оцінки похибок визначення навігаційних параметрів.
- Проведення імітаційного моделювання роботи комплексної системи.
- Висновки.
Об’єктом дослідження є безплатформна інерціальна навігаційна система
побудована на лазерних гіроскопах.
Предметом дослідження є сучасні методи корекції БІНС та способи
підвищення точності.
Методи дослідження. Огляд сучасних існуючих систем з оптичною
корекцією; опис алгоритму та математичної оптичної системи; опис принципів
побудови методів орієнтації та навігації оптико – електронної системи. Синтез
фільтра Калмана та оцінювання похибок комплексної системи.
Наукова новизна дисертації полягає у розробці методики комплексування
та підвищення точності роботи БІНС шляхом дослідження основних
характеристик які впливають на точність роботи системи.
Практичне значення отриманих результатів визначається вимогами, які
висуваються до системи та природою роботи об’єкта на якому буде
розташовуватися комплексна система. Такий підхід дозволить визначити
доцільність використання комплексної системи на об’єкті та модернізувати
алгоритми обробки інформації.
6
Апробація результатів дисертації XIV міжнародна науково – технічна
конференція «Приладобудування 2015: стан і перспективи», 22-23 квітня
2015р., м. Київ Україна, ПБФ, НТУУ «КПІ».
Публікації. Опубліковано 2 друковані роботи:
- XIV науково – технічна конференція «Приладобудування 2015: стан і
перспективи», 22-23 квітня 2015р., м. Київ Україна, ПБФ, НТУУ «КПІ»
2015, с. 38 – 39;
- VII Научно-техническая конференция аспирантов и молодых ученых
«ВООРУЖЕНИЕ. ТЕХНОЛОГИЯ. БЕЗОПАСНОСТЬ. УПРАВЛЕНИЕ»,
в городе Ковров, 21-23 апрель 2015г. С. 200-208, г. Ковров.
Ключові слова. БЕСПЛАТФОРМНА ІНЕРЦІАЛЬНА НАВІГАЦІЙНА
СИСТЕМА, ОПТИКО – ЕЛЕКТРОННА СИСТЕМА ОРІЄНТАЦІЇ ТА
НАВІГАЦІЇ, КОРЕКЦІЯ БІНС, ФІЛЬТР КАЛМАНА.
7
РЕФЕРАТ
Магистерская диссертация состоит из введения, пяти глав, выводов, 109
источников литературы. Общий объём диссертации составляет 146 страниц, из
них 113 стр. основного текста, 45 рисунков и 10 таблиц, списка ссылок на 10
страницах и 2 приложения на 17 страницах.
Актуальность теми. Исследования в направлении безплатформних
инерциальных навигационных систем (БИНС) направлены на решение одной из
важнейших задач - повышение точности системы за счет комплексирования
БИНС с другими навигационными система, основанные на неинерциальной
природе. Прогресс в электронике, оптике, компьютерной технике и
программировании дает возможность использовать оптические приборы и
системы в широком спектре устройств потребительского рынка. Проблеме
исследования коррекции БИНС от спутниковой навигационной системы и
радионавигационных систем посвящено много работ, однако данное
исследование не теряет своей актуальности. Поэтому необходимо проводить
эффективную коррекцию и исследовать новые вспомогательные (вторичные)
системы для коррекции БИНС. Также нужно помнить, что при
комплексировании БИНС с другими системами необходимо разработать и
синтезировать регуляторы (фильтры), которые уменьшают погрешности уже
комплексной системы, как правило используют фильтр Калмана. Поэтому для
достижения желаемой точности БИНС важное внимание необходимо уделять
исследованию вторичных систем, их характеристикам и способам
комплексирования и корректировки, а также алгоритма работы навигационного
комплекса в целом. Исходя из этого, такой подход к решению задачи
повышения точности БИНС позволит получить максимально возможные
точные характеристики, на основе которых и будет состоять математическая
модель навигационного комплекса и алгоритм работы и именно поэтому эта
работа является актуальной научно - исследовательской работой.
8
Связь работы с научными программами, планами, темами. Научно -
исследовательская работа по теме диссертации проводилась по собственной
инициативе на предприятии ПАО НТК «ЭЛЕКТРОНПРИБОР», Киев.
Цель диссертационной работы заключается в представленных методов и
алгоритмов, которые позволяют исследовать точностные характеристики БИНС
и провести коррекцию БИНС.
Достижение цели предполагает решение следующих задач:
- Анализ построения алгоритмов и математической модели БИНС;
- Анализ результатов моделирования математических моделей и
алгоритмов работы БИНС;
- Исследования и анализ ошибок БИНС;
- Исследование алгоритмов и обзор построения математической модели
оптико - электронной системы ориентации и навигации;
- Исследование схемы комплексирования БИНС и ОЭСОН и синтез
фильтра Калмана для оценки погрешностей определения
навигационных параметров.
- Проведение имитационного моделирования работы комплексной
системы.
- Выводы.
Объектом исследования является безплатформенная инерциальная
навигационная система построена на лазерных гироскопах.
Предметом исследования являются современные методы коррекции
БИНС и способы повышения точности.
Методы исследования. Обзор современных существующих систем с
оптической коррекцией; описание алгоритма и математической оптической
системы; описание принципов построения методов ориентации и навигации
оптико - электронной системы. Синтез фильтра Калмана и оценки
погрешностей комплексной системы.
9
Научная новизна диссертации состоит в разработке методики
комплексирования и повышения точности работы БИНС путем исследования
основных характеристик влияющих на точность работы системы.
Практическое значение полученных результатов определяется
требованиями, предъявляемым к системе и физикой работы объекта на котором
будет располагаться комплексная система. Такой подход позволит определить
целесообразность использования комплексной системы на объекте и
модернизировать алгоритмы обработки информации.
Апробация результатов диссертации ХIV научно - техническая
конференция «Приборостроение 2015: состояние и перспективы», 22-23 апреля
2015г., г. Киев Украина, ПСФ, НТУУ «КПИ».
Публикации. Опубликовано 2 печатные работы:
- ХIV научно - техническая конференция «Приборостроение 2015:
состояние и перспективы», 22-23 апреля 2015г., Г.. Киев Украина, ПСФ, НТУУ
«КПИ» 2015, с. 38 – 39;
- VII Научно-техническая конференция аспирантов и молодых ученых
«ВООРУЖЕНИЕ. ТЕХНОЛОГИЯ. БЕЗОПАСНОСТЬ. УПРАВЛЕНИЕ», в
городе Ковров, 21-23 апрель 2015г. С. 200-208, г. Ковров.
Ключевые слова. БЕСПЛАТФОРМЕННАЯ ИНЕРЦИАЛЬНАЯ
НАВИГАЦИОННАЯ СИСТЕМА, ОПТИКО - ЭЛЕКТРОННАЯ СИСТЕМА
ОРИЕНТАЦИИ И НАВИГАЦИИ, КОРРЕКЦИЯ БИНС, ФИЛЬТР КАЛМАНА.
10
ABSTRACT
Master's thesis consists of introduction, five chapters, conclusions, list of 109
names. The total volume of the thesis is 146 pages, including 113 pages of main text,
45 figures and 10 tables, a list of links to pages 10 and 2 additions on 17 pages.
Actuality of topic. Research towards strapdown inertial navigation system
(SINS) aimed at solving one of the most important tasks - increasing the accuracy of
the system by combining SINS with other navigation systems, which are based on
non-inertial nature. Advances in electronics, optics, computer technology and
programming enables the use of optical devices and systems in a wide range of
devices consumer market. The problem of studying SINS correction of satellite
navigation systems and radionavigation systems devoted a lot of work, but this study
does not lose its relevance. Because it is necessary to conduct effective correction and
explore new auxiliary (secondary) system for SINS correcting. We must also
remember that the integration of SINS with other systems necessary to develop and
synthesize regulators (filters) that reduce errors are complex systems typically use a
Kalman filter. Therefore, to achieve the desired accuracy of SINS important attention
should be paid to research secondary systems, their characteristics and methods of
integration and adjustment, and the algorithm of the navigation system as a whole.
Therefore, the approach to solving the problem increasing the accuracy SINS will
provide the most accurate possible characteristics on which and will consist
navigation system mathematical model and algorithm that is why this work is actual
scientific - research work.
Relationship with academic programs, plans, themes. Scientific - research
work on the topic of the thesis carried on its own initiative to "Elektronprylad", Kyiv,
JOINT STOCK COMPANY SCIENTIFIC AND TECHNICAL ENTERPRISE.
The aim of the thesis is presented methods and algorithms that allow to explore
the accuracy of the characteristic and SINS correction.
Purpose achievement foresees the decision of such tasks:
- Analysis of the construction of algorithms and mathematical models SINS;
- Analysis of the simulation of mathematical models and algorithms of SINS;
11
- Research and analysis of errors SINS;
- Analysis and review of algorithms of mathematical modeling of optical -
electronic system for orientation and navigation;
- Research and schemes combining SINS OESON and synthesis Kalman filter
for estimation error of navigation parameters.
- Carrying out of simulation of complex systems.
- Conclusions.
Research object is starpdown inertial navigation system based on laser
gyroscopes.
The subject of research – modern methods of starpdown inertial navigation
system correction and increasing of accuracy.
Methods of research. Review existing systems with modern optical correction;
algorithm and a mathematical description of the optical system; description of the
principles of design methods orientation and navigation opto - electronic system.
Kalman filter synthesis and evaluation of complex system errors.
The scientific novelty of the thesis is to develop techniques for combining and
improve the accuracy of SINS by examining key features that impact the accuracy of
the system.
The practical significance of the results determined by the requirements that
apply to the system and the nature of the object which will be based integrated
system. This approach will determine the feasibility of using the integrated system at
the facility and modernize algorithms processing.
Approbation result of study XIV scientific and technical conference
"Instrument 2015: state and prospects", April 22-23, 2015., M. Kyiv Ukraine, PBF,
NTU "KPI".
Publications. Published 2 printed works:
- XIV scientific and technical conference " Instrument 2015: state and
prospects", 23-25 April 2015., Kyiv Ukraine, PBF, NTU "KPI" 2015, p. 38-
39;
12
- VII-commercial-Scientific Conference of Young scientists and graduate
students "WEAPONS. TECHNOLOGY. SECURITY. MANAGEMENT "in
the city Kovrov, 21-23 April 2015h. pp. 200-208, Kovrov.
Keywords. STRAPDOWN INERTIAL NAVIGATION SYSTEM, OPTICAL -
ELECTRONIC SYSTEM OF ORIENTATION AND NAVIGATION, SINS
CORRECTION, KALMAN FILTER.
13
ЗМІСТ
ВСТУП………………………………………………………………………………16
1 ОГЛЯД СТАНУ СУЧАСНИХ СИСТЕМ ОРІЄНТАЦІЇ ТА НАВІГАЦІЇ…….18
1.1 Аналіз існуючих систем орієнтації та навігації та систем, що мають
перспективи розвитку. Різновид та практичне значення……………………..18
1.2 Проблемна ситуація у розробці та комплексуванні систем орієнтації та
навігації…...……………………………………………………………………..25
ВИСНОВКИ ДО ПЕРШОГО РОЗДІЛУ…………………………………..…28
2 РОЗРОБКА МОДЕЛІ БЕЗПЛАТФОРМНОЇ ІНЕРЦІАЛЬНОЇ
НАВІГАЦІЙНОЇ СИСТЕМИ………………………………………………….29
2.1 Початкове визначення орієнтації та вимоги до чутливих елементів…….29
2.2 Основний режим роботи безплатформної інерціальної навігаційної
системи…………………………………………………………………………...40
2.2.1 Кінематичні рівняння орієнтації……………………………………...42
2.2.1.1 Рівняння орієнтації Ейлера……………………………………...42
2.2.1.2 Рівняння орієнтації Пуассона………………………………...…45
2.2.2 Рівняння навігації……………………………………………………...48
2.3 Похибки безплатформної інерціальної навігаційної системи……………53
2.3.1 Вплив похибки акселерометра……………………………………..…54
2.3.2 Вплив похибки гіроскопа…………………………………………...…55
2.3.3 Похибка алгоритмічного дрейфа……………………………………...55
2.3.4 Похибка безплатформної інерціальної навігаційної системи у
вигляді простору станів………………………………………………………57
ВИСНОВКИ ДО ДРУГОГО РОЗДІЛУ……………………………………...61
3 ОПТИКО – ЕЛЕКТРОННА СИСТЕМА ОРІЄНТАЦІЇ ТА НАВІГАЦІЇ…...62
3.1 Опис та принцип роботи оптико – електронної системи орієнтації та
навігації…………………………………………………………………………..62
3.2 Метод лідарної зйомки. Опис та принцип роботи. Характеристики деяких
оптико – електронних систем орієнтації та навігації та їх ЧЕ………………..64
14
3.3 Математичні основи оптико – електронних систем орієнтації та
навігації…………………………………………………………………………..77
ВИСНЛВКИ ДО ТРЕТЬОГО РОЗДІЛУ……………………………………...82
4 КОРЕКЦІЯ БЕЗПЛТАФОРМНОЇ ІНЕРЦІАЛЬНОЇ НАВІГАЦІЙНОЇ
СИСТЕМИ. РЕЗУЛЬТАТИ МОДЕЛЮВАННЯ БІНС ТА ОЕСОН……………..83
4.1 Синтез фільтра Калмана комплексної системи орієнтації та навігації….84
4.2 Алгоритм дослідження та результати моделювання……………………...87
ВИСНОВКИ ДО ЧЕТВЕРТОГО РОЗДІЛУ………………………………….98
5 ОХОРОНА ПРАЦІ ТА БЕЗПЕКА У НАДЗВИЧАЙНИХ СИТУАЦІЯХ…...99
5.1 Аналіз умов праці і робочого місця………………………………………..99
5.1.1 Вібрації…………………………………………………………………99
5.2 Аналіз небезпечних та шкідливих чинників на виробництві…………...100
5.3 Аналіз можливих уражень струмом при експлуатації устаткування…..100
5.4 Аналіз пожежної безпеки приміщення…………………………………...102
5.5 Аналіз рівня електромагнітного випромінювання в приміщенні………102
5.6 Аналіз повітряного середовища робочої зони………………………...…102
5.7 Виробниче освітлення……………………………………………………..104
5.7.1 Розрахунок природного освітлення…………………………………105
5.7.2 Розрахунок штучного освітлення……………………………………107
5.8 Безпека в надзвичайних ситуаціях………………………………………..110
5.8.1 Дії у разі виникнення НС на підприємстві, що спеціалізується на
виготовленні спеціальних систем…………………………………………..111
ВИСНОВКИ ДО П’ЯТОГО РОЗДІЛУ……………………………………..113
ВИСНОВКИ ПО РОБОТІ…………………………………………………...114
СПИСОК ВИКОРИСТАННОЇ ЛІТЕРАТУРИ………………………………..…116
ДОДАТОК A. Список наукових праць.......……………………...………………126
ДОДАТОК B. Вихыдний код MatLAB розрахунків……………………………131
15
ПЕРЕЛІК СКОРОЧЕНЬ ТА УМОВНИХ ПОЗНАЧЕНЬ
БСО – безплатформна системи орієнтації
БІНС – безплатформна інерціальна навігаційна система;
БПЛА – безпілотний літальний апарат;
ЛГ – лазерний гіроскоп;
КЛГ – кільцевий лазерний гіроскоп;
ЛА – літальний апарат;
ІНС – інерціальна навігаційна системи;
GPS – система глобального позиціонування;
СНС – супутникова навігаційна система;
РНС – радіонавігаційна система;
ЧЕ – чутливий елемент;
АЦП – аналого-цифровий перетворювач;
ПК- персональний комп’ютер;
СК – система координат;
БГА – блок гіроскопів та акселерометрів;
БКВ – безплатформна курсовертикаль;
ФК – фільтр Калмана;
ОФК – оптимальний фільтр Калмана;
КПО - коефіцієнт природної освітленості;
ОЕСОН – оптико – електронна система орієнтації та навігації;
МНК – матриця напрямних косинусів;
GLONASS – глобальна навігаційна супутникова система;
ПЕОМ - персональна електроно-обчислювальна машина.
16
ВСТУП
Після Другої Світової Війни 1939 – 1945р.р. у світі почався стрімкий
розвиток освоєння як повітряного простору так і космічного. Проте освоєння
таких середовищ вимагає різноманітну кількість інформаційно –вимірювальних
систем, а також відповідну точність цих систем. Однією із найважливіших
систем для освоєння космічного простору є навігаційна система, тобто така
система яка дозволяє визначити місцеположення об’єкта у просторі та
визначити параметри руху об’єкта.
Починаючи з 1955 року розробляється новий тип навігаційного
обладнання – Інерціальна Навігаційна Система (ІНС). ІНС, у більшості
випадків, у своєму складі містять інерціальні чутливі елементи (ЧЕ), такі як
гіроскопи та акселерометри. Така комбінація дозволяє визначити як параметри
руху так і місцеположення об’єкта та його орієнтацію у просторі (кути Ейлера).
Така система має одну перевагу перед іншими системами – автономність
роботи.
З розвитком радіотехніки та оптики стала можливість комплексування
ІНС з різними радіосистемами та оптичними системами. Також з розвитком
технологій виготовлення електромеханічних систем з’явився підвид ІНС який
міг розміщувати свої чутливі елементи безпосередньо на корпусі об’єкта. Такий
вид отримав назву – Безплатформна (Безкарданна) Інерціальна Навігаційна
Система (БІНС). Перевага БІНС перед ІНС це зменшена вага всієї системи.
На даний момент існує багато робіт, в яких описані алгоритми роботи
БІНС та ІНС, вимоги до ЧЕ та різновиди похибок які виникають як у БІНС так і
у ІНС. Основною похибкою БІНС є похибка від часу, тобто з плином часу БІНС
може накопичувати похибки визначення координат, орієнтації об’єкта. Для
коректної роботи БІНС та зменшення похибок проводять виставку системи.
Традиційно виставка БІНС проводилася перед початком робити, такий
етап називався – початкова виставка. Проте накопичення похибок БІНС у часі
змусили впровадити новий метод виставки системи – виставка на рухомій
17
основі. Виставка на рухомій основі це комплексний процес, який може
використовувати методи прогнозу – корекції та зовнішні джерела інформації.
Дана робота спрямована на дослідження та вивчення методів корекції
БІНС за допомогою застосувань оптико – електронних систем орієнтації та
навігації.
18
1 ОГЛЯД СТАНУ СУЧАСНИХ СИСТЕМ ОРІЄНТАЦІЇ ТА
НАВІГАЦІЇ
1.1 Аналіз існуючих систем орієнтації та навігації та систем, що мають
перспективи розвитку. Різновид та практичне значення
Сучасні навігаційні системи по принципу дії можна поділити на такі типи:
- Спутникові системи навігації та орієнтації
Принцип роботи супутникових систем навігації та орієнтації заснований на
вимірюванні відстані від антени на об'єкті (координати якого необхідно
отримати) до супутників, положення яких відомо з великою точністю. Таблиця
положень всіх супутників називається альманахом, яким повинен
розташовувати будь-який супутниковий приймач до початку вимірювань.
Зазвичай приймач зберігає альманах у пам'яті з часу останнього виключення і
якщо він не застарів - миттєво використовує його [16, 67]. Кожен супутник
передає у своєму сигналі весь альманах. Таким чином, знаючи відстані до
декількох супутників системи, за допомогою звичайних геометричних побудов,
на основі альманаху, можна обчислити положення об'єкту в просторі [42, 67].
Метод виміру відстані від супутника до антени приймача заснований на
визначеності швидкості поширення радіохвиль [54, 55, 67]. Для провадження
можливості виміру часу розповсюджуваного радіосигналу кожен супутник
навігаційною системи випромінює сигнали точного часу, використовуючи
точно синхронізований з системним часом атомний годинник (точний
годинник) [55, 67, 71, 108]. При роботі супутникового приймача його годинник
синхронізуються з системним часом супутника, і при подальшому прийомі
сигналів обчислюється затримка між часом випромінювання, що містяться в
самому сигналі, і часом прийому сигналу [55, 67, 108].
Маючи цю інформацію, навігаційний приймач обчислює координати
антени. Решта всіх параметрів руху (швидкість, курс, пройдена відстань, кутове
положення у просторі) обчислюються на основі затримки часу, який сигнал
витратив на переміщення від супутника до антени приймача, що розташований
на об’єкті [54, 55, 67, 71].
19
У реальності робота системи відбувається значно складніша. Нижче
перераховані певні проблеми, та деякі спеціальні технічних прийоми, які
дозволяють вирішити ці проблеми [54, 55, 67, 95, 108]:
1. Відсутність атомного годинника в більшості навігаційних приймачів.
Цей недолік усувається зазвичай вимогою отримання інформації не менше ніж
з трьох (2-мірна навігація при відомій висоті) або чотирьох (3-мірна навігація)
супутників; (При наявності сигналу хоча б з одного супутника можна
визначити поточний час з гарною точністю) [61, 67, 71].
2. Неоднорідність гравітаційного поля Землі, що впливає на орбіти
супутників. Вирішення цієї проблеми досягається шляхом використання систем
стабілізування орбіти супутника [61, 68, 71];
3. Неоднорідність атмосфери, через яку швидкість і напрямок поширення
радіохвиль може змінюватися в деяких межах [55, 71];
4. Відбиття сигналів від наземних об'єктів, що особливо помітно в місті
[55, 67, 71];
5. Неможливість розмістити на супутниках передавачі великої
потужності, через що прийом їх сигналів можливий тільки в прямій видимості
на відкритому повітрі [42, 61, 67, 108].
- Інерціальні системи орієнтації та навігації
Суть інерціальної навігації та орієнтації складається у визначенні
прискорення об’єкта та його кутових швидкостей за допомогою встановлених
приладів і пристроїв на рухомому об’єкті, а за цими даними – позиціонування
(координат) цього об’єкта, його курсу, швидкості, дистанції, а також у
визначенні параметрів, необхідних для стабілізації об’єкту і автоматичного
управління його рухом [2, 3, 14, 15, 18, 95]. Це здійснюється за допомогою:
– датчиків лінійного прискорення (акселерометрів);
– гіроскопічних пристроїв, що відтворюють на об’єкті інерційну систему
відліку (наприклад, за допомогою гіростабілізованої платформи) та
дозволяють визначати кути повороту і нахилу об’єкта (орієнтацію у
просторі), використовувані для його стабілізації та управління рухом;
20
– обчислювальних пристроїв (ЕОМ), які за прискоренням (шляхом їх
інтегрування) знаходять швидкість об’єкту, його координати та інші
параметри руху.
Практична реалізація методів інерціальної навігації та орієнтації пов’язана
із значними труднощами, викликаними необхідністю забезпечити високу
точність і надійність роботи всіх пристроїв при заданій вазі, і габаритах [25, 35,
95]. Подолання цих труднощів стає можливою завдяки створенню спеціальних
технічних засобів – інерціальної навігаційної системи. Переваги методів
інерціальної навігації та орієнтації полягають у автономністі (не потрібна
інформація від зовнішніх джерел, для роботи системи), перешкодозахищеності
та можливості повної автоматизації всіх процесів навігації та орієнтації [25, 35,
36]. Завдяки цьому методи інерціальної навігації та орієнтації отримують все
більш широке застосування при вирішенні проблем навігації та орієнтації
надводних суден, підводних човнів, літаків, космічних апаратів та інших
рухомих об’єктів [25, 26, 28, 33, 35].
Інерціальні системи навігації та орієнтації мають у собі датчики лінійного
прискорення (акселерометри) і кутової швидкості (гіроскопи або пари
акселерометрів, що вимірюють відцентрове прискорення) [25, 28, 35]. За їх
допомогою можна визначити відхилення пов’язаної з корпусом приладу
системи координат від системи координат, пов’язаної з Землею, отримавши
кути орієнтації: рискання (курс) тангажа і крену [35]. Лінійне відхилення
координат у вигляді широти, довготи і висоти визначається шляхом
інтегрування показань акселерометрів. Алгоритмічно ІНС складається з
курсовертикалі і системи визначення координат [6, 33, 35]. Курсовертикаль
забезпечує можливість визначення орієнтації в географічній системі координат,
що дозволяє правильно визначити розташування об’єкта [33, 35, 95]. При цьому
до неї постійно мають надходити дані про становище об’єкта. Проте технічно
система, як правило, не поділяється і акселерометри, наприклад, можуть
використовуватись при виставці курсовертикальної частини [28, 35, 36, 50, 95].
21
Інерціальні системи навігаційні та орієнтації поділяються на ті, що мають
гіростабілізовану платформу (ІНС) і не мають її (БІНС) [15, 18, 35, 95]. У
платформних ІНС взаємний зв’язок блоку вимірювачів прискорень і
гіроскопічних пристроїв, які забезпечують орієнтацію акселерометрів в
просторі та визначають як орієнтований об’єкт у просторі, визначає тип
інерціальної системи. Відомі три основні типи інерціальних систем [15, 25, 26]:
I. Інерціальна система геометричного типу має дві платформи. Одна
платформа з гіроскопами орієнтована і стабілізована в інерціальному просторі,
а друга з акселерометрами – щодо площини горизонту. Координати літака
визначаються в обчислювачі з використанням даних про взаємне розташування
платформ.
II. У інерціальних системах аналітичного типу, акселерометри і гіроскопи
нерухомі в інерціальному просторі (відносно скільки завгодно далеких зірок).
Вхідними координатами та сигналами для використання алгоритмів орієнтації
та навігації є: координати початкового місця положення об’єкта – широта та
довгота, сигнали акселерометрів та гіроскопів.
III. Інерціальні системи напіваналітичного типу мають платформу, що
безупинно стабілізується по місцевому горизонту. На платформі є гіроскопи і
акселерометри. Координати літака чи іншого літального апарата визначаються
в обчислювачі, розташованому поза платформи.
- Оптико – електронні системи орієнтації та навігації
Оптико – електронні системи орієнтації та навігації засновані на
використанні технологій «машинного зору», які включають у себе кореляційно
– екстремальні методи обробки інформації та методи обробки та розпізнавання
образів [16, 24, 39, 41, 42, 60].
Практична реалізація технологій «машинного зору», в значній мірі,
заснована на досягненнях у мікроелектроніці та розробках сенсорних бортових
приладів які можуть працювати у видимому, інфрачервоному та
радіолокаційному діапазонах випромінювання електромагнітного спектру, а
22
також на можливостях реалізації складних алгоритмів обробки поточної
інформації отриманої від декількох джерел [16, 24, 31, 39, 56, 60].
Використання технологій «машинного зору» для навігації та орієнтації
літальних апаратів формулюється таким чином [16, 39, 42, 56, 75].
Необхідно порівняти поточне та еталонне зображення та з подальшою
локалізацією, на поточному зображенні, заданих об’єктів сцен визначити
значення поточної орієнтації об’єкта у просторі та його координат [19, 42, 75].
Згідно з таким принципом роботи, можна виділити три основні напрямки
[16, 42, 75, 76, 78, 98]:
1) Розробка методів, технічних засобів та технологій формування
еталонних зображень, на основі використання,попередньо отриманих,
аеро- та космозображень заданих ділянок земної поверхні, опису
характерних особливостей природного та штучного походжень.
2) Розробка бортових алгоритмів порівняння еталонних та поточних
зображень, локалізація та визначення орієнтації та координат об’єкта,
оптимальна організація обчислювальних процесів та процесів запису і
зберігання інформації.
3) Розробка алгоритмів швидкого доступу до інформації та передачі її
іншим системам.
Основним завданням розробки та проектування оптико – електронних
систем орієнтації та навігації є розроблення та створення алгоритмів аналізу,
обробки та формування вихідних сигналів, які несуть в собі інформацію про
поточне місцеположення та про поточну орієнтацію у просторі об’єкта. У
роботі [16, 42, 60, 75, 76, 98, 99] розглядається метод, який дозволяє оцінювати
кути тангажа та крена ЛА. Приведений метод базується на оцінюванні
зображення з попереднім виділенням лінії горизонту та подальшою
морфологічною обробкою та перетворенням Хо [16, 42, 75, 98].
У роботі [81] продемонстровано застосування відеометричного методу для
контролю безпілотного літального апарата. Відеометрична система у своєму
складі має дві відкалібровані високоточні камери. Дана система може видавати
23
інформацію про поточне місцеположення та кутове положення з частотою не
менше ніж 50Гц та з похибкою яка не перевищує 5 метрів за визначенням
координат та не перевищує 7 кутових мінут за визначенням орієнтації об’єкта у
просторі [16, 41, 42, 75, 98].
На сьогоднішній день перспективи розвитку технологій «машинного зору»
є досить широким. Недоліками таких систем є високі вимоги до алгоритмів
обробки інформації, швидкодії системи та високій точності і камер, які
встановлюються на борту об’єкта, що може ускладнювати виготовлення
бортової обчислювальної системи та системи аналізу зображень [16, 42, 72, 75].
- Інтегровані навігаційні системи
Під інтегрованими навігаційними системами в даній роботі мається на
увазі система, що об’єднує в собі різноманітні, за принципом роботи,
навігаційні системи. Найбільш поширеною інтегрованою системою є
комплексування супутникової навігаційної системи (GLONASS, GPS) та
інерціальної навігаційної системи (ІНС) [16, 35, 36, 71]. Але на даний момент
ведуться роботи з інтеграції ІНС та оптико – електронних систем орієнтації та
навігації [12, 27, 41, 54, 75, 76, 99, 100].
До недавнього часу висока вартість інерціальних приладів була основною
перешкодою для використання їх в навігаційних системах в цілому ряді
прикладних областей, в тому числі і в автомобільної навігації. У останні
декілька років стали доступні інерціальні пристрої нового типу з компактними
чутливими елементами (наприклад, датчики кутових швидкостей і
акселерометри на основі технології MEMS) з низькою вартістю і малими
габаритами [36, 50, 54, 95]. Проте нові чутливі елементи мають не високу,
порівняно з механічними чутливими елементами, точність вихідного сигналу
[95].
Необхідність інтеграції двох таких несхожих систем навігації обумовлена
принципово різним характером помилок, властивих кожній з систем [26, 36,
95]. У той час як помилки GPS обумовлені наявністю перешкод у каналі
передачі інформації і зміною геометрії сузір’я супутників, похибки ІНС мають
24
характер довгоперіодичної хвилі і не схильні до впливу зовнішніх факторів [26,
67, 68, 71]. Крім того, в умовах обмеженій видимості супутників, приймач GPS
припиняє видачу навігаційної інформації, що в ряді завдань абсолютно
неприпустимо [26, 71]. Помилками оптичних систем є не чіткий вихідний
сигнал обумовлений поганою якістю вхідного зображення [16, 34, 72, 75, 98].
Затримка у часі вихідного сигналу оптичної системи, яка обумовлена складним
та багатокроковим процесом фільтрації [6, 9, 99]. Отже, справедливо очікувати,
що дві системи за спільної їхньому застосуванні будуть доповнювати та
змінювати одна одну, підвищуючи надійність і точність навігаційного
комплексу [16, 42, 94, 95, 96, 98].
Спільне застосування ІНС та GPS або ІНС та оптико – електронної системи
дозволяють вирішувати ряд завдань прикладної навігації, в яких потрібна
безперервна видача інформації. Безперервність видачі інформації в
супутниковій навігаційній системі GPS порушується через зникнення
супутників і інтервалу між двома просторами. Відомо, що періодичність видачі
інформації приймачем GPS становить 1 секунду (деякі більш дорогі моделі
дозволяють отримати частоту видачі даних до 6 разів на секунду, а над дорогі
до 10-15 разів на секунду) [4, 16, 77, 94, 98]. Отже, наприклад, зі швидкістю
руху 100 км/год невизначеність розташування може скласти понад 30 м. У той
же час для ІНС передача навігаційних даних із частотою 50 Гц і вище є
нормою. Звідси випливає перша перевага інтеграції – безперервність видачі
інформації [77, 95].
Іншою проблемою використання GPS у реальному часу є стрибкоподібні
зміни показань, викликані орбітальним рухом та зміною сузір’я супутників.
Залежно від кількості та положення супутників ці стрибки призводять до
похибки визначення місцеположення в десятки метрів [61, 67, 71].
Використання ІНС дозволяє ефективно фільтрувати ці стрибки і згладжувати
навігаційні дані [61, 71, 87, 95].
Основним недоліком ІНС є те, що вона накопичує похибки визначення
параметрів руху об’єкта у часі [35, 36, 57, 95]. Але оптико – електронна
25
система немає таких недоліків як накопичення похибок у часі та періодична
відсутність сигналу у часі, як у СНС [56, 63, 72, 81, 96]. Для довготривалих
подорожей та завдань, які вимагають високої точності визначення навігаційних
параметрів комплексування ІНС та оптико – електронної системи орієнтації та
навігації є найбільш вдалим методом інтеграції, який може вирішити проблему
виставки ІНС на рухомій основі [56, 81, 96, 97].
Підвищення вимог до безпеки життя і майна, розвиток страхової індустрії
передбачають включення бортового навігаційного комплексу до складу
обладнання транспортних засобів оперативних служб, вантажних і
пасажирських автомобілів. І тут неможливо обійтися виключно
радіонавігаційної системою, так як інерціальні датчики дозволяють ефективно і
точно вимірювати такі параметри, як кутове становище транспортного засобу,
кутові швидкості, поздовжні і поперечні прискорення [1, 7, 16, 35, 95].
Інтеграція різнорідних навігаційних систем дозволяє підвищити надійність
навігаційного комплексу, покращити точність вихідних параметрів, які
характеризують рух та місцеположення об’єкта [71, 83, 85, 95]. Також
інтеграція навігаційних систем дозволяє компенсувати похибки однієї системи
за рахунок іншої та зменшити загальні масо – габаритні характеристики
навігаційного комплексу [15, 16, 18, 50, 51, 57].
Історію розвитку засобів інерціальної навігації можна прочитати в
монографії [15, 95].
1.2 Проблемна ситуація у розробці та комплексуванні систем орієнтації та
навігації
Сучасні вимоги до навігаційних систем такі щоб система могла якісно,
надійно, неперервно та з високою точністю визначати навігаційні параметри
об’єкта у будь – яких обставинах, які супроводжуються перешкодами, при
різних метеорологічних умовах, та у будь – який час [14, 15, 16]. Беручі до
уваги такі вимоги, сучасні рухомі об’єкти різного призначення (авіаційні,
ракетно – космічні, морські, наземні),для виконання поставлених задач та
26
цілей, обладнуються високоінтелектуальними навігаційними системами [16, 26,
35, 36].
Найбільш поширеною системою яка може вирішувати, з високою
точністю, задачі орієнтації та навігації є безплатформна інерціальна
навігаційна система (БІНС) [35, 36]. Проте використання БІНС залежить від
типу інерціальних сенсорів які використовуються. Так наприклад, якщо БІНС у
своєму складі буде мати лазерні гіроскопи (ЛГ) тоді похибки визначення
місцеположення будуть складати до 1 морської милі, а похибки визначення
орієнтації об’єкта у просторі будуть складати 3 – 9 кутових мінут [11, 16, 29].
Проте вартість такої системи може складати від 40 до 100 тис. доларів, а також
може мати значні масові габарити, тобто вага такої системи може складати
більше ніж 10 кг, що може бути критичним у використанні такої системи на ЛА
типу безпілотний літальний апарат [4, 14 – 16, 50, 54, 71, 85, 95].
Розглядаючи дані недоліки маємо таке протиріччя: методи, які призводять
до покрашення точності навігації, одночасно призводять до втрати системою
перешкодозахищеності та підвищення габаритів і загальної вартості системи.
Таке протиріччя вирішуються шляхом комплексування різнорідних систем
орієнтації та навігації, тобто таких систем, які мають різних характер похибок.
Оскільки сигнали СНС можуть бути не постійні у часі, тому поступово
корекцію БІНС проводять не тільки від СНС, але і від інших систем орієнтації
та навігації. Одним з таких методів є комплексування БІНС та систем оптико –
електронної орієнтації та навігації (машинний зір), від якої, також, проходить
корекція БІНС в умовах відсутності сигналів від СНС. Таке комплексування
стале можливим завдяки розвитку комп’ютерних систем та систем обробки
інформації. Такі системи дозволяють проводити обробку та аналіз знімків
різного діапазону: видимого, інфрачервоного, електронного, теплового, та
синтезувати отриману інформацію у цифровий вигляд та проводи детальний
аналіз з еталонними зображеннями за відносно короткий проміжок часу –
приблизно 1 – 5 секунд. Але для того аби досягти бажаної точності вихідної
інформації потрібно розробити складні алгоритми обробки та аналізу
27
інформації. Проте такі алгоритми, також, не завжди є надійними і, як
компенсаційний, використовується додатковий алгоритм фільтрації. Але в
екстремальних умовах втрати навігації та орієнтації об’єктом затримка у часі
вихідного сигналу є критичним параметром. З урахуванням цього
розробляються методи засновані на визначенні навігаційних параметрів без
апріорної інформації [8, 9, 79, 82, 84, 85].
28
ВИСНОВКИ ДО ПЕРШОГО РОЗДІЛУ
1. Основними джерелами корекції інформації можуть бути супутникові
системи та оптико – електронні системи орієнтації та навігації
2. Основними навігаційними системами являються супутникові системи,
однак вони мають свої недоліки. Основним недоліком є їх залежність від
зовнішньої інформації.
3. Оптико – електроні системи орієнтації та навігації мають стабільність та
безперервність роботи. Основними недоліками оптико – електронної
системи є складність алгоритмів та методів обробки інформації,
формування еталонних зображень та затримка видачі сигналу у часі від
процесу фільтрації.
4. Інерціальні навігаційні системи мають незалежність роботи, проте вони
мають суттєвий недолік. Основним недоліком ІНС є накопичення
похибок визначення орієнтації та місцеположення у часі.
5. Методи комплексування навігаційних систем з різним характером
похибок дозволяють покращити точність визначення орієнтації та
навігації і підвищити надійність навігаційного комплексу.
29
2 РОЗРОБКА МОДЕЛІ БЕЗПЛАТФОРМНОЇ ІНЕРЦІАЛЬНОЇ
НАВІГАЦІЙНОЇ СИСТЕМИ
2.1 Початкове визначення орієнтації об’єкта та вимоги до чутливих
елементів.
Згідно з [53] БІНС має два режими роботи:
- ВИМКНЕНО;
- НАВІГАЦІЯ (з під режимом Виставка).
Режим НАВІГАЦІЯ є основним режимом роботи БІНС. Під режим
Виставка призначений для визначення початкової орієнтації об’єкта. Як відомо
більшість алгоритмів роботи БІНС описуються системою диференціальних
рівнянь, і, як відомо, для вирішення цієї системи необхідно знати початкові
умови, які можна отримати у процесі визначення початкової орієнтації.
Сутність алгоритму БІНС – подвійне інтегрування прискорень, для
визначення лінійної швидкості та координат поточного місцеположення. Для
інтегрування прискорень необхідно задати початкові умови: 3 проекції
початкової лінійної швидкості та 3 координати початкового місцеположення
(декартові координати, географічні координати і т.п.). Для визначення
навігаційних початкових умов застосовуються зовнішні джерела інформації
(наприклад СНС) та відомості про рух об’єкта [36, 42]. Для початку роботи
БІНС у режимі НАВІГАЦІЯ, після ввімкненні напруги живлення системи, на
нерухомому об’єкті, запускається під режим Виставка. У такому випадку
проекції лінійного прискорення дорівнюють нулеві, а координати початкового
місцеположення об’єкта отримуються від СНС або відомі заздалегідь.
Для визначення орієнтації також необхідно задати початкові умови.
Початкові значення орієнтації залежать від алгоритму який використовується:
кути Ейлера – Крилова, рівняння Пуассона, кватерніонне рівняння. Відповідно
до цих методів задаються: початкові кути, початкова матриця напрямних
косинусів, початковий кватерніон [33, 57].
На відміну від початкових навігаційних умов, початкова орієнтація може
бути визначена автономно самою БІНС [2, 3, 35, 36, 42, 68, 70, 85].
30
Визначення початкової орієнтації БІНС проходить аналітично. За
інформацією, яка поступає від чутливих елементів визначаються початкові
значення орієнтації [2, 35, 36].
Чутливими елементами (ЧЕ) є акселерометри та датчики кутової
швидкості (наприклад лазерні гіроскопи, сферичні гіроскопи, поплавкові
гіроскопи, мікромеханічні гіроскопи), які вимірюють абсолютну кутову
швидкість [33, 35, 36].
У початковій момент часу об’єкт займає довільне положення O
відносно географічного базиса Oξηζ, яке визначається кутами рис. 2.1
[35, 36, 42, 52].
Рис. 2.1 Взаємне розташування зв’язаного базиса та географічного базиса
у початковий момент часу
Згідно з рис. 2.1 у початковий момент часу проекції вектора прискорення
сили тяжіння g, які вимірюють акселерометри, на вісі зв’язаного базиса будуть
мати вигляд:
31
Користуючись показаннями акселерометрів (2.1) знаходимо початкові кути
тангажу та крена [35, 36, 50]:
(
)
(
)
Оскільки у початковий момент часу основа нерухома, то проекції кутової
швидкості Землі на географічний базис Oξηζ будуть мати вигляд [33, 35, 36]:
де Ω = 7.29*
– кутова швидкість обертання Землі, φ – широта місця.
Показання датчиків кутової швидкості будуть такими:
Початкове значення кута рискання може бути отримане по одному із
сигналів повздовжнього ДКШ та знайденому куту тангажа [35, 36].
З (2.4) отримаємо:
(
)
Але вираз (2.5) не дозволяє визначити знак початкового курсу об’єкта. Для
того аби визначити знак початкового курсу об’єкта зробимо такі перетворення з
урахуванням раніше визначених кутів тангажа та крена :
Отримавши , , з (2.4) знайдемо:
32
Використовуючи відомі тригонометричні залежності отримаємо:
Враховуючи (2.6) – (2.8), початкове значення курса можна знайти за таким
виразом [35, 36, 89]:
(
) (
)
Використання виразу (2.9) дозволяє повністю визначити початковий курс
об’єкта, урахуванням знака [16, 35].
Після визначення початкових кутів , та можна розрахувати
початкову матрицю напрямних косинусів або початковий кватерніон, в
залежності від того який алгоритм використовується [14, 15, 16, 33, 35, 36].
Отже процес визначення початкової орієнтації є першочерговим процесом,
який повинен виконуватися з високою точністю. Для того аби цей процес
виконувався з високою точністю необхідно аби ЧЕ відповідали певним
критеріям точності. Щоб дізнатися якої точності повинні бути ЧЕ БІНС, аби
забезпечити бажану точність початкового орієнтування, проваріюємо вирази
(2.2) та (2.9) [1, 6, 28, 36].
Нехай
(
)
(
)
(
)
. Проваріювавши данні вирази отримаємо:
33
Аналізуючи вираз (2.10) можна зробити висновок, що похибки кутів
тангажа та крена лінійно залежать від похибок чутливих елементів.
Промоделюємо отримані значення (2.10), для того аби оцінити вплив похибки
чутливого елемента.
Нехай маємо однакові акселерометри та похибка вихідного сигналу
акселерометрів змінюється від g до 0:
Рис. 2.2 Залежність похибки кута тангажа від похибки вихідного
сигналу акселерометра розташованого на осі Y1.
10-8
10-6
10-4
10-2
100
10-4
10-2
100
102
104
Error of Accelerometer Y, m/sec2
Err
or
of in
arc
se
c
Error of pitch angle from accelerometer Y error
34
Рис. 2.3 Залежність похибки кутакрена від похибки вихідного
сигналу акселерометра розташованого на осі X1.
Похибка визначення кута курса у загальному випадку залежить не
тільки від похибки ЧЕ, але і від широти, на якій розташовується об’єктта від
початкового курсу [36, 62, 67, 90, 95].
Розглянемо як впливає кожна з похибок на похибку визначення
початкового курсу.
Будемо вважати, що всі ЧЕ елементи однакові, і мають однаковий діапазон
похибок, тоді:
1) Вплив похибки ДКШ:
( )
де Ω = 7.29*
– кутова швидкість обертання Землі, φ = 57.27° –
широта місця, – похибка ДКШ
, = 0° – початковий
курс.
10-8
10-6
10-4
10-2
100
10-4
10-2
100
102
104
Error of Accelerometer X, m/sec2
Err
or
of
in a
rcse
c
Error of pitch angle from accelerometer X error
35
Рис. 2.4 Залежність похибки визначення кута курсу від похибки
ДКШ.
2) Вплив похибки акселерометрів
[ ]
де φ = 57.27° – широта місця, – похибка
акселерометрів
, = 0° – початковий курс.
10-7
10-6
10-5
10-4
-3
-2.5
-2
-1.5
-1
-0.5
0x 10
5
Errors of Gyro X and Gyro Y, rad/sec
Err
or
of
in
arc
se
c
Error of yaw angle from Gyros error
36
Рис. 2.5 Залежність похибки визначення кута курсу від похибки
акселерометрів.
3) Вплив широти
Нехай похибки чутливих елементів однакові
та , тоді:
[
]
Похибки чутливих елементів дорівнюють:
10-7
10-6
10-5
10-4
10-3
10-2
-3500
-3000
-2500
-2000
-1500
-1000
-500
0
Errors of Accelerometers X, Y, Z, m/sec2
Err
or
of
in
arc
se
c
Error of yaw angle from Acelerometers errors
37
Рис. 2.6 Залежність похибки визначення кута курсу від значення широти
місця розташування об’єкта.
Рис. 2.7 Залежність похибки визначення кута курсу від значення
початкового курсу .
Аналіз залежностей (2.10) показує, що похибки визначення початкових
кутів орієнтації суттєво залежать від похибок ЧЕ [35, 36, 95]. Наведемо
0 5 10 15 20 25 30 35 40 45 50 55 60 65 70 75 80 85 90
-169.6846-162.6846-155.6846-148.6846-141.6846-134.6846-127.6846-120.6846-113.6846-106.6846-99.6846-92.6846-85.6846-78.6846-71.6846-64.6846-57.6846-50.6846-43.6846-36.6846-29.6846-22.6846-15.6846-8.6846
Latitude , deg
Err
or
of
in
arc
se
c
Error of yaw angle from latitude cahnge
0 20 40 60 80 100 120140 160180200 220240 260280300 320340 360
-23.4075-21.4075-19.4075-17.4075-15.4075-13.4075-11.4075-9.4075-7.4075-5.4075-3.4075-1.40750.59252.59254.59256.59258.5925
10.592512.592514.592516.592518.592520.5925
Initial yaw 0, deg
Err
or
of
in
arc
se
c
Error of yaw angle from initial yaw 0
38
характеристики деяких чутливих елементів, які застосовуються у
безплатформнихінерціальних навігаційних системах.
Таблица 2.1 Характеристики акселерометрівза ТУ та рекламними
проспектами
Назва параметру Од.
виміру
Вимоги до
акселером
етра
А-18 АК-15 ВА-3 А-18Т АК-6 Е1
Невідтворюванн
ість
масштабного
коефіцієнта
Відн.
Од. 9·10
-5 15·10
-5 20·10
-5 24·10
-5 10·10
-5 8·10
-5 5·10
-5
Невідтворюванн
ість зміщення
нуля g 8·10
-5 20·10
-5 3·10
-5 16·10
-5 10·10
-5 6·10
-5 8·10
-5
Зміна кутів
орієнтації
базової
площини
" 40 30 4 20 20 10 20
Діапазон
вимірювань g 40 40 20 50 40 20 50
Діапазон
робочих
температур
оС -50…+85
-60…
+80
-60…
+70
-55…
+85
-50…
+85
-60…
+80
-55…
+85
Матеріал
маятника
кремний кварц кварц кремний кварц кварц
Виробник ИТТ МИЭА Электро
-оптика
ИТТ Серп.за
вод
Металл
ист
Китай
Ціна Дол. $ 3310,82 3659,33 4356,34 4356,34 3833,58 2265,30
Таблиця 2.2 Характеристики ДНГ
Хар-стики,
розмірність
Модель, розробник КИ-05-5А КИНД
05-049
РВГ-1М ГВК-6-1 ГВК-10 ГВК-18 ДНГ-4
НИИ ПМ ОКБ
«ТЕМП»
РПКБ ПНППК
Діапазон
виміру
кутових
швидкостей,
град/сек
±15
±30
±150
±160
±50
----
±15
Випадковий
дрейф,
град/год
(від пуска до
пуска)
±0.3
±0.15
±5.0
±0.01
±0.03÷
±0.2
±0.01
±0.5
Дрейф,
град/год ---- ---- ---- ±3 ±25 ±3 ±50
Дрейф, ---- ---- ---- ±1 ±3 ±3 ±6
39
залежний від g,
град/год
Нестабільність
масштабного
коефіцієнта, %
0.05 0.05 0.1 ---- 0.1 ---- ----
Крутизна ДМ,
град/(с*А) ---- ---- ---- 23 130 80 450±70
Габаритні
розміри, мм Ø54×78 Ø33×27 Ø25×30 Ø54×45 Ø56×63 Ø54×45 ----
Маса, г 430 80 50 265 460 250 80
Таблиця 2.3.1 Характеристики ВОГ
Характеристики,
розмірність
Модель, розробник
ВГ910Ф ВГ941-3 ВГ951 ДУС-500 Д-36
«ФИЗОПТИКА» ПНППК
Діапазон виміру
кутових
швидкостей,
град/сек
±300
±500
±80
±500
±60
Систематична
похибка,
град/год
5 30 1÷3 2 36
Випадкова
похибка,
град/год
0.08 0.3 0.017 0.03 ----
Нестабільність
масштабного
коефіцієнта, %
0.1
0.3
0.1
0.01
----
Споживча
потужність, Вт 1.1 0.8 1.5 15 5
Габаритні
розміри, мм Ø80×20 Ø24.5×58 Ø150×30 ---- ----
Маса, г 110 40 650 1500 ----
Таблиця 2.3.2 Характеристики ВОГ підприємства «ОПТОЛИНК»
Характеристики,
розмірність
Модель, розробник
SRS-2000 SRS-1000 SRS-500 SRS-200 TRS-500
ОПТОЛИНК
Діапазон виміру
кутових
швидкостей,
град/сек
±10
±30
±100
±200
±500
Дрейф зміщення
при постійній
температурі,
град/год
≤0.01
<0.1
<1.0
<10.0
<10.0
Похибка
40
масштабного
коефіцієнта, % ≤0.01 ≤0.02 ≤0.02 ≤0.05 ≤0.5
Смуга
пропускання, Гц >10 10÷30 >100 >100 100÷500
Шум,
√ ⁄ ≤0.001 ≤0.003 ≤0.005 ≤0.01 ≤0.05
Довжина ВК, м 2000 1000 500 200 500 Розмір, мм Ø250×40 Ø150×40 Ø150×40 Ø150×40 110×110×90 Маса, кг 1.2 0.8 0.8 0.8 1.1
Таблиця 2.4.1 Характеристики ЛГ (КЛГ)
Характеристики,
розмірність
Модель, розробник
LG-8028 GG-1342 ASLIG KM-11 БЛГ-1 ЛГ-1 «Морион»
Litton
Inc.
Honeywell
Sperry
НИИ
«Полюс»
ОКБ
«Темп»
МИЭА
НПО
«Астро-
физика»
Діапазон виміру
кутових
швидкостей,
град/сек
±600
±800
±100
----
±100
----
±200
Систематична
похибка,
град/год
0.01÷0.2
0.01
0.1
0.01
0.5÷1.5
0.01
0.2÷2
Випадкова
похибка,
град/год
0.003
0.003
0.05
0.003
----
0.003
0.03
Стабільність
масштабного
коефіцієнта
5*
5*
5*
----
5*
----
Споживча,
потужність, Вт 3 ---- ---- ---- 6 ---- 9
Периметр, с м 28 32 ---- 44 16 28 26 Ресурс, год 15 20 30 10 0.5-1.0 ---- 5-7 Маса, кг 1.7 1.9 1.8 ---- 1.6 1.8 ---- Кількість
вимірювальних
вісей
1
----
----
1
3
1
3×2
Таблиця 2.4.2 Характеристики ЛГ заводу «Арсенал», Київ
Модель лазерного гіроскопа RL-08 RL-16 RL-28A
Діапазон кутових швидкостей, °/сек ±1000 ±400 ±250
Нестабільність зміщення нуля, °/год. 0,1....0,6 0,05 0,005...0,02
Нестабільність масштабного
коефіцієнта ≤ ≤ ≤
Випадковий відхід в куті, √ 0,06 0,02 0,005...0,01
Температурній діапазон, -40....+65 -40....+65 -40....+65
41
Лазерні гіроскопи з дрейфом нуля 0.005 – 0.01 град/год мають великі
габарити та, як наслідок, велику масу > ніж 3 кг. Такі гіроскопи потребують
високої технології виготовлення, і, відповідно, вартість таких гіроскопів може
бути критичною [35, 90, 92, 95].
2.2 Основний режим роботи безплатформної інерціальної навігаційної
системи
Основний режим безплатформної інерціальної навігаційної системи
забезпечує видачу споживачам параметрів орієнтації об’єкта та параметри
навігації, тобто як розміщений об’єкта у просторі відносно початкової системи
координат та які координати місцеположення має об’єкт [2, 3, 14, 25, 95].
У загальному випадку задачі, які вирішує БІНС можна зобразити у такому
вигляді, рис. 2.8.
Рис. 2.8 Загальна обчислювальна задача у БІНС.
З рис. 2.8 обчислювальну задачу, яка виконується у БІНС можна розділити
на два канали: канал обчислення орієнтації об’єкта та канал обчислення
навігації. Канал орієнтації є швидкодіючим каналом, в якому обчислення
проходять з високою частотою [33, 90, 95]. Обчислення, які виконуються у
цьому каналі є першочерговими обчисленнями для вирішення задачі навігації.
Канал навігації є повільним каналом, у якому процеси проходять з низьким
періодом часу, який зазвичай дорівнює періоду Шюлера. У каналі навігації
42
відбувається перетворення прискорень із зв’язаної системи координат до
навігаційної системи та шляхом двократного інтегрування обчислюються
відповідно швидкість об’єкта та його поточне місцеположення [33, 90, 91, 95].
Розглянемо етапи проектування основного режиму роботи без
платформної інерціальної навігаційної системи. При проектуванні
безплатформних інерціальних навігаційних систем (БІНС) необхідно проводити
обґрунтований вибір алгоритмів функціонування системи. Основним
елементом алгоритмічного забезпечення БІНС є алгоритм чисельного
інтегрування кінематичних рівнянь. Вибір цього алгоритму впливає на
метрологічні характеристики без платформної інерціальної навігаційної
системи, і для його обґрунтування необхідно визначати похибку, виникаючу
при інтегрування кінематичних рівнянь у самому алгоритмі [19, 33, 35, 92, 95].
2.2.1 Кінематичні рівняння орієнтації
Для визначення орієнтації об’єкта у просторі, на практиці, застосовують
три вимірювачі кутової швидкості, які називають датчиками кутової швидкості
(ДКШ). ДКШ жорстко встановлені (закріплені) на об’єкті та вимірюють
поточні проекції абсолютної кутової швидкості тіла на три ортогональні осі
системи координат пов’язаної з тілом. У цьому випадку задача визначення
поточної орієнтації зводиться до встановлення диференціальних рівнянь,
інтегруючи які, можна визначити поточні значення параметрів орієнтації. Ці
рівняння називаються кінематичними рівняннями орієнтації [15, 28, 33, 95].
2.2.1.1 Рівняння орієнтації Ейлера
Найбільш поширеною формою кінематичних рівнянь є така, в якій вони
подані через кути Ейлера або Ейлера – Крилова послідовних поворотів [33, 95].
Подання повороту через кути повороту не є єдиними, у цьому випадку
форма кінематичних рівнянь визначається цією послідовністю поворотів і
також не є єдиною. Далі розглянемо послідовність поворотів наведену
на рис. 2.9.
43
Рис. 2.9 Послідовність поворотів Ейлера – Крилова.
Тут вісь напрямлена вздовж вертикалі місця.
При чому будемо вважати, що вектор кутової швидкості ω опорної
системи координат відносно зв’язаної з об’єктом буде визначатися як
ψω
(2.14)
Спроектуємо кутову швидкість ω на вісі зв’язаної системи координат:
sincoscos
sin
cossincos
z
y
x
(2.15)
Тепер визначимо отриману систему рівнянь відносно кутів ,, . З
другого рівняння знайдемо:
sin y (2.16)
Далі, помножимо перше рівняння на cos , а третє рівняння на sin та
складемо їх:
2 2
cos sin cos sin cos
cos cos cos sin sin .
x z
(2.17)
44
Тепер помножимо перше рівняння на sin , а третє рівняння на cos та
складемо їх:
2
2
sin cos cos sin cos sin
cos cos sin cos cos .
x z
(2.18)
В результаті отримаємо систему з трьох рівнянь:
sin y
sincos zx
cossincos zx (2.19)
З останнього рівняння отримаємо:
cos
1sincos xz
(2.20)
Тоді перше рівняння набуде наступного вигляду:
tgxzyy sincossin
(2.21)
В результаті ми тримаємо наступну систему рівнянь:
cos
1sincos
sincos
sincos
xz
zx
xzy tg
(2.22)
Співвідношення (2.22) називають кінематичними рівняннями Ейлера.
Згідно з рівняння (2.22) у середовищі MatLAB була створена програма, яка
реалізує чисельне інтегрування методами Рунге - Кутта. Проінтегрувавши
рівняння (2.22) (методами Рунге - Кутта) ми отримаємо кути орієнтації .
Інтегрування проводилося трьома методами Рунге – Кутта, при таких
параметрах:
( )
( )
( )
45
(2.23) – закон руху основи, в якому:
– частота коливання;
– амплітуда коливань;
– зсув фази коливань.
Рис. 2.10 Результати моделювання кінематичних рівнянь Ейлера 2.22
Представлення орієнтації за допомогою кутів Ейлера широко
розповсюджене завдяки зрозумілому фізичному сенсу та прямій відповідності
кутам між рамками карданового підвісу гіростабілізованої платформи [2, 3, 33].
Очевидною перевагою такого представлення є мінімальна кількість
параметрів, яка відповідає кількості степенем вільності. Але існує суттєвий
недолік при представлені орієнтації кутами Ейлера – Крилова. При куті
тангажа відбувається виродження однієї з ступенів вільності і через
це, два інших кута вже неможливо однозначно визначити [33, 35, 95]. Така
ситуація відповідає «складанню рамок» карданового підвісу. Звідси випливає,
що використовувати таке представлення орієнтації можна тільки в обмеженому
діапазону куті або виключати некоректну ситуацію ввівши для цього
0 5 10 15 20 25 30 35 40 45 50-0.2
-0.1
0
0.1
0.2Euler modify method (2.2)
Time, sec
Angle
, ra
d
0 5 10 15 20 25 30 35 40 45 50-0.2
-0.1
0
0.1
0.2Hoine (3.2)
Time, sec
Angle
, ra
d
0 5 10 15 20 25 30 35 40 45 50-0.2
-0.1
0
0.1
0.2Runge-Kutta (4.2)
Time, sec
Angle
, ra
d
Integrate Euler equation
=(ycos-
zsin)/cos
=ysin-
zcos
=x-*sin
Motion Low
=o+
ot+
msin(
t+
)
=o
ot+
msin(
t+
)
=o+t
o+
msin(
t+
)
Initial conditions
o=0
o=0
m=0
=1
=0
o=0
o=0
m=0.2
=1
=0
o=0
o=0
m=0.2
=1
=1.5708
Step of integration
h=0.02
-------------------------------BSO
-Euler.m
Gurynenko S.O., PSON, PG-31m
21: 9: 30-Mar-2015
46
допоміжні алгоритми. Крім цього, використання кутів Ейлера – Крилова
вимагає значних обчислювальних затрат, які зв’язані з обчисленням
тригонометричних функцій [2, 14, 15, 33, 35, 36, 91 – 95].
2.2.1.2 Рівняння орієнтації Пуассона
Матричне подання векторного співвідношення [33, 95]:
(2.24)
де - локальна похідна вектора (його швидкість відносно рухомої системи
відліку), - вектор абсолютної кутової швидкості рухомої системи, матиме
такий вид:
(2.25)
де, і матриці-стовпці із проекцій вектора на осі відповідно нерухомої (S)
й рухомої (P) системи відліку, - матриця напрямних косинусів переходу
від нерухомої (S) до рухомої (P) системи відліку [33, 35, 36].
Якщо подати за основу кінематику поворотів зображену на рис.2.9, де S –
система координат ξηζ, а P – система відліку xyz, то матриця набуде
вигляду:
coscossincossin
cossincossinsincoscoscossinsinsincos
sinsincoscossinsincossinsinsincoscosspC
- позначення кососиметричної матриці із проекцій вектора кутової
швидкості системи Р відносно системи S на її осі:
(2.26)
Зіставляючи із (2.25), можна зробити
висновок, що:
aωaa
PP
dt
d
dt
d~
dt
dPa~
a
Pω
P
PS
PPStSP A
dt
dA
dt
dAC )()(
SA PA
SPC
)( PS
P
0
0
0
)(
12
13
23
PS
P
PS
P
PS
P
PS
P
PS
P
PS
P
PS
P
P
SPtSPPStSP A
dt
dCC
dt
dA
dt
dAC )()(
47
(2.27)
Це співвідношення зв'язує проекції кутової швидкості обертання системи Р
відносно S на осі системи Р з матрицею напрямних косинусів між цими
системами відліку [33, 95]. В дещо зміненій формі
(2.28)
це ж співвідношення може прислужиться задля визначення похідної від МНК за
заданими проекціями кутової швидкості. Воно називається рівнянням
Пуассона.
Якщо матричне подання (2.25) записати через проекції на осі абсолютної
системи
(2.29)
де,
(2.30)
то, порівнюючи (2.25) з , можна одержати аналогічну
залежність для проекцій кутової швидкості на нерухомі осі
(2.31)
звідки
(2.32)
яке є однією з форм рівняння Пуассона.
У випадку, коли за основу прийнята кінематика поворотів зображена на
рис.2.9, а вираз (2.26) можна подати у вигляді.
dt
dCC
SPtSPPS
P )()(
)( PS
P
SPSP
Cdt
dC
S
PS
SPSPS A
dt
dAC
dt
dA )(
0
0
0
)(
12
13
23
PS
S
PS
S
PS
S
PS
S
PS
S
PS
S
PS
S
P
SP
PSPS Adt
dC
dt
dAC
dt
dA
tSPSP
PS
S Cdt
dC)()(
SPPS
S
SP
Cdt
dC )(
48
0
0
0
xy
xz
yz
Тут роль осей 1р, 2р і 3р відіграють відповідно осі x, y, z, а осі 1s, 2si 3s –
осі .
Результати моделювання рівнянь Пуассона проводилися при русі основи,
яке задано співвідношенням (2.23) та інтегрувалося методами Рунге – Кутта:
Рис. 2.11 Результати моделювання процесу інтегрування
рівнянь Пуассона 2.32
2.2.2 Рівняння навігації
В основі рівнянь навігації покладений принцип використання другого
закона Ньютона. Згідно з другим законом Ньютона, вектор абсолютного
прискорення центра мас матеріального тіла можна виразити через вектор усіх
рівнодіючих прикладених до тіла сил F та масу цього тіла m [15, 16, 25, 35, 67]:
Силу F можна представити у вигляді суми двох сил: суми всіх діючих на
тіло активних сил Fакт (сила тяги, аеродинамічна сила, сили, що створюються
органами керування і т.п.) та сил гравітаційної взаємодії G тіл. З урахуванням
такого припущення рівняння (2.33) можна представити у вигляді [15, 16, 95]:
0 5 10 15 20 25 30 35 40 45 50-0.2
-0.1
0
0.1
0.2
Time, s
Ru
ng
e -
- K
utta
4.1
0 5 10 15 20 25 30 35 40 45 50-0.2
-0.1
0
0.1
0.2
Runge -- Kutta method
Ru
ng
e -
- K
utta
2.2
Programe of integration matrixPoisson motion equation
Law of motion body frame
= 0; =
msin(t+
);
= m
sin(t+);
Parameter of motion body frame
m = 0.2;
m = 0.2;
= = 90
o;
= 0
o; = 1 rad/s;
Parameter of integration processStart time t = 0 s; End time Tk = 50 s;Step of integration h = 0.01;
=================================================IntPoisEq.m - control programe
Author - Gurynenko S.O., PG-31m,PSON21:52: 02-Apr-2015
=================================================
49
Відношення
, представляє собою питому силу, яку називають уявним
прискоренням і позначають a. Відношення
представляє собою гравітаційне
прискорення об’єкта gгр(R, φ), яке визначається геопотенціалом у даній точці,
яке у загальному випадку є функцією від геоцентричного радіус – вектора
місцеположення та широти місцеположення [15, 16, 35, 67, 71, 95].
Абсолютне прискорення Wможна виразити, як другу похідну від зміни
геоцентричного місцеположення радіус – вектора R:
Підставляючи вирази (2.34) та (2.35), з урахуванням усіх припущень, у
вираз (2.33), отримаємо:
Отриманий вираз (2.36) називають основним рівнянням інерціальної
навігації і яке слугує основою для розробки алгоритмів функціонування всіх
типів ІНС. Це рівняння дозволяє визначити радіус – вектор місцеположення R
(відповідно координати об’єкта) та похідні від радіус – вектора (відповідно
швидкість та прискорення об’єкта) на основі векторів aта gгр(R, φ). Вектор
уявного прискорення a може бути вимірян трьома ортогонально
розташованими акселерометрами [35, 36, 94]. Гравітаційне прискорення gгр(R,
φ) зазвичай задається у вигляді відомої функції від радіус – вектора R та
широти місця φ для конкретно заданої моделі фігури Землі [35, 71, 90, 91, 95].
Рівняння (2.36) можна записати у формі Коші:
де V – вектор абсолютної швидкості руху об’єкта.
50
При розглядання рівнянь (2.37) та (2.38) у деякій рухомій системі
координат, яка обертається х кутовою швидкістю Ω, похідні векторів R таV
можна представити у такому вигляді:
де
та
– локальні похідні від векторів R та V, які взяті у базисі, що
обертається з кутовою швидкістю Ω. Вектор Ω абсолютної кутової швидкості
координатного базиса (тригранника) у загальному випадку вибирається при
проектуванні системи у відповідності до вимог які висуваються до системи.
У загальному випадку проекції кутової швидкості обертання Землі мають
вигляд [2, 3, 25, 33, 95]:
( )
( )
де = 6378.137 км – радіус Землі, V – швидкість руху об’єкта, u =
7.27*
– кутова швидкість обертання Землі, – широта та швидкість
зміни широти місцеположення, –швидкість зміни довготи місцеположення.
У проекціях на осі географічного базиса, прискорення можна подати у
таком вигляді [35, 36, 89, 94]:
Враховуючи те, що та підставляючи (2.41) у (2.42)
отримаємо [15, 16, 35, 36, 89, 94]:
51
( ) (
)
( )
Вираз (2.43) показую ті складові, які вимірює акселерометр, тобто складові
активної сили.
З іншого боку показання акселерометра мають вигляд виразу (2.36). У
проекціях на вісі об’єкта вираз (2.36) має вигляд [35, 36, 95]:
Враховуючи вираз (2.44) та вираз для обчислення МНК, запишемо вирази
прискорень, і будуть інтегруватися у навігаційному каналі БІНС [36, 67, 95]:
( )
( )
( )
Рівняння (2.45) інтегруються у навігаційному обчислювачі.
Найчастіше, у якості базової системи координат обирають географічну
систему координат Oξηζ, яка характеризується географічними координатами:
широта φ, довгота λ. З урахуванням (2.45) географічні координати
визначаються за такими виразами [14, 15, 35, 36, 67, 71, 95]:
∫
∫
∫ ∫
Продемонструємо результат роботи ідеального навігаційного
обчислювача.
Моделювання будемо проводити з такими параметрами:
52
загальний час інтегрування Tk – 600с;
крок інтегрування h – 0.005;
метод інтегрування – Рунге-Кутта 1-го порядку;
Дослід 1
1) Швидкість руху об’єктаV = 720
= 200
;
2) Координати початкового місцеположення об’єкта:
Широта φ = 50.21°;
Довгота λ = 30.3°;
3) Початковий курс = 0°;
Рис.2.12 Інтегрування прискорень (2.44) та визначення географічних координат
(2.46) місцеположення об’єкта, рух курсом = 0°;
Дослід 2
1) Швидкість руху об’єктаV = 720
= 200
;
2) Координати початкового місцеположення об’єкта:
Широта φ = 50.21°;
Довгота λ = 30.3°;
3) Початковий курс = 30°;
0 100 200 300 400 500 6000
10
20
30
40
50
60
Time, second
Na
vig
ation
coo
rdin
ate
s,
deg
rees,
ide
al
Object coordinates: latitude and longitude , ideal
I
I
Integration ofideal navigation equation
Wi = C
gb*a
W = C
gb(1,1)*a
x+C
gb(1,2)*a
y+C
gb(1,3)*a
z
W = C
gb(2,1)*a
x+C
gb(2,2)*a
y+C
gb(2,3)*a
z
W = C
gb(3,1)*a
x+C
gb(3,2)*a
y+C
gb(3,3)*a
z
Initial coordinates
0 = 50.27
o; 0
= 30.3o;
Accelerometers errors
Law of motion of body frame =
msin(t+
);
= m
sin(t+);
= m
sin(t+);
Parameter of motion of body frame
m = 0;
m = 0.1;
m = 0.2;
= = 90
o;
= = 0
o;
= 1 rad/s;Parameters of integration processStart time t = 0 s;End time Tk = 600 s;Step of integration h = 0.005;
=====================================AHRS-SINS
-mod.m - control program
Author - Gurynenko S.O.19:21: 12-Apr-2015
=====================================
53
Рис.2.13 інтегрування прискорень (2.44) та визначення географічних координат
(2.46) місцеположення об’єкта, рух курсом = 30°;
2.3 Похибки безплатформної інерціальної навігаційної системи
Розглянутий у пункті 2.2 принцип побудови БІНС характеризує основний
режим роботи БІНС у тому випадку, коли всі елементи працюють без похибок,
а початкові умови визначенні та введені ідеально. Відповідно до цього, усі
параметри, які видає БІНС також будуть безпомилковими. Такі алгоритми
роботи називаються ідеальними [2, 3, 35, 36, 95]. У реальних системах всі
елементи мають певні похибки такі як: зміщення нуля вихідного сигналу, шум,
зміна масштабного коефіцієнту в залежності від температури і т.п. Усі чинники
слугують збуреннями для БІНС [15, 16, 90, 95]. Рівняння похибок БІНС
дозволяє визначити на скільки показання реальної роботи будуть відрізнятися
від показань ідеальної роботи [15, 35, 36, 95].
Аналіз похибок БІНС базується на розв’язанні рівнянь похибок, які
отримуються шляхом варіації алгоритмів роботи БІНС. У загальному випадку
отримуються звичайні диференціальні рівняння дев’ятого порядку зі змінними
коефіцієнтами [35, 73, 95].
Розглянемо вплив суттєвих похибок ЧЕ, які сильно впливають на роботу
БІНС: зміщення нуля [35, 36, 50, 54].
0 100 200 300 400 500 6000
10
20
30
40
50
60
Time, second
Na
vig
ation
coo
rdin
ate
s,
deg
rees,
ide
al
Object coordinates: latitude and longitude , ideal
I
I
Integration ofideal navigation equation
Wi = C
gb*a
W = C
gb(1,1)*a
x+C
gb(1,2)*a
y+C
gb(1,3)*a
z
W = C
gb(2,1)*a
x+C
gb(2,2)*a
y+C
gb(2,3)*a
z
W = C
gb(3,1)*a
x+C
gb(3,2)*a
y+C
gb(3,3)*a
z
Initial coordinates
0 = 50.27
o; 0
= 30.3o;
Law of motion of body frame =
msin(t+
);
= m
sin(t+);
= m
sin(t+);
Parameter of motion of body frame
m = 0;
m = 0.1;
m = 0.2;
= = 90
o;
= = 0
o;
= 1 rad/s;Parameters of integration processStart time t = 0 s;End time Tk = 600 s;Step of integration h = 0.005;
=====================================AHRS-SINS
-mod.m - control program
Author - Gurynenko S.O.20:0: 12-Apr-2015
=====================================
54
Зміщенням нуля називається усереднений вихідний сигнал, який не
зв’язаний з вхідною величиною вимірюваною датчиком. Це сигнал, який не
відповідає значенню нуля, при відсутності прискорення (кутової швидкості) на
вході. Зміщення нуля можна розкладати на постійну складову, температурно-
залежну частину та випадковий дрейф [35, 36, 95]. Постійну складову зміщення
та його температурну залежність можна визначити за допомогою калібрування.
Випадковий дрейф зазвичай повинен бути описаний в моделі у вигляді
випадкового процесу [29, 57, 95].
Масштабний коефіцієнт відображає відношення приросту вихідного
сигналу до приросту вимірюваної величини. Зазвичай від визначається як нахил
прямої апроксимуючої залежність вихідного сигналу датчика від вимірюваної
величини. У масштабному коефіцієнті можна виділити такі складові: постійна
складова, температурно-залежна та випадкова частини. При необхідності
врахування нелінійності вихідних сигналів датчиків, масштабні коефіцієнти
можуть бути представлені поліномами більш високого порядку [36, 50, 91, 95].
2.3.1 Вплив похибки акселерометра
Як ми знаємо, акселерометр вимірює прискорення сили тяжіння, похибку
вимірювання лінійної швидкості можна визначити за допомогою однократного
інтегрування зміщення нуля акселерометра, а для знаходження похибки
місцеположення – двократне інтегрування його зміщення нуля ab [25, 36, 95]:
21
2a a ap b dtdt b tdt b t
(2.47)
Із залежності (2.47) можна побачити, що зміщення нуля акселерометра
призводить до похибки в визначенні швидкості, прямо пропорційній часу і
похибку місцезнаходження, яка пропорційна квадрату часу [25, 95].
Продемонструємо залежність (2.47) числовим прикладом:
Нехай об’єкт рухається з постійною швидкістю 200
протягом 3 годин
(10800 секунд) у північному напрямку, зміщення нуля акселерометра складає
10-4
g, g = 9.8
– гравітаційне прискорення, приймемо Землю сферою з
55
радіусом R = 6400000 м. Згідно з такими даними місцеположення об’єкта буде
визначатися:
P =
+
=
[рад]
Отже похибка визначення місцеположення буде складати 0.01786 радіан =
1.023 градуси.
2.3.2 Вплив похибки гіроскопа
Для гіроскопа, його нескомпенсоване зміщення вихідного сигналу gb
призводить до створення похибки в орієнтації, яка пропорційна часу [25, 95]:
g gb dt b t (2.48)
Цей невеликий кут похибки орієнтації призводить до неправильного
проектування прискорення на осі опорної СК, що в свою чергу призводить до
похибки в одному з вимірювальних каналів навігаційної системи, яка має
величину sin( ga g g gb t , де g – прискорення вільного падіння.
2 31 1
2 6g g gp gb tdtdt gb t dt gb t (2.49)
Таким чином, зміщення нуля у гіроскопа призводить до похибки
швидкості, яка пропорційна квадрату часу та похибку місцезнаходження, яка
пропорційна часу у третій степені [25, 95].
Наведемо чисельний приклад:
Нехай гіроскоп має нескомпенсоване зміщення нуля gb = 0.005 град/год =
2.42*10-8
рад/сек, час польоту, 3 годин (10800 секунд), g = 9.8
– гравітаційне
прискорення. Підставивши дані у вираз (2.49) отримаємо:
[рад]
Отже похибка визначення місцеположення від нескомпенсованого дрейфу
гіроскопа буде складати 0.007793 радіан = 0.45 градуса.
2.3.3 Похибка алгоритмічного дрейфа
У загальному випадку похибка алгоритмічного дрейфа залежить від
порядка алгоритма інтегрування N, кроку інтегрування h, амплітуди та частоти
56
коливань основи та зсуву фаз між цими коливаннями [11, 19, 33]. Дану похибку
можна представити таким виразом [33]:
⟨ ⟩ (
)
Наведемо результати моделювання формули алгоритмічного дрейфу
(2.50):
Рис. 2.14 Залежність дрейфу від зміщення фази коливання
Рис. 2.15 Залежність дрейфу від значення кроку інтегрування
-150 -100 -50 0 50 100 150-2.5
-2
-1.5
-1
-0.5
0
0.5
1
1.5
2
2.5x 10
-6 RK 21
Drift
ra
d/s
, deg
-150 -100 -50 0 50 100 150-4
-3
-2
-1
0
1
2
3x 10
-8 RK 31
Drift
ra
d/s
, deg
-150 -100 -50 0 50 100 150-8
-6
-4
-2
0
2
4
6
8x 10
-11 RK 41
Drift
ra
d/s
, deg
Law of motion
= 0;
= m
sin(t+);
= m
sin(t);
The parameter of motion
= 1 rad/s;
m
= 0.1; m
= 0.1;
Step of integration process h = 0.05;
==============================probe4
-1
-poxubku - control programme
Author - Gurynenko S.O., PG - 31m, PSON
16:6: 28-Sep-2014==============================
10-3
10-2
10-1
10-16
10-14
10-12
10-10
10-8
10-6
10-4
10-2
Maximum Drift of method Runge -- Kutta, depend from step of integration
Step, h
Drift
ra
d/s
RK--2.1, =90o
RK--3.1, =0o
RK--4.1, =90o
Law of motion
= 0;
= m
sin(t+);
= m
sin(t);
The parameter of motion
= 1 rad/s;
m
= 0.1; m
= 0.1;
Integrate a Poisson
equation of orientation motion
Slope: DR21 = 0.000477667;
DR31 = 8.0415e-005;DR41 = 2.54965e-006;
================================
probe4-2
-poxubku - control programme
Author - Gurynenko S.O., PG - 31m, PSON
13:59: 15-Dec-2014
================================
57
Рис. 2.16 Залежність дрейфу від частоти коливань
Рис. 2.17 Залежність дрейфу від амплітуди коливань
2.3.4 Похибки безплатформної інерціальної навігаційної системи у
вигляді простору станів.
У загальному випадку похибки БІНС можуть бути виражені у матричному
рівнянні простору станів [19, 35, 36, 70, 71, 73, 90, 95]:
де x = [ ]T – вектор зміни стану,
- похибки визначення кутів рискання, тангажа та крена
10-1
100
10-16
10-14
10-12
10-10
10-8
10-6
10-4
Errors depending from Frequency of oscillations
Frequency of oscillation, Fr
Drift
ra
d/s
RK--2.1, =90o
RK--3.1, =0o
RK--4.1, =90o
Approx RK--2.1
Approx RK--3.1
Approx RK--4.1
Law of motion
= 0;
= m
sin(t+);
= m
sin(t);
The parameter of integrationStep of integration h = 0.05
Integrate a Poissonequation of orientation motion
Slope: DR21 = 2.3687e-006;
DR31 = 2.97703e-008;DR41 = 6.85531e-011;
================================probe4
-8
-3
-poxubku
-max
-pois - control programme
Author - Gurynenko S.O., PG - 31m, PSON
13:45: 08-Nov-2014================================
10-2
10-1
10-14
10-12
10-10
10-8
10-6
10-4
Maximun Errors, depending from Amplitude of oscillations
Amplitute of oscillation, A
Drift
ra
d/s
RK--2.1, =90o
RK--3.1, =0o
RK--4.1, =90o
Approx RK--2.1
Approx RK--3.1
Approx RK--4.1
Law of motion
= 0;
= m
sin(t+);
= m
sin(t);
The parameter of integrationStep of integration h = 0.05
Integrate a Poissonequation of orientation motion
Slope: DR21 = 4.64056e-005;
DR31 = 5.97261e-007;DR41 = 1.04881e-009;
================================probe4
-7
-3
-poxubku
-max
-pois - control programme
Author - Gurynenko S.O., PG - 31m, PSON
13:23: 08-Nov-2014================================
58
відповідно, – похибки визначення складових вектора
швидкості швидкості, - похибки визначення координат
географічного місцеположення: широти, довготи та висоти, F – перехідна
матриця простору стану, B – матриця коефіцієнтів вхідного збурення або
керування, u – вектор керування = [ ], [ ] -
зміщення нуля акселерометрів, [ ] - систематичний дрейф
гіроскопів, Y – вектор – стовпець вихідних величин, H – матриця вимірювання.
Матриця F має вигляд:
(
)
де
(
)
(
)
(
)
(
) (
)
(
)
59
(
)
(
)
(
)
Отже рівняння (2.51) та (2.52) є основними рівняннями похибок БІНС, за
допомогою яких досліджуються основні характеристики точності БІНС.
Як приклад наведемо результат моделювання рівнянь (2.51) та (2.52) при
таких значеннях:
початкове місце положення - широта φ = 50.27°, довгота λ = 30.3° -
координати Києва;
зміщення нуля акселерометрів - g;
систематичний дрейф гіроскопів - 0.01
;
швидкість руху V = 200
;
час руху 4 години.
60
Рис. 2.18 Інтегрування рівнянь похибок БІНС
0 0.5 1 1.5 2 2.5 3 3.5 4-2000
0
2000
4000
6000
8000
Inertial State Space Error Modeling
po
sitio
n e
rro
r in
arc
-se
co
nd
s
lat error
lon error
0 0.5 1 1.5 2 2.5 3 3.5 4-200
-100
0
100
200
ve
locity e
rro
r in
m/s
dvx
dvy
0 0.5 1 1.5 2 2.5 3 3.5 4-5000
0
5000
10000
att
itu
de
err
or
in a
rc-s
eco
nd
s
time in hours
roll
pitch
yaw
61
ВИСНОВКИ ДО ДРУГОГО РОЗДІЛУ
1. Безплатформна інерціальна навігаційна система є автономною системою
орієнтації та навігації рухомого об’єкта.
2. Згідно із вимогами до режимів роботи та точносних характеристик БІНС
висувають певні вимоги до ЧЕ, які входять до складу БІНС.
3. Математична модель БІНС (спосіб описання) майже не впливає на
похибки БІНС при нормальному режимі роботи. Вибір того чи іншого
способу може бути продиктований практичними вимогами.
4. Дрейфи гіроскопів мають на багато більший вплив на точність
навігаційної системи, а ніж акселерометри, що в свою чергу підкреслює
важливість підвищення точності ІВМ для визначення орієнтації та
навігації рухомого об’єкта.
5. Для того аби зменшити дрейф гіроскопа збільшується маса самого
приладу, що впливає на вагові характеристики всієї системи та на
загальну вартість всієї системи.
6. У загальному випадку похибки БІНС мають степеневу залежність від
змінюваних параметрів кроку, амплітуди і частоти
7. Зі зменшенням кроку інтегрування похибки зменшуються
8. Рівняння похибок БІНС у формі простору станів дає можливість
аналізувати точність роботи всієї системи, дослідити вплив кожної
величини на точність усієї системи, а також при необхідності розглядати
різні способи корекції БІНС від різних систем, які мають різні, за
характером змінювання, похибки.
62
3 ОПТИКО – ЕЛЕКТРОННА СИСТЕМА ОРІЄНТАЦІЇ ТА НАВІГАЦІЇ
На сьогоднішній день оптико – електронні системи дуже швидко
розвиваються. Використання оптичних систем, як систем корекції БІНС є
доцільним та перспективним дослідженням оскільки в оптичних системах
відсутні такі недоліки, як затримка сигналу у часі (властиво СНС), відсутність
накопичення похибок з плином часу.
3.1 Опис та принцип роботи оптико – електронної системи орієнтації та
навігації.
На сьогоднішній день відомо два класи оптико – електронних систем які
можна реалізувати [5, 8, 16, 96]:
- Автоматизовані бортові системи обробки та розпізнавання об’єктів сцен
із участю людини – оператора;
- Автоматичні (автономні) бортові системи обробки та розпізнавання
зображень та об’єктів сцен.
У автоматизованих системах головним об’єктом при обробці інформації та
прийнятті рішення є людина – оператор. У таких випадках у якості еталонного
зображення використовується зображення, отримане за допомогою бортових
сенсорів ЛА [12, 16, 60, 72, 96].
У автоматичних (автономних) системах підготовка еталонних зображень
виконується заздалегідь, за допомогою спеціального центру підготовки
польотів [5, 12, 16, 60, 96].
У автоматичних системах формування еталонного зображення найбільш
доцільно використовувати геометричний підхід, який заснований на
формуванні символьної інтерпретації сцени за одним або декількома
зображеннями цієї сцени у вигляді тривимірної геометричної моделі [16, 96].
Формування та отримання тривимірної геометричної моделі можна подати
за допомогою такого процесу рис. 3.1 [12, 16]:
1 – й рівень – зображення;
2 – й рівень – двовимірне представлення (2D – опис);
3 – й рівень – тривимірні структури (3D – опис);
63
4 – й рівень – тривимірні геометричні моделі.
Рис. 3.1 Ілюстрація формування тривимірної геометричної моделі.
При побудові таких структур з двовимірних елементів підсумовуються
дані про роздільну здатність апаратури, знання про розглянутій області і дані,
отримані з зображення, що дозволяє отримати в результаті цього етапу
перетворень тривимірне так зване «розсіяне» контурне зображення сцени [16,
96, 97].
64
При побудові тривимірної геометричної моделі з набору тривимірних
структур використовується опис меж поверхонь, об'ємів [81, 86, 96, 97].
Цілком очевидно, що практична реалізація описаних, в загальному вигляді,
процесів перетворень і формування тривимірних геометричних моделей
наземних сцен допускає використання різних підходів і процедур на кожному
рівні [16, 60, 81, 86].
Такі відмінності грунтуються на різних особливостях конкретних сцени і
об'єктів, можливості ідентифікації їх характерних рис у двовимірних
зображеннях, складності та подробиці побудови тривимірних структур,
точностних характеристиках вихідної інформації та форми подання кінцевої
тривимірної геометричної моделі і т.д [16, 86, 96, 97].
Більшість таких процедур не може бути повною мірою формалізовано
строгими математичними описами і значною мірою базується на використанні
евристичних підходів та інтерактивних технологій з широким застосуванням
спеціалізованих програмних продуктів машинної графіки для представлення
тривимірних геометричних моделей реальних сцен і в кінцевій формі - у
вигляді цифрової моделі місцевості (ЦММ), що включає моделі рельєфу
місцевості та об'єктового складу розглянутої сцени [96, 97].
Крім методів побудови тривимірних геометричних моделей сцени за
одним або кількома зображень, можливі й інші підходи до формування
еталонних зображень і еталонних описів [16, 97].
3.2 Метод лідарної зйомки. Опис та принцип роботи. Характеристики
деяких оптико – електронних систем орієнтації та навігації та їх ЧЕ.
На сьогоднішній день одним з найпоширеніших методів отримання та
формування зображень є метод лідарної зйомки [16, 96, 97, 102].
Лідар (транслітерація LIDAR англ. LIght Identification Detection and
Ranging - світлове виявлення і визначення дальності) - технологія отримання та
обробки інформації про віддалених об'єктах за допомогою активних оптичних
систем, що використовують явища відбиття світла і його розсіювання в
прозорих і напівпрозорих середовищах [86, 96, 97, 102].
65
Лідар це активний оптичний сенсор, що випускає оптичні промені в бік
цілі під час руху транспортного засобу за спеціальними знімальним
маршрутами. Узагальнений процес лідарної зйомки зображений на рис. 3.2
[102].
Рис. 3.2 Узагальнений процес лідарної зйомки.
Відображення оптичного випромінювання від об'єкта приймається і
аналізується сенсором. Приймачі записують точний час, що минув з моменту
випускання оптичного імпульсу системою до моменту його повернення, для
того щоб обчислити відстань між сенсором і тілом [16, 54, 86, 102]. Спільно з
інформацією про внутрішнє і зовнішнє позиціонуванні (GPS і БІНС), ці відстані
перетворюються в тривимірні точки, що відображають поверхню відображення
оптичних імпульсів [54, 56, 86, 102].
Точкові дані піддаються подальшій обробці після збору з урахуванням
діапазону часу роботи лазера, кута сканування, координат GPS, інформації про
внутрішній позиціонуванні (БІНС), що дозволяє отримати точні координати
місцеположення x, y, z та просторові координати кутового положення ψ, ϑ, γ.
Дані лідарної зйомки являють собою набір даних, що містять хмари точок,
які можуть управлятися, відображатися, аналізуватися і спільно
використовуватися за допомогою спеціального програмного забезпечення.
Основою для створення топографічних карт і планів, цифрових моделей
рельєфу і місцевості, а також інших документів у паперовому або цифровому
вигляді є хмара точок і ортофотоплани в заданій системі координат, які
виходять в результаті виконання наступних технологічних процесів:
66
1. обробка даних СРНС / БІНС системи в диференціальному режимі з метою
отримання кінцевої траєкторії руху повітряного судна і кутів
орієнтування знімальної апаратури в кожен момент часу;
2. обчислення кутів неузгодженості скануючого блоку і
інерціальнійсистеми;
3. генерація кінцевого хмари точок у заданій системі координат і висот;
4. класифікація точок лазерних відображень;
5. обчислення кутів неузгодженості аерофотокамери і інерціальнійсистеми
шляхом побудови мережі фототріангуляції;
6. створення ортофотопланів;
7. створення кінцевих документів у цифровому вигляді.
Представлена послідовність процесів є загальною і може коригуватися в
залежності від моделі використовуваних лідаров, призначення лідарної зйомки,
необхідної точності та інформативності графічних документів і цифрових
моделей та інших факторів.
Розташування системи на об’єкті зображено на рис. 3.3:
Рис. 3.3 Розташування системи на об’єкті.
67
Приведемо характеристики та опис декількох систем та елементів, за
допомогою яких може здійснюватися оптична навігація та орієнтація, заснована
на лідарній зйомці.
1. Лазерний сканер для БПЛА Ultra – Ligth UAV LIDAR Scaning System
фірми YellowScan
Рис. 3.4 Легкий лазерний сканер для БПЛА
Ultra – Ligth UAV LIDAR Scaning System призначена для безпілотних
літальних апаратів та інших надлегких ЛА. Система має патарею для
автономного живляння. Час автономної роботи складає 2 години.
Технічні характеристики системи
1. Лазерний оптичний датчик с довжиною хвилі 905 нм;
2. Максимальна висота сканування 150 м
3. Розподільна здатність 4 см
4. Абсолютна точні по вісям XY: 0.1 м ±1%
5. Абсолютна точні по вісі Z: 0.1 м ±0,5%
6. Кутовий діапазон сканування 100° (±50°)
7. Частота сканування 80 кГц
8. Вага та габаритні розміри: 2.2 кг; 172мм x 206мм x 147мм
68
2. Повітряна лазерна скануючи система LMS-Q240i-60(-80) фірми RIEGL
Рис. 3.5 Повітряна лазерна скануючи система LMS-Q240i-60(-80)
Прилад призначений для роботи в якості датчика захисту від зіткнень для
безпілотних літальних апаратів, але також може бути успішно використаний
для наземних транспортних засобів і систем відеоспостереження. Він
заснований на вимірюванні часу польоту за допомогою коротких
інфрачервоних лазерних імпульсів і оптико-механічного сканування, щоб
забезпечити оперативні дані дальності і кута цілям в області-огляду сканера
(FOV). Прилад має тільки одине джерело живлення і забезпечує дискретні дані
лінії сканування, безпосередньо збережені в комп'ютерній системі, за
допомогою інтегрованого інтерфейсу локальної мережі TCP / IP. Потік
двійкових даних може бути легко декодований і оброблений з використанням
доступного програмного забезпечення бібліотеки. Механічна структура LMS-
Q240і-60(-80) дуже легка, але, з іншого боку досить міцна, щоб витримувати
ударне навантаження і вібрацію в наземних або повітряних транспортних
засобах.
Характеристики
Общие сведения
Производитель RIEGL
Модель LMS-Q240i-60(-80)
Тип инструмента воздушная лазерная сканирующая система
69
Сканирующая система
Высота сканирования (над
поверхностью), м 2-260 (-60), 2-230 (-80)
Лазер ближнего инфракрасного спектра, класс 1
Расхождение лазерного луча 2.7 мрад
Частота лазерного излучения,
кГц 30
Поле зрения 60 градусов (-60), 80 градусов (-80)
Скорость сканирования,
сканы/с 6-80 (-60), 5-60 (-80)
Угловое разрешение, градусы 0.005
Другие характеристики
системы
Интерфейсы TCP/IP Ethernet 10/100; RS232; ECP
Монтаж компонентов резьбовые вставки М6 и М8
Физические и
эксплуатационные параметры
Питание постоянный ток 18-32 В; 1.8 А при 24 В
Соответствие стандартам
защищённости
электрооборудования
IP64
Условия окружающей среды рабочая температура: -10°C ... +50°C;
температура хранения: -20°C ... +60°C
Размеры компонентов 180 х 374 х 180 мм
Массы компонентов 7 кг
70
3. Оптико – інерціальна безплатформна курсовертикаль компанії Inertial
Labs, USA
Рис. 3.6 Оптико – інерціальна безплатформна курсовертикаль компанії
Inertial Labs, USA
Оптико – інерціальна безплатформна курсовертикаль компанії Inertial
Labs, USA призначена для визначення площини горизонту за допомогою
мікромеханічних датчиків та визначення курсу за допомогою оптичного
сенсора. Дана система може використовуватися на малих безпілотних ЛА та
для невеликих знарядь залпового вогню.
Параметр Одиниці виміру Додаткова інформація
Вихідний сигнал Висота, Курс, Тангаж,
Крен, Азимут
Частота оновлення Гц 100
Час готовності сек 10
Діапазон град 0 ÷ 360
Кутова резолюція Град 0.05
Точність град 0.2
Шум (100 Гц) Гц 0.02
71
Вимірювання тангажу та
крену (БКВ)
град 0 ÷ 360
Вимірювання тангажу та
крену (оптична частина)
Град ±22.5
Діапазон виміру
гіроскопів
Град/сек ±300
Дрейф гіроскопа Град/сек 0.02
Шум Гіроскопів Град/(сек√ ) 0.035
Смуга пропускання Гц 50
Діапазон вимірювання
акселерометрів
g ±2
Зміщення нуля
акселерометра
mg 0.05
Шум акселерометрів 0.04
Смуга пропускання Гц 50
Напруга живлення В (постійний струм) 12 ÷ 36
Споживча потужність Вт 3.5
Роз’єм підключення Стандарт USB
Передача даних Мбіт/сек 480
Габаритні розміри мм 132.3 × 59.5 × 47.5
Вага Грам 400 (w/o camera cap,
splitter box, and
cable); 600 (full set)
72
4. Цифрова аерофотокамера Rollei AIC
Рис. 3.7 Цифрова аерофотокамера AIC
Камера призначена для формування початкових знімків місцевості та
безпосередній зйомці місцевості при роботі оптико – електронної системи
орієнтації та навігації.
Камера має мінімум механічних частин, висока надійність. Корпус
виготовлений з алюмінію, стандартні Т-подібні прорізи забезпечують просте,
але жорстке кріплення. Користувач може вибирати з широкого діапазону
об'єктивів Rollei System 6000 і Schneider KREUZNACH з вбудованим
пелюстковим затвором. Інтерфейс RS 232 для управління камерою зі
стандартного комп'ютера або спеціалізованої комп'ютерної системи.
Високошвидкісний інтерфейс отримання цифрових знімків - IEEE 1394.
Характеристики
Общие сведения
Тип фотокамеры цифровая аэрофотокамера
Производитель Rollei
Модель AIC
Спецификации получаемых
изображений
Размер кадра 5440 x 4080 (22 Мп)
73
Форматы данных 8 и 16-бит TIFF
Камера
ПЗС Phase One H25 CCD-сенсор
Размер точки 9 мкм
Объективы 2 Super Aglulon 50 мм
Диафрагма f/2.8
Затвор электронно контролируемый лепестковый
затвор, скорость затвора 125-1000 сек
Диапазон выдержек максимум 4 сек
Периферийные устройства
Накопление и обработка
данных
внутренний накопитель: защищённый RAID
320Гб; заменяемый накопитель: 320 Гб
Управление интерфейсы: Ethernet, TTL, RS-232C
Эксплуатация
Электропитание камера: 15-18 В; управляющий компьютер: 300
Вт/28 В
Условия окружающей
среды
рабочая температура: –10 °C ... +40 °C;
влажность: 5 ... 90%
Размеры и массы
Размеры компонентов камера: 150 х 150 х 250 мм; управляющий
компьютер: 146 х 483 х 406 мм
Массы компонентов камера: 3.6 кг; управляющий компьютер: 7 кг
Дополнительная
информация
Сайт производителя http://www.optech.ca
74
5. Лазерний сканер LUX 8L(prototype) фірми ibeo
Рис. 3.8 Лазерний сканер LUX 8L(prototype)
Система призначена для БПЛА та використовується як лазерний сканер
поверхні. Програмний комплекс,з отриманої інформації від сканера, дозволяє
будувати 3D моделі та використовувати їх у якості еталонних зображень для
визначення орієнтації та навігації БПЛА.
Технічні характеристики
Лазер:
- Клас лазера – Клас 1, безпечний для зору;
- Довжина хвилі – 905нм;
- Діапазон дії – до 1км (середня ціль, нульовий кут напрямку бачення)
Вимірювання:
- Горизонтальна площина – 110° (від 50° до -60°)
- Вертикальна площина - 6.4° (0° - напрямок польоту)
- Багатошаровість знімків – 8 шарів (2 пари по 4 шари)
- Багатовідлунність - до 3 вимірювань за один імпульс (при вимірюванні
через атмосферну забрудненість, дощ, або пил)
- Частота оновлення даних: 6.25 / 12.5 / 25 Гц, дійсні для обох пар
альтернативних шарів сканування.
75
- Діапазон робочих температури – від -40°С до +80°С;
- Точність – 10см / 3.9 дюйми
- Горизонтальна кутова роздільна здатність – до 0.125°;
- Вертикальна кутова роздільна здатність – до 0.8°;
- Роздільна здатність за відстанню – см/1.57 дюйми
Програмне забезпечення
- обробки первинних даних: Всі вимірювання будуть класифіковані і
помічені дійсними / земля / безлад (доступно 6.25 Гц тільки)
- в режимі реального часу об'єкт відстеження: Властивості об'єкта:
положення, розмір, швидкість(Доступно 6.25 Гц тільки)
- компенсація руху Его: необхідні дані руху автомобіля его через CAN
Механічні розміри та живлення
- Живлення: 9 до 27 В
- Потужність: 8 Вт (в середньому), <10 Вт (макс)
- Розміри (Ш х Д х В): 164,5 х 93,2 х 88 mm³
- Клас захисту: IP 69 К (МЕК 60529, DIN 40050-9 (з використанням
захищених роз'ємів підключаються)) IP 68 (IEC 60529 (2 м, 24 год))
Інтерфейс передачі даних
- 100 Мбіт Ethernet: Вихід: Raw- і об'єкти даних
- Вхід: Конфігурація / синхронізації часу через сервер NTP
- CAN: Вихід: дані Об'єкт, вхід: дані Его рух
- RS232: Синхронізація
Приведемо деякі характеристики оптичних систем російської фірми
«Перегам». Оптичні системи фірми «Перегам» призначені для маневрових ЛА
та гелікоптерів спеціального призначення. Данні системи засновані на
оптичному принципі та дозволяють визначати швидкість руху об’єкта,
місцеположення, кутове положення та висоту польоту.
Характеристики цих систем наведені у таблиці 3.1:
76
Таблиця 3.1 Основні характеристики лазерних бортових сканерів фірми
«Перегам»
Модель
сканера
LMS-Q160 LMS-Q240J VQ-480 LMS-
Q560
LMS-
Q680
Максимальная
дальность
измерений, м
до 60;
до 200
320;
650
350 −
500;
600 −
900
580 −
1200;
580 −
1800
1250 −
2000;
2000 −
3000
Минимальная
дальность, м
2 10 30
Погрешность,
мм
20 25 20
Точность, мм 15 25 20
Эффективная
скорость
измерений
(округлен-
ные величины),
изм./с
10000 10000 (25 –
100)*103
до 24*104
120 -
160)*103
Разрешение при
наличии
нескольких
целей, м
8
Расходимость
пучка, мрад.
2.7 ≤0,5
Диапазон углов
сканирования,
град.
±40 – 80 ±30 – 60;
±40 – 80
±30 – 60 ±22,5 –
45;
±30 – 60
±22,5 –
45;
±30 – 60
Скорость
сканирования,
разверток/с
5 − 60 6 − 80; 5 − 60 10 − 100 10 − 160 10 − 200
Угловое
разрешение,
град.
0,01
0,005 0,001
Число
измерений на
одну линию
сканирования
500 при 20
развертках/с;
1000 при 10
развертках/с
6 − 8 разверток/с;
5 − 60 разверток/с
10 − 100
разверто
к/с
100 − 160
разверток/
с
Интерфейс
TCP/IP/Ethernet,1
0/100 Мбит/с
TCP/IP/Ethernet, 10/100
Мбит/с, RS232, 19,2
кбит/с, ЕСР (порт с
расширенными
возможностями),паралле
льный
RS232, вход TTL
для импульса синхронизации
Энергопотребле
ние, Вт
при питании от
=18 − 32 В
48 (типично) 65
(типично
)
Ток
потребления, А
2.0 типично при –
24 В
~1,8 при –24 В ~5 при –
24 В
Габариты, мм ∅170х350 ∅180х374 ∅180х34
5
420х212х
228
420х212х
230
Масса, кг ~4,6 ~7 11,5 16 17,5
77
Диапазон
рабочих
температур,°С
−10…+50 −10…+4
0
0…+40
Частота
повторения
лазерных
импульсов
(округленные
величины), Гц
30 000 до 200 240 000
Типичная
высота полета
над уровнем
земли, м
280 −
450;
рабочая
высота
до 5000
450 −
1000
1000 −
1600
Величина шага
изменения угла
луча Δ между
последовательн
ыми замерами,
град.
≥0,04 0,06 ≤ Δ ≥0,004
3.3 Математичні основи оптико – електронних систем орієнтації та
навігації
Математичне обґрунтування оптико – електронної системи орієнтації та
навігації в основному базується на принципі доплерівських та радіолокаційних
вимірювачів [16, 31, 34, 60, 102, 103, 104, 109]. Прешочерговим кроком
оптичного вимірювача є визначення відстані до поверхні, де якій знаходиться
об’єкт. Залежно від цієї відстані залежить розмір оптичної «плями», яка несе в
собі інформацію про поточне місцеположення об’єкта та його орієнтацію у
просторі. Також оптична «пляма» залежить від кута під яким скануються
поверхня.
Рис. 3.9 Розмір оптичної «плями» на поверхні.
78
Розмір оптичної «плями» може визначатися за формулою [71, 102, 109]:
( )
де h – висота сканера над поверхнею у метрах, γ – розходимість (розсіяння)
лазерного рис 3.8(а). Якщо враховувати що значення D набагато менше висоти
h рис. 3.8(б), тоді (3.1) матиме такий вигляд
Для загального випадку рис.3.8(в) вираз (3.1) має такий вигляд:
(
)
( )
де ( ) ( )
, i – кут
нахилу ділянки місцевості в площині, перпендикулярній напрямку руху об’єкта
[102, 103].
Мінімальна висота судна над поверхнею задається виходячи з необхідної
щільності сканування, розміру пікселя фотознімків на місцевості, а також
діючих норм руху судна над цією місцевістю і мінімальною відстанню
безпечного використання оптичного сканера [54, 102, 103, 109].
Згідно з рис. 3.3 та рис. 3.8 на ЛА можна розташувати оптичні сенсори, які
забезпечують сканування простору строковим способом. Спеціальні приймачі,
які приймають відображений сигнал формують три сигнали кутового
положення α1, α2, α3 та три сигнали лінійного положення , в
географічній системі координат. Методами аналітичної та сферичної геометрії
отримується система рівнянь, розв’язок якої дає інформацію про поточні
орієнтацію та поточне місцеположення об’єкта [5, 12, 54, 56, 60, 72, 97]:
( ) ( )
( ) ( )
(3.3)
79
де – шукані величини координат поточного місцеположення сенсора, а
відповідно і об’єкта; ψ, ϑ, γ – шукані кутові координати поточної орієнтації
об’єкта; – початкові координати розташування та стоянки об’єкта; α1,
α2, α3, – вихідні сигнали оптичних сенсорів, які несуть в собі
інформацію про поточне місцеположення та просторову орієнтацію об’єкта [5,
12, 54, 60].
Систему (3.3) можна привести до системи трьох рівнянь, склавши вирази з
однаковими вихідними сигналами оптичних сенсорів:
( )
( ) ( )
( )
де – елементи матриці напрямних косинусів.
Оскільки система (3.4) є неперервною системою то для легшого
обчислення її перетворюють у дискретну систему. Використовуючи відомі
перетворення МНК до кватерніону положення зображення подається у вигляді
кватерніону поворота, тоді координати поточного місцеположення, згідно із
зображенням можна записати у такому вигляді [5, 72, 97, 102]:
[
( ( ) )
( ( ) )
( ( ) )
( ( ) )
]
[
]
де q – кватерніон повороту, – початкові координати, r – поточні координати,
– кількість змінних, які необхідно знайти.
Згідно з (3.5) можемо отримати узагальнене диференціальне рівняння [66,
72, 102]:
Розв’язок рівняння (3.6) дає загальну функцію, яка залежить від
місцеположення об’єкта, швидкості руху та просторового положення, поданого
у вигляді кватерніону повороту Λ=Λ( ). Далі з
80
отриманого розв’язку знаходимо кути які характеризують поточну просторову
орієнтацію:
(
(
))
( ( ))
(
(
))
Координати місцеположення визначаються за такими виразами:
(
∬
)
де С – матриця напрямних косинусів визначена за допомогою (3.6), Δt – час
знімання і-тої ділянки, – прискорення з яким рухається об’єкт, – початкова
швидкість руху, – прискорення руху зображення.
Загальній алгоритм визначення орієнтації та навігації за допомогою оптичних
сенсорів [5, 12, 72, 97, 102].
81
Рис. 3.10 Загальний алгоритм визначення орієнтації та навігації за допомогою
оптичних сенсорів.
82
ВИСНОВКИ ДО ТРЕТЬОГО РОЗДІЛУ
1. Оптико – електронні системи орієнтації та навігації засновані на
технології машинного зору, яка потребує попереднього формування
еталонних зображень.
2. Визначення поточних параметрів орієнтації та навігації може бути
здійснена або людиною оператором або автоматично – спеціальним
програмним комплексом.
3. Сучасні оптико – електронні системи мають високу розподільну
здатність, високу точність визначення параметрів навігації та параметрів
орієнтації, також мають малі габарити та вагу.
4. Математичне обґрунтування роботи оптико – електронної системи та
алгоритми роботи базуються на відомих принципах роботи радіо- та
доплерівсько – навігаційних систем, та можуть уточнюватися згідно із
призначення ЛА.
5. Оптико – електронні системи орієнтації та навігації можуть
використовуватися як основна або допоміжна система навігації та
орієнтації рухомого об’єкта.
83
4 КОРЕКЦІЯ БЕЗПЛАТФОРМНОЇ ІНЕРЦІАЛЬНОЇ НАВІГАЦІЙНОЇ
СИСТЕМИ. РЕЗУЛЬТАТИ МОДЕЛЮВАННЯ БІНС ТА ОЕСОН.
Як показує теорія, практика та досвід експлуатації, основним недоліком
БІНС, як і інших електромеханічних систем – зростання похибок у часі [14, 15,
16, 35, 72, 95]. Зменшення похибок за рахунок вдосконалення та покрашення
характеристик ЧЕ пов’язане з великими труднощами та збільшенням витрат
[35, 95]. Зменшення зростання похибок у часі та підвищення точності може
бути досягнуто за допомогою корегування БІНС від інших навігаційних систем,
які мають високі точносні характеристики [71, 72, 90, 91, 95]. Такими
системами можуть бути доплерівські системи, супутникові навігаційні системи
та оптико – електронні системи орієнтації та навігації [90, 95].
При інтегруванні БІНС та оптико – електронної системи можуть
використовуватися різні способи. Найбільш поширеним є спосіб компенсації
[35, 50, 72].
Рис. 4.1 Структурна схема комплексування за компенсаційним способом
Зазвичай, у схемі комплексування за компенсаційним способом,
використовується фільтр Калмана [35, 50, 72, 91, 95]. Вхідними величинами
фільтра є різниця вихідних координат навігаційної системи 1 та навігаційної
системи 2. Ця різниця подається у вигляді простору станів (2.51) та (2.52),
тільки з однією відмінністю – рівняння простору станів отримане шляхом
віднімання вихідних величин навігаційних систем [35, 50, 60, 72, 95].
84
Згідно з рис. 4.1 функціональна схема може бути реалізована таким
способом [41, 72, 75].
Рис. 4.2 Функціональна схема комплексування БІНС та ОЕСОН.
Різниця похибок, яка проходить через фільтр Калмана, зменшує похибки
вихідного сигналу різниці двох навігаційних систем [60, 67, 68, 70, 71]. Оцінки
помилок використовуються для корекції кутів орієнтації, обчислених за
сигналами ДКШ, і для демпфірування помилок БКВ. Вихідний сигнал фільтра
Калмана віднімається від реальних значень БІНС і на виході ми отримаємо
скореговані та дійсні значення навігаційних параметрів, які характеризують
місцеположення та просторову орієнтацію об’єкта [60, 70, 71, 91, 95].
4.1 Синтез фільтра Калмана комплексної системи орієнтації та навігації.
З теоретичної точки зору, фільтр Калмана є тим, що називається лінійно-
квадратичною проблемою, якою є задача оцінки миттєвого значення лінійної
динамічної системи збудженої білим шумом з вимірювань, які лінійно зв’язані
з станом системи, але є пошкодженими білим шумом [29, 70, 77, 91, 95].
Отримана оцінка, являє собою статично оптимальну по відношенню до будь
якої квадратичної функції похибки оцінки [29, 91, 95].
85
З практичної точки зору, фільтр Калмана являється одним із великих
відкриттів в історії статичних теорій оцінювання. Найбільшого використання
фільтр набув в управлінні складних динамічних систем, такі як неперервні
процеси виробництва, в літаках, кораблях та космічних апаратах [70, 71, 73, 95].
При використанні фільтра Калмана динамічна система повинна бути
представлена у вигляді моделі динаміки і моделі вимірювання, відповідно до
структури фільтру [29, 70, 73]. Для кожного такту k роботи фільтру необхідно
у відповідності до наведеного нижче опису визначити матрицю еволюції
процесу k , матрицю спостерігання kH , коваріаційну матрицю процесу kQ ,
коваріаційну матрицю шуму вимірювань kR ; при наявності керуючих впливів –
матрицю їх коефіцієнтів kB [29, 66, 70, 73].
Модель процесу передбачає, що істинне значення у момент k визначається
з істинного стану у момент ( 1)k у відповідності до рівняння [29, 66, 70, 73]:
1k k k k k kB ux x w , (4.1)
де ku — вектор детермінованих керуючих впливів, kw — нормальний
випадковий процес з нульовим математичним очікуванням та коваріаційною
матрицею kQ , яка потребує визначення дисперсії процесу [29, 70, 95].
Результат спостереження (вимірювання) ky істинного вектора стану kx у
момент k зв’язан з ним рівнянням:
k k k kH xy v , (4.2)
де kv — нормальний випадковий процес з нульовим математичним
очікуванням і коваріаційною матрицею kR , описуючий білий шум у
результатах вимірювання [29, 70].
Початковий стан та вектори випадкових процесів на кожному етапі тракту
0 1 1 , , , , ,k k x w w v v вважаються незалежними.
В загальному випадку фільтр Калмана працює в два етапи: прогноз і
корекцію [70, 73]. Оцінка фазового стану системи, яка була отримана на
попередньому кроці роботи 1ˆ kx розповсюджується на поточний крок k у
вигляді прогнозу оцінки ˆ k
x відповідно до моделі динаміки системи (4.1). Після
86
цього, отримана оцінка та вимірювання, які були отримані на даному кроці, ky
використовують для отримання відкорегованої оцінки стану системи ˆ k
x [70].
При цьому прогнозування можу відбуватися безліч разів між двома
корекціями. Це використовується для розповсюджування фазового стану, коли
вимірювання за якимось причинами виявляються недоступними або
недостовірними [9, 16, 70, 73]. Також це дозволяє вимірюванням поступати
асинхронно по відношенню до роботи фільтра [29, 70, 73].
Похибка оцінки стану системи описується в термінах коваріаційної
матриці похибки [29, 70, 73]:
ˆ ˆ( ) [ ][ ]Tk k k kP t E x x x x (4.3)
де E x це математичне очікування.
Робота фільтру починається з ініціалізації оцінки вектора стану і
коваріаційної матриці похибки [29, 70, 73, 95]:
0 0ˆ( )t x x (4.4)
0 0 0 0 0ˆ ˆ[ ( ) ( )][ ( ) ( )]TP E t t t t x x x x (4.5)
Далі, відбувається розповсюдження оцінки стану і матриці коваріацій
відповідно до моделі (4.1) для отримання некоректованої оцінки [29, 70, 95]:
1 1 1 1ˆ ˆk k k k kB x x u (4.6)
1 1 1 1T
k k k k kP P Q (4.7)
На наступному кроці визначаються коефіцієнт підсилення (підсилення
Калмана) kK та відбувається корекція оцінки з урахуванням отриманих
вимірювань ky [29, 70, 73, 95]:
1[ ]T Tk k k k k k kK P H H P H R (4.8)
ˆ ˆ ˆ[ ]k k k k k kK H x x y x (4.9)
[ ]k k k kP I K H P (4.10)
Описаний фільтр Калмана є орієнтований на вирішення лінійних моделей,
але існує декілька різновидів фільтру, які мають за мету вирішення нелінійних
87
задач. Найбільш розповсюдженим серед них є розширений фільтр Калмана. По
своїй структурі він схожий на лінійну версію, але має відмінність у тому, що в
рівняннях динаміки присутні нелінійні функції [29, 70, 73, 95].
4.2 Алгоритм дослідження та результати моделювання
У загальному випадку рівняння роботи БІНС можна подати у формі Коші
векторного вигляду [14, 15, 36, 60, 66, 67, 85, 95, 99]:
де
- місцеположення та швидкість об'єкта відносно земної системи
координат, - матриця напрямних косинусів між земною СК та зв'язаною з
об'єктом СК, - показання акселерометра, - вектор кутової швидкості
обертання Землі, - вектор прискорення сили тяжіння, - кутова швидкість
отримана з гіроскопів [60, 66, 67, 85, 95, 99].
Рис. 4.3 Модель площини роботи оптичного сенсора
Математично, згідно з рис.4.3, рівняння роботи оптичного сенсора можна
подати у такому вигляді [12, 34, 96, 97, 99, 100]:
(
)
88
де
- радіус-вектор місцеположення об'єкта,
- радіус-вектор
місцеположення площини (ділянки) знімка, - радіус-вектор місцеположення
центра оптичного сенсора, - матриця повороту між земною СК та системою
координат пов'язаною з оптичним сенсором. Скалярне вираження може мати
такий вигляд [12, 34, 60, 66, 99, 100]:
[
] [
]
де - отримані координати про місцеположення та швидкість об'єкта з
оптичного сенсора, - еталонні координати та швидкість, - фокальна
довжина [12, 34, 60, 66, 99].
Згідно з (4.12) та (4.13) рівняння вимірювання оптичного сенсора
записуються у такому вигляді:
[
] [
]
[
]
[
]
де [
] - шум вимірювача.
Розташування оптичної системи відносно БІНС може бути киконано таким
чином [5, 12, 34, 60, 66, 99, 102]:
Рис. 4.4 варіант розташування ОЕСОН відносно БІНС на об'єкті
89
Згідно з рис. 4.4 можна вивести рівняння похибок комплексної системи,
яке буде застосовуватися для реалізації фільтрації по Калману. Рівняння
похибок БІНС, у загальному випадку можна представити у такому вигляді:
де - вектор похибки визначення кутів орієнтації, - вектор похибки
визначення швидкості об'єкта, - вектор похибки визначення поточного
місцеположення об'єкта, - похибки визначення прискорення сили тяжіння,
- похибки ЧЕ БІНС.
Рівняння похибок оптико - електронної системи можна представити у
такому вигляді:
де - радіус-вектор місцеположення центра оптичного сенсора відносно
земної системи координат, - місцеположення БІНС відносно земної системи
координат, - вектор різниці між БІНС та оптико - електронною системою у
системі координат зв'язаної з об'єктом.
Рівняння (4.12) може бути представлено у такому вигляді:
(
)
де - матриця повороту між системою координат зв'язаною з камерою та
системою координат зв'язаною з об'єктом, яка визначається шляхом
калібрування системи.
Підставляючи рівняння (4.16) у рівняння (4.17) отримаємо нове рівняння,
яке пов'язує значення показань БІНС та оптико - електронної системи:
(
(
)
)
Лінійна модель може бути представлена у двокроковому процесі. Перший
крок - визначення місцеположення об'єкта за даним з оптико-електронної
системи. Відповідно до рівняння (4.14) визначення місцеположення об'єкта, за
даними від оптико - електронної системи, можна записати у такому вигляді:
90
[
]
Другий крок - визначення похибки орієнтації та місцеположення об'єкта за
даними від БІНС:
[
] [
] [
]
Підставивши рівняння (4.10) у (4.9) отримаємо рівняння вимірювання
комплексної системи:
[
]
Рівняння (4.21) може використовуватися, рівняння вимірювання для
формування вихідних величин фільтра Калмана.
Згідно з (4.15) та (4.21) продемонструємо результати моделювання
комплексної навігаційної системи.
Моделювання проводилося при таких параметрах:
- Швидкість руху об’єкта – V = 200 м/с = 720 км/год;
- Початкові координати місцеположення – координати Києва:
Широта φ = 50.21°;
Довгота λ = 30.3°;
- Зміщення нуля акселерометра - 10-4
g, g = 9.8
- Дрейф гіроскопа – 0.05 град/год = 2.4241 рад/сек;
91
Рис. 4.5 Похибки визначення швидкості руху об’єкта
Рис. 4.6 Похибки визначення широти та довготи
Графіки зображені на рис. 4.5 та рис. 4.6 демонструють похибки
визначення швидкості руху об’єкта та поточних координат місцеположення
0 5 10 15 20-3
-2
-1
0
1
2
3
4
Correction SINS from OESON
ve
locity e
rro
r in
m/s
time in minutes
east unaided
north unaided
east SINS vs OESON
north SINS vs OESON
0 5 10 15 20-40
-35
-30
-25
-20
-15
-10
-5
0
5
Correction SINS from OESON
err
or
in a
rc-s
econd
time in minutes
lat err unaided
long err unaided
lat err SINS vs OESON
lon err SINS vs OESON
92
нескорегованої БІНС (синя та зелена лінії) та скорегованої БІНС від ОЕСОН із
застосуванням філтра Калмана (червона та бірюзова лінії).
Рис. 4.7 Похибки визначення кутів крену та тангажу.
Графік на рис. 4.7 демонструє похибки визначення кутів крену та тангажу
за допомоги ОЕСОН та фільтра Калмана. На перших хвилинах роботи система
проходить самовизначення а потім робить зменшення похибки визначення
якомога мінімальним.
0 5 10 15 20-6
-5
-4
-3
-2
-1
0
1
2
3
4
Euler Angle Errors Correction SINS from OESON
err
or
in d
egre
es
time in minutes
roll unaided
pitch unaided
roll SINS vs OESON
pitch SINS vs OESON
93
Рис. 4.8 Похибка визначення кута курсу
Нижче зображені похибки оцінювання визначення швидкості руху
об’єкта, поточного місцеположення об’єкта та похибки оцінювання визначення
кутів крену, тангажу та курсу.
Рис. 4.9 Похибки оцінювання визначення швидкості руху об’єкта
0 5 10 15 20-1
-0.5
0
0.5
1
1.5
Euler Angle Errors Correction SINS from OESON
ya
w e
rro
r in
de
gre
es
time in minutes
yaw unaided
yaw SINS vs OESON
0 5 10 15 20 25 30 35 40 45-1
0
1
2
Estimate Velocity Error in Meters/Second
east
0 5 10 15 20 25 30 35 40 45-1
0
1
nort
h
0 5 10 15 20 25 30 35 40 45-1
-0.5
0
0.5
up
time in minutes
94
Рис. 4.10 Похибки оцінювання визначення місцеположення об’єкта
Рис. 4.11 Похибки оцінювання визначення кутів крену та тангажу об’єкта
0 5 10 15 20 25 30 35 40 45-20
0
20
East/North/Up Estimate Position Errors in Meters
east
0 5 10 15 20 25 30 35 40 45-20
-10
0
10nort
h
0 5 10 15 20 25 30 35 40 45-5
0
5
up
time in minutes
0 5 10 15 20 25 30 35 40 45-300
-200
-100
0
100
200
Attitude/PSI Estimation Error in Arc-seconds
roll
0 5 10 15 20 25 30 35 40 45-200
-100
0
100
200
pitch
time in minutes
95
Рис. 4.12 Похибка оцінювання визначення кута курсу.
Також продемонструємо результати досліджень закордонних компаній, які
займаються проектуванням та розробкою інтегрованих систем орієнтації та
навігації.
У роботі [60] наведено продемонстровано один із варіантів побудови
алгоритму комплексування БІНС та ОЕСОН.
У роботі [66] продемонстровано роботу одного з перших прототипів
комплексної системи орієнтації та навігації. Приведені результати оцінювання
визначення параметрів навігації та орієнтації.
0 5 10 15 20 25 30 35 40 45-1
0
1
2
3
4
5
6
Heading/PSI Estimation Error in Arc-minutes
ya
w
time, minutes
96
Рис. 4.13 Похибки оцінювання місцеположення.
Рис. 4.14 Похибки оцінювання орієнтації
На рис. 4.13 та рис. 4.14 продемонстровано результати моделювання
системи описаній у роботі [66]. Сині лінії відповідають діапазону бажаної
97
точності визначення місцеположення та орієнтації системи, червона лінія –
результат випробовування прототипу системи.
У роботі [99] досліджувалася робота першого прототипу інтегрованої
інерціально – оптичної системи орієнтації та навігації. У ході дослідження були
отримані такі результати похибок оцінювання визначення навігаційних
параметрів.
Рис. 4.15Похибки оцінювання визначення навігаційних параметрів
98
ВИСНОВКИ ДО ЧЕТВЕРТОГО РОЗДІЛУ
1. Найбільш поширеною схемою корекції БІНС є схема компенсації, яка
використовується у більшості навігаційних комплексах.
2. Для найкращого оцінювання та зменшення похибок використовують
фільтр Калмана. Використання фільтра Калмана, в даній роботі,
дозволило розробити адекватну модель схеми корекції БІНС від оптико –
електронної системи орієнтації та навігації і є оправдане.
3. Використання рівняння похибок у вигляді простору станів дозволяє
проводити комплексування БІНС з різними системами, наприклад з
оптико – електронною системою орієнтації та навігації. Корекція БІНС
від ОЕСОН зменшує, накопичені у часі, похибки БІНС.
4. Результати моделювання показують, що на комплексну систему
практично не діють зовнішні збурюючі фактори, які перешкоджають
визначенню швидкості та поточного місцеположення об’єкта;
визначення, оптичною системою, кутів орієнтації, займає приблизно 5
хвилин, але після визначення поточних кутів система визначає ці кути з
похибкою, яка не перевищую ±0.5° градуса.
5. Похибки оцінювання визначення кутів орієнтації не перевищують 180
кутових секунд.
6. Моделювання похибок оцінювання визначення швидкості руху об’єкта та
поточного місцеположення об’єкта показує, що вихідні величини не
перевищують 2 м/с за визначення швидкості та 10 м за визначення
місцеположення.
99
5. ОХОРОНА ПРАЦІ ТА БЕЗПЕКА У НАДЗВИЧАЙНИХ СИТУАЦІЯХ
5.1 Аналіз умов праці і робочого місця
Розглядатимемо питання охорони праці на прикладі конструкторського
бюро «ЕЛЕКТРОНПРИЛАД» (КБ), у якому і проводяться дослідницькі роботи.
У приміщенні конструкторського бюро розташовано 11 робочих місць (10
наукових співробітників, 1 начальник). Обчислимо площу й обсяг вільного
простору, що приходить на одного співробітника [20]:
27м11
108
n
baSc , (5.1)
3м 5,245,37 hSV cc , (5.2)
де Sc – площа, яка приходиться на одного співробітника; n – кількість робочих
місць; Vс – обсяг вільного простору, що приходиться на одного співробітника.
Розміри приміщення показано на рисунку 5.1.
Рисунок 5.1 Розміри приміщення
1– вікна; 2 – шафа для документів та обладнання; 3 – стіл письмовий
(12шт.). Висота – h = 3,5 м; ширина – a = 8 м; довжина – b = 10 м.
100
Згідно СН 245-71 норми площі вільного простору для конструкторських
приміщень і лабораторій на одну людину складає відповідно Sc =6м2 і Vс=21 м
3.
Отже приміщення задовольняє вимогам СН 245-71.
При зборці даного приладу, а саме блоку гіроскопів і акселерометрів та
оптичного сенсора , навантаження зосереджене на сам процес та налаштування
приладу на потрібні умови дослідницької роботи [20, 21, 22, 23].
Санітарно-гігієнічна обстановка, що визначає умови праці,
характеризується метеорологічними параметрами, такими як наявність
вібрацій.
5.1.1 Вібрації
Несприятливо на здоров'я людини діють вібрації, причиною яких є
неврівноважений силовий вплив, що виникає при роботі машин і станків.
Вплив вібрації не тільки погіршує самопочуття працюючого і знижує
продуктивність праці, але часто наводить до важкого профзахворювання –
віброхвороби [44, 45].
5.2 Аналіз небезпечних та шкідливих чинників на виробництві
Згідно ГОСТ 12.0.003-74, при роботі в КБ працівник може піддатися таким
небезпечним та шкідливим чинникам:
‒ небезпека ураження електричним струмом;
‒ підвищений рівень утворення пожежі в приміщені;
‒ незадовільні параметри мікроклімату;
‒ електромагнітне випромінювання;
‒ несприятлива освітленість;
‒ підвищений рівень шуму [20, 44, 45].
5.3 Аналіз можливих уражень струмом при експлуатації устаткування
Усі приміщення поділяються по ступені ураження людей електричним
струмом на 3 класи:
‒ без підвищеної небезпеки;
‒ з підвищеною небезпекою;
‒ особливо небезпечні.
101
У КБ є обчислювальні машини, які працюють від мережі 220В.
Випадки ураження людини струмом можливі лише при замиканні
електричного кола через тіло людини не менш чим до двох точок кола, між
якими існує деяка напруга. Небезпека такого включення залежить від ряду
факторів: значення струму, схеми включення людини в коло, напругу в колі,
схеми кола, ізоляції. Схеми включення людини в електричне коло можуть бути
різними. Найбільш характерними є дві схеми: між двома проводами і між
одним проводом і землею. Двофазне включення, як правило, більш небезпечне,
оскільки до тіла людини подається найбільша в даній мережі напруга – лінійна,
і тому через тіло людини піде великий струм.
, (5.3)
де Iпр – струм, який проходить через людину, А; UTM – фазна напруга, V; Rr –
опір людини, Ом, у середньому дорівнює 2 кОм;
Розрахуємо струм, який пройде через людину, якщо вона замкне на собі
220В. Згідно з (5.3) маємо:
Такий струм э небезпечним для життя людини. Для попередження
враження електричним струмом проводиться спеціальний комплекс заходів.
Комплекс заходів щодо забезпечення електробезпеки.
Основними заходами від ураження електричним струмом є:
‒ забезпечення недоступності провідників, що знаходяться під
напругою, від випадкового дотику;
‒ електричний поділ мережі;
‒ усунення небезпеки ураження з появою напруги на корпусах, в
кожухах та ін. частинах електроустаткування, що досягається
застосуванням малих напруг, використанням подвійної ізоляції, захисним
зануленням, захисним відключенням та ін.
rТМпр RUI /73,1
102
Доцільним є застосування занулення мережі. Занулення – це навмисне
з'єднання з нульовим захисним проводом металевих струмоведучих частин, що
можуть виявитися під напругою [44, 45].
5.4 Аналіз пожежної безпеки приміщення
У приміщенні конструкторського бюро є: меблі, книги, папір і т.д. їхнє
загорання може послужити причиною пожежі.
Відповідно до СНіП 11-2-80 усі виробництва поділяють на пожежо-, і
вибухонебезпечної категорії. Дане КБ відноситься до категорії Д. Приміщення
побудоване з цегли, що підвищує вогнестійкість будинку. У якості пожежного
оповіщувача в КБ застосовується димовий оповіщувач ГИП-1, включений у
загальну мережу пожежної сигналізації будинку. У приміщенні знаходяться
пінні вогнегасники, які застосовуються при гасінні електроустановок, а також
інструкція з пожежної безпеки [44, 45, 47].
5.5 Аналіз рівня електромагнітного випромінювання в приміщенні
Особливо шкідливе для людини випромінювання екрана дисплея.
Напруженість електромагнітного поля на робочому місці оператора на
протягом робочого дня не повинна перевищувати значень вказаних у таблиці 5.1.
Табл. 5.1 Напруженість електромагнітного поля
Діапазон частот Напруженість
60 кгц- 3 МГц 2,5 Ут
3 МГц – 30 МГц 400 мвт
30 МГц – 300 ГГц 200 мвт
Заходи для захисту від випромінювань:
- застосування захисних екранів;
- застосування монохромних екранів;
- застосування спеціальних екранів зі слабким випромінюванням [44,
45, 47].
5.6 Аналіз повітряного середовища робочої зони
Умови, що визначають стан повітря робочої зони, характеризуються:
‒ температурою навколишнього повітря;
103
‒ відносною вологістю;
‒ швидкістю руху повітря.
Основними джерелами тепла, що впливають на температуру
навколишнього повітря в приміщенні є сонячна радіація 600-700 ккал/м,
опалення, працюючий у приміщенні персонал (при легкій роботі людина
випромінює до 150 ккал/година), прилади (комп'ютери) пристрої освітлення
[44].
Основним джерелом вологи є зовнішнє повітря. Мікроклімат у лабораторії
нормується відповідно до ДСТ 12.1.005-76 ССБТ.
Розглянемо загальні санітарно-гігієнічні вимоги до повітря робочої зони.
Значення норм встановлюється в залежності від пори року і ступеня важкості
робіт.
Таблиця 5.2 відповідає роботі обслуговуючого персоналу, який виконує
роботу категорії "легка 1".
Табл. 5.2 Санітарно-гігієнічні вимоги
Пора
року
Температура
навколишнього повітря,
С°
Відносна вологість
повітря, %
Швидкість руху
повітря, м/с
Оптимальна Припусти
ма
Оптима
льна
Припуст
има
Оптимал
ьна
Припуст
има
Холодн
а 20-23 19-25 60-40 <75 <0,2 <0,2
(середн
я) 19-22 65-55 <0,2
Тепла 22-25 22-28 60-40 <75 <0,2 0,2-0,5
(середн
я) 24-27 65-55 <0,2
У даній лабораторії температура навколишнього повітря, відносна
вологість, швидкість руху повітря знаходиться в нормі і забезпечується
системою опалення, вентиляції і кондиціонування.
104
Для підтримки в приміщеннях нормальних параметрів повітряного
середовища, що задовольняють санітарно-гігієнічним і технологічним вимогам,
влаштовують вентиляцію. Вентиляція призначена для забезпечення санітарно-
гігієнічних і технологічних вимог.
5.7 Виробниче освітлення
Значення освітлення в процесі життєдіяльності й особливо у виробничій
діяльності велике [20, 38, 40]. При незадовільному освітленні зорова здатність
очей знижується і можуть появитися такі захворювання, як короткозорість,
катаракта. Правильно виконана система має велике значення в зниженні
виробничого травматизму, зменшуючи потенційну небезпеку більшості
виробничих чинників; створює нормальні умови для роботи органом зору і
підвищує загальну праце-спроможність організму [38, 40]. Освітлення можна
назвати раціональним, якщо воно є економічним і враховує властивості й
особливості ока [38, 40, 44, 45].
Часта пристосовуваність ока, різкі затінки, освітлення занадто яскравим
світлом стомлюють око, знижують його захисну реакцію, око втрачає
контрастну чутливість і гостроту зору [38].
У освітленні лабораторії бере участь природне освітлення, що
здійснюється бічним світлом через світлові пройми в зовнішніх стінках (вікна),
штучне, утворюване електричними лампами і суміщене, при якому у світлий
час доби недостатнє по нормам природне освітлення додається тучним.
Для забезпечення високої освітленості на робочих поверхнях застосовують
комбіноване освітлення, якщо застосування загального освітлення
неекономічне.
Також є евакуаційне освітлення для евакуації людей із приміщення при
відключенні робочого освітлення.
Виробниче освітлення нормується СНІП П-4-79
Згідно з положенням СНІП П-4-79 рівень освтленості спеціальних
приміщень та лабораторій має дорівнювати значенням, які наведені у
таблиці 5.3:
105
Помещения
Плоскость
(Г—
горизон-
тальная,
В—верти-
кальная)
нор-
мирования
освещен-
ности
и КЕО,
высота
плоскости
над полом,
м
Искусственное освещение Естественное освещение
освещен-
ность
рабочих
поверхнос
тей, лк
цилинд
-ри-
ческая
освеще
нность,
лк
показа-
тель
дис-
ком-
форта,
не
более
коэффи
циент
пульса-
ции
освеще
нности,
%,
не
более
,%ІІІ
нКЕОе
при
верх-
нем или
верхнем
и
боково
м
освеще-
нии
При боковом
освещении
в зоне с
устойчи-
вым
снежным
покровом
на
осталь-
ной
тер-
итории
СССР
Здания управления,
конструкторских
и проектных
организаций,
научно-
исследовательских
учреждений
1. Кабинеты и рабочие
комнаты, проектные
кабинеты
2. Проектные залы и
комнаты,
конструкторские, чер-
тежные бюро
Г—0,8
Г—0,8
300*
500*
—
—
40
40
15
10
—
5
0,8
1,6
1
2
Наше приміщення належить до приміщень І групи за зоровою роботою.
5.7.1 Розрахунок природного освітлення
Природне освітлення здійснюється бічним світлом через віконні пройми у
стінах. У лабораторії є три віконних проміжки.
106
Попередній розрахунок площі світлових проміжків або перевірочний
розрахунок КПО (коефіцієнт природної освітленості) здійснюється по формулі:
ЗД
Н
n
Kr
eK
S
S
10
030100
(5.4)
де:
0S – площа світлових проміжків при бічному освітленні, м ;
nS – площа підлоги, м2;
Íe – нормоване значення КПО, %;
3K - коефіцієнт запасу, для приміщень із повітряним середовищем, що
містять у повітряній зоні пил не менше 1мг/м3 при вертикальному заскленні
3,13 K ;
ÇÄK – коефіцієнт, що враховує затінення вікон будинками, що протистоять,
(1…1,7);
0 – загальний коефіцієнт світлопропускання (0,1..0,8);
1r – коефіцієнт, що враховує підвищення КПО за рахунок відбитого
світла(1…10);
0 – світлова характеристика вікна.
Тому, виходячи з цієї формули розрахуємо значення коефіцієнту
природного освітлення:
03
100100
KKS
rSe
ÇÄn (5.5)
Лабораторія має три вікна, кожне розміром 2x3 м тобто загальна площа
віконних проймів 18 м2. По таблиці у додатку СНІП ІІ-4-79 (витяг з
«Будівельних норм та правил») η=10, то 0 – загальний коефіцієнт
світлопропускання, дорівнює:
543210
звідси:
1 – коефіцієнт світлового пропускання скла, 0,8 – для подвійного скла.
2 – коефіцієнт втрат світла в переплетеннях пройму – 0,85.
107
3 – коефіцієнт втрат у несучих конструкціях при бічному освітленні;
враховує забруднення скла – 1,85.
1, 54 – для бічного освітлення при шторах, що забираються.
68,011185,08,00
1r – коефіцієнт, що враховує підвищення КПО при бічному освітленні через
відбиток від поверхні приміщення і підстильного шару прилягаючого до
будинку, при 21 r .
321
0
SSS
SSSïëïëccn
ñð
(5.6)
ñð – середньо виміряний коефіцієнт відбитка стін, стелі і підлоги:
n - 70% = 0,7 – коефіцієнт відбиття підлоги;
âñ - 50% = 0,5 – коефіцієнт відбиття стін;
ñm - 10% = 0,1 – коефіцієнт відбиття стелі;
nS - площа підлоги;
cS - площа стін;
cmS – площа стелі.
Тоді:
44,04,361818
4,365,0187,0181,0
ср
Нормоване значення КПО для Києва, що знаходиться в четвертому
світловому поясі в приміщенні лабораторії буде виражатися такою формулою:
mcee 34
де:
т – коефіцієнт світлового клімату;
с – коефіцієнт сонячності.
Для четвертого пояса т = 0,9; для Києва с = 0,85. Природне освітлення і
штучне освітлення" значення КПО при природному освітленні нормуються в
залежності від характеристики зорової роботи. Установлено вісім розрядів
зорової роботи. Роботи зі зборки даної системи відносяться до робіт третього
108
розряду (високої точності - найменший розмір об'єкта розрізнення – 0,3...0,5
мм)
5,33 H
e %
тоді:
68,285,05,39,04 H
e %
4,101103,118
268,018100
e 6
Розрахований КПО більше нормованого, тобто освітленість у приміщенні
відповідає нормам [38, 40, 45].
5.7.2 Розрахунок штучного освітлення
При штучному освітленні нормується абсолютне значення освітленості.
Норми установлені СНІП 4-79 у залежності від характеру зорової роботи, фона
( світлий, середній, темний). А також контрасту об'єкта і фона (малий, середній,
великий) і системи освітлення (нормоване або загальне).
Нормуються також показники якості освітлення.
Передбачаємо освітлення по нормі третього розряду. Система освітлення,
що рекомендується – комбінована. Тип ламп – люмінесцентний.
Методи штучного освітлення характеризуються різноманітною точністю й
областю застосування. Найбільш часто застосовують два методи:
1) метод коефіцієнта використання світлового потоку – для розрахунку
загального рівномірного освітлення горизонтальної поверхні при світильниках
будь-якого типу;
2) крапковий метод – для розрахунку загального рівномірного,
локалізованого і місцевого освітлення, а також для розрахунку освітлення
негоризонтальних поверхонь. Для даного розрахунку використовуємо метод
коефіцієнта використання світлового потоку.
N
ZSEKF
(Лм) (5.7)
F – потік світла від ламп у світильнику;
E – мінімальна освітленість по нормам;
109
K – коефіцієнт запасу К=1,4 (із таблиці);
N – число світильників;
Z – коефіцієнт мінімальної освітленості, рівний відношенню:
minE
EZ
ô (5.8)
2,1Z
S – площа приміщення, м2;
– коефіцієнт використання світлового потоку, що падає на розраховану
поверхню до сумарного потоку усіх ламп.
Для визначення коефіцієнту світлового потоку приймаємо наступні
позначення:
H – висота приміщення;
h – висота світильників над робочою поверхнею;
L – відстань між світильниками;
l – відстань між центрами світильників;
hc – підвіс світильника, hc = 0,3 м.
Відстань L між світильниками повинні відповідати найвигіднішим
значенням:
ch
L (5.9)
Вибираємо світильники philips, призначені для нормальних умов (для
приміщень із помірною вологістю і запиленістю).
Потужність світильника – 2×80 Вт.
Висота світильника над робочою поверхнею – hcв = 273 мм.
Показник приміщення і = 1,5.
По таблиці значень коефіцієнтів використання в залежності від показників
приміщення і коефіцієнта відбитка для світильників типу ОДОР знаходимо
312,0 . Тоді:
85,336532,1312,0
304,1300
Z
SKEF câ
ðîçð[Лм] (5.10)
Кількість світильників визначається по формулі:
110
538,423840
85,33653
nF
FN
Ä
ðîçð[шт]
де: n – число ламп.
Щоб освітлити лабораторію відповідно до норм СНіП необхідно п’ять
світильників.
Висновок: Виходячи з проведених розрахунків можна сказати, що
природне і штучне освітлення в нашій лабораторії відповідає санітарним
нормам [20, 38, 44, 45].
5.8 Безпека в надзвичайних ситуаціях
Залежно від обсягів заподіяних надзвичайною ситуацією техногенного та
природного характеру наслідків, обсягів технічних і матеріальних ресурсів,
необхідних для ліквідації її наслідків, визначаються такі рівні надзвичайних
ситуацій техногенного та природного характеру:
– державний;
– регіональний;
– місцевий;
– об'єктовий.
У разі виникнення пожежі з вибухами та горінням на об'єкті і території
потрібно проводити наступні заходи:
а) першочергові заходи: – негайно повідомити пожежні частини міста про
виникнення пожежі; – провести оповіщення працівників про можливе ураження
внаслідок пожежі з вибухами та горінням від 5 до 15 хв. у залежності від місця
НС; – забезпечити захист працівників від можливих наслідків пожежі з
вибухами та горінням, проведення у разі необхідності відселення (або
евакуації) та інших заходів; – негайно приступити до локалізації і ліквідації
наслідків пожежі з горінням силами формувань у взаємодії з силами ППС та
МНС України від 5 до 30 хв. і проводити їх до повного завершення; – комісії з
питань НС провести через 30 хв. попередню оцінку можливої обстановки і
підготувати рішення на проведення РНАВР робіт у зонах ураження;
111
б) подальші заходи: – основні зусилля направляти на захист працівників,
студентів та ліквідацію наслідків пожежі з горінням, надання допомоги
потерпілим; – локалізацію і ліквідацію наслідків пожежі з наступним горінням
проводити силами формувань ЦО та з НС у взаємодії з ППС службою МВС
України та територіальними силами; роботи проводити негайно з моменту їх
виникнення і до повного завершення; – взаємодію проводити з силами інших
міністерств, відомств, у першу чергу з МНС України, ППС МВС України та
територіальними органами управління і силами ЦО [20, 40, 44, 45].
План евакуації при надзвичайній ситуації показано на рисунку 4.2.
Рисунок. 5.2 План евакуації
5.8.1 Дії у разі виникнення НС на підприємстві, що спеціалізується на
виготовленні спеціальних систем
Організаційні та правові дії у разі виникнення конкретної НС на даному
підприємстві слід організувати у відповідності з Законом України “Про захист
населення і територій від надзвичайних ситуацій техногенного та природного
характеру” [20, 37, 40, 44] на наступних принципах:
- пріоритетності завдань, спрямованих на рятування життя та
збереження здоров’я робітників;
112
- безумовного надання переваги раціональній та превентивній
безпеці;
- особистої відповідальності та піклування працівників про власну
безпеку, неухильного дотримання ними правил поведінки та дій у НС;
- відповідальність у межах своїх повноважень посадових осіб за
дотримання вимог цього Закону;
- обов’язковості завчасної реалізації заходів, спрямованих на
запобігання виникненню НС техногенного характеру та мінімізацію їх
негативних психосоціальних наслідків;
- урахування економічних, природних та інших особливостей
територій і ступеня реальної небезпеки виникнення НС;
- максимально можливого, ефективного і комплексного
використання наявних сил і засобів, які призначені для запобігання НС
техногенного характеру і реагування на них.
Проводячи дані дослідження, а саме – налаштування, відладка та
тестування систем орієнтації та навігації, можна зробити один із головних
висновків даного розділу: сам процес не визиває загрози та виникненню
надзвичайно небезпечної ситуації, як в лабораторії де проводиться
налаштування, відладка та тестування системи, так і на підприємстві.
113
ВИСНОВКИ ДО П’ЯТОГО РОЗДІЛУ
1. В магістерській дисертації опрацьовано вимоги, щодо виявлення та
аналізу потенційних небезпечних і шкідливих факторів, що створюються
технологічними процесами під час конструювання об'єкту (на операціях
виготовлення, спеціальної обробки, складання, монтажу, регулювання
або налаштування, контролю чи випробувань), та заходи їх усунення.
2. Проведено аналіз небезпечних та шкідливих виробничих факторів, які
виявлені на працівників з завантаженням робочих місць при складанні та
налаштуванні приладів. Згідно цих факторів розроблено розрахунок
освітлення.
3. Виділено основи безпеки праці при виконанні основних видів робіт на
підприємстві.
4. Розроблені заходи з пожежної безпеки завантаження робочих місць при
складанні та налаштуванні приладів.
114
ВИСНОВКИ ПО РОБОТІ
В магістерській дисертації було розглянуто питання корекції
безплатформної інерціальної навігаційної системи від оптико – електронної
системи орієнтації та навігації.
Були досягнені такі результати:
1. Пророблено та проаналізовано роботу БІНС в цілому. Визначено
основні режими роботи БІНС. Визначено причини та джерела похибок,
які впливають на точність БІНС.
2. Розглянуто початкову виставку БІНС та вимоги до ЧЕ. Розглянуто та
проаналізовано алгоритми роботи каналів орієнтації та навігації БІНС,
складено математичну модель основного режиму роботи БІНС.
3. Проаналізовано алгоритмічний дрейф інтегрування математичної
моделі БІНС. Виявлено степеневу залежність похибки визначення
параметрів орієнтації та навігації від кроку інтегрування, амплітуди
вхідного збурення, частоти вхідного збурення та зсуву фаз вхідного
збурення.
4. Розроблена програма моделювання роботи БІНС в середовищі MatLAB
та Simulink, яка дозволяє проводити аналіз роботи алгоритмів БІНС в
залежності від характеристик ЧЕ та методів інтегрування.
5. Представлено сучасні оптико – електронні засоби, які можуть
використовуватися для корекції безплатформної інерціальної
навігаційної системи. Приведено математичні основи оптико –
електронних систем орієнтації та навігації.
6. Обґрунтовано використання алгоритму комплексування систем за
допомогою синтезування фільтра Калмана, для оцінки похибки
визначення параметрів орієнтації та навігації комплексної системи, а
також для створення найбільш оптимального навігаційного комплексу,
заснованого на БІНС та ОЕСОН.
7. Проведено моделювання, яке демонструє коректність роботи
синтезованого алгоритму фільтра Калмана для комплексної системи, а
115
також показані результати моделювання роботи корекції БІНС від
ОЕСОН. За модельований період часу в 25 хв., похибка визначення
швидкості руху об’єкта не перевищує 4 м/с, похибка визначення
місцеположення об’єкта не перевищую 2 м/с.
8. Оцінка похибки визначення кутів орієнтації не перевищує 180 кутових
секунд.
116
СПИСОК ВИКОРИСТАНОЇ ЛІТЕРАТУРИ
1. Алексеев К.Б., Бебенин Г.Г. Управление космическими летательными
аппаратами. – М.: Машиностроение, 1974. – 343с.
2. Андреев В.Д. Теория инерциальной навигации. Автономные системы. –
М.: Наука, 1966. – 578с.
3. Андреев В.Д. Теория инерциальной навигации. Корректируемые
системы. – М.: Наука, 1967. – 648с.
4. Анучин О.Н., Емельянцев Г.И. Интегрированные системы ориентации и
навигации морских подвижных объектов. – СПб.: ГНЦ РФ – ЦНИИ
«Электроприбор», 2003. – 390с.
5. Аршакян А.А., Будков С.А. Оптическое определение пространственного
положения подвижного объекта. //Известия Тульского Государственного
Университета. 2013. 10
6. Бабич О.А. Обработка информации в навигационных комплексах. – М.:
Машиностроение, 1991. – 512с.
7. Бажинов И.К., Ястребов В.Д. Навигация в совместном полете
космических кораблей «Союз» и «Аполлон». – М.: Наука, 1978.
8. Баклицкий В.К. Корреляционно – экстремальные методы навигации и
наведения. – Тверь: ТО «Книжный клуб».2009. – 306с.
9. Баклицкий В.К. Методы фильтрации в корреляционно – экстремальных
системах навигации. – М.: Радио и связь, 1986. – 216с.
10. Белоглазов И.Н., Джанджгава Г.И., Чигин Г.П. Основы навигации по
геофизическим полям. – М.: Наука, 1985. – 328с.
11. Боданский Е.Д., Фурман В.Д. О погрешностях численного
интегрирования кинематических уравнений Пуассона. – Космические
исследования, 1970, т. 8, вып. 6, с. 944 – 948.
12. Бондарев В.Г., Гривенная Н.В., Хабаров А.Н. Локальная оптико –
электронная навигационная система. Актуальные вопросы исследований
в авионике: Теория, обслуживание, разработки – Сб. тез.докладов
всероссийской науч. – практич.конф. «АВИАТОР», Воронеж, 2014
117
13. Бранец В.Н., Шмыглевский И.П. Применение кватернионов в задачах
ориентации твердого тела. – М.: Наука, 1973 – 303с.
14. Бранец В.Н., Шмыглевский И.П. Введение в теорию бесплатформенных
инерциальных навигационных систем. – М.: Наука, 1992. – 280с.
15. Бромберг Л.В. Теория инерциальных систем навигации. – М.: Наука,
1979. – 296с.
16. Веремеенко К.К., Красильщиков М.Н., Сыпало К.А. и др. Управление и
наведение беспилотных маневренных летательных аппаратов на основе
современных информационных технологий. – М.: ФИЗМАТЛИТ, 2003. –
280с.
17. Веселов К.Е., Сагитов М.У. Гравиметрическая разведка. – М.: Недра,
1968. – 512с.
18. Горенштейн И.А, Шульман И.А. Инерциальные навигационные системы.
– М.: Машиностроение, 1970. – 279с.
19. Дмитриченко Л.А., Лопатин В.И. Особенности алгоритмов БИНС. – В
кн.: Вопросы управления космическими аппаратами. – М.: Мир, 1975, с.
95 – 117.
20. Закон України 2695 – XII від 14.10.1992 «Про охорону праці»
21. Закон України „Про пожежну безпеку” від 17 грудня 1993 року.
22. Закон України „Про загальнообов’язкове державне соціальне
страхування від нещасного випадку на виробництві та професійного
захворювання, які спричинили втрату працездатності” від 23 вересня
1999 року.
23. Закон України “Про захист населення і територій від надзвичайних
ситуацій техногенного і природного характеру” від 8 червня 2000 року.
24. Илюхин И.М., Дикарев В.Н. Оптико – электронные приборы угловой
ориентации космических летательных аппаратов (КЛА). Ч.1. Приборы
орбитальной ориентации КЛА: учебное пособие/ под ред. Дикарева В.Н.
– М.: Изд-во МГТУ им. Н.Э. Баумана, 1998. – 44с.
118
25. Инерциальная навигация. Анализ и проектирование./ Под ред. К. Ф.
О’Доннела. – М.: Наука, 1969. – 592с.
26. Интегрированные инерциально – спутниковые системы навигации/
составитель Степанов О.А.; под общей ред. Пешехонова В.Г. – СПб.:
ГНЦ ЦНИИ «Электроприбор», 2004 – 235с.
27. Ишанин Г.Г., Попков Э.Д. и др. Источники и приемники излучения. –
СПб.: Политехника, 1991. – 240с.
28. Ишлинский А.Ю. Ориентация, гироскопы и инерциальная навигация. –
М.: Наука, 1976. – 670с.
29. Кузовков Н.Т., Салычев О.С. Инерциальная навигация и оптимальная
фильтрация. – М.: Машиностроение, 1982. – 216с.
30. Кузьмин В.С., Федосеев В.И. Оптико – электронные приборы
ориентации и навигации КА: опыт разработки, проблемы и тенденции //
Оптический журнал. 1996. 7 С. 4 – 9.
31. Купервассер О., Рубинштейн А. Система навигации беспилотных
летательных аппаратов с помощью видео. – ООО «Транзист Видео».
32. Лаврова Н.П., Стеценко А.Ф. Аэрофотосъемка. Аэрофотосъемочное
оборудование. – М.: Недра 1981. – 296с.
33. Лазарєв Ю.Ф., Бобровицька Я.Г. Розроблення і моделювання алгоритмів
безплатформової системи орієнтації. Електронний навчальний посібник.
– К.: НТУУ «КПІ», 2011. – 135с.
34. Малышева Ю.А. Система навигации и ориентации летательного
аппарата на оптических датчиках. Национальный технический
университет Украины «Киевский политехнический институт», Матеріали
VIII міжнародної конференції, "Гіротехнології, навігація, керування
рухом і конструювання авіаційно-космічної техніки", НТУУ «КПІ», 2011.
35. Матвеев В.В., Распопов В.Я. Основы построения бесплатформенных
инерциальных навигационных систем. – СПб.: ГНЦ РФ ОАО «Концерн
«ЦНИИ «Электроприбор», 2009. – 280с
119
36. Мелешко В. В., Нестеренко О. И. Бесплатформенные инерциальные
навигационные системы. Учебное пособие. - Кировоград: ПОЛИМЕД -
Сервис, 2011. - 172 с.
37. Методичні вказівки до виконання розділу “Охорона праці та безпека в
надзвичайних ситуаціях” в дипломних проектах для підготовки студентів
факультету електроніки за освітньо-кваліфікаційним рівнем “Спеціаліст”
та ”Магістр”. / С.С. Козлов. – м. Київ: НТУУ ”КПІ”, 2013, - 64 с.
38. Методические указания к лабораторным работам «Исследование
освещения» для студентов всех специальностей / Ю.К. Френзе, М.Я.
Подколзин, В.С. Сабарно, Н.И. Чаповский. – г. Киев: КПИ, 1988. - 48 с.
39. Мошкин В.И., Петров А.А., Титов В.С., Якушенков В.Г. Техническое
зрение роботов. – М.: Машиностроение, 1990. – 272с.
40. Наказ 255 від 15.11.04 «Типове положення про службу охорони праці».
Держнаглядохоронпраці України.
41. Нестеренко О.И. РАЗРАБОТКА МАЛОГАБАРИТНЫХ СИСТЕМ
ОРИЕНТАЦИИ В КОМПАНИИ INERTIAL LABS, USA., Матеріали VIII
міжнародної конференції, "Гіротехнології, навігація, керування рухом і
конструювання авіаційно-космічної техніки", НТУУ «КПІ», 2011.
42. Ориентация и навигация подвижных объектов: современные
информационные технологии/ Под.ред. Алёшина Б.С., Веремеенко К.К.,
Черноморского А.И. – М.: ФИЗМАТЛИТ, 2006. – 424с.
43. Осипик В.А., Саблин В.К., Федосеев В.И. Анализ способов сканирования
при измерении угловых координат точеного источника // Оптико –
механическая промышленность. 1980. 10, С. 8 – 11.
44. Основи охорони праці: підручник / О. І. Запорожець, О. С.
Протоєрейський, Г. М. Франчук, І. М. Боровик. – К: Центр учбової
літератури, 2009. – 264 с.
45. Павлов С.П. Охрана труда в приборостроении: учеб. для приборостроит.
спец. вузов. / С.П. Павлов, З.И. Губонина. – Г.: Высш. шк., 1986. - 215 с.
120
46. Попков В.М. Высокоточный инфракрасный прибор ориентации по Земле
с использованием информации с двух параллельных траекторий
сканирования // Оптический журнал. 1996 7, С. 54 – 55.
47. Практикум з охорони праці : навчальний посібник/ Жидецький В.Ц.,
Джигирей В.С., Сторожук В.М. – Львів, Афіша, 2000 – 352 с.
48. Прохорцов А.В. Возможности определения угловой ориентации по
сигналам одной антены СНС. //Известия Тульского Государственного
Университета. 2013. 11
49. Рак В.С., Половко М.В., Степанченко В.В. Охорона праці на
підприємстві. Навчально-практичний посібник для модульної системи
навчання. – К.: Видавництво друкарня ФПУ, 2004. – 120 с.
50. Распопов В.Я. Микросистемная авионика: учебное пособие. – Тула
«Гриф и К», 2010. – 248с.
51. Руководство по воздушной навигации авиации вооруженных сил
Российской Федерации. – М.: Военное издательство, 1994.
52. Савельев В.М., Антонов Д.А. Выставка Бесплатформенной
инерциальной навигационной системы беспилотного летательного
аппарата на подвижном основании. Труды МАИ, вып. 45.
53. Самолет Ту – 214. Руководство по летной эксплуатации. Издание первое.
– ОАО «Туполев», 2003 – 2352с.
54. Современные информационные технологии в задачах навигации и
наведения беспилотных маневренных летательных аппаратов/ Под ред.
Красильщикова М.Н., Серябрякова Г.Г. – М.: ФИЗМАТЛИТ, 2009. – 556с.
55. Соловьев Ю.А. Системы спутниковой навигации. – М.: Эко – Трендз,
2000. – 270с.
56. Федоссеев В.И., Колосов М.П. Оптико – электронные приборы
ориентации и навигации космических аппаратов: учебное пособие. – М.:
Логос, 2007. – 248с.
57. Эдвардс А. Бесплатформенные инерциальные навигационные системы //
Вопросы ракетной техники. – 1973. 5 С. 47 – 70.
121
58. Bo Xu, Yang Liu, Wei Shan, Yi Zhang, and Guochen Wang. Error Analysis
and Compensation of Gyrocompass Alignment for SINS on Moving Base.
Mathematical Problem in Engineering, Harbin Engineering University, Harbin
China, Hindawi, 2014.
59. Bennett K. MATLAB applications for the practical engineer. 2014.
60. Corke P., Lobo J., Dias J., “An introduction to inertial and vision sensing”, Int.
J. Rob. Res. 26(6): 519 – 535, 2007.
61. Crassidis J.L. and Markley F.L. New algorithm for attitude determination
using global positioning system signals. Journal of Guidance, Control, and
Dynamics, 20(5):891–896, 1997.
62. Crassidis J.L. and Markley F.L. Predictive filtering for attitude estimation
without rate sensors. Journal of Guidance, Control, and Dynamics, 20(3):522–
527, 1997.
63. Du W.Y., and Dickerson S.L. Passive component inspection using machine
vision. International Conference in Multichip Modules and High Density
Packaging (1998), p. 74-79.
64. Dusha Damien, Walker Rodney, Boles Wageeh. Fixed – wing attitude
estimation using vision based horizon detection // 22nd
International Unmanned
Air Vehicle Systems Conference 2007.
65. Ejiri M. Machine vision technonlgy: Past, present and future. Intelligent
Robots and Systems (1990), XXIX-XXXX.
66. Faraz M. Mirzaei, Stergios I. Roumeliotis. A Kalman Filter – Based
Algorithm for IMU – Camera Calibration: Observability Analysis and
Performance Evaluation. IEEE TRANSACTIONS ON ROBOTICS, VOL. 24,
NO. 5, OCTOBER 2008.
67. Farrell J.A. and Barth M. The Global Positioning and Inertial Navigation.
McGraw-Hill, New York, 1999.
68. Farrell J.A. AIDED NAVIGATION GPS with High Rate Sensors McGraw-
Hill, New York, 2008.
122
69. Fu-Jun Pei, Xuan Liu, Li Zhu. In – Flight Alignment Using Filter for
Strapdown INS on Aircraft. Beijing University of Technology, Beijing, China,
Hindawi, 2014.
70. Grewal M.S., Andrews A.P. Kalman filtering: theory and practice using
MATLAB. 3rd
ed. J. Wiley & Sons, Inc. – 2008.
71. Grewal M.S., Weill L.R., Andrews A.P. Global Positioning Systems, Inertial
navigation and integration, 2nd
ed., Wiley, New York, 2007.
72. Hol J. D. “Pose Estimation and Calibration Algorithms for Vision and Inertial
Sensors”, Lic. Thesis no 1379, Dept. Electr. Eng., Linkopings University,
Sweden, May 2008.
73. Huddle J. Inertial Navigation System Error Model Considerations in Kalman
Filtering Applications. In Control and Dynamic Systems. Vol. 20., Academic
Press, 1983.
74. Hung J.C. and White H.V. Self-alignment techniques for inertial measurement
units. IEEE Trans. on Aerospace and Electronic Systems, 11(6):1232–1247,
1975.
75. Integrated Vision-Based Navigation System for Autonomous Vehicles in
GPS-denied Environments (Autonomous Weapons Summit and GNC
Challenges for Miniature Autonomous Systems Workshop, October 25-27,
2010)
76. Jarvis R.A. Application – oriented robotic vision. – a review // Robotica, 1984
N 2, P. 3 – 15.
77. Jekeli C. Inertial Navigation Systems with Geodetic Applications. Walter de
Gruyter, Berlin, 2001.
78. Jeroen D. Hol, Thamas B. Schon, Henk Luinge, Per J. Slycke, Frederik
Gustafsson. “Robust real – time tracking by fusing measurement from inertial
and vision sensors”, Journal of Real – Time Image Processing.
79. Jeroen D. Hol, Thamas B. Schon, Frederik Gustafsson “Relative Pose
Calibration of Spherical Camera and an IMU”
123
80. Johnson C., Ohlmeyer E.J. and Pepitone T.R., “Attitude Dilution of Precision
- A New Metric for Observability of In-flight Alignment Errors” AIAA 2000-
4277.
81. Jose Guivant, Eduardo nebot, Stephan Baiker “Autonomous Navigation and
Map Building Using Laser Range Sensors in Outdoor Applications”, Journal of
Robotic Systems, Vol. 17 10, October 2000, pp. 565 – 583.
82. Karsenti S.M. A Study of IMU Alignment Transfer. MASSACHUSETTS
INSTITUTE OF TECHNOLOGY. MASTER’S THESIS of SCIENCE in
AERONAUTICS AND ASTRONAUTICS. 1989.
83. Kayton M., Fried W.R. Avionics Navigation Systems. Wiley, New York, 1997
84. Lefferts E.J., Markley F.L., and Shuster M.D., “Kalman Filtering for
Spacecraft Attitude Estimation,” Journal of Guidance, Control, and Dynamics,
Vol. 5, No. 5, Sept.-Oct. 1982, pp. 417–429.
85. Lin, Ching-Fang. Modern Navigation, Guidance, and Control Processing.
Prentice Hall, Englewood Cliffs, New Jersey, 1991
86. Price Sean. A Simultaneous Position and Orientation Estimate Feature Finder
for Machine Vision. Electrical and Computer Engineering. A thesis submitted
to the Faculty of the WORCESTER POLYTECHNIC INSTITUTE. 2000.
87. Pue A. Integration of GPS with Inertial Navigation Systems. short courses
notes, Navtech Seminars, Springfield, VA, 2003.
88. Rogers R.M., “IMU In-motion Alignment Without Benefit of Attitude
Inialization,” Journal of the Institute of Navigation, Vol. 44, No. 3, 1997, pp.
301-311.
89. Rogers R.M., “Comparison of Inertial Navigation System Error Models in
Application to IMU Transfer Alignment” AIAA 97-3599.
90. Rogers R. M. Applied Mathematics in Integrated Navigation Systems. AIAA
Educational Series. American Institute of Aeronautics and Astronautics, Inc,
Reston, VA, 2nd edition, 2003.
91. Salychev O.S. Applied Inertial Navigation: Problems and Solutions. BMSTU
Press, Moscow, Russia, 2004.
124
92. Savage P.G. Strapdown System Algorithms. Advances in Strapdown Inertial
Systems. AGARD Lecture Series 133, 1984.
93. Savage P. G. Strapdown Inertial Navigation Integration Algorithm Design
Part 2: Velocity and Position Algorithms., Journal of Guidance, Control, and
Dynamics, vol. 21, no. 2, pp. 208-211, 1998.
94. Siouris G. Aerospace Avionics Systems – A Modern Synthesis. Academic
Press, San Diego, 1993.
95. Titterton D.H., Weston J.L. Strapdown Inertial Navigation Technology, —
American Institute of Aeronautics and Astronautics & Institution of Electrical
Engineers, 2004.
96. Vision-Aided Inertial Navigation for Flight Control. AIAA Guidance,
Navigation, and Control Conference and Exhibit 15 - 18 August 2005, San
Francisco, California
97. Visual Navigation Aid for Planetary UAV Risk Reduction — The Charles
Stark Draper Laboratory, Inc. — USA Intelligent robots and computer vision
XXV 9-11 September, 2007, Boston, Massachusetts, USA
98. Waldmann Jacques. In – Flight Alignment in INS – Aiding with Switched
Feedforward/Feddback of Error Estimates.
99. Weiping Jiang, Li Wang, Xiaoji Niu, Quan Zhang, Hui Zhang, Min Tang,
Xiangyun Hu. High – Precision Image Aided Inertial Navigation with Known
Features: Observability Analysis and Performance Evaluation.
Sensors 2014, 14(10), 19371-19401
100. Xiang Zhou, Zhihui Lei, Qifeng Yu, Hongliang Zhang, Yang Shang,
Jing Du, Yang Gui, Pengyu Guo. Videometric Terminal Guidance Method and
System for UAV Accurate Landing. Society of Photo-Optical Instrumentation
Engineers (SPIE).
101. Yang C., Lin C.F., Tarrant D., Roberts C. and Ruffin P., “Transfer
Alignment Design and Evaluation,” AIAA-93-3892-CP, pp. 1724-1733.
102. Лазерная локация [Электронный ресурс] http://www.laserlocation.ru/
125
103. Основы фильтрации оптических сигналов [Электронный ресурс]
http://www.ipi.uni-hannover.de/fileadmin/institut/pdf/Kurz_mueller_etal.pdf
104. Параметры воздушной лидарной съемки [Электронный ресурс]
http://www2.geog.ucl.ac.uk/~mdisney/teaching/GEOGG141/papers/Baltsavias_Lidar
105. http://www.ncbi.nlm.nih.gov/pubmed/20820078 - електронний ресурс.
106. http://www.darpa.mil/ - електронний ресурс.
107. http://www.ssci.com – електронний ресурс.
108. Спутниковая система навигации [Электронный ресурс]
http://ru.wikipedia.org/wiki/Спутниковая_система_навигации.
109. Воздушная лидарная съемка [Электронный ресурс]
http://lidar.pro/wiki/
126
ДОДАТОК A
Список наукових праць
Гуриненко Станіслава Олеговича
пп Назва
Друков
ані або
на
правах
рукопи
су
Видавництво, журнал
(назва, номер, том) або
номер авторського
свідоцтва
Кількі
сть
друк.
арк.
Прізвища
співавторі
в
1 2 3 4 5 6
1
Бесплатформенная
инерциальная
навигационная
система с оптико –
электронной
системой корекции
Друк
XIV науково – технічна
конференція
«Приладобудування
2015: стан і
перспективи», 22-23
квітня 2015р.
ЗБІРНИК ТЕЗ
ДОПОВІДЕЙ
2
2
Скалярная
диагностика
инерциально –
измерительного
модуля
Друк
VII Научно-
техническая
конференция
аспирантов и молодых
ученых
«ВООРУЖЕНИЕ.
ТЕХНОЛОГИЯ.
БЕЗОПАСНОСТЬ.
УПРАВЛЕНИЕ», в
городе Ковров, 21-23
апрель 2015г., С. 200 –
208, г. Ковров
9
Аврутов
В.В.,
Головач
С.В.
127
128
129
130
131
ДОДАТОК B
ВИХІДНИЙ КОД MATLAB РОЗРАХУНКІВ
Вихідний код MatLAB моделювання БІНС
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%------------------AHRS and SINS-------------------%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clear all,clc global Ps0 Th0 Ga0 Pm Tm Gm epsT epsG om global PsA ThA GaA OMdr % Change from degree to rad and from rad to degree R2D = 180/pi; D2R = pi/180; % Earth Parameters U = 7.2921e-5; % Angular Velocity of The Earth ReqZ = 6378137; % Equatorial Earth radius % gravity value (emperic equation) g0 = 9.78; % Initial Gravity acceleration, m/s^2 fi0 = 50.27*D2R; % Initial geographic latitude, deg, Kyiv lam0 = 30.30*D2R; % Initial geographic longitude, deg, Kyiv bet1 = 0.0053024; % gravity constant 1(Geodetic Refernce System 1967) bet2 = 5.8e-6; % gravity constant 2(Geodetic Refernce System 1967) s2 = power(sin(fi0),2); % sin(fi) in square s22 = power(sin(2*fi0),2); % sin(2*fi) in square Kh = 3.086e-6; % Altitude constant in 1/sec^2 H = 0; % Baro Altitude, m g = g0*(1 + bet1*s2 - bet2*s22) - Kh*H; G = [0 0 -g]; g01 = g; % Projection on the Initial(Basis) Frame % Movement time and Prepearing for integration tI = 0; Tk = 600; tR = 0; % Start and final time of integration h = 0.05; ki = 1; % step of integration, sec and counter register ttI(ki) = tI; ttR(ki) = tR; % Accelerometer and Gyro Parameters OMdr = 0.005*D2R; % Gyro range = +/-250 DPS => span = 500 DPS % OMdr = Bias is 0.1%(0.01%) of span => % => Bias = 0.5 DPS - Gyro Drift SpanAcc = 10*g; % Accelerometer range = +/-5g => span = 10g dA = 0.1*SpanAcc/100; % Accelerometer Bias; Bias is 0.1 % of span % Initial Angles Parameters Ps0 = 30*D2R; % Intial Yaw Th0 = 0*D2R; % Initial Pitch Ga0 = 0*D2R; % Initial Roll % Alignment Angles PsA = 0*D2R; ThA = -0.010155; GaA = -0.0098071; % Angular Movement -- movement initial angles Pm = 0; Tm = 0.1; Gm = 0.2; epsT = pi/2; epsG = 0; om = 1; % Orientation Initial motion % Ideal [UgId,UGskId] = UgDvigOsnId(tI); PsiId = UgId(1); ThetaId = UgId(2); GammaId = UgId(3); % Real [UgR,UGskR] = UgDvigOsnR(tI); PsiR = UgR(1); ThetaR = UgR(2); GammaR = UgR(3); % Trigonometric functions % Ideal cPI = cos(PsiId);sPI = sin(PsiId); cTI = cos(ThetaId);sTI = sin(ThetaId); cGI = cos(GammaId);sGI = sin(GammaId); % Real cPR = cos(PsiR);sPR = sin(PsiR); cTR = cos(ThetaR);sTR = sin(ThetaR); cGR = cos(GammaR);sGR = sin(GammaR);
132
% Initial cosine matrix % Ideal CgbI = ug2mnk(cPI,sPI,cTI,sTI,cGI,sGI); % Real CgbR = ug2mnk(cPR,sPR,cTR,sTR,cGR,sGR); % Initial angles PsId(ki) = UgId(1); ThId(ki) = UgId(2); GaId(ki) = UgId(3); PsR(ki) = UgR(1); ThR(ki) = UgR(2); GaR(ki) = UgR(3); % Initial errors dPsId(ki) = 0; dThId(ki) = 0; dGaId(ki) = 0; dPsR(ki) = 0; dThR(ki) = 0; dGaR(ki) = 0; % Initial Parameters of lineier movement fiId = fi0; lamId = lam0; fiR = fi0 + 0.0001*rand(1); lamR = lam0 + 0.0001*rand(1); V = 200; Vh = 0; % Object Velocity Vn = V*cos(Ps0); % North Velocity Ve = V*sin(Ps0); % East Velocity VnR = Vn; VeR = Ve; VhR = Vh; % Real Vrlocity Wksi = 0; Weta = 0; Wdzeta = 0; % Object Acceleration % Trigonometric function of coordinates cfId = cos(fiId); sfId = sin(fiId); tfId = tan(fiId); clId = cos(lamId); slId = sin(lamId); cfR = cos(fiR); sfR = sin(fiR); tfR = tan(fiR); clR = cos(lamR); slR = sin(lamR); wb = waitbar(0,'ON GOING'); set(findobj(wb,'type','patch'),'edgecolor','b','facecolor','b') while tI <= Tk ki = ki + 1; % Counter register % Ideal Rocking movement [UgId,UGskId] = UgDvigOsnId(tI); PsiTId(ki) = UgId(1); ThetaTId(ki) = UgId(2); GammaTId(ki) = UgId(3); OMxId = cossem(UGskId); % Real Rocking movement [UgR,UGskR] = UgDvigOsnR(tI); PsiTR(ki) = UgR(1); ThetaTR(ki) = UgR(2); GammaTR(ki) = UgR(3); OMxR = cossem(UGskR); % Earth movement % Ideal OMksiId = -Vn/ReqZ; OMetaId = U*cfId + Ve/ReqZ; OMdzetaId = U*sfId + (Ve/ReqZ)*tfId; OMzi = [OMksiId OMetaId OMdzetaId]; OMzemId = cossem(OMzi); % Real OMksiR = -VnR/ReqZ; OMetaR = U*cfR + VeR/ReqZ; OMdzetaR = U*sfR + (VeR/ReqZ)*tfR; OMzr = [OMksiR OMetaR OMdzetaR]; OMzemR = cossem(OMzr); % Acceleration in Base Frame Ideal WxId = Wksi*CgbI(1,1) + Weta*CgbI(1,2) + Wdzeta*CgbI(1,3); WyId = Wksi*CgbI(2,1) + Weta*CgbI(2,2) + Wdzeta*CgbI(2,3); WzId = Wksi*CgbI(3,1) + Weta*CgbI(3,2) + Wdzeta*CgbI(3,3); % Accelerometers Signals Ideal axId = WxId; % - CgbI(1,3)*G(3); ayId = WyId; % - CgbI(2,3)*G(3); azId = WzId; % - CgbI(3,3)*G(3); % Apparent Acceleration in Geographic axis Ideal aeI = CgbI(1,1)*axId + CgbI(1,2)*ayId + CgbI(1,3)*azId; % East anI = CgbI(2,1)*axId + CgbI(2,2)*ayId + CgbI(2,3)*azId; % North ahI = CgbI(3,1)*axId + CgbI(3,2)*ayId + CgbI(3,3)*azId; % Altitude Ve = Ve + h*aeI; % Velocity Ideal Vn = Vn + h*anI; Vh = Vh + h*ahI;
133
fiId = fiId + h*Vn/ReqZ; % Navigation Coordinate Ideal lamId = lamId + h*Ve/(ReqZ*cfId); % Acceleration in Base Frame Real WxR = Wksi*CgbR(1,1) + Weta*CgbR(1,2) + Wdzeta*CgbR(1,3); WyR = Wksi*CgbR(2,1) + Weta*CgbR(2,2) + Wdzeta*CgbR(2,3); WzR = Wksi*CgbR(3,1) + Weta*CgbR(3,2) + Wdzeta*CgbR(3,3); % Accelerometers Signals Real axR = WxR + dA; % - CgbR(1,3)*G(3); ayR = WyR + dA; % - CgbR(2,3)*G(3); azR = WzR + dA; %- CgbR(3,3)*G(3); % Apparent Acceleration in Geographic axis Real aeR = CgbR(1,1)*axR + CgbR(1,2)*ayR + CgbR(1,3)*azR; % East anR = CgbR(2,1)*axR + CgbR(2,2)*ayR + CgbR(2,3)*azR; % North ahR = CgbR(3,1)*axR + CgbR(3,2)*ayR + CgbR(3,3)*azR; % Altitude % Velocity Real VeR = VeR + h*aeR; VnR = VnR + h*anR; VhR = VhR + h*ahR; % Navigation Coordinate Real fiR = fiR + h*VnR/ReqZ + 1e-4*randn(1,length(tI)); lamR = lamR + h*VeR/(ReqZ*cfR) + 1e-5*randn(1,length(tI)); % Integrate Poisson Equation % Ideal CgbI = CgbI + h*(CgbI*(OMxId)); % Real CgbR = CgbR + h*(CgbR*(OMxR)); % Integration of Coordinats s2 = power(sin(fiId),2); % sin(fi) in square s22 = power(sin(2*fiId),2); % sin(2*fi) in square g = g0*(1 + bet1*s2 - bet2*s22)- Kh*H; % gravity value (emperic equation) % Final Data % Ideal angles and angular velocity UgId = mnk2ug(CgbI); PsId(ki) = UgId(1); ThId(ki) = UgId(2); GaId(ki) = UgId(3); dPsId(ki) = PsiTId(ki) - PsId(ki); dThId(ki) = ThetaTId(ki) - ThId(ki); dGaId(ki) = GammaTId(ki) - GaId(ki); % Real angles and angular velocity UgRe = mnk2ug(CgbR); PsR(ki) = UgRe(1); ThR(ki) = UgRe(2); GaR(ki) = UgRe(3); dPsR(ki) = PsiTR(ki) - PsR(ki); dThR(ki) = ThetaTR(ki) - ThR(ki); dGaR(ki) = GammaTR(ki) - GaR(ki); % Velocity Ideal Vid(1,ki) = Vn; Vid(2,ki) = Ve; % Vid(3,ki) = Vh; % Navigation Parameters Ideal NavCoor(1,ki) = fiId*R2D; NavCoor(2,ki) = lamId*R2D; % Velocity Real Vr(1,ki) = VnR; Vr(2,ki) = VeR; % Vr(3,ki) = VhR; % Navigation Parameters Real NavCoorR(1,ki) = fiR*R2D; NavCoorR(2,ki) = lamR*R2D; % Gravitation Graviti(ki) = g; tI = tI + h; ttI(ki) = ki + 1; waitbar(tI/Tk,wb); end TT = ttI*h; close(wb);
134
Код для моделювання комплексної системи орієнтації та навігації.
clear all close all fly_over_rms = 3; % RMS error (in meters) of the external 2-D position % % fix (in each dimension); also known as a fly-over fix load dyn_flt_ins_dat % output from the program DYN_FLIGHT_INS.M randn('state',0) C = [0 1 0; 1 0 0; 0 0 -1]; % conversion between NED and ENU H = zeros(2,15); H(1,1) = 1; H(2,2) = 1; % % The measurement vector z consists of % % an x and y position difference only. % % Since we are working in the L frame % % this corresponds to east and north % % position difference (since alpha=0). Peast_pos = fly_over_rms^2; Pnorth_pos = fly_over_rms^2; Pup_pos = 0; Peast_vel = 2^2; Pnorth_vel = 2^2; Pup_vel = 0; Ppsi_x = 0.001^2; Ppsi_y = 0.001^2; Ppsi_z = 0.001^2; Pacc_x = (100*9.81e-6)^2; Pacc_y = (100*9.81e-6)^2; Pacc_z = (100*9.81e-6)^2; Pgyr_x = (0.05)^2; Pgyr_y = (0.05)^2; Pgyr_z = (0.05)^2; P_pre = zeros(15,15); P_pre(1,1)=Peast_pos; P_pre(2,2)=Pnorth_pos; P_pre(3,3)=Pup_pos; P_pre(4,4)=Peast_vel; P_pre(5,5)=Pnorth_vel; P_pre(6,6)=Pup_vel; P_pre(7,7)=Ppsi_x; P_pre(8,8)=Ppsi_y; P_pre(9,9)=Ppsi_z; P_pre(10,10)=Pacc_x; P_pre(11,11)=Pacc_y; P_pre(12,12)=Pacc_z; P_pre(13,13)=Pgyr_x; P_pre(14,14)=Pgyr_y; P_pre(15,15)=Pgyr_z; P_pre_KF = P_pre; % initial prediction error covariance matrix R = [fly_over_rms^2 0; 0 fly_over_rms^2]; % measurement error covariance matrix sigma_acc = 0.3*9.81e-6; sigma_gyr = 1e-6; G = zeros(15,15); G(10,10)=sigma_acc; G(11,11)=sigma_acc; G(12,12)=sigma_acc; G(13,13)=sigma_gyr; G(14,14)=sigma_gyr; G(15,15)=sigma_gyr; W = zeros(15,15); W(10,10)=1; W(11,11)=1; W(12,12)=1; W(13,13)=1; W(14,14)=1; W(15,15)=1; tau_accel = 100; tau_gyro = 10000; update = 0; count = 0; X_pre = zeros(15,1); est_lat_KF(1) = lat_prof(1); % INITIALIZATION IN THIS SECTION est_lon_KF(1) = lon_prof(1); est_alpha_KF(1) = 0; est_vel_l_KF(1,:) = vel_l(1,:); est_roll_KF(1) = est_roll(1); est_pitch_KF(1) = est_pitch(1); est_yaw_KF(1) = est_yaw(1); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% h = waitbar(0,' Time Loop '); for i = 2:npts, % Generate the inertial error model system dynamics matrix F_ins = F_ins_gen(est_lat(i),del_Vl(:,i),tdint(i),... omega_el_L(:,i),omega_ie_L(:,i),g_extr(i),est_height(i,1)); % Generate the system dynamics matrix for opto correction and for the whole
filter F = zeros(15,15); F(1:9,1:9) = F_ins; F(10:12,10:12) = (-1/tau_accel)*eye(3); F(4:6,10:12) = C*est_DCMbn(:,:,i); F(13:15,13:15) = (-1/tau_gyro)*eye(3); F(7:9,13:15) = -*C*est_DCMbn(:,:,i); % Generate the system noise [Q,PHI] = q_gen(tdint(i),F,G,W,15); % Calculate the Kalman gain K = P_pre_KF*H'*inv(H*P_pre_KF*H' + R); % keep track of time (in seconds) last Kalman update count = count + tdint(i); %%if count >= 630, % 10.5 minute intervals (i.e., 1/8 Schuler cycle)
135
%%if count >= 60, % 1 minute intervals if count >= 1, % 1 Hz updates update = 1; count = 0; end if update == 1, % Generate the free-inertial estimates of lat, long and ECEF x-y-z ins_lat = lat_prof(i) + est_lat_err(i); ins_lon = lon_prof(i) + est_lon_err(i); est_pos_xyz = llh2xyz([ins_lat ins_lon height_prof(i)]); % Calcuate the ECEF x-y-z coordinates of the true position tru_pos_xyz = llh2xyz([lat_prof(i) lon_prof(i) height_prof(i)]); % Calculate the free-inertial position error in east-north-up % coordinates ins_pos_err_enu = xyz2enu(est_pos_xyz,tru_pos_xyz); % Simulate the errors of the position aiding source fly_over_fix_errors = fly_over_rms*randn(2,1); % Simulate the position difference between INS and position aiding % source Z = ins_pos_err_enu(1:2,1) + fly_over_fix_errors; % Compute Kalman estimate X_est = X_pre + K*(Z - H*X_pre); X_est(3) = 0; X_est(6) = 0; % Assuming perfect vertical aiding P_est_KF = (eye(15) - K*H)*P_pre_KF; update = 0; else X_est = X_pre; X_est(3) = 0; X_est(6) = 0; % Assuming perfect vertical aiding P_est_KF = P_pre_KF; end X_pre = PHI*X_est; P_pre_KF = PHI*P_est_KF*PHI' + Q; state_est(:,i) = X_est; x_rms_KF(i) = sqrt( P_est_KF(1,1) ); y_rms_KF(i) = sqrt( P_est_KF(2,2) ); x_v_rms_KF(i) = sqrt( P_est_KF(4,4) ); y_v_rms_KF(i) = sqrt( P_est_KF(5,5) ); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% [rm,rp] = radicurv(est_lat(i)); radius_e = sqrt(rm*rp); theta(1,1) = -X_est(2)/radius_e; % converting position error from linear theta(2,1) = X_est(1)/radius_e; % units to angular units theta(3,1) = tan(est_lat(i))*theta(2); % this is for alpha=0 and ENU frame; psi = X_est(7:9); phi_angle = psi + theta; % computing total attitude error %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % This is the Kalman estimate of the true DCM est_DCMbn_KF = C*(eye(3) + skewsymm(phi_angle))*C*est_DCMbn(:,:,i); eul_vect = dcm2eulr(est_DCMbn_KF); est_roll_KF(i) = eul_vect(1); est_pitch_KF(i) = eul_vect(2); est_yaw_KF(i) = eul_vect(3); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % This is the Kalman estimate of the true DCM est_DCMel_KF = (eye(3) + skewsymm(theta))*est_DCMel(:,:,i); llw_vect = dcm2llw(est_DCMel_KF); est_lat_KF(i) = llw_vect(1); est_lon_KF(i) = llw_vect(2); est_alpha_KF(i) = llw_vect(3); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% est_vel_l_KF(i,:) = ((eye(3) + skewsymm(theta))*(vel_l(i,:)' - X_est(4:6)))'; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% waitbar(i/npts,h)
136
end close(h) h = waitbar(0,' Computing Position, Velocity, Attitude Errors '); N = max(size(est_lat0)); % Compute horizontal position for i = 1:N, % error by finding the ENU truxyz = llh2xyz([est_lat0(i) est_lon0(i) 0]); % coordinates of the insxyz_unaided = llh2xyz([est_lat(i) est_lon(i) 0]); % INS-derived
position enu = xyz2enu(insxyz_unaided,truxyz); % relative to the
truth east_pos_err_unaided(i) = enu(1); north_pos_err_unaided(i) = enu(2); up_pos_err_unaided(i) = enu(3); horz_pos_err_unaided(i) = norm(enu(1:2));
insxyz_KF = llh2xyz([est_lat_KF(i) est_lon_KF(i) 0]); enu = xyz2enu(insxyz_KF,truxyz); east_pos_err_KF(i) = enu(1); north_pos_err_KF(i) = enu(2); up_pos_err_KF(i) = enu(3); horz_pos_err_KF(i) = norm(enu(1:2)); waitbar(i/N,h) end close(h) lat_err_unaided = est_lat-est_lat0; lon_err_unaided = est_lon-est_lon0; alpha_err_unaided = est_alpha - est_alpha0; vel_l_err_unaided = vel_l - vel_l_0; roll_err_unaided = est_roll - est_roll0; pitch_err_unaided = est_pitch - est_pitch0; yaw_err_unaided = est_yaw - est_yaw0;
lat_err_KF = est_lat_KF-est_lat0; lon_err_KF = est_lon_KF-est_lon0; alpha_err_KF = est_alpha_KF - est_alpha0; vel_l_err_KF = est_vel_l_KF - vel_l_0; roll_err_KF = est_roll_KF - est_roll0; pitch_err_KF = est_pitch_KF - est_pitch0; yaw_err_KF = est_yaw_KF - est_yaw0; close all
Код програми моделювання похибок оцінювання визначення навігаційних
параметрів
clear all close all load prof_1_ins_truth % output from GEN_INS_TRUTH.M dph2rps = (pi/180)/3600; % conversion constant from deg/hr to rad/sec earthflg = 1; % earth shape flag: 1 for ellipsoidal C = [0 1 0; 1 0 0; 0 0 -1]; % conversion between NED and ENU %%%%%%%% SPECIFICATION OF INS ERRORS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% init_vel_e_err = 0.02; init_vel_n_err = 0.02; % 0.02 m/s initial north velocity error init_x_tilt = 0.0001; % 0.1 milli-radian body-x tilt error init_y_tilt = 0.0001; init_z_mis = 0*0.001; g = gravity(0,0); vxbias = 200e-6*g; % 200 micro-g x-accel bias vybias = 150e-6*g; % 150 micro-g y-accel bias vzbias = -100e-6*g; vxsferr = 0; % 0 accel scale-factor errors vysferr = 0; vzsferr = 0;
137
vxstdev = 0; % 0 accel white noise standard deviation vystdev = 0; vzstdev = 0; thxbias = 1; % 1 deg/hr x-gyro bias thybias = 1.5; % 1.5 deg/hr y-gyro bias thzbias = -2; thxsferr = 0; % 0 gyro scale-factor errors thysferr = 0; thzsferr = 0; thxstdev = 0.09; % 0.09 deg/root-hour gyro white noise standard deviation thystdev = 0.05; % 0.05 deg/root-hour gyro white noise standard deviation thzstdev = -0.04; dvparam = [vxbias vybias vzbias; % Set up parameter matrix for vxsferr vysferr vzsferr; % GENDVERR vxstdev vystdev vzstdev]; dthparam = [thxbias thybias thzbias; % Set up parameter matrix for thxsferr thysferr thzsferr; % GENTHERR thxstdev thystdev thzstdev]; %%%%%%%%%%%% END OF INS ERROR SPECIFICATION %%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% accum_gyro_x = zeros(npts,1); accum_gyro_y = zeros(npts,1); accum_gyro_z = zeros(npts,1); accum_accel_x = zeros(npts,1); accum_accel_y = zeros(npts,1); accum_accel_z = zeros(npts,1); est_lat = zeros(npts,1); est_lon = zeros(npts,1); est_alpha = zeros(npts,1); est_height = zeros(npts,1); est_roll = zeros(npts,1); est_pitch = zeros(npts,1); est_yaw = zeros(npts,1); state_est = zeros(18,npts); x_rms_KF = zeros(npts,1); y_rms_KF = zeros(npts,1); z_rms_KF = zeros(npts,1); x_v_rms_KF = zeros(npts,1); y_v_rms_KF = zeros(npts,1); z_v_rms_KF = zeros(npts,1); x_psi_rms_KF = zeros(npts,1); y_psi_rms_KF = zeros(npts,1); z_psi_rms_KF = zeros(npts,1); x_accel_rms_KF = zeros(npts,1); y_accel_rms_KF = zeros(npts,1); z_accel_rms_KF = zeros(npts,1); x_gyro_rms_KF = zeros(npts,1); y_gyro_rms_KF = zeros(npts,1); z_gyro_rms_KF = zeros(npts,1); est_roll_KF = zeros(npts,1); est_pitch_KF = zeros(npts,1); est_yaw_KF = zeros(npts,1); est_lat_KF = zeros(npts,1); est_lon_KF = zeros(npts,1); est_alpha_KF = zeros(npts,1); est_height_KF = zeros(npts,1); est_vel_l_KF = zeros(npts,3); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%% INS MECHANIZATION ALGORITHM INITIALIZATION %%%%%%%%%%%%%%%%% est_roll(1) = tru_roll(1) + init_x_tilt; est_pitch(1) = tru_pitch(2) + init_y_tilt; est_yaw(1) = tru_yaw(3) + init_z_mis; laterr=0; longerr=0; alphaerr=0; est_alpha(1) = tru_alpha(1); est_height(1) = tru_height(1);
138
height1 = tru_height(1); height2 = tru_height(1); est_lat(1) = tru_lat(1); est_lat(2) = tru_lat(1); est_lon(1) = tru_lon(1); vx1 = tru_vel_L(1,1) + init_vel_e_err; vx2 = vx1; vy1 = tru_vel_L(1,2) + init_vel_n_err; vy2 = vy1; vel_l(1,:) = [vx2 vy2 0]; vel2 = [vx1 vy1 0]; vel1 = vel2; lat2 = est_lat(1); lat1 = est_lat(1) - (est_lat(2)-est_lat(1)); est_DCMbn = ( eulr2dcm([est_roll(1) est_pitch(1) est_yaw(1)]) )'; est_DCMel = llw2dcm([est_lat(1) est_lon(1) est_alpha(1)]); vertmech = 0; omega2_el_L =
crafrate(est_lat(1),vx1,vy1,est_height(1),est_DCMel,earthflg,vertmech); %%%%%%%%%%%% END INS MECHANIZATION INITIALIZATION %%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%% BEGIN GENERATION OF IMU MEASUREMENTS %%%%%%%%%%%%%%%%%%%%%%% dtherr = gentherr(deltheta,time,dthparam,98765); % generate delta-theta errors est_dtheta = deltheta + dtherr; % form profiles of 'measured' delta-theta's dverr = gendverr(dvtot,time,dvparam,76543); % Generate delta-V errors est_dv = dvtot + dverr; % form profile of 'measured' delta-V's %%%%%%%%%%% END GENERATION OF IMU MEASUREMENTS %%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%% BEGIN OPTO INITIALIZATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% mpmat=mpgen(24,3600,1,54321); % simulate OPTO multipath error loadopto % load the OPTO constellation parameters into global memory optotime = 40000; % specify the OPTO time for the simulation %%optotime = 22000; % specify the OPTO time for the simulation %%%%%%%%%%% END OPTO INITIALIZATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%% INSERT KF INITIALIZATION PARAMETERS %%%%%%%%%%%%%%%% H = zeros(3,18); H(1,1) = 1; H(2,2) = 1; H(3,3) = 1; % % The measurement vector z consists of % % an x, y and z position differences only. % % Since we are working in the L frame % % this corresponds to east, north and up % % position differences (since alpha=0). % Defining the components of the initial prediction error covariance % matrix %%init_pos_rms = 3; % initial position error (RMS in meters) init_pos_rms = 1.5; % initial position error (RMS in meters) Peast_pos = init_pos_rms^2; Pnorth_pos = init_pos_rms^2; Pup_pos = init_pos_rms^2; Peast_vel = 2^2; Pnorth_vel = 2^2; Pup_vel = 2^2; Ppsi_x = 0.0001^2; Ppsi_y = 0.0001^2; Ppsi_z = 0.0001^2; Pacc_x = (100*9.81e-6)^2; Pacc_y = (100*9.81e-6)^2; Pacc_z = (100*9.81e-6)^2; Pgyr_x = (0.05*dph2rps)^2; Pgyr_y = (0.05*dph2rps)^2; Pgyr_z = (0.05*dph2rps)^2; P_pre = zeros(18,18); P_pre(1,1)=Peast_pos; P_pre(2,2)=Pnorth_pos; P_pre(3,3)=Pup_pos; P_pre(4,4)=Peast_vel; P_pre(5,5)=Pnorth_vel; P_pre(6,6)=Pup_vel; P_pre(7,7)=Ppsi_x; P_pre(8,8)=Ppsi_y; P_pre(9,9)=Ppsi_z; P_pre(10,10)=Pacc_x; P_pre(11,11)=Pacc_y; P_pre(12,12)=Pacc_z; P_pre(13,13)=Pgyr_x; P_pre(14,14)=Pgyr_y; P_pre(15,15)=Pgyr_z; P_pre(16,16)=init_pos_rms^2; P_pre(17,17)=init_pos_rms^2;
P_pre(18,18)=init_pos_rms^2; P_pre_KF = P_pre; % initial prediction error covariance matrix % measurement error covariance matrix R = init_pos_rms^2*eye(3); % calculate the system noise covariance matrix (Q). sigma_acc = 0.3*9.81e-6; sigma_gyr = 1e-9; G = zeros(18,18); G(10,10)=sigma_acc; G(11,11)=sigma_acc; G(12,12)=sigma_acc; G(13,13)=sigma_gyr; G(14,14)=sigma_gyr; G(15,15)=sigma_gyr; W = zeros(18,18);
139
W(10,10)=1; W(11,11)=1; W(12,12)=1; W(13,13)=1; W(14,14)=1; W(15,15)=1; % Time constants (in seconds)used to model the accelerometer and gyro biases tau_accel = 100000; tau_gyro = 100000; update = 0; count = 0; X_pre = zeros(18,1); % Initialize the state prediction est_lat_KF(1) = est_lat(1); est_lon_KF(1) = est_lon(1); est_height_KF(1) = est_height(1); est_alpha_KF(1) = est_alpha(1); est_vel_l_KF(1,:) = vel_l(1,:); est_roll_KF(1) = est_roll(1); est_pitch_KF(1) = est_pitch(1); est_yaw_KF(1) = est_yaw(1); vertmech = 0; earthflg = 1; omega_ie_E = [0 0 7.292115e-5]'; init_corrections = 0; prev_KF_update = 0; KF_corrections = 0; %%%%%%%%%%%%%%%%%%%% END OF KALMAN FILTER INITIALIZATION %%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%% BEGIN TIME LOOP %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% accum_accel_bias = zeros(3,1); accum_gyro_bias = zeros(3,1); fprintf(1,' . . . . . . . . . \n') fprintf(1,' Starting nav computations \n') npts = length(time); h = waitbar(0,' Time Loop '); for i = 2:npts, td12 = time(i) - time(i-1); tdex = 0.5*td12; tdint = time(i) - time(i-1); est_dtheta(i-1,1:3) = est_dtheta(i-1,1:3) - (accum_gyro_bias*td12)'; accum_gyro_x(i) = accum_gyro_bias(1); accum_gyro_y(i) = accum_gyro_bias(2); accum_gyro_z(i) = accum_gyro_bias(3); est_DCMbn = bodupdat(est_DCMbn,est_dtheta(i-1,1:3)); [DCM_ll_I, omega_el_L, omega_ie_L] = lclevupd(lat1,lat2,vx1,vx2,vy1,vy2,... height1,height2,td12,tdex,tdint,est_DCMel,vertmech,1,earthflg); est_DCMbn = C*(DCM_ll_I*(C*est_DCMbn)); eul_vect = dcm2eulr(est_DCMbn); est_roll(i) = eul_vect(1); est_pitch(i) = eul_vect(2); est_yaw(i) = eul_vect(3); est_delv_b = est_dv(i-1,1:3) - (accum_accel_bias*td12)'; accum_accel_x(i) = accum_accel_bias(1); accum_accel_y(i) = accum_accel_bias(2); accum_accel_z(i) = accum_accel_bias(3); del_Vl = C*(est_DCMbn*est_delv_b'); omega1_el_L = omega2_el_L; omega2_el_L = omega_el_L; [est_DCMel, DCM_ll_E] = navupdat(omega1_el_L,omega2_el_L,td12,est_DCMel,1); h_extr = extrapol(height1,height2,td12,tdex); lat_extr = extrapol(lat1,lat2,td12,tdex); g_extr = gravity(lat_extr,h_extr); vtmp = ...
velupdat(vel2,vel1,td12,tdex,del_Vl,omega_el_L,est_DCMel,g_extr,0,tdint); vel_l(i,:) = vtmp'; est_height(i,1)= est_height(i-1,1) + tdint*mean([vel_l(i,3); vel_l(i-1,3)]); height1 = height2; height2 = est_height(i,1); vx1 = vx2; vy1 = vy2; vx2 = vel_l(i,1); vy2 = vel_l(i,2); vel1 = vel2; vel2 = vel_l(i,:); llw_vect = dcm2llw(est_DCMel);
140
est_lat(i) = llw_vect(1);est_lon(i) = llw_vect(2);est_alpha(i) =
llw_vect(3); lat1 = lat2; lat2 = est_lat(i); %%%%%%%%%%%%%% BEGIN KALMAN FILTER ALGORITHM %%%%%%%%%%%%%%%%%%%%%%% % Forming the system dynamics matrix [rm,rp] = radicurv(est_lat(i)); radius_e = sqrt(rm*rp); accel_vect_L = del_Vl*(1/tdint); est_DCMel = llw2dcm([est_lat(i) est_lon(i) est_alpha(i)]); omega_el_L = crafrate(est_lat(i),vel_l(i,1),vel_l(i,2),... est_height(i),est_DCMel,earthflg,vertmech); F11 = -1*skewsymm(omega_el_L); % L-frame F12 = eye(3); F13 = 0*eye(3); grav = gravity(est_lat(i),est_height(i)); F21=eye(3); F21(1,1)=-grav/radius_e; F21(2,2)=-grav/radius_e; F21(3,3)=2*grav/(radius_e+est_height(i,1)); omega_ie_L = est_DCMel*omega_ie_E; F22 = -1*skewsymm(2*omega_ie_L + omega_el_L); % L-frame F23 = skewsymm(accel_vect_L); F31 = 0*eye(3); F32 = 0*eye(3); F33 = (-1)*skewsymm(omega_ie_L + omega_el_L); % L-frame F_ins = [F11 F12 F13; F21 F22 F23; F31 F32 F33]; F = zeros(18,18); F(1:9,1:9) = F_ins; est_DCMbn = ( eulr2dcm([est_roll(i) est_pitch(i) est_yaw(i)]) )'; F(10:12,10:12) = (-1/tau_accel)*eye(3); F(4:6,10:12) = C*est_DCMbn; F(13:15,13:15) = (-1/tau_gyro)*eye(3); F(7:9,13:15) = -1*C*est_DCMbn; % Numerical calculation of the state transition matrix (PHI) and % system noise covariance matrix (Q) A = zeros(36,36); A(1:18,1:18) = -1*F; A(1:18,19:36) = G*W*G'; A(19:36,19:36) = F'; A = A*tdint; B = expm(A); PHI_trans = B(19:36,19:36); PHI = PHI_trans'; Q = PHI*B(1:18,19:36); % Calculate the Kalman gain % THE NEXT LINE IMPLEMENTS THE TRADITIONAL KALMAN GAIN: K = (P_pre_KF*H')/(H*P_pre_KF*H' + R); % keep track of time (in seconds) last Kalman update count = count + tdint; if count >= .1, %Hz updates update = 1; update_interval = count; count = 0; end if update == 1, %% Simulate OPTO Receiver output here optotime = optotime + update_interval; tru_pos_xyz = llh2xyz([tru_lat(i) tru_lon(i) tru_height(i)]); [svxyzmat,svid] = gensv(tru_pos_xyz,optotime,5); %%[prvec,adrvec] = genrng(1,tru_pos_xyz,svxyzmat,svid,optotime,[1 1 0 1
1],[],mpmat); [prvec,adrvec] = genrng(1,tru_pos_xyz,svxyzmat,svid,optotime,[1 0 0 0
0],[],mpmat); gps_xyzt = olspos(prvec,svxyzmat); gps_llh = xyz2llh(gps_xyzt(1:3)); ins_lat = est_lat(i); ins_lon = est_lon(i); ins_height = est_height(i); % Kalman observation vector (Z) formed by position differences. % Angular position differences converted to linear differences. Z(1,1) = (ins_lon - gps_llh(2))*rp*cos(ins_lat); % east component
141
Z(2,1) = (ins_lat - gps_llh(1))*rm; % north component Z(3,1) = ins_height - gps_llh(3); % up component X_est = X_pre + K*(Z - H*X_pre); P_est_KF = (eye(18) - K*H)*P_pre_KF; prev_KF_update = 1; update = 0; else X_est = zeros(18,1); P_est_KF = P_pre_KF; end X_pre = PHI*X_est; P_pre_KF = PHI*P_est_KF*PHI' + Q; %%%%%%%%%%% SAVE VARIABLES FOR PLOTTING LATER %%%%%%%%%%%%%%%%%%%%% state_est(:,i) = X_est; x_rms_KF(i) = sqrt( P_est_KF(1,1) ); y_rms_KF(i) = sqrt( P_est_KF(2,2) ); z_rms_KF(i) = sqrt( P_est_KF(3,3) ); x_v_rms_KF(i) = sqrt( P_est_KF(4,4) ); y_v_rms_KF(i) = sqrt( P_est_KF(5,5) ); z_v_rms_KF(i) = sqrt( P_est_KF(6,6) ); x_psi_rms_KF(i) = sqrt( P_est_KF(7,7) ); y_psi_rms_KF(i) = sqrt( P_est_KF(8,8) ); z_psi_rms_KF(i) = sqrt( P_est_KF(9,9) ); x_accel_rms_KF(i) = sqrt( P_est_KF(10,10) ); y_accel_rms_KF(i) = sqrt( P_est_KF(11,11) ); z_accel_rms_KF(i) = sqrt( P_est_KF(12,12) ); x_gyro_rms_KF(i) = sqrt( P_est_KF(13,13) ); y_gyro_rms_KF(i) = sqrt( P_est_KF(14,14) ); z_gyro_rms_KF(i) = sqrt( P_est_KF(15,15) ); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% theta(1,1) = -X_est(2)/rm; % converting position error from linear theta(2,1) = X_est(1)/rp; % units to angular units theta(3,1) = tan(est_lat(i))*theta(2); % this is for alpha=0 and ENU frame; psi = X_est(7:9); phi_angle = psi + theta; % computing total attitude error %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % This is the Kalman estimate of the true DCM est_DCMbn_KF = C*(eye(3) + skewsymm(phi_angle))*C*est_DCMbn; eul_vect = dcm2eulr(est_DCMbn_KF); est_roll_KF(i) = eul_vect(1); est_pitch_KF(i) = eul_vect(2); est_yaw_KF(i) = eul_vect(3); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % This is the Kalman estimate of the true DCM est_DCMel_KF = (eye(3) + skewsymm(theta))*est_DCMel; llw_vect = dcm2llw(est_DCMel_KF); est_lat_KF(i) = llw_vect(1); est_lon_KF(i) = llw_vect(2); est_alpha_KF(i) = llw_vect(3); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% est_height_KF(i,1) = est_height(i) - X_est(3); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% est_vel_l_KF(i,:) = ( (eye(3) + skewsymm(theta))*(vel_l(i,:)' - X_est(4:6)) )'; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%% END KALMAN FILTER ALGORITHM %%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%% THE FOLLOWING SECTION INCORPORATES THE KALMAN FILTER CORRECTIONS %%%%%%% INTO THE INS POS/VEL/ATT UPDATE ALGORITHM if (prev_KF_update==1), time_in_minutes = time(i)/60; accum_accel_bias = accum_accel_bias + X_est(10:12); accum_gyro_bias = accum_gyro_bias + X_est(13:15); est_DCMbn = est_DCMbn_KF; vel_l(i,:) = est_vel_l_KF(i,:); vel_l(i-1,:) = est_vel_l_KF(i-1,:);
142
est_DCMel = est_DCMel_KF; est_height(i) = est_height_KF(i,1); est_height(i-1) = est_height_KF(i-1,1); vel1 = vel_l(i,:); vel2 = vel_l(i,:); vx1 = vel_l(i,1); vy1 = vel_l(i,2); vx2 = vel_l(i,1); vy2 = vel_l(i,2); lat1 = est_lat_KF(i); lat2 = est_lat_KF(i); height2 = est_height(i); height1 = est_height(i); X_pre = zeros(18,1); KF_corrections = 1; prev_KF_update = 0; end %%%%%%%%%%%%% END OF KALMAN FILTER CORRECTION FEEDBACK %%%%%%%%%%%%%% if rem(i,1000)==0, waitbar(i/npts,h) end end close(h) %%%%%%%%%%%%%%%% END OF TIME LOOP %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% h = waitbar(0,' Computing Position, Velocity, Attitude Errors '); east_pos_err_KF = zeros(npts,1); north_pos_err_KF = zeros(npts,1); up_pos_err_KF = zeros(npts,1); for i = 1:npts, truxyz = llh2xyz([tru_lat(i) tru_lon(i) tru_height(i)]); insxyz_KF = llh2xyz([est_lat_KF(i) est_lon_KF(i) est_height_KF(i)]); enu = xyz2enu(insxyz_KF,truxyz); east_pos_err_KF(i) = enu(1); north_pos_err_KF(i) = enu(2); up_pos_err_KF(i) = enu(3); if rem(i,1000)==0, waitbar(i/npts,h) end end close(h) lat_err_KF = est_lat_KF-tru_lat; lon_err_KF = est_lon_KF-tru_lon; alpha_err_KF = est_alpha_KF - tru_alpha; vel_l_err_KF = est_vel_l_KF - tru_vel_L; roll_err_KF = est_roll_KF - tru_roll; pitch_err_KF = est_pitch_KF - tru_pitch; yaw_err_KF = est_yaw_KF - tru_yaw; close all time_end = max(time/60); save kf_fb_out plot_kf_fb