МИНИСТЕРСТВО ОБРАЗОВАНИЯ И НАУКИ РФ
ФЕДЕРАЛЬНОЕ ГОСУДАРСТВЕННОЕ
БЮДЖЕТНОЕ ОБРАЗОВАТЕЛЬНОЕ УЧРЕЖДЕНИЕ
ВЫСШЕГО ОБРАЗОВАНИЯ
«ВОРОНЕЖСКИЙ ГОСУДАРСТВЕННЫЙ УНИВЕРСИТЕТ»
ОБЗОР ПРИНЦИПОВ ПОСТРОЕНИЯ БЕСПЛАТФОРМЕННОЙ
ИНЕРЦИАЛЬНОЙ НАВИГАЦИОННОЙ СИСТЕМЫ
Учебно-методическое пособие для вузов
Составители: Дегтярев И.С., Яковлев А.Ю.
Воронежский государственный университет
2015
Стр.1
3
СОДЕРЖАНИЕ
1.
2.
Введение
Основные механико-математические соотношения,
используемые при построении ИНС
2.1. Принцип работы ИНС. Получение входных данных
2.2. Вывод формул интегрирования в случае поступательного
плоскопараллельного движения
2.3. Формулы поворота в случае сложного
плоскопараллельного движения
2.4. Итоговые соотношения для интегрирования
3.
Перевод вектора ускорения из локальной в глобальную
систему координат
3.1. Шарнирный замок
3.2. Матричный подход к заданию вращения
3.3. Кватернионный подход к заданию вращения
3.4. Взаимосвязь между самолетными углами, матрицей
поворота и кватернионом
4.
Фильтрация данных, полученных с датчиков
4.1. Основы фильтрации
4.2. Линейный фильтр
4.3. Комплементарный фильтр
4.4. Фильтр Калмана
Заключение
Список литературы
11
11
14
17
19
21
22
23
28
29
5
6
9
10
4
Стр.3
6
при вращении резонатора с переносной угловой скоростью, вектор которой
перпендикулярен вектору количества движения, или момента количества
движения, соответственно для поступательных или вращательных первичных
колебаний инерционной массы.
Основой алгоритма ИНС выступает второй закон Ньютона, который
устанавливает зависимость между ускорением 𝑎
приложенной силой 𝐹
�
𝑚𝑎
� = 𝐹
�.
� тела, его массой 𝑚 и
(1)
Измерив внешние силы, действующие на тело, произведем
интегрирование этого уравнения, и при заданных начальных условиях
получим координаты тела [1].
К сожалению, в реальных условиях измерение внешних сил
представляет большую сложность. Поэтому целесообразно измерять не
приложенные к объекту силы, а ускорения, возникающие при воздействии
сил на тело.
2.2. Вывод формул интегрирования в случае поступательного
плоскопараллельного движения.
Рассмотрим плоский случай без поворота тела относительно глобальной
системы координат, т. е. когда оси акселерометра сонаправлены с осями
глобальной системы координат. Движение тела в данном случае будет
поступательным.
Рисунок 1. Двумерный случай.
По определению скорости точки имеем
или
𝑣̅ = 𝑑𝑥̅
𝑑𝑡 ,
(2)
Стр.6
7
�
𝑣𝑥 = 𝑑𝑥
𝑑𝑡 ,
𝑣𝑦 = 𝑑𝑦
𝑑𝑡 ,
(3)
где 𝑣𝑥, 𝑣𝑦 – проекции компонент вектора скорости точки на оси 𝑋 и 𝑌
соответственно, а 𝑥 и 𝑦 – величины перемещения объекта вдоль этих осей.
Проинтегрируем эту систему уравнений по времени, получим
𝑡1
⎪
⎧� 𝑣𝑥𝑑𝑡
⎪
⎪
⎨
⎩
⎪
𝑡0
� 𝑣𝑦𝑑𝑡
𝑡1
𝑡0
где (𝑥0, 𝑦0) – координаты точки, где было тело в момент времени 𝑡 = 𝑡0.
Если предположить, что скорость была постоянной на промежутке
времени ∆𝑡 = 𝑡1 −𝑡0, то получим следующие выражения
�𝑣𝑥∆𝑡 = 𝑥 − 𝑥0,
𝑣𝑦∆𝑡 = 𝑦 − 𝑦0.
По определению ускорения
𝑎
или в проекциях
�
𝑎𝑥 = 𝑑𝑣𝑥
𝑑𝑡 ,
𝑎𝑦 = 𝑑𝑣𝑦
𝑑𝑡 .
Проинтегрируем (7) по времени, получим
𝑡1
⎪
⎧� 𝑎𝑥𝑑𝑡
⎪
⎪
⎨
⎩
⎪
𝑡0
� 𝑎𝑦𝑑𝑡
𝑡1
𝑡0
= 𝑣𝑦 − 𝑣0𝑦,
= 𝑣𝑥 − 𝑣0𝑥,
(8)
(7)
� = 𝑑𝑣̅
𝑑𝑡 ,
(6)
(5)
= 𝑦 − 𝑦0,
= 𝑥 − 𝑥0,
(4)
Стр.7
8
где 𝑣0𝑥, 𝑣0𝑦 – проекции вектора скорости в момент времени 𝑡 = 𝑡0.
Для постоянного ускорения (8) примет вид
�𝑎𝑥∆𝑡 = 𝑣𝑥 − 𝑣0𝑥,
𝑎𝑦∆𝑡 = 𝑣𝑦 − 𝑣0𝑦.
Выразим скорость
�𝑣𝑥 = 𝑣0𝑥 +𝑎𝑥∆𝑡,
𝑣𝑦 = 𝑣0𝑦 +𝑎𝑦∆𝑡.
Проинтегрируем (8) еще раз по времени
𝑡1
𝑡1
⎪
⎧� � 𝑎𝑥𝑑𝑡
⎪
⎪
⎨
⎩
⎪
𝑡0
𝑡1
𝑡0
� � 𝑎𝑦𝑑𝑡
𝑡1
𝑡0
𝑡0
Подставим (4) в (9)
𝑡1
⎪
⎪
⎨
⎩
⎪
и ускорение
𝑡0
𝑡1
𝑡1
⎪
⎧� � 𝑎𝑥𝑑𝑡
𝑡0
� � 𝑎𝑦𝑑𝑡
𝑡1
𝑡0
𝑡0
Откуда получаем выражение координат тела через начальную скорость
𝑡1
𝑡1
⎪
⎧𝑥 = 𝑥0 + 𝑣0𝑥∆𝑡 + � � 𝑎𝑥𝑑𝑡
⎪
⎪
⎨
Если предположить, что ускорение было постоянным с 𝑡 = 𝑡0 по 𝑡 = 𝑡1,
то выражение (10) перепишется в виде
⎩
⎪
𝑡0
𝑡0
�𝑥 = 𝑥0 +𝑣0𝑥∆𝑡 + 𝑎𝑥∆𝑡2,
𝑦 = 𝑦0 +𝑣0𝑦∆𝑡 +𝑎𝑦∆𝑡2.
(11)
𝑡0
𝑡1
𝑡0
𝑦 = 𝑦0 + 𝑣0𝑦∆𝑡 + � � 𝑎𝑦𝑑𝑡
𝑡1
𝑑𝑡 .
𝑑𝑡 ,
(10)
𝑑𝑡 = 𝑦 − 𝑦0 − 𝑣0𝑦∆𝑡.
𝑑𝑡 = 𝑥 − 𝑥0 −𝑣0𝑥∆𝑡,
𝑑𝑡
𝑑𝑡
= � 𝑣𝑥𝑑𝑡
𝑡1
𝑡0
= � 𝑣𝑦𝑑𝑡
𝑡1
𝑡0
−𝑣0𝑦∆𝑡.
−𝑣0𝑥∆𝑡,
(9)
(8.1)
Стр.8
9
Таким образом, зная ускорение, начальную скорость и начальные
координаты тела, мы можем вычислить положение тела.
При написании программного кода по приведенному алгоритму
(𝑣0𝑥, 𝑣0𝑦). По этим величинам, используя формулу (11) для вычисления
новых координат тела и формулу (8.1) для вычисления новой скорости,
будем находить текущие координаты и скорость. Затем, после нахождения
текущих значений, заменяем старые координаты и скорость на вновь
посчитанные. После чего процесс расчета повторяется.
необходимо будет хранить старые координаты тела (𝑥0, 𝑦0) и его скорость
2.3. Формулы поворота в случае сложного плоскопараллельного
движения.
Рассмотрим плоский случай с поворотом, т. е. когда оси акселерометра
не совпадают с осями глобальной системы координат, а повернуты
относительно них на некоторый угол 𝜃.
Рисунок 2. Двумерный случай с поворотом.
В этом случае вектор ускорения, который мы будем измерять, будет
определен (это связано положением датчиков, связанных с телом) в
локальной системе координат 𝑂1𝑋′𝑌′. Нам необходимо будет перевести
вектор ускорения из локальной системы координат в глобальную 𝑂𝑋𝑌 с
помощью поворота локальной системы координат на угол 𝜃.
Осуществим поворот по формулам
�𝑎𝑥 = 𝑎𝑥
𝑙 cos 𝜃 − 𝑎𝑦
𝑙 sin 𝜃 ,
𝑎𝑦 = 𝑎𝑥
𝑙 sin 𝜃 + 𝑎𝑦
𝑙 cos 𝜃 ,
(12)
системе координат.
Осуществив переход, определять положение тела можно по
соотношениям (8.1) и (11).
где (𝑎𝑥
𝑙 , 𝑎𝑦
𝑙 ) – проекции вектора ускорения тела, измеренные в локальной
Стр.9
10
В трехмерном случае мы будем иметь три угла поворота – углы
поворота вокруг оси 𝑋, 𝑌 и 𝑍. Эти углы называют самолетными углами или
углами Крылова. Поворот вокруг оси 0Y называется углом крена (𝜃), вокруг
оси 0X – углом тангажа (𝜑), и поворот вокруг оси 0Z – углом рысканья (𝜓).
Вместо самолетных углов можно использовать углы Эйлера.
В трехмерном пространстве поворот вектора ускорения можно
осуществить по формуле
⎪
⎧
⎪
⎪
⎨
⎩
⎪
𝑎𝑥 = 𝑎𝑥
𝑙 (cos 𝜃 cos 𝜓)+𝑎𝑦
𝑙 (−cos 𝜃 sin 𝜓) +𝑎𝑧
𝑙 (sin 𝜃),
𝑎𝑦 = 𝑎𝑥
𝑙 (sin 𝜑 sin 𝜃 cos 𝜓 + sin 𝜓 cos 𝜑)+ 𝑎𝑦
𝑙 (−sin 𝜑 sin 𝜃 sin 𝜓 + cos 𝜑 cos 𝜓)+
+𝑎𝑧
𝑙 (−cos 𝜃 sin 𝜑),
𝑎𝑧 = 𝑎𝑥
𝑙 (−sin 𝜃 cos 𝜑 cos 𝜓 + sin 𝜓 sin 𝜑)+ 𝑎𝑦
𝑙 (sin 𝜃 sin 𝜓 cos 𝜑 +sin 𝜑 cos 𝜓)+
+𝑎𝑧
𝑙 (cos 𝜃 cos 𝜑).
2.4. Итоговые соотношения для интегрирования.
Перепишем (8.1) и (11) в векторном виде, получим
𝑣̅ = 𝑣0��� + 𝑎
� ∙ ∆𝑡,
𝑥̅ = 𝑥0��� +𝑣0��� ∙ ∆𝑡 + 𝑎
� ∙ ∆𝑡2.
(13)
(14)
После преобразования вектора ускорения из локальной системы
координат в глобальную, из него нужно вычесть вектор гравитации 𝑔̅. С
учетом этого (14) перепишется в виде
𝑣̅ = 𝑣0��� + (𝑎
� −𝑔̅) ∙ ∆𝑡,
𝑥̅ = 𝑥0��� +𝑣0��� ∙ ∆𝑡 + (𝑎
� − 𝑔̅) ∙ ∆𝑡2.
(15)
Формулы (14) и (15) записаны в глобальной системе координат.
Подставлять значение измеренного вектора ускорения в них нужно после
перевода вектора ускорения из связанной с летательным аппаратом системы
координат в глобальную систему координат, связанную с Землей.
Контрольные вопросы.
1. Перечислите набор чувствительных элементов, который необходим
для построения инерциальной навигации. Дайте краткую
характеристику по каждому элементу.
2. Охарактеризуйте ситуации с различным вариантом ориентировки
осей датчиков.
Стр.10