Пособие БИНСа

Мелешко В.В., Нестеренко О.И.
БЕСПЛАТФОРМЕННЫЕ
ИНЕРЦИАЛЬНЫЕ
НАВИГАЦИОННЫЕ СИСТЕМЫ
Учебное пособие
Параметры БИНС
Ошибки определения скорости [Vn(-), Ve(-.-)]
1.5
м/c
1
φ0= 45
0.5
αy0= 0.001
αx0= 0.001
0
Угловые минуты (мили)
-0.5
0
0.2
0.4
0.6
0.8
1
1.2
1.4
1.6
1.8
2
αz0= 0.1
o
o
o
V= 20 м/с
Oшибки определения широты, долготы [φ(-), λ (-.-)]
3
ω dr= 0.01 град./час
2
2
da= 0.0001 м/с
1
Шаг интегрирования h= 0.5 с
0
-1
0
0.2
0.4
0.6
0.8
1
1.2
1.4
1.6
1.8
2
ω k= 0.01 (1/c)
Km = 1.14592
Ошибки ориентации платформы [курс (-), тангаж (-.-), крен (...)]
10
Угловые минуты
o
θm = 5.72958
5
γm = 5.72958
0
Wη = 0 м/с
-5
0
2
Wξ= 0 м/с
0.2
0.4
0.6
0.8
1
1.2
Время, часы
1.4
2011
1.6
1.8
2
2
o
o
o
ББК 34.9
УДК 629.056.6
М-47
РЕЦЕНЗЕНТЫ:
Леонец А.А., доктор технических наук
Панов А.П., доктор технических наук
М-47
Мелешко В.В., Нестеренко О.И.
Бесплатформенные инерциальные навигационные системы.
Учебное пособие. – Кировоград: ПОЛИМЕД - Сервис, 2011. – 164с.
ISBN 978-966-7813-75-8
Изложены основы теории бесплатформенных инерциальных систем.
Приведены основные сведения для пояснения принципа работы, показаны разновидности уравнений ориентации, различные формы записи
скоростей и ускорений в подвижных системах координат. Представлены примеры возможных алгоритмов работы. Рассматривается начальная выставка. Значительное внимание уделено калибровке параметров
чувствительных элементов как в производстве, так и в полете. Приведены алгоритмы коррекции от спутниковой навигационной системы и
астровизиров. Приведены примеры моделирования.
Для студентов приборостроительных специальностей высших
учебных заведений. Может быть полезна инженерам этих специальностей.
Ил. 59, табл. 11
ББК 34.9
УДК 629.056.6
М-47
© Мелешко В.В., Нестеренко В.В.
ISBN 978-966-7813-75-8
В.В.Мелешко, О.И.Нестеренко
БЕСПЛАТФОРМЕННЫЕ ИНЕРЦИАЛЬНЫЕ НАВИГАЦИОННЫЕ СИСТЕМЫ
СОДЕРЖАНИЕ
Предисловие .
.
.
.
.
.
.
.
.
.
1. Основы инерциальной навигации .
.
.
.
.
.
1.1. Фигура Земли
.
.
.
.
.
.
.
.
.
1.2. Гравитационное поле и поле силы тяжести
.
.
.
.
1.3. Системы координат
.
.
.
.
.
.
.
.
1.4. Акселерометры. Основное уравнение инерциальной навигации .
1.5. Обобщенная схема ИНС
.
.
.
.
.
.
.
1.6. Принцип построения бесплатформенной ИНС .
.
.
.
2. Уравнения ориентации .
.
.
.
.
.
.
.
2.1. Уравнения ориентации Эйлера
.
.
.
.
.
.
2.2. Уравнения ориентации Пуассона .
.
.
.
.
.
.
.
.
2.3. Уравнения с параметрами Родрига-Гамильтона.
2.4. Уравнения в параметрах вектора ориентации .
.
.
.
3. Представления скоростей и ускорений
.
.
.
.
4. Алгоритмы БИНС .
.
.
.
.
.
.
.
4.1. БИНС в инерциальной системе координат
.
.
.
.
4.2. БИНС в географическом сопровождающем базисе
с обобщенными уравнениями Пуассона .
.
.
.
.
4.3. БИНС с уравнением в параметрах Родрига-Гамильтона
.
.
4.4. БИНС с вектором ориентации
.
.
.
.
.
.
4.5. Методы и алгоритмы вычислений
.
.
.
.
.
.
.
.
.
.
.
.
4.6. Избыточность измерений
5. Уравнения идеальной и реальной работы.
.
.
.
.
5.1. Уравнения идеальной работы в инерциальной системе координат
5.2. Уравнения реальной работы в инерциальной системе координат .
5.3. Уравнения идеальной работы в географической системе координат
5.4. Уравнения реальной работы в географической системе координат
6. Уравнения ошибок .
.
.
.
.
.
.
.
.
7. Начальная выставка
.
.
.
.
.
.
.
.
8. Демпфирование колебаний
.
.
.
.
.
.
.
8.1. Демпфирование с помощью внутренних связей
.
.
8.2. Демпфирование по скорости от внешней системы .
.
.
. 5
. 7
. 8
. 11
. 15
. 19
. 22
. 25
. 29
. 29
. 31
. 36
. 38
. 43
. 49
. 49
. 53
. 55
. 57
. 60
. 65
. 69
. 69
. 71
. 73
. 74
. 75
. 81
. 88
. 88
. 91
9. Коррекция БИНС .
.
.
.
.
.
.
.
9.1. Разомкнутая схема комплексирования (интегрирования)
со спутниковой навигационной системой (СНС)
.
.
9.2. Замкнутая схема комплексирования с СНС.
.
.
.
9.3. Астрокоррекция в географической системе координат
.
.
.
.
.
.
.
10. Калибровка БИНС. .
10.1. Модели выходных сигналов инерциальных измерителей .
10.1.1. Модель выходных сигналов акселерометров .
.
10.1.2. Модель выходных сигналов гироскопов .
.
.
10.1.3. Упрощение моделей выходных сигналов измерителей
10.2. Калибровка измерительного модуля в производстве
.
10.2.1. Калибровка акселерометров .
.
.
.
.
10.2.2. Калибровка гироскопов
.
.
.
.
.
10.2.3. Температурная калибровка инерциальных измерителей
10.3. Использование результатов калибровки в алгоритме БИНС
10.4. Калибровка и довыставка БИНС на подвижном основании
10.4.1. Алгоритм калибровки и довыставки
.
.
.
10.4.2. Расширение возможностей калибровки и довыставки
11. Моделирование БИНС
.
.
.
.
.
.
Литература
.
.
.
.
.
.
.
.
Приложения
1. Сведения из алгебры кватернионов
2. Характеристики БИНС
.
.
.
.
.
.
.
.
.
.
.
. 95
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
96
105
108
117
117
117
119
121
122
123
126
132
136
140
141
146
152
156
.
.
158
163
5
Предисловие
Инерциальные навигационные системы (ИНС) стали основой навигационных комплексов современных подвижных объектов. Это обусловлено тем,
что они дают полную информацию о навигационных параметрах движения углах курса, тангажа (дифферента), крена; ускорении, скорости движения и координатах места объекта. При этом они полностью автономны, т.е. не требуют
какой-либо информации извне. Благодаря возможности определять угловое положение объекта с высокой точностью в любом диапазоне углов и с высокой
частотой выдачи информации, ИНС к настоящему времени не имеют альтернативы.
Несмотря на активность разработок бесплатформенных инерциальных
систем (БИНС), большое количество научной литературы по БИНС, учебная
литература по БИНС практически отсутствует или устарела. С целью дополнить учебную литературу по БИНС подготовлено это учебное пособие. Оно
может быть использовано как студентами соответствующих специальностей,
так и инженерами. Пособие предполагает наличие базовой подготовки.
Авторы стремились осветить все аспекты работы БИНС. В работе рассматриваются как основной режим работы, так и начальная выставка БИНС.
Выставка рассматривается как на неподвижном, так и на подвижном основании.
Значительное внимание уделяется режимам калибровки параметров и коррекции. В качестве корректирующих систем рассматриваются спутниковая и астронавигационная системы. В пособии освещены наиболее массовые БИНС,
использующие датчики угловой скорости. Рассмотрение БИНС с другими измерителями угловой ориентации объекта выходит за рамки пособия.
Пособие содержит достаточное количество примеров численной оценки
характеристик системы.
Главы 1-9, 11, приложения 1,2 подготовлены Мелешко В.В., гл. 10 подготовлена Нестеренко О.И., п. 2.4 подготовлен совместно.
Авторы высказывают большую благодарность рецензентам Леонцу А.А.,
Панову А.П. за труд по прочтению рукописи. Большинство их замечаний были
учтены и способствовали улучшению качества пособия.
Авторы примут все замечания и пожелания, которые постараются учесть
при подготовке следующего издания. Их можно направить по адресу: Украина,
Киев, 03056, проспект Победы, 37, НТУУ “Киевский политехнический институт”, кафедра приборов и систем ориентации и навигации, Мелешко Владиславу Валентиновичу.
6
Основные сокращения и обозначения
БИНС – бесплатформенная инерциальная навигационная система
ИНС – инерциальная навигационная система
СНС – спутниковая навигационная система
БЦВМ – бортовая цифровая вычислительная машина
ДУС – датчик угловой скорости
ИИМ – инерциальный измерительный модуль
ПК
– преобразователь координат
АЦП – аналого-цифровой преобразователь
ОФК – оптимальный фильтр Калмана
МНК – метод наименьших квадратов
ψ – угол курса
ϑ – угол тангажа
γ – угол крена
ϕ – широта
λ – долгота
r
r – радиус-вектор положения объекта
v – относительная линейная скорость
V , r& – абсолютная линейная скорость
W – абсолютное ускорение
а – кажущееся ускорение
g – ускорение силы тяжести
g' – ускорение силы тяготения (гравитационное)
ω – угловая скорость
u – угловая скорость вращения Земли
R – радиус Земли
RN (R2) – радиус кривизны земного эллипсоида в плоскости меридиана
RE (R1) – радиус кривизны земного эллипсоида в плоскости первого вертикала
(перпендикулярной плоскости меридиана
r
ω – вектор угловой скорости
Ω – матрица угловой скорости
ω × – матричная форма представления вектора
XYZ – инерциальная система координат (базис i )
ξηζ (ENH ) – географический сопровождающий трехгранник (базис g )
ξ Гη Г ζ Г – геоцентрическая земная система координат (базис е )
xc yc zc – связанный с объектом трехгранник (базис b )
C ib – матрица направляющих косинусов, соответствующая преобразованию
от связанного базиса b к инерциальному i
ib
Ω b – матрица угловой скорости вращения от связанного базиса b
к инерциальному i в проекциях на оси связанного базиса b
ib
Λ
– кватернион поворота от связанного базиса к инерциальному
Основы инерциальной навигации
7
1. Основы инерциальной навигации
Идея создания инерциальных навигационных систем (ИНС) возникла в
начале 20-го века. Базовым чувствительным элементом такой системы стал
акселерометр – прибор для измерения ускорения движения объекта. Поскольку
акселерометр, а также и используемые в ИНС гироскопы являются
инерциальными чувствительными элементами (основаны на использовании
инерции массы), системы называют инерциальными.
Метод навигации, используемый в системе – метод счисления пути. Суть
его в том, что в заданной системе координат измеряют проекции абсолютного
ускорения, интегрируя их дважды, получают скорость и текущие координаты.
Поскольку измерение должно проводиться в заданной системе координат,
необходимы гироскопические устройства, обеспечивающие моделирование
(физическое или математическое) этой системы. Только к 40-м годам прошлого
века уровень гироскопов и акселерометров поднялся до такого, который
позволил приступить к реальной разработке ИНС.
Если схема построения системы для измерения координат в
инерциальном пространстве (относительно звезд) сравнительно проста, то
решение задачи определения параметров движения относительно вращающейся
Земли было сложнее. Для решения такой задачи плодотворным стало
использование маятника Шулера. Хотя такой маятник является математической
абстракцией, в ИНС удалось построить его электромеханический аналог,
который обеспечил невозмущаемость системы ускорениями движения
относительно Земли и значительно снизил рост ошибок.
С 60-х годов прошлого века началась активная разработка
бесплатформенных
инерциальных
навигационных
систем
(БИНС).
Привлекательность таких систем в том, что в них не требуется
гиростабилизированная платформа (ГСП) – наиболее сложный, следовательно,
дорогостоящий узел ИНС. В БИНС акселерометры и гироскопы устанавливают
непосредственно на борту объекта, а электромеханическую модель (ГСП)
системы координат заменяют математической моделью. Однако эти
преимущества не удавалось сразу реализовать в связи с жесткими условиями
работы чувствительных элементов (ЧЭ) непосредственно на борту при высоких
требованиях к их характеристкам. При этом также предъявлялись высокие
требования к вычислительным устройствам. Наконец, к 80 годам было освоено
серийное производство БИНС, в первую очередь, в США.
В настоящее время возможности БИНС существенно возрастают. Связано
это с прогрессом в производстве высокоточных гироскопов и акселерометоров,
вычислительной техники. Появились массовые микрогабаритные БИНС,
построенные на микромеханических ЧЭ. При этом большое значение имеют
возросшие возможности коррекции от спутниковых навигационных систем
(СНС), также получивших существенное развитие в последние десятилетия. В
результате сфера применения БИНС заметно расширяется - беспилотные
летательные аппараты, наземные транспортные средства, роботы и др.
Основы инерциальной навигации
8
1.1.
Фигура Земли
Наиболее простая модель фигуры (формы) Земли – сфера радиусом
R=6371,3 км. Объем именно такой сферы наиболее близок к объему реальной
Земли. Для такой модели введены геоцентрические координаты:
геоцентрическая широта ϕ ′ - угол между плоскостью экватора и геоцентрической вертикалью места, изменяется в пределах ± 90o , положительные значения
соответствуют северной широте, отрицательные - южной ;
долгота λ - угол между нулевым (гринвичским) меридианом и меридианом
места, изменяется в пределах от 0o...360o или ± 180o , положительные значения
соответствуют восточной долготе, отрицательные - западной.
Длина окружности сферической Земли равна примерно 40000 км. Длине дуги в
1 градус соответствует 111 км. Дуга в одну
угловую минуту (1' ) равна 1853,3 метра и
получила название морская миля. Более
точно, перемещение на одну морскую милю
вдоль меридиана примерно соответствует
изменению на одну минуту широты. Международная морская миля равна 1852 метрам. Международный кабельтов равен 0,1
морской мили и соответствует 185,2 м.
Такое представление нашей планеты подхоРис.1.1. Сечение эллипсоида
дит для задач, точность вычислений в которых не превышает 0,5%.
Более точно фигура Земли описывается двухосным эллипсоидом,
сплюснутым у полюсов. Меридиональное сечение эллипсоида показано на
рис.1.1. Здесь а – большая полуось эллипсоида, b – малая полуось. Основные
характеристики эллипсоида
a−b
a 2 − b2
- сжатие,
- эксцентриситет, e 2 ≈ 2α = 6,72е-3.
α=
e=
a
a
Важными характеристиками эллипсоида являются радиусы кривизны его поверхности:
радиус кривизны в плоскости, перпендикулярной плоскости меридиана
R1 ≡ RE =
a+h
2
2
1 − e sin ϕ
и радиус кривизны в плоскости меридиана
,
(1.1)
Основы инерциальной навигации
R2 ≡ R N =
RE ( 1 − e 2 )
1 − e 2 sin 2 ϕ
=
9
a( 1 − e 2 )
(1 − e
2
2
sin ϕ
)
3
.
(1.2)
2
Индексы E и N означают восточную и северную составляющие соответственно.
В этих выражениях ϕ - географическая (геодезическая) широта, равная углу
между плоскостью экватора и нормалью к поверхности эллипсоида. Между
геоцентрической и географической широтой существует соотношение
ϕ − ϕ' = α sin 2ϕ' ,
α ≈ 11,5 угл.мин.
Радиусы кривизны могут быть записаны приближенно, используя разложение в
1
≈ 1 m nx + ...
ряд в виде
1 ± xn
⎞
⎞
⎛ 1
⎛ 3
R1 ≡ RE = a ⎜1 + e 2 sin 2 ϕ + ... ⎟ ,
R2 ≡ RN = a( 1 − e 2 )⎜1 + e 2 sin 2 ϕ + ...⎟ .
⎠
⎠
⎝ 2
⎝ 2
Радиус параллели ρ = R1 cos ϕ .
В разных странах существуют различные модели эллипсоидов. Например,
сжатие их находится в пределах α =1/295 … 1/299. Для того, чтобы наилучшим
образом приблизить к эллипсоиду реальную поверхность Земли на ограниченной территории, введены референц-эллипсоиды.
Референц-эллипсоид (от лат. referens – сообщающий, вспомогательный) –
земной эллипсоид с определёнными размерами и положением в теле Земли,
служащий вспомогательной математической поверхностью, к которой приводят
результаты всех геодезических измерений на земной поверхности.
Референц-эллипсоид наилучшим образом согласуется с поверхностью фигуры,
наиболее точно соответствующей форме Земли - геоидом на ограниченной части его поверхности.
Требования к референц-эллипсоиду:
1) Ось вращения должна быть параллельна оси вращения Земли
2) Плоскость экватора должна быть параллельна плоскости земного экватора
3) Сумма квадратов отступлений геоида от общеземного эллипсоида должна
быть наименьшей из всех возможных для данной территории.
Существуют и другие требования.
На территории Советского Союза использовали референц-эллипсоид
Красовского (1964 г.) с параметрами a=6378245 м , b=6356863 м, α =1/298,3,
e 2 ≈ 2α = 6,72е-3.
В США и Канаде используют эллипсоид Кларка, в Италии – Хейфорда, в
Норвегии – Бесселя. С расширением использования глобальных навигационных
10
Основы инерциальной навигации
систем (например, спутниковых) широко используют общеземной эллипсоид
(ОЗЭ).
Общеземной эллипсоид – наилучшим образом согласующийся с поверхностью геоида в целом.
Требования к общеземному эллипсоиду:
1) Центр эллипсоида должен совпадать с центром масс Земли
2) Плоскость экватора и малая ось его должны совпадать соответственно с
плоскостью экватора и осью вращения Земли
3) Объем его должен быть равен объему геоида
4) Сумма квадратов отступлений геоида от общеземного эллипсоида должна
быть по всей Земле наименьшей из всех возможных:
5) Сумма квадратов уклонений отвесных линий (см. п.1.3) должна быть по всей
Земле наименьшей из всех возможных.
В США принят общеземной эллипсоид системы World Geodetic System (WGSb=6356752,3 м α =1/298,257.
84) с параметрами а=6378136,991 м
В России действует государственная система координат «Параметры
Земли 1990 года» (ПЗ-90). За отсчетную поверхность в государственной геоцентрической системе координат (ПЗ-90) принят общий земной эллипсоид со
следующими геометрическими параметрами:
– большая полуось 6378 136 м;
– сжатие 1:298,257839;
– центр этого эллипсоида совмещен с началом геоцентрической системы координат; плоскость начального (нулевого) меридиана совпадает с плоскостью ZX
этой системы.
Масса Земли M, включая массу ее атмосферы, умноженная на постоянную тяготения G, составляет
геоцентрическую гравитационную постоянную GM = 398600,44 км3/c2,
гравитационное ускорение на экваторе Земли - 978 032,84 мгал (1 Гал=1 см/с2,
1 миллиГал=10-3 см/с2).
Поправка к гравитационному ускорению на уровне моря, обусловленная
влиянием атмосферы Земли - 0,87 мГал.
Основные параметры общеземных эллипсоидов сведены в таблицу 1.1.
Геоид - фигура, соответствующая эквипотенциальной поверхности силы
тяжести (соответствует уровню океана). По определению эквипотенциальной
поверхности, поверхность геоида везде перпендикулярна отвесной линии.
Форма геоида отличается от фигуры (формы) Земли. Эти отклонения внесены
в карту. На ней приведены изолинии равных отклонений уровня геоида от
уровня эллипсоида. Отрицательное отклонение достигает величины - 105 метров и находится на севере Индийского океана, а положительное отклонение
достигает 85 метров и находится на западе Тихого океана.
Основы инерциальной навигации
11
Таблица 1.1. Параметры общеземных эллипсоидов систем WGS – 84 и ПЗ-90
Параметры
Обозна-
WGS – 84
ПЗ-90
чения
Большая полуось эллипсоида
а
6378137 м
6378136 м
Сжатие
α
1/298,25722356
1/298,257839
7,292115·10-5
7,292115·10-5
рад/с
рад/c
398600,5 км3/с2
398600,44 км3/c2
Угловая скорость вращения Земли
Геоцентрическая гравитационная
постоянная с учетом массы атмосферы Земли
1.2.
u
GM
Системы координат
Определения координат объектов, их навигация могут проводиться в различных системах координат.
Инерциальная геоцентрическая система координат OXYZ имеет ось OX, направленную по линии равноденствия в точку весеннего равноденствия [1], ось
OZ направлена по оси вращения Земли, ось OY образует с осями OX и OZ правый координатный трехгранник. В иностранной литературе эту систему координат иногда обозначают ECI (Earth-centered inertial), кратко обозначают буквой i . (Иногда в качестве инерциальной принимают стартовую систему координат, положение которой известно относительно системы i )
Относительно этой системы
координат вместе с Землей вращается геоцентрическая земная
система координат О ξ Гη Г ζ Г .
Ее обозначают ECIF (Earthcentered earth-fixed), кратко
обозначают e. Угол поворота ее
соответствует величине ut , где
u - угловая скорость вращения
Земли, t – время. Ось Oη Г находится в плоскости гринвичского меридиана.
Сопровождающая
система
координат
(сопровождающий
трехгранник) имеет начало в
Рис.1.2. Системы координат
точке на поверхности Земли,
положение которой задано широтой ϕ и долготой λ . Если широта задана как
12
Основы инерциальной навигации
угол между плоскостью экватора и геоцентрическим радиусом (вертикалью),
она называется геоцентрической и обозначается ϕ ' . Оси сопровождающего географического трехгранника O1ξηζ (или O1ENH) направлены так: ось O1ξ по
касательной к параллели на восток, ось O1η направлена на Север, ось O1ζ направлена по вертикали. Иногда этот трехгранник обозначают буквой g. Относительно нее повернута на угол χ в плоскости горизонта свободная в азимуте
система координат. Если угол χ является путевым углом ортодромии, то
O1ξ0η0ζ - ортодромическая система координат. Связанную с объектом систему
координат (на рис. не показана) будем обозначать O1 xc yc zc , O1 yc - продольная
ось объекта, O1 xc - поперечная ось объекта (на правый борт), O1 zc - нормальная
ось объекта (вверх). Такую систему координат иногда обозначают буквой b (от
слова body).
Сопровождающие трехгранники
Сопровождающий трехгранник вращается в инерциальном пространстве
с угловой скоростью, проекции которой можно выразить соответствующими
соотношениями.
Географический сопровождающий трехгранник
Проекции угловой скорости вращения трехгранника (системы координат,
базиса) относительно инерциального пространства (абсолютной угловой скорости) можно записать в следующем виде
ωξ = −v N / (R2 + h );
ωη = vΕ / (R1 + h ) + u cosϕ ;
ως = (v Ε / (R1 + h ))tg ϕ + u sin ϕ ;
h = h0 + vζ t;
(1.3)
ως = ωη tgϕ .
Кроме ранее введенных обозначений, здесь v N = v cos χ - северная составляющая вектора относительной скорости движения объекта, v E = v sin χ - восточная
составляющая, h – высота объекта, h0 - ее начальное значение, vζ - вертикальная составляющая скорости, t – время.
Иногда вектор угловой скорости представляют в виде суммы
r r r
ω = u + ωo ,
r
r
где u - вектор угловой скорости вращения Земли, а ω o - вектор угловой скорости из-за перемещения объекта относительно Земли (относительная угловая
скорость). Эти векторы можно записать в проекциях на оси сопровождающего
базиса как
r
r
u = [0, uη , uξ ]T ;
ωξo = [ωξo , ωηo , ωζo , ]T .
(1.4)
Основы инерциальной навигации
13
Здесь uη = u cos ϕ , uζ = u sin ϕ ;
ωξo = −v N / (R2 + h ), ωηo = v E / (R1 + h ), ωζo = v E tgϕ / (R1 + h ).
Абсолютная линейная скорость равна сумме относительной линейной
скорости и линейной скорости из-за вращения Земли. Проекции абсолютной
линейной скорости представляют в следующем виде
VN = V cos χ , VE = V sin χ , VN = v N , VE = vΕ + (R1 + h )u cosϕ .
Отсюда следует форма записи проекций абсолютной угловой скорости географического сопровождающего трехгранника
ωξ = −VN / (R2 + h );
ωη = VΕ / (R1 + h );
ως = V Ε tg ϕ / (R1 + h );
Если использовать выражение скорости изменения долготы
λ& = v Ε / ((R 1 + h ) cos ϕ ) ,
то (1.3) можно переписать в другом виде:
ωξ = −VN / (R2 + h ) ;
ωη = (u + λ& )cosϕ ;
ως = (u + λ& )sin ϕ .
Ортодромический сопровождающий трехгранник
Выражения для проекций абсолютной угловой скорости ортодромического трехгранника можно получить перепроектированием проекций угловой скорости географического трехгранника [2]
ωξо = −u cos ϕ sin χ − v / (R + h );
ωηо = u cos ϕ cos χ ;
ωςо = u sin ϕ ;
R – радиус сферической Земли
Если есть боковое движение со скоростью vξo :
ωξо = −u cos ϕ sin χ − vηo / R ; R – радиус сферической Земли
ωηо = u cosϕ cos χ + vξo / R ;
ωςо = u sin ϕ −
vξ o
R
tg Φ ,
Основы инерциальной навигации
14
где Ф – ортодромическая широта1 [1].
Существуют другие варианты описания движения ортодромического сопровождающего трехгранника, в том числе c учетом эллиптичности Земли [1].
Матрица направляющих косинусов между осями гринвичской системы
координат 0ε Г η Г ς Г и осями полусвободной (ортодромической) в азимуте системы координат 0ε 0η0ς (рис.1.4) имеет вид:
Таблица 1.2. Направляющие косинусы между осями
гринвичского и ортодромического трехгранников
C ge ξ Г
ξ0
η0
ς0
ηГ
c11 = − sin ϕ cos λ sin ε −
ςГ
c12 = − sin ϕ sin λ sin ε +
− sin λ cos ε ;
+ cos λ cos ε ;
c21 = − sin ϕ cos λ cos ε + c22 = − sin ϕ sin λ cos ε −
+ sin λ sin ε ;
− cos λ sin ε ;
c31 = cos λ cos ϕ ;
c32 = sin λ cos ϕ ;
c13 = cos ϕ sin ε ;
c23 = cos ϕ cos ε ;
b33 = sin ϕ .
Матрица обозначена как C ge , где вторая буква индекса обозначает исходный
трехгранник, а первая буква индекса – конечный трехгранник.
Рис.1.3. Гринвичский и ортодромический трехгранники
Трехгранник 0ε 0η0ζ движется с относительной угловой скоростью ω o , свяr
r
r r
занной с путевой скоростью v соотношением v = ω o × R , где R - радиус кривизны земного эллипсоида в плоскости траектории.
1
В ортодромической системе координат ортодромия является экватором, полюс системы координат
соответствует точке пересечения земной сферы с перпендикуляром к плоскости ортодромии, проведенном из центра Земли.
Основы инерциальной навигации
15
ro
Проекции вектора ω на оси трехгранника 0ε 0η0ζ представим в виде:
ω oξ 0 = −
vη 0
Rη 0
−
vξ 0
a
e 2b13b23 ,
ω oη 0 =
vξ 0
Rξ 0
−
vη 0
a
e 2b13b23 .
Здесь Rη 0 и Rξ 0 - радиусы кривизны нормальных сечений эллипсоида в плоскостях Oη0ζ и Oξ0ζ соответственно, e 2 - квадрат эксцентриситета земного эллипсоида.
Величины, обратные радиусам кривизны, вычисляются по соотношениям:
1
1
h⎞
⎛ 1
2
2
= ⎜1 − e 2b33
+ e 2b13
− ⎟ a;
Rξ 0 ⎝ 2
a⎠
2
1
1
h⎞
⎛ 1
2
2
= ⎜1 − e 2b33
+ e 2b23
− ⎟ a,
Rη 0 ⎝ 2
a⎠
2
где h – высота, а – большая полуось земного эллипсоида.
1.3.
Гравитационное поле и поле силы тяжести Земли
В соответствии с законом всемирного тяготения все тела притягиваются друг
к другу с силой, прямо пропорциональной произведению масс этих тел и обратно пропорциональной квадрату расстояния между ними:
F =G
Mm
,
r2
(1.5)
где F — сила притяжения; М и т — массы двух взаимно притягивающихся тел
(гравитирующие массы); r — расстояние между ними; G — коэффициент пропорциональности, называемый гравитационной постоянной и являющийся
фундаментальной константой физики. С другой стороны, сила притяжения,
действующая на материальную точку массы m, определяется формулой
r
r
F = mg ' ,
(1.6)
r
где g ' — гравитационное ускорение, или ускорение силы тяготения. Сила приr
тяжения F и гравитационное ускорение имеют одинаковое направление.
Сопоставляя формулы (1.5) и (1.6), находим
M
g' = G 2 .
(1.7)
r
Основы инерциальной навигации
16
Величина гравитационной постоянной G = 6,6742 ⋅ 10−11 м 3 ( с 2 кг ) . Как показано в табл.1.1., для эллипсоида ПЗ-90 произведение GM ≡ K =398600,44 км3/c2.
Приведенное выражение представляет простейшую модель гравитационного поля. Реальное поле имеет более сложный характер. Оно может быть
r
представлено составляющими вектора g ' в плоскости меридиана.
Радиальная составляющая, направленная к центру Земли [1]
2
(
)
K
3μ ⎛ a ⎞
2
g r′ = − 2 [1 +
⎜ ⎟ 1 − 3 sin ϕ ' ] .
2 ⎝r⎠
r
(1.8)
Здесь общий знак минус говорит о том, что составляющая направлена против
направления радиуса из центра Земли, μ = 1,09 ⋅ 10−3 - коэффициент, характеризующий распределение масс Земли, а – большая полуось эллипсоида, r - геоцентрический радиус, ϕ ' - геоцентрическая широта.
Трансверсальная составляющая,
направленная в плоскости горизонта к
плоскости экватора (противоположно
отсчету широты)
2
K 3μ ⎛ a ⎞
gϕ′ = − 2 ⋅ ⎜ ⎟ sin 2ϕ ' .
r 2 ⎝r⎠
(1.9)
Сила тяжести есть равнодействующая силы тяготения и центробежной силы (силы инерции центростремительного
ускорения из-за вращения
r
r r v
Земли Wцс = u × (u × R ) ). Ускорение силы тяжести в общем случае соответст-
Рис.1.4. Проекции ускорений
вует выражению
r r r r v
g = g '−u × (u × R ) .
(1.10)
Wцс = u 2 RE cosϕ .
Величина центростремительного ускорения
В [1] получены составляющие ускорения силы тяжести в следующем виде
g r = − g э0
a2
(a + h )
gϕ = − g э0α
2
(1 + β sin2 ϕ '),
(1.11)
a2
(a + h )
2
sin 2ϕ ' .
Основы инерциальной навигации
17
Здесь g э0 - ускорение силы тяжести на экваторе, α - сжатие эллипсоида.
Величины β и g э0 имеют следующие значения
β =0,005317,
g э0 =978,049 см/с2.
Отношение величин
gϕ
≈ α sin 2ϕ '
(1.12)
gr
r
соответствует тангенсу угла между вектором g ускорения силы тяжести и геоцентрическим радиусом вектором точки местоположения. В силу малости величины тангенса, можно принять, что он равен углу. Как видно из формулы,
максимальный угол на широте 45о имеет величину 0,003356, что соответствует
величине около 11,5 угловых минут.
Абсолютную величину ускорения силы тяжести g = g r2 + gϕ2 в силу малости (1.11) можно принять равной абсолютной величине g r .
Для расчетов величины ускорения силы тяжести иногда применяют формулу
Гельмерта (1909 г.):
g = 9,78030(1 + 0,005302 sin 2 ϕ − 0,000007 sin 2 2ϕ ) ,
где используют географическую широту ϕ .
Формула Кассиниса (1930 г.), называемая международной, имеет вид:
g = 9,78049 (1 + 0,0052884 sin2 ϕ - 0,0000059 sin22 ϕ ).
(1.13)
Для свободной в азимуте системы координат ξ0η0ζ трансверсальная составляющая гравитационного ускорения может быть представлена формулами
gξ' o =
− u 2 RE sin 2ϕ sin χ
,
2
gη' o =
u 2 RE sin 2ϕ cos χ
.
2
(1.14)
Появление этих проекций можно увидеть из рис.1.4 (проекция g 'η ).
На рис.1.5 представлены графики изменения ускорений, рассчитанные по формулам (1.9), (1.11).
С изменением высоты ускорение изменяется в соответствии с зависимостью
g( h ) =
gζ′
(1 + h / R )2
.
Приращения ускорения силы тяжести можно рассчитать по формуле
Основы инерциальной навигации
18
Δg = −
2g
Δh .
R
Если Δg измеряется в мГал, а Δh в метрах, то Δg = −0,3086Δh . При увеличении высоты на 100 м сила тяжести уменьшится на 30,9 мГал. Для объектов, находящихся под водой, действует следующий закон изменения ускорения силы
тяжести
δg = 0,3086Δh − 0,0838σ ⋅ Δh ,
где σ = 1,03 г/см3 – плотность морской воды. С учетом приведенной плотности
δg = +0,225Δh .
Радиальная составляющая ускорения
-9.77
-9.78
силы тяжести
-9.8
r
g , м/с2
-9.79
гравитационная
-9.81
-9.82
-9.83
-9.84
0
10
20
30
40
50
60
70
80
90
70
80
90
Трансверсальная составляющая ускорения
0
-0.005
f
g , м/с2
-0.01
гравитационная
-0.015
-0.02
-0.025
силы тяжести
-0.03
-0.035
0
10
20
30
40
50
широта, град.
60
Рис.1.5. Графики проекций ускорений
При углублении в воду на 100 м сила тяжести увеличится на 22,5 мГал.
Иногда необходимо знать проекции гравитационного ускорения на оси
инерциальной системы координат. Для этого сначала необходимо рассчитать
модуль ускорения g ' в точке с заданными инерциальными координатами
X,Y,Z, а затем получить необходимые проекции в соответствии с рис. 1.6.
r
fM r
fM r
g = − 2 r0 = − 3 r ;
r
r
Основы инерциальной навигации
19
Рис.1.6. Проекции гравитационного
ускорения
g ' x = − g ' cosα1 = − g '
X
Y
Z
; g ' y = − g ' cosα 2 = − g ' ; g ' z = − g ' cosα1 = − g ' ; (1.15)
r
r
r
Существуют более точные выражения (п. 4.1).
Аномалии гравитационного поля
Гравитационное поле Земли, следовательно, и поле силы тяжести, весьма
неоднородно. Реальные значения составляющих поля отличаются от расчетного
«нормального» поля. Отклонения величины ускорения могут достигать 0,05% и
составить величину до 160 мГал . Направления вектора ускорения также могут
меняться. Уклонение (отклонение) отвесной линии может достигать 40'' (угловых секунд). На территории СНГ максимальные уклонения достигают 4''. Картину изменения составляющих гравитационного поля представляют на соответствующих картах.
1.4.
Акселерометры.
Основное уравнение инерциальной навигации
Инерциальные навигационные системы в качестве измерителей используют
инерциальные чувствительные элементы: акселерометры и гироскопы.
Акселерометры предназначены для измерения ускорения движения объекта,
на котором они установлены. Рассмотрим принцип его работы.
В корпусе прибора (рис.1.7), установленного на подвижном объекте, расположена инертная масса 1, движение которой ограничено пружиной 2. При действии ускорения движения объекта W, инертная масса вследствие инерции перемещается вдоль оси чувствительности x до тех пор, пока сила инерции не
уравновесится силой упругости пружины. Но кроме силы инерции от действия
ускорения W, на инертную массу действует также сила тяготения с гравитаци-
Основы инерциальной навигации
20
онным ускорением g'. Для уменьшения времени переходного процесса исполь1
3
2
Рис.1.7. Осевой акселерометр
зуют демпфер 3. Выходной сигнал акселерометра (обычно электрический) соответствует перемещению инертной массы относительно корпуса акселерометра по оси x.
Составим уравнение движения инертной массы в корпусе, с которым связана система xy (связанная с объектом). Используем для этого метод кинетостатики. В соответствии с принципом дАламбера, сумма активных сил, сил
инерции и сил реакции связей равна
r нулю
r
r[3]:
Fa + FR + Fи = 0 .
Поскольку инертная масса имеет лишь одну степень свободы относительно оси
x , составим уравнение равенства проекций сил на эту ось:
Fax = mg ′x - силы тяготения,
FRx = − cx − fx& - реакций связей (пружины и демпфера),
Fиx = −m( W x + &x& ) - силы инерции.
В этих выражениях m – масса , с – линейная жесткость пружины, f – линейный
коэффициент демпфирования, &x& - относительное ускорение инертной массы
(относительно корпуса).
В результате
− m( Wx + &x& ) − cx − fx& + mg ′x =0,
m&x& + fx& + cx = − m( Wx − g ′x ) = − ma x .
(1.16)
Здесь a x = W x − g ′x - называют кажущимся ускорением.
В общем случае [1]
r r r
a = W − g'
- основное уравнение акселерометра
- основное уравнение инерциальной навигации
(1.17)
Основное уравнение подчеркивает важнейшую особенность акселерометра –
инертная масса всегда находится под действием как силы инерции, так и силы
тяготения.
Поскольку эту особенность обязательно необходимо учитывать при проектировании алгоритма работы ИНС, уравнение называют также основным
уравнением инерциальной навигации.
Основы инерциальной навигации
21
Рассмотрим характерные примеры сигнала акселерометра.
Примеры:
Уравнение статики акселерометра можно записать из (1.16) в виде
m
x = − ax .
c
1. На рис. 1.8а акселерометр движется с ускорением W так, что ось чувствительности его горизонтальна. В этом случае
m
Wx = W , g ' x = 0, x = − W т.к. a x = W − 0 .
c
а)
2.
3.
б)
Рис.1.8. Поведение акселерометра
в)
На рис. 1.8б акселерометр стоит на столе так, что ось чувствительности его вертикальна. Ускорение движения отсутствует. В
этом случае
m
Wx = 0, g ' x = − g ' , x = − g ' т.к. a x = 0 − g '.
c
На рис. 1.8в акселерометр свободно падает так, что ось чувствительности его вертикальна. В этом случае
x = 0 т.к. a x = − g '+ g ' , Wx = − g ' ,
g'x = −g';
Эти примеры показывают, что для определения величины ускорения движения
объекта необходимо в выходном сигнале акселерометра учитывать или исключать из него проекцию гравитационного ускорения.
Существует множество вариантов конструкций акселерометров. Можно
отметить, что в настоящее время в ИНС применяют, в основном, маятниковые
акселерометры компенсационного типа. Проектирование акселерометров - отдельное направление в навигационном приборостроении и в настоящем учебном пособии не рассматривается. Приведем только таблицу характеристик 1.3.
Основы инерциальной навигации
22
1.5.
Обобщенная схема ИНС
В основе построения инерциальных навигационных систем (ИНС) лежит
метод счисления пути. Суть его состоит в том, что сигналы измерителей скорости или ускорения, оси чувствительности которых удерживаются в заданной
системе координат, интегрируются. Интегралы скорости соответствуют приращениям пути, интегралы ускорений соответствуют приращениям скорости.
Складывая значения приращений с начальными значениями пути или скорости,
получают текущие значения пройденного пути и скорости. В системах, где первичными измерителями являются акселерометры, для получения скорости сигнал акселерометра интегрируют один раз, а для получения пройденного пути
этот интеграл (скорость) интегрируют второй раз. Зная направления проекций
пройденного пути на оси системы координат, определяют координаты подвижного объекта.
Устройство ИНС, реализующей метод счисления пути, можно проиллюстрировать обобщенной схемой, приведенной на рис.1.9.
+
Рис.1.9. Обобщенная схема платформенной ИНС
На рис.1.9. показаны:
1 - ГСП – гиростабилизированная платформа, удерживающая оси чувствительности акселерометров в заданной системе координат; 2 - А – блок акселерометров (трехосный акселерометр); 3,6,8 - сумматоры; 4,7 - интеграторы; 5 – вычислитель вектора гравитационного ускорения; 9 - обратные связи;
введены следующие обозначения:
r r
r r
a , g ' - векторы кажущегося и гравитационного ускорений соответственно; W ,V
- векторы
абсолютного ускорения и абсолютной скорости соответственно;
r r
ΔV , Δr - приращения абсолютной скорости и радиуса-вектора местоположения
r r
r
объекта; r - радиус-вектор местоположения объекта; ΔV0 , Δr0 - начальные значения векторов; ψ ,ϑ , γ - углы ориентации объекта (возможно, курс, тангаж,
крен).
Если в системе имеются обратные связи 9 по скорости или по координатам, систему называют замкнутой, если обратных связей нет – система разомкнутая.
КИ-67-11
«Ротор»
(Россия)
КЕАФ-67
«Электромех
аника»
(Россия)
Ракетные ИНС
АТ-1104
«Темп»
(Россия)
АК-10
«Дельфин»
(Россия)
А-4Т
Litton,
Sagem (Фр.)
Основная область
Корабельные Наземные
применения
ИНС
ИНС
Диапазон измерения, g
±10; ±20;
±20;
±10
±10
±20; ±40
±10; ±20
±2
±10
Пороговая
0.5
0.5
0.5
1
5
1
0.1
5
-6
чувствительность, *10 g
Основная погрешность
«нуля»:
-в запуске, *10-5 g/час
0.5
2
3
5
10
2
2.5
10
-5
-от запуска к запуску, *10
5
3 ... 5
5
15
60
10
3 ... 5
10 ... 15
g
0.5 ... 1
2
3 ... 5
3 ... 5
2
5
1
5
Основная погрешность
КП, *10-6 %
Температурные
погрешности
1 ... 3
2
3
5
10
2
-«нуля» , *10-5 g⋅ 0С-1
-4 0 -1
1
0.5
1.8
1.5
5
0.5
-КП
, *10 ⋅ С
Диапазон рабочих
-60 ...+80
-60 ...+80
-60 ...+85
-60 ...+80
-60 ...+80
-50 ...+85
-50 ...+70
-40 ...+55
0
температур, С
Габаритные размеры , мм
∅30×20
∅25×21
∅20×24
38×38×25
(масса) ,г
(50)
(80)
(100)
(70)
(50)
(40)
(45)
(115)
Время готовности, с
30
60
30
30
120
60
250
Цена , тыс.$
3 ... 4
3 ... 4
2 ... 2.5
4 ... 5
1.5 ... 2
1.2
Вид выходного сигнала
U=/f
I=/f
U=/f
I=/f
U=/f
U=/f
U=
U=/f
А/К
КП – коэффициент передачи, А/К – аналоговый сигнал/кодовый сигнал, U= постоянное напряжение, I= - постоянный ток, f – частота
Характеристика
A-4 Litton,
А-12, А-15
АК-6
РПКБ
«Авиаприбор
QA-3000
Sundstrand
(Россия)
»
(США)
(Россия)
Авиационные ИНС
Модель акселерометра, фирма-изготовитель.
Таблица 1.3. Основные технические характеристики прецизионных маятниковых компенсационных акселерометров.
23
24
Основы инерциальной навигации
В БИНС вместо гиростабилизированной платформы используют блок
гироскопов и акселерометров с вычислителем. Гироскопы с акселерометрами
(блок инерциальных чувствительных элементов) при этом устанавливают
жестко на борту объекта. На рис.1.10. показаны 1 – блок чувствительных
элементов, выдающих информацию о векторе кажущегося ускорения a xyz в
проекциях на оси связанной с объектом системы координат xyz , а также о
векторе угловой скорости θ&xyz в проекциях на оси той же системы координат. В
вычислителе 2 происходит преобразование проекций ускорения из связанной
системы в навигационную (например, географическую сопровождающую). Для
этого по данным об угловой скорости (или другой информации) вычисляют
направляющие косинусы между осями указанных систем координат. Из них
также вычисляют углы положения объекта: курс ψ , крен ϑ , тангаж γ .
Рис.1.10. Обобщенная схема БИНС
В БИНС используют различные гироскопические датчики. Они могут
выдавать информацию о различных угловых параметрах. В таблице 1.4
приведены данные о типах используемых в настоящее время в БИНС
гироскопических датчиков.
Тип гироскопа
Таблица 1.4. Типы применяемых гироскопических датчиков
Угловая
Выходные
скорость ухода параметры
Поплавковые ДУС
Лазерные гироскопы
Волоконно-оптические гироскопы
Динамически настраиваемые гироскопы
Микромеханические гироскопы
Твердотельные волновые гироскопы
Сферические гироскопы с
электростатическим подвесом ротора
Сферические гироскопы с магнитным
подвесом ротора
Сферические гироскопы с воздушным
подвесом ротора
до 0,01 о/час
до 0,001 о/час
до 0,01 о/час
до 0,01 о/час
до 10 о/час
до 0,01 о/час
до 10-5 о/час
до 10-4 о/час
до 0,01 о/час
Угловая скорость
Угл. скор., Угол
Угловая скорость
Угловая скорость
Угловая скорость
Угл. скор., Угол
Направляющие
косинусы
Направляющие
косинусы
Направляющие
косинусы
Основы инерциальной навигации
25
Достоинства ИНС:
1. Измерение полного набора навигационных параметров – ускорения,
скорости, координат, углов положения объекта (курс, крен, тангаж),
угловых скоростей объекта и ряда других вспомогательных.
2. Полная автономность, т.е. возможность работать независимо ни от
видимости ориентиров, маяков, светил, ни от положения или движения
объекта; помехозащищенность (невозможно создать помехи, мешающие
работе ИНС).
3. Высокая скорость определения и выдачи данных (100 Гц и более)
4. Невозмущаемость относительными ускорениями, т.е. отсутствие
колебаний гиростабилизированной платформы или ее аналитического
аналога (в БИНС) при действии относительных (относительно Земли)
ускорений. Как следствие, отсутствуют погрешности в выходных данных
по всем навигационным параметрам. При этом собственная частота
колебаний погрешностей, вызванных различными возмущающими
факторами, в основном, соответствует частоте колебаний маятника
Шулера.
Недостатки ИНС:
1. Необходимость ввода начальных условий (начального положения
платформы (объекта для БИНС), начальных значений скорости,
начальных координат), что необходимо для использования метода
счисления пути; необходимость учета формы Земли и параметров
гравитационного поля в точке расположения подвижного объекта.
2. Требуется непрерывность работы (или после перерыва в работе
необходимо вновь вводить начальные условия)
3. Нарастание ошибок со временем
1.5.
Y
yc
Принцип работы БИНС
Y
i
Г
Ax
Ay
xc
Г
i
O1
g
X
r
O
X
Рис.1.10. Положение объекта в
инерциальной системе координат
Рассмотрим принцип работы БИНС в
инерциальной системе координат на
примере плоской задачи. Пусть XOY –
инерциальная
система
координат.
Положение объекта, с которым связана
система координат xcO1 yc , задано
r
радиусом-вектором r .
Гравитационное ускорение в точке
О1 соответствует вектору g ' .по формуле
(1.7)
r
GM r
GM r
g ' = − 2 r0 = − 3 r ,
r
r
26
Основы инерциальной навигации
r
r
где r0 - орт радиуса-вектора r . Знак – учитывает, что ускорение направлено
против направления радиуса-вектора.
На борту объекта жестко установлены акселерометры А и гироскопы Г.
В соответствии с основным уравнением (1.17)
W = a + g' .
Отсюда выражения для проекций абсолютного ускорения в инерциальной
системе координат можно записать в виде
X&& = a xc c11 + a yc c12 + g ' X ;
Y&& = a xc c21 + a yc c22 + g 'Y .
В этих формулах a xc и a yc , проекции кажущегося ускорения, измеряемые
акселерометрами, c11 … c22 - направляющие косинусы между осями
инерциальной и связанной систем координат. Для рис.1.10 матрица
направляющих косинусов
C xc
yc
X c11 = cos θ ; c12 = − sin θ ;
Y c21 = sin θ ; c22 = cos θ ;
Далее запишем проекции гравитационного ускорения:
GM
GM
g ′X = − 2 cos( rX ), gY′ = − 2 cos( rY ) .
r
r
Знак – учитывает, что проекция ускорения направлена против направления оси.
Направляющие косинусы из рис.1.10 можно записать
cos( rX ) = X / r, cos( rY ) = Y / r .
После подстановки
GM
GM
′
g
X
,
=
−
Y.
y
r3
r3
В векторно-матричной форме выражение для ускорения имеет вид
g ′x = −
&rr& = Car − GM rr .
r3
Это соотношение лежит в основе построения и полноразмерной БИНС.
Зная вектор абсолютного ускорения, получим вектор абсолютной линейной
скорости
t
r& ≡ V = ∫ &r&dt + r&0 .
0
Основы инерциальной навигации
27
t
r = ∫ r&dt + r0 .
Аналогично получим радиус вектор места
0
Уравнения идеальной работы в скалярном виде в инерциальной системе
координат
Работу рассмотренной схемы можно описать системой скалярных
уравнений. Предположим, что в качестве гироскопического измерителя
используется датчик угловой скорости. Угол поворота объекта получим как
интеграл от угловой скорости с учетом начального значения угла
t
ϑ = ∫ ω z dt + ϑ0 .
0
В дифференциальной форме это уравнение получает вид ϑ&i = ωz . Выражения
для кажущихся ускорений в инерциальной системе координат имеют вид
a X = a xc cosϑi − a yc sin ϑi ,
aY = a xc sin ϑi + a yc cosϑi .
Проекции абсолютной скорости получим в виде
t
V X = ∫ ( a X + g ' X )dt +V X 0 ,
0
t
VY = ∫ ( aY + g 'Y )dt +VY 0 .
0
В дифференциальной форме
V&X = a X + g ' X ,
V&Y = aY + g 'Y .
Аналогично можно записать выражения для вычисления координат
t
t
0
0
X = ∫ V X dt + X 0 , Y = ∫ VY dt +Y0 .
Уравнения идеальной работы
во вращающейся (земной) системе координат
Подавляющее большинство инерциальных систем решают задачи
навигации на земной поверхности, т.е. в сопровождающем трехграннике.
Северный канал такой БИНС показан на рис. 1.11. Рассмотрим уравнения
идеальной работы в географическом трехграннике.
Угол поворота объекта (связанного базиса 0 xc yc ) в сопровождающем
трехграннике обозначим ϑ . Гироскоп измеряет абсолютную угловую скорость
ω z . Переносная угловая скорость из-за перемещения объекта ωξ = −Vη / (R + h ) ,
28
Основы инерциальной навигации
где R – радиус Земли, h - высота объекта. Исходя из положения, что
абсолютная угловая скорость равна сумме переносной и относительной
скорости, запишем следующие выражения.
t
ϑ& = ωz − Vη / (R + h ), ϑ0 = 0; (ϑ = ∫ ϑ&dt +ϑ0 ; )
(1.18)
0
Эти уравнения можно назвать уравнениями ориентации.
В блоке вычисления направляющих косинусов и преобразования
координат НК и ПК выполняется преобразование ускорений из связанной в
навигационную систему координат
aη = − a xc cosϑ + a yc sin ϑ ,
(1.19)
aζ = a xc sin ϑ + a yc cosϑ .
Рис.1.11. Северный канал БИНС
Из полученных проекций кажущегося ускорения, учитывая основное уравнение
инерциальной навигации ( a = W − g ' ), после интегрирования получим
северную составляющую скорости
t
V&η = aη , Vη 0 = 0; (Vη = ∫ V&η dt +Vη 0 )
(1.20)
0
и вертикальную составляющую скорости
t
V&ζ = aζ + g ' , Vζ 0 = 0; (Vζ = ∫ V&ζ dt +Vζ 0 ) .
(1.21)
0
Далее получаем широту и высоту в соответствии с очевидными выражениями
t
Vη
&
, ϕ 0 = 0; (ϕ = ∫ ϕ&dt +ϕ 0 );
ϕ=
R+h
(1.22)
0
h& = V , h = 0.
ζ
0
Уравнения ориентации
29
2. Уравнения ориентации
Уравнениями ориентации называют дифференциальные уравнения, в результате решения которых получают параметры, характеризующие положение
подвижного объекта относительно выбранной системы координат. Такими параметрами могут быть углы Эйлера – Крылова, направляющие косинусы, параметры Родрига-Гамильтона и др. Исторически первыми уравнениями ориентации были известные в теоретической механике кинематические уравнения Эйлера. Дополним эти уравнения учетом переносного движения сопровождающего трехгранника.
2.1. Уравнения ориентации Эйлера
Уравнения позволяют по измеренным проекциям абсолютной угловой
скорости объекта и вычисленным проекциям угловой скорости сопровождающего трехгранника вычислить углы курса, тангажа и крена объекта.
На рис.2.1 Oξηζ - географический сопровождающий трехгранник, ось
Рис.2.1. Углы и угловые скорости
Oη -направлена на Север, Oxc yc zc - связанный с объектом трехгранник, ψ угол курса, ϑ - угол тангажа, γ - угол крена; ωξ , ωη , ωζ - проекции угловой
скорости сопровождающего трехгранника; ω xc , ω yc , ω zc - проекции абсолютной
угловой скорости объекта, измеряемые датчиками угловой скорости; ψ& ,ϑ&, γ& -
проекции относительной угловой скорости. Исходя из того, что абсолютная
скорость равна сумме относительной и переносной скоростей
Уравнения ориентации
30
r
r
r
ωабс = ωпер + ωотн ,
составим выражения проекций угловых скоростей.
Сумма проекций на ось О1 даст
γ& = ω ус + tgϑ (ωxс sin γ − ωzс cosγ ) −
1
(ωη cosψ + ωξ sinψ ).
cosϑ
(2.1)
Сумма проекций на на ось О2 даст
ψ& = ω xс sin γ − ω zс cos γ + tgϑ (ωη cosψ + ωξ sinψ ) + ωζ .
(2.2)
Сумма проекций на на ось О3 даст
ϑ& = ω xс cos γ + ω zс sin γ + ωη sin ψ − ωξ cos ψ .
(2.3)
Недостаток уравнений (2.1) и (2.2) – их некорректность при ϑ = ±90o . Следовательно, использовать их можно только ограниченном диапазоне углов ϑ или же
необходимо исключать некорректную ситуацию дополнительными алгоритмами.
Уравнения могут быть представлены в векторно-матричной форме
⎡ψ& ⎤ ⎡ sinγ
⎢ϑ& ⎥ = ⎢ cosγ
⎢ ⎥ ⎢
⎢⎣γ& ⎥⎦ ⎢⎣tgϑ sinγ
− cosγ ⎤ ⎡ωxс ⎤
⎡ sinψ sinϑ cosψ sinϑ cosϑ ⎤ ⎡ωξ ⎤
1 ⎢
⎢ ⎥
0
sinγ ⎥ ⋅ ⎢ωyс ⎥ +
− cosψ cosϑ sinψ cosϑ
0 ⎥ ⋅ ⎢ωη ⎥ .
⎥ ⎢ ⎥ cosϑ ⎢
⎥
1 − tgϑ cosγ ⎥⎦ ⎢⎣ωzс ⎥⎦
− cosψ
0 ⎥⎦ ⎢⎣ωζ ⎥⎦
⎢⎣ − sinψ
0
По данным о полученных после интегрирования углах курса, крена и тангажа вычисляют матрицу направляющих косинусов
C gb
ξ
η
ς
xc
c11 = cos γ cosψ +
+ sin γ sinψ sin ϑ ;
c21 = - cos γ sinψ +
+ sin γ cosψ sin ϑ ;
c31 = − cosϑ sin γ ;
yc
c12 = cosϑ sinψ
c22 = cosϑ cosψ ;
c32 = sin ϑ ;
zc
c13 = cosψ sin γ −
− sinψ cos γ sin ϑ ;
c23 = − sinψ sin γ −
− cos γ cosψ sin ϑ ;
c33 = cosϑ cos γ .
(2.4)
Уравнения ориентации
31
2.2. Уравнения ориентации Пуассона
Производная вектора в абсолютной системе координат, если вектор представлен в подвижной системе координат, определяется формулой
r ~r
de d e r r
=
+ω ×e ,
dt dt
(2.5)
где первое слагаемое (т.н.локальная производная) описывает скорость изменения вектора (производную) в подвижной системе, а второе – его скорость, выr
занную движением подвижной системы с угловой скоростью ω .
На рис.2.2 показаны инерциальная и подвижная системы координат,
r r r
i 0 , j 0 , k 0 - орты инерциальной системы координат 0XYZ ,
r r r
i , j , k - орты связанной системы координат 0xсyсzс ,
r
ω - вектор угловой скорости этой
системы координат.
Введем матрицу направляющих косинусов
C bi X
xc c11
yc c21
zc c31
или
r
C bi i0
r
i
c11
r
j c21
r
k c31
Y
c12
c22
c32
Z
c13
c23
c33
r
j0
c12
c22
c32
r
k0
c13
c23
c33
Рис.2.1. Инерциальная и связанная системы координат
Пользуясь этой матрицей, можно записать выражения
r
r
r
r
i0 = c11i + c21 j + c31k ,
r
r
r
r
j0 = c12i + c22 j + c32 k ,
r
r
r
r
k0 = c13i + c23 j + c33k .
В соответствии с (2.5)
r
i
v
r
r
r
di0
= c&11i + c&21 j + c&31k + ω xc
dt
c11
r
j
r
k
ω yc ωzc ,
c21
c31
Уравнения ориентации
32
с vдругой стороны скорость изменения орта инерциальной системы координат
di0
= 0 . Аналогично
dt
r
r
r
i
j
k
r
r
r
r
r
dj0
dj0
= c&12 i + c&22 j + c&32 k + ω xc ω yc ωzc ,
= 0;
dt
dt
c12 c22 c32
r
i
r
r
r
r
dk 0
= c&13i + c&23 j + c&33k + ω xc
dt
c13
r
j
r
k
ω yc ω zc ,
c23
c33
r
dk 0
= 0.
dt
Суммируя коэффициенты при ортах, получим 3 системы уравнений
⎧ c&11 = ωzc c21 − ω yc c31
⎪
⎨ c&21 = ω xc c31 − ωzc c11
⎪c& = ω c − ω c
yc 11
xc 21
⎩ 31
⎧ c&12 = ω zc c22 − ω yc c32
⎪
⎨ c&22 = ω xc c32 − ωzc c12
⎪c& = ω c − ω c
yc 12
xc 22
⎩ 32
⎧ c&13 = ω zc c23 − ω yc c33
⎪
⎨ c&23 = ω xc c33 − ω zc c13
⎪c& = ω c − ω c
yc 13
xc 23
⎩ 33
(2.6)
Это уравнения Пуассона. В матричном виде
bi 1
C& bi = Ω bi
bC
(2.7)
где
⎡ c11 c12
C = ⎢c21 c22
⎢
⎢⎣c31 c32
bi
⎡ 0
c13 ⎤
⎢
bi
⎥
c23 , Ωb = ⎢ − ω zc
⎥
⎢ ω yc
c33 ⎥⎦
⎣
ωzc
0
− ω xc
− ω yc ⎤
⎥
ω xc ⎥ .
0 ⎥⎦
Уравнение (2.7) можно транспонировать
Т
Т
Т
C& bi = C bi Ω bi
b .
Т
bi
Отметим, что Ωbi
b = − Ω b и тогда
Т
Т
C& bi = −C bi Ω bi
b
(2.8)
1
Отметим, что в обозначении матрицы направляющих косинусов верхние индексы показывают название исходного базиса (правый индекс) и конечного базиса (левый индекс). В наbi
шем примере C обозначает преобразование от базиса i к базису b . В обозначении матрицы угловой скорости верхние индексы используют аналогично, а нижний индекс показывает,
на какую систему координат проектируется угловая скорость.
Уравнения ориентации
Другие формы записи
33
bi
C& bi − Ωbi
bC =0 ;
Т
Т
C& bi + C bi Ωbi
b = 0.
Т
Если учесть, что C bi = C ib , можно записать
C& ib + C ibΩbi
b = 0 или
C& ib = C ibΩib
b .
Свойства матрицы направляющих косинусов
Напомним основные свойства матриц направляющих косинусов.
Матрица направляющих косинусов ортогональна:
C
ibТ
Т
⋅C
ib
= I,
−1
C ib = C ib ,
⎡ 1 0 0⎤
I = ⎢0 1 0⎥ - единичная матрица;
⎢
⎥
⎢⎣0 0 1⎥⎦
т.е. транспонированная матрица есть обратная матрица.
Для матрицы C bi (как и для другой любой матрицы направляющих косинусов)
существуют соотношения для элементов строк
2
2
2
+ c12
+ c13
= 1,
c11
2
2
2
+ c22
+ c23
= 1,
c21
2
2
2
c31 + c32 + c33 = 1;
c11c21 + c12 c22 + c13c23 = 0,
c11c31 + c12 c32 + c13c33 = 0,
c31c21 + c32 c22 + c33c23 = 0.
(2.9)
(2.9а)
Аналогичные соотношения можно привести и для элементов столбцов.
Формулы (2.9) описывают условие масштаба (нормировки), (2.9а) – условие ортогональности осей системы координат.
В общем виде
δαβ = 1 при α = β
.
cα 1cβ 1 + cα 2 cβ 2 + cα 3cβ 3 = δαβ
где символ Кронекера
δαβ = 0 при α ≠ β
Определитель матрицы направляющих косинусов равен 1:
det(С)=1.
Раскроем определитель по элементам первой строки
Уравнения ориентации
34
c11 c12
c21 c22
c31 c32
c13
c23 = c11 (c22 c33 − c23c32 ) + c12 (c23c31 − c21c33 ) + c13 (c21c32 − c22 c31 ) .
l33
Сравнивая полученное выражение с первым элементом (2.8), можно записать
c11 = c22 c33 − c23c32 ,
c12 = c23c31 − c21c33 ,
(2.10)
c13 = c21c32 − c22 c31.
Аналогично можно записать еще 2 тройки соотношений
c21 = c12 c33 − c13c32 ,
c31 = c12 c23 − c13c22 ,
c22 = c13c31 − c11c33 ,
c32 = c13c21 − c11c23 ,
c23 = c31c12 − c32 c11;
c33 = c11c22 − c12 c21;
(2.11)
т.е. каждый элемент равен своему алгебраическому дополнению.
Обобщенное уравнение Пуассона
Обобщенное уравнение Пуассона запишем в виде [5,13]
ig gb
C& gb = C gbΩib
b − Ωg C ,
(2.12)
⎡ 0
− ω zc ω yc ⎤
⎢
⎥
Ωib
− ω xc ⎥ - матрица проекций на оси связанного базиса b век0
b = ⎢ ω zc
⎢ − ω yc ω xc
0 ⎥⎦
⎣
тора угловой скорости связанной системы координат b относительно инерциr
альной (i) , этой матрице соответствует вектор ωb = [ω xc ω yc ω zc ]T ;
⎡ 0
⎢
Ωigg = ⎢ ωζ
⎢ − ωη
⎣
− ωζ
0
ωξ
ωη ⎤
⎥
− ωξ ⎥ - матрица проекций на оси базиса g вектора угловой
0 ⎥⎦
скорости географического сопровождающего базиса g относительно инерциr
ального i , этой матрице соответствует вектор ω g = [ωξ ωη ωζ ]T .
Обобщенное уравнение применяется, когда необходимо определить направляющие косинусы одной вращающейся системы координат относительно
другой вращающейся системы. В нашем случае в развернутом виде уравнение
имеет вид
Уравнения ориентации
35
⎡c&11 c&12 c&13⎤ ⎡c11 c12 c13⎤ ⎡ 0 − ωzc ωyc ⎤ ⎡ 0 − ωζ ωη ⎤ ⎡c11 c12 c13⎤
⎥ ⎢
⎥ ⎢
⎢c& c& c& ⎥ = ⎢c c c ⎥ ⋅ ⎢ ω
⎥
0
ω
ω
0
ω
−
−
−
zc
xc
ζ
ξ
21
22
23
21
22
23
⎥ ⎢
⎥ ⋅ ⎢c21 c22 c23⎥
⎢
⎥ ⎢
⎥ ⎢
0 ⎥⎦ ⎢⎣− ωη ωξ
0 ⎥⎦ ⎢⎣c31 c32 c33⎥⎦
⎢⎣c&31 c&32 c&33⎥⎦ ⎢⎣c31 c32 c33⎥⎦ ⎢⎣− ωyc ωxc
В скалярном виде
c&11 = c12ωzc − c13ω yc + ωζ c21 − ωη c31;
c&12 = −c11ωzc + c13ω xc + ωζ c22 − ωη c32 ;
c&13 = c11ω yc − c12ω xc + ωζ c23 − ωη c33 ;
c&21 = c22ωzc − c23ω yc − ωζ c11 + ωη c31;
c&22 = − c21ωzc + c23ω xc − ωζ c12 + ωξ c32 ;
(2.13)
c&23 = c21ω yc − c22ω xc − ωζ c13 + ωξ c33 ;
c&31 = c32ωzc − c33ω yc − ωξ c21 + ωη c11;
c&32 = − c31ωzc + c33ω xc + ωη c12 − ωξ c22 ;
c&33 = c31ω yc − c32ω xc + ωη c13 + ωξ c23 ;
Если известны направляющие косинусы, можно найти углы курса, крена, тангажа из (2.4)
⎞
⎛ c
⎛c ⎞
⎛c ⎞
32
⎟ (2.14)
ψ = arctg ⎜⎜ 12 ⎟⎟, γ = − arctg ⎜⎜ 31 ⎟⎟, ϑ = arcsin( c32 ) или ϑ = arctg ⎜
2
⎜ 1− с ⎟
⎝ c22 ⎠
⎝ c33 ⎠
32 ⎠
⎝
Вместо обобщенного уравнения Пуассона можно использовать два обычных уравнения Пуассона. Первое уравнение описывает движение связанного
базиса b относительно инерциального i:
C& ib = C ibΩib
b .
Второе уравнение описывает движение сопровождающего базиса g :
C& ig = C ig Ωig
g .
Далее используют понятное соотношение
C ib = C ig C gb ,
откуда искомая матрица
( )−1C ib .
C gb = C ig
(2.15)
Уравнения ориентации
36
Еще один вариант получения направляющих косинусов предполагает использование вектора ориентации или вектора конечного поворота [13] (см.
п.2.4, формулы (2.25),(2.26)).
Контроль правильности вычисления направляющих косинусов основан на
свойствах матрицы направляющих косинусов. Для правильно вычисленных направляющих косинусов должны выполняться условия масштаба (нормирования) (2.9) и ортогональности (2.9а). Если условия не выполняются, может быть
выполнена коррекция:
- если 1 − ( ci21 + ci22 + ci23 ) = Δ i , i = 1,2,3 корректируем значения направляю1
(
щих косинусов ci1 = ci1 − Δ i ci1 .
2
В общем виде, если обозначить Ci строку i матрицы направляющих косинусов, а C j - другую строку этой же матрицы, то можно записать
Δ ij = Ci C Tj . Если Δ ij ≠ 0 , выполняем коррекцию [6]:
(
1
- нормализацию Ci = Ci − Δ ii Ci ,
2
(
(
1
1
- ортогонализацию Ci = Ci − Δ ij C j , C j = C j − Δ ij Ci .
2
2
Пояснения к формулам можно найти в [13].
2.3. Уравнения ориентации с параметрами Родрига-Гамильтона
В этих уравнениях информацию об угловом положении объекта содержат
параметры Родрига-Гамильтона, которые являются компонентами кватерниона
Λ , описывающего этот поворот (см. приложение 1). Начальные значения параметров кватерниона определяют по известным начальным значениям углов
курса ψ 0 , тангажа ϑ0 и крена γ 0 (см. п1.3).
λ0 = cos
λ1 = cos
ψ0
2
ψ0
λ2 = cos
λ3 = cos
2
ψ0
2
ψ0
2
cos
sin
2
ϑ0
2
cos
sin
ϑ0
ϑ0
2
ϑ0
2
cos
γ0
2
cos
γ0
sin
γ0
sin
2
2
γ0
2
+ sin
ψ0
2
+ sin
ψ0
− sin
ψ0
− sin
2
2
ψ0
2
sin
ϑ0
cos
ϑ0
2
2
sin
ϑ0
cos
ϑ0
2
2
sin
γ0
sin
γ0
cos
γ0
cos
γ0
2
2
2
2
,
,
,
.
Матрица направляющих косинусов через параметры кватерниона может
быть представлена в следующем виде
Уравнения ориентации
37
C gb
xc
yc
zc
ξ
η
ζ
2λ20 + 2λ12 − 1
2( −λ0λ3 + λ1λ2 )
2(λ1λ3 + λ0λ2 )
2(λ0λ3 + λ1λ2 )
2(λ1λ3 − λ0λ2 )
2λ20
+ 2λ22
−1
2(λ0λ1 + λ2λ3 )
2( −λ0λ1 + λ2λ3 )
.
(2.16)
2λ20 + 2λ23 − 1
В соответствии с (2.14)
⎛ 2( λ1λ3 − λ0λ2 ) ⎞
⎛ 2( −λ0λ3 + λ1λ2 ) ⎞
⎟
⎟ , γ = −arctg ⎜
2
2
2
2⎟
⎜ λ2 − λ2 − λ2 + λ2 ⎟ ,
λ
λ
λ
λ
−
+
−
2
3⎠
2
3⎠
⎝ 0 1
⎝ 0 1
⎛ 2( λ λ + λ λ ) ⎞
0 1
2 3
⎟
ϑ = arcsin( 2( λ0λ1 + λ2λ3 )) или ϑ = arctg ⎜
⎜ 1 − 4( λ λ + λ λ )2 ⎟
0 1
2 3
⎠
⎝
ψ = arctg ⎜⎜
(2.17)
Дифференциальное уравнение, описывающее изменение параметров Родрига-Гамильтона в зависимости от проекций абсолютной угловой скорости
ω x , ω y , ωz и проекций переносной угловой скорости ωξ , ωη , ωζ , может быть записано
− (ω x − ωξ ) − (ω y − ωη ) − (ω z − ωζ )⎤ ⎡λ0 ⎤
⎡ 0
⎡λ&0 ⎤
⎢ω − ω
⎢&⎥
ω z + ωζ
0
− (ω y + ωη )⎥ ⎢ λ1 ⎥
ξ
⎥ ⋅ ⎢ ⎥ . (2.18)
⎢ λ1 ⎥ = 1 ⎢ x
ω x + ωξ ⎥ ⎢λ2 ⎥
0
⎢λ&2 ⎥ 2 ⎢ω y − ωη − (ω z + ωζ )
⎢ω − ω
⎥ ⎢ ⎥
⎢& ⎥
ω y + ωη
0
− (ω x + ωξ )
ζ
⎣ z
⎦ ⎣λ3 ⎦
⎣ λ3 ⎦
Если обозначить матрицу угловых скоростей Ω , то уравнение можно записать в компактной векторно-матричной форме
& gb = 1 Ω ⋅ Λgb .
Λ
2
Если в уравнении (2.18) пренебречь проекциями переносной угловой скорости, получим уравнение, описывающее изменение кватерниона в инерциальной системе координат.
Вместо кинематического уравнения (2.18) можно использовать 2 кинематических уравнения и операцию перемножения кватернионов, аналогично
(2.15).
Для решения уравнений применяют широко известные численные методы
первого, второго, третьего, четвертого порядка [8] или специализированные методы [9].
Контроль точности вычислений проводят по отклонению нормы кватерниона (п1.1) от 1:
Δ = 1 − (λ20 + λ12 + λ22 + λ23 ) .
38
Уравнения ориентации
Если Δ < Δ d , т.е.вычисленная норма отличается от 1 в допуске Δ d , параметры
кватерниона уточняют по формуле
(
Λ
Λ=
,
1− Δ
(
1
или по формуле Λ = Λ + Δ ⋅ Λ [13], которая соответствует разложению в ряд
2
предыдущей формулы.
2.4. Уравнения ориентации c параметрами вектора ориентации
Параметры Родрига-Гамильтона, Кейли-Клейна, координаты вектора
ориентации, координаты вектора конечного поворота относятся к группе векторных параметров [14] и все они основываются на известной теореме Эйлера:
любое вращательное движение твердого тела эквивалентно плоскому вращению вокруг некоторой оси и может быть задано конечным поворотом вокруг
этой оси.
Из этой теоремы следует, что поворот одной системы координат (связанной) относительно другой (исходной) в любой момент может быть описан неr
которым единичным вектором e (называемым иногда вектором Эйлера), перпендикулярным плоскости конечного поворота, и углом φ на который осуществляется поворот. Различные кинематические параметры, относящиеся к группе
r
векторных, являются некоторыми функциями e и θ [13,14].
Вектор Эйлера (вектор ориентации)
⎡θe x ⎤
⎡φ x ⎤
r ⎢ ⎥
⎥
⎢
φ = φ y = θe = θe y ,
⎢ ⎥
⎢ ⎥
⎢⎣θez ⎥⎦
⎢⎣φz ⎥⎦
r
(2.19)
r
где ex , e y , ez - направляющие косинусы орта e в подвижном базисе.
r
Вектор конечного поворота Θ по направлению совпадает с вектором
Эйлера, а по величине равен 2tg
θ
2
:
θ⎤
⎡
2e x tg ⎥
⎢
2
⎡Θ ⎤
⎢
r ⎢ x⎥
θr
θ⎥
Θ = Θ y = 2tg e = ⎢2e y tg ⎥ .
⎢ ⎥
2⎥
2
⎢
⎢⎣ Θ z ⎥⎦
⎢ 2e tg θ ⎥
⎢⎣ z 2 ⎥⎦
Уравнения ориентации
39
Кинематическое уравнение, описывающее изменения вектора конечного
поворота во времени, имеет вид:
r
dΘ r 1 r r 1 r r r
= ω + Θ × ω + (ω ⋅ Θ)Θ ,
2
4
dt
r
r
Θ(t0 ) = Θ0 .
(2.20)
r
Здесь вектор ω описывает угловую скорость связанной системы координат
0xyz относительно исходной. Если измерителями являются гироскопические
r
измерители угловой скорости, то ω - вектор абсолютной угловой скорости относительно инерциальной системы координат.
Как кинематические параметры используются проекции (координаты) вектора
конечного поворота на оси подвижной системы координат.
В скалярном виде получим
1
1
4
2
1
1
θ&y = ω y + (ω zθ x − ω xθ z ) + θ y (ω xθ x + ω xθ y + ω zθ z ),
4
2
1
1
θ&z = ω z + (ω xθ y − ω yθ x ) + θ z (ω xθ x + ω xθ y + ω zθ z ).
2
4
θ&x = ω x + (ω yθ z − ωzθ y ) + θ x (ω xθ x + ω xθ y + ωzθ z ),
r
Кинематическое уравнение для вектора Эйлера φ имеет вид:
r
dφ r 1 r r
1
1 φ sin φ r r r
= ω + (φ × ω ) + 2 (1 − ⋅
)φ × (φ × ω ) ;
dt
2
2 1 − cos φ
φ
r
r
φ (t0 ) = φ0 ,
(2.21)
φ
sin φ
= ctg , раз1 − cos φ
2
ложением коэффициента при третьем слагаемом (2.21) в степенной ряд до первого порядка относительно величины φ можно показать, что
r
где ω - вектор угловой скорости поворота. Отметим, что
1 φ sin φ
1
−
⋅
≈
.
(
1
)
2 1 − cos φ 12
φ2
1
Это уравнение нелинейное и вырождается при φ = 2πk , (k = 0,1,...) .
В качестве кинематических параметров используют проекции вектора Эйлера
на оси подвижной системы координат.
Уравнение (2.21) может быть записано в векторно-матричной форме
r
φ⎞ r
dφ r 1 r 1 ⎛ φ
= ω − Ωφ + 2 ⎜1 − ctg ⎟Φ 2ω ,
dt
2
2⎠
φ ⎝ 2
(2.22)
Уравнения ориентации
40
⎡ 0
⎡φx ⎤
⎡ω x ⎤
r ⎢ ⎥
⎢
⎢
⎥
где φ = φ y ; ω = ω y ; Ω = ⎢ ω z
⎢ ⎥
⎢ ⎥
⎢− ω y
⎢⎣φz ⎥⎦
⎢⎣ω z ⎥⎦
⎣
r
− ωz
0
ωx
⎡ 0
ωy ⎤
⎥
⎢
− ω x ⎥ ; Φ = ⎢ φz
⎢− φ y
0 ⎥⎦
⎣
− φz
0
φx
φy ⎤
⎥
− φx ⎥ . .
0 ⎥⎦
r
На малых интервалах времени h обычно выполняется условие φ <<1. Тогда нелинейный член уравнения, который имеет порядок (ωh)3 , можно не учитывать
и рассматривать уравнение
r
dφ r 1 r
= ω − Ωφ .
dt
2
(2.23)
С учетом интегрального характера первичной информации численные
методы решения уравнения (2.23) целесообразно строить на основе метода поr ∞ r
следовательных приближений, когда φ = ∑ φ ( n ) .
n =0
Нулевое приближение: φ (0 ) = 0 .
Первое приближение: φ
(1)
ti
= ∫ ω (τ )dτ .
t i −1
Второе приближение: φ
(2 )
τ′
1 i
′
= ∫ ω (τ )dτ − ∫ Ω(τ ) ∫ ω (τ )dτdτ ′ .
2t
t
t
ti
t
i −1
i −1
i −1
и т.д.
Если в качестве измерителей используют интегрирующие гироскопы (или
интегрирующие выходы лазерного гироскопа, твердотельного волнового гироскопа и др.), то для численного интегрирования можно использовать простейший одношаговый алгоритм (интервал интегрирования равен периоду съема
информации) третьего порядка точности [9]
r
r
φi =θ i+
r
1
Θi −1θi ,
12
⎡ 0
⎢θ x ,i ⎥
− θ z ,i −1 θ y ,i −1 ⎤
⎢
⎥
⎢ ⎥
− θ x ,i −1 ⎥ ,
θ i = ⎢θ y ,i ⎥; Θi −1 = ⎢ θ z ,i −1
0
⎢ − θ y ,i −1 θ x ,i −1
⎢⎣θ z ,i ⎥⎦
0 ⎥⎦
⎣
r
где θx, θy, θz, - показания интегрирующих гироскопов, если гироскопы имеют
ортогональные оси чувствительности, или результаты приведения показаний к
Уравнения ориентации
41
ортогональным осям, если оси чувствительности датчиков не ортогональны.
Ошибка численного метода пропорциональна величине (ωh)4.
Могут применяться и другие методы различных порядков и различной
шаговости (см.п.4.5).
Вектор ориентации и вектор конечного поворота часто более удобны для
анализа вращения твердого тела, чем например, матрица направляющих косинусов или параметры Родрига-Гамильтона. После их вычисления переходят к
матрице направляющих косинусов или параметрам Родрига-Гамильтона (кватерниону).
Вычисление кватерниона Λ = [λ0 , λ1 , λ2 , λ3 ]T через вектор ориентации
r
φ = φ x φ y φ z Т (2.19) достаточно просто, хотя и требует вычисления тригонометрических функций:
[
]
φy
φ
⋅ sφ 2 ; λ3 = z ⋅ sφ 2 ,
(2.24)
φ
φ
φ
φ
где для краткости обозначено cφ 2 = cos , s φ 2 = sin , φ = φ x2 + φ y2 + φ z2 .
λ0 = cφ 2 ;
λ1 =
φx
⋅s ;
φ φ2
λ2 =
2
2
Вычисление матрицы направляющих косинусов может быть выполнено
по выражению (2.16) после вычисления элементов кватерниона (2.24) или непосредственно через компоненты вектора ориентации (2.19)
⎡ φ x2 + cφ (1 − φ x2 )
⎢
C ib = ⎢φ xφ y (1 − cφ ) − φ z sφ
⎢φ xφ z (1 − cφ ) + φ y sφ
⎣
φ xφ y (1 − cφ ) + φ z sφ φ xφ z (1 − cφ ) − φ y sφ ⎤
⎥
φ y2 + cφ (1 − φ y2 )
φ yφ z (1 − cφ ) + φ x sφ ⎥ ,
φ yφ z (1 − cφ ) − φ x sφ
φ z2 + cφ (1 − φ z2 ) ⎥⎦
(2.25)
где обозначено cφ = cosφ , sφ = sin φ .
Матрицу (2.25) можно записать компактно, используя формулу Родригеса:
C ib = P + ( I − P) cosφ + Q sin φ ,
⎡ φx2
r r
⎢
где P = φ ⋅ φ Т = ⎢φxφ y
⎢φ φ
⎣ x z
φ xφ y φ xφ z ⎤
⎥
φ y2 φ yφz ⎥ ,
φ yφz φz2 ⎥⎦
⎡1 0 0⎤
I = ⎢0 1 0 ⎥ ,
⎢
⎥
⎢⎣0 0 1⎥⎦
⎡ 0
⎢
Q = ⎢ φz
⎢− φ y
⎣
(2.26)
− φz
0
φx
φy ⎤
⎥
− φx ⎥ .
0 ⎥⎦
Однако, в отличие от параметров Родрига-Гамильтона, уравнения с вектором ориентации (2.22) являются в общем случае нелинейными и решение
этих уравнений вырождается при φ = 2πk , (k = 0, 1, ...) . Это затруднение исчезает при использовании рекуррентной процедуры .
Уравнения ориентации
42
Когда найдены компоненты вектора Эйлера φi , x , φi , y , φi , z , на i-м интервале
интегрирования, вычисляется матрица направляющих косинусов ΔСі , описывающая поворот от исходной системы к связанной. Для этого используется соотношение (с учетом малого угла φ ) [13]
1
1
1
1
ΔC = eΦ = I + Φ + Φ2 + Φ3 + ... = I + Φ sin φ + Φ2 2 (1 − cos φ ) , (2.27)
φ
2
6
φ
где I – единичная матрица размера 3 × 3, Ф – матрица из (2.22).
Матрицу направляющих косинусов Ci определяют путем перемножения
C i = C i − 1 ΔC i .
(2.28)
Если входной информацией является абсолютная угловая скорость, измеr
ряемая датчиками угловой скорости (вектор ω и соответствующая ему матрица
Ω ) , то вектор Эйлера покажет поворот относительно исходного инерциального
базиса i , а определяемая матрица направляющих косинусов может быть обозначена C ≡ C ib .
Аналогичная рекуррентная процедура может быть использована при вычислении кватерниона ориентации
Λ i = Λ i −1 o ΔΛ i ,
(2.29)
где приращение кватерниона на шаге интегрирования ΔΛ i определяют по приращению вектора ориентации по (2.24).
Если необходимо определять поворот связанного базиса b относительно
вращающегося географического сопровождающего базиса g , то тогда вместо
указанных в (2.22) вектора и матрицы используют вектор относительной угловой скорости
T r
r
r
r
ω o = ω − C gb ω g ,
ω g = [ωξ ωη ωζ ]T ,
( )
и соответствующую ему матрицу
( ) Ωigg
Ωo = Ω − C
gb T
⎡ 0
⎢
, где Ωig
g = ⎢ ωζ
⎢ − ωη
⎣
− ωζ
0
ωξ
ωη ⎤
⎥
− ωξ ⎥ .
0 ⎥⎦
Вычисления по формулам (2.22), (2.23) в этом случае дают матрицу направляющих косинусов C gb .
Представления скорости и ускорения объекта
43
3. Представления скорости и ускорения объекта
Представление скорости
Как уже упоминалось, абсолютной называют скорость в инерциальной
системе координат. Относительной называют скорость в системе координат,
перемещающейся относительно абсолютной системы. Это перемещение может
быть как поступательным, так и вращательным. Представлен вектор скорости
может быть в проекциях на оси разных систем координат.
В алгоритмах ИНС чаще всего рассматривают в качестве относительной
скорость во вращающемся сопровождающем трехграннике Оξηζ (OENH ) , обозначаемом иногда буквой g:
vξ ≡ v E - восточная составляющая;
vη ≡ v N - северная составляющая;
vζ ≡ v H - вертикальная составляющая.
Для получения абсолютной линейной скорости к относительной скорости
добавляют переносную линейную скорость
r r r
V = v + v пер .
(3.1)
В рассматриваемом примере переносной является скорость из-за вращения
Земли.
Эта
скорость
имеет
только
восточную
составляющую
vξЗ = ru cosϕ , r = R + h (для геоцентрической системы координат, R - радиус
Земли,
h - высота). Таким образом, составляющие абсолютной линейной
скорости в проекциях на оси географического трехгранника
Vξ = vξ + ru cosϕ , Vη ≡ vη , Vζ ≡ vζ .
Вертикальная составляющая скорости соответствует скорости изменения высоVζ = h& .
ты (или глубины)
В векторной форме относительную скорость записывают в виде
r
v g = [v E , v N , h&]T .
r
Вектор переносной линейной скорости представим в виде vЗ = [ ru cosϕ , 0, 0]T .
Абсолютную линейную скорость можно получить как производную радиусавектора в инерциальной системе координат
r drr r
V=
≡ r& .
dt
Представления скорости и ускорения объекта
44
В инерциальной системе координат i радиус-вектор представляют в виде
r
r
ri = [ X , Y , Z ]T , в сопровождающем базисе rg = [0, 0, r ]T .
Если радиус-вектор представлен в сопровождающем базисе, абсолютную скорость записывают в виде
~r
r d rg
r
r
r
+ (u g + ωgo ) × rg .
(3.2)
Vg =
dt
~r
d rg
Здесь
- производная радиуса-вектора в сопровождающем базисе (локальdt
r
ная производная), u g = [0, u cosϕ , u sin ϕ ]T - вектор угловой скорости вращения
r
v
v
v
Земли, ω go = [ − N , E , E tgϕ ]T - вектор угловой скорости сопровождающего
r
r
r
базиса из-за перемещения относительно Земли. Сумма
r
r
r
ω g = u g + ω go
обозначает абсолютную угловую скорость сопровождающего базиса, представленную в проекциях на оси сопровождающего базиса g.
Представление ускорения
При записи абсолютного
ускорения в подвижной системе rкоординат абr
солютное ускорение W есть сумма относительного ускорения v& , переносного
r
r
ускорения W пер и кориолисового ускорения W кор :
r r r
r
W = v& + W пер + W кор .
Составляющие ускорения
r
r r r
W пер = u × (u × r ) - центростремительное ускорение из-за вращения Земли;
r
r r
W кор = 2u × v - кориолисово ускорение.
Рассмотрим абсолютное ускорение объекта, перемещающегося на поверхности Земли или около нее, т.е. в сопровождающем базисе.
r
r
r
dVg d 2 rg &r&
Wg ≡
≡ 2 ≡ rg .
dt
dt
Далее индекс g для упрощения записи употреблять не будем, молчаливо подразумевая его наличие.
Дифференцируя (3.2), получим
Представления скорости и ускорения объекта
r
r
~r
d 2 r d ⎛ d r ⎞ d r r o r r r o dr
= ⎜ ⎟ + (u + ω ) × r + (u + ω ) × .
dt
dt 2 dt ⎜⎝ dt ⎟⎠ dt
45
(3.3)
r
r r
Учтем, что u = Const ( u × u = 0 ) и подставим в последнее слагаемое выражение
(3.2):
r
~r
~r
r r o r⎤
d 2r d ⎛ d r ⎞ d r o r r r o ⎡ d r
⎜
⎟
ω
(
ω
)
(3.4)
=
+
×
r
+
u
+
×
+
u
+ω )×r⎥.
(
⎢ dt
dt 2 dt ⎜⎝ dt ⎟⎠ dt
⎣
⎦
Учтем, что
Тогда
~r
d r r&
= h,
dt
r ~r
~r
dr d r
r ro r dr r r ro r
=
+ (u + ω ) × r =
+ u × r + ω × r.
dt dt
dt
(3.5)
~r
~r
~ r
d r r&
d ⎛ dr ⎞ d 2r
r r o r& &r& r r o r&
⎜ ⎟=
+ (u + ω ) × h = h + (u + ω ) × h , так как
=h.
dt ⎜⎝ dt ⎟⎠ dt 2
dt
Преобразуем далее (3.3):
r
r
~r
d 2 r d ⎛ d r ⎞ d r r o r r r o dr
= ⎜ ⎟ + (u + ω ) × r + (u + ω ) × =
dt
dt 2 dt ⎜⎝ dt ⎟⎠ dt
r
r
&r& r r o r& d r r d r o r r dr r o dr
= h + (u + ω ) × h + u × r + ω × r + u × + ω × .
dt
dt
dt
dt
Учтем, что
d r
u = 0 и подставим (3.5):
dt
r
~r
~r
d 2r &r& r r o r& d r o r r ⎛ d r r r r o r ⎞ r o ⎛ dr r r r o r ⎞
= h + (u + ω ) × h + ω × r + u × ⎜⎜ + u × r + ω × r ⎟⎟ + ω × ⎜⎜ + u × r + ω × r ⎟⎟ = .
2
dt
dt
⎝ dt
⎠
⎝ dt
⎠
r& d r r r r& r r r r r r r r& r r r r
r r
&r& r r
= h + (u + ωo ) × h + ωo × r + u × h + u × (u × r) + u × (ωo × r) + ωo × h + ωo × (u × r) + ωo × (ωo × r) =
dt
r& r r r r
r r r r r r
r r
r r d r r
&r&
= h + 2(u + ω o ) × h + u × (u × r ) + ω o × (ω o × r ) + u × (ω o × r ) + ω o × (u × r ) + ω o × r .
dt
(3.6)
ro ro
Преобразуем последнее слагаемое, учитывая, что ω × ω = 0 :
~
~
~
d ro r ⎛ d ro r ro ro ⎞ r ⎛ d ro r ro ⎞ r d ro r r ro r
ω × r = ⎜⎜ ω + (u + ω ) × ω ⎟⎟ × r = ⎜⎜ ω + u × ω ⎟⎟ × r = ω × r + (u × ω ) × r .
dt
dt
⎝ dt
⎠
⎝ dt
⎠
Учтем также такое преобразование:
46
Представления скорости и ускорения объекта
r
r r
r r r
r r
r r
r r
(u × ω o ) × r + ω o × (u × r ) = − r × (u × ω o ) + ω o × (u × r ) =
r r r r r r r r r
r r r
= −u ( r ⋅ ω o ) + ω o ( r ⋅ u ) + u (ω o ⋅ r ) − r (ω o ⋅ u ) =
r r r r r r
r r r r r r
r r r
= ω o ( r ⋅ u ) − r (ω o ⋅ u ) = ω o (u ⋅ r ) − r (u ⋅ ω o ) = u × (ω o × r ).
В итоге из (3.6) , учитывая, что
r
r r
u + ω o = ω - абсолютная угловая скорость базиса g,
r r r
ω o × r = vг - горизонтальная составляющая относительной линейной скорости,
~
d ro r
ω = ε - тангенциальное ускорение,
dt
получим формулу абсолютной линейной скорости в проекциях на оси сопровождающего базиса g ( Oξηζ )
r
r r& r r r
r r r r
&r&
Wg = h + 2ω × h + ω × (ω × r ) + 2u × vг + ε × r
(3.7)
r r
Рассмотрим содержание слагаемого ε × r :
~
′ ⎤Т
⎡
d ro r
v& v&
v
ω = ε = ⎢− N , E , ⎛⎜ E tgϕ ⎞⎟ ⎥ (штрих означает производную).
dt
⎠ ⎥⎦
⎢⎣ r r ⎝ r
r
r
r
ξ0
r r
v&
ε ×r = − N
r
0
η0
v& E
r
0
ζ0
′ r
r
r
⎛ vE
⎞
⎜ tgϕ ⎟ = ξ 0v&E + η0 v& N = v&г
⎝ r
⎠
к
- горизонтальная составляющая относительного ускорения.
В (3.7)
r
r &
2ω × h - кориолисово ускорение, вызванное изменением высоты в сопровождающем базисе;
r r r
ω × (ω × r ) - центростремительное ускорение;
r r
2u × vг - кориолисово ускорение, возникающее при горизонтальном движении.
Уравнение (3.7) может быть преобразовано к следующему виду
или
~
r
d r
r r
r r r r
Wg = v + (2u + ω o ) × v + u × (u × r )
dt
(3.8)
~
r
d r r r r r r r
Wg = v + (u + ω ) × v + u × (u × r )
dt
(3.9)
Такие формулы приведены, например, в [1].
Представления скорости и ускорения объекта
47
Эти формулы можно также получить другим путем.
Представим формулу абсолютной скорости (3.2) в виде
r d~rr r r r o r
(3.10)
+u ×r +ω ×r .
V=
dt
В соответствии с ранее введенными обозначениями, (3.10) можно записать в виде
r r& r r r r r r
V = h + u × r + vг = v + u × r .
(3.11)
Таким образом, вектор абсолютной линейной скорости в сопровождающем баr
зисе g равен сумме относительной линейной скорости v в этом базисе (относиr
r r
тельно Земли) и переносной линейной скорости v пер = u × r из-за вращения
Земли.
Возьмем производную от (3.11)
r d r d r r d~ r r r o r d r r r d r
W = v + (u × r ) = v + (u + ω ) × v + u × r + u × r .
dt
dt
dt
dt
dt
Учтем, что
d r
d r r r r
u = 0 и r = v + u × r . Тогда
dt
dt
r r r r
r r r r r
W = v& + (u + ω o ) × v + u × ( v + u × r ).
Отсюда получим формулу, соответствующую (3.8)
r r
r r
r r r r
W = v& + ( 2u + ω o ) × v + u × (u × r ).
r r r r r
Учитывая выражение для ускорения силы тяжести g = g '−u × (u × r ) ,
можно записать выражение кажущегося ускорения
r
r
r r
r r
a g = v& + ( 2u + ω o ) × v − g .
Такая формула приведена, например, в [6].
Второе слагаемое иногда представляют через определитель
r
r
r
ξ0
η0
ζ0
r r
r
( 2u + ω o ) × v = 2uξ + ωξo 2uη + ωηo 2uζ + ωζo =
vξ
vη
vζ
r
[(
[(
[(
) (
) (
) (
) ]
) ]
) ]
= ξ0 2uη + ωηo vζ − 2uζ + ωζo vη +
r
+ η0 2uζ + ωζo vξ − 2uξ + ωξo vζ +
r
+ ζ 0 2uξ + ωξo vη − 2uη + ωηo vξ .
(3.13)
Представления скорости и ускорения объекта
48
r r r
Здесь ξ0 , η0 , ζ 0 - орты сопровождающего базиса.
Иногда используют векторно-матричную форму записи
~r
d vg
dt
r
r
r
ie r
gb r
+ (Ωig
g + Ω g ) v g = C a b + g g , v g ( t0 ) = v g , 0 ;
⎡
0
⎢
⎢
v
где (Ωigg + Ωieg ) = ⎢ E tgϕ + 2u sin ϕ
⎢ RE
⎢ v
⎢ − E − 2u cos ϕ
⎢⎣ RE
⎤
vE
+ 2u cos ϕ ⎥
RE
⎥
vN
⎥;
⎥
RN
⎥
⎥
0
⎥⎦
⎞
⎛v
− ⎜⎜ E tgϕ + 2u sin ϕ ⎟⎟
⎠
⎝ RE
0
−
vN
RN
r
g g - вектор силы тяжести в точке нахождения объекта,
ускорения в связанном базисе.
Отметим, что
⎡
⎞
⎛v
0
− ⎜⎜ E tgϕ + u sin ϕ ⎟⎟
⎢
⎠
⎝ RE
⎢
v
Ωigg = ⎢ E tgϕ + u sin ϕ
0
⎢ RE
⎢ v
v
⎢ − E − u cos ϕ
− N
RN
⎣⎢ RE
0
⎡
Ωieg = ⎢ u sin ϕ
⎢
⎢⎣ − u cos ϕ
− u sin ϕ
0
0
(3.14)
r
ab - вектор кажущегося
⎤
vE
+ u cos ϕ ⎥
RE
⎥
vN
⎥,
⎥
RN
⎥
⎥
0
⎦⎥
u cos ϕ ⎤
0 ⎥.
⎥
0 ⎥⎦
Абсолютное ускорение можно также получить путем дифференцирования
вектора абсолютной скорости, записанного во вращающейся системе координат
[3]
r
r
~& r r
W
= V + ω ×V .
ξηζ
В проекциях
Wξ = V&ξ + ωηVζ − ωζ Vη ,
Wη = V&η + ωζ Vξ − ωξVζ ,
Wζ = V&ζ + ωξVη − ωηVξ .
Здесь, учитывая радиусы кривизны земного эллипсоида,
(3.15)
Представления скорости и ускорения объекта
49
vN
V
v
V
= − N , ωη = u cos ϕ + E = E ,
R2 + h
R2 + h
R1 + h R1 + h
V
v
ωζ = u sin ϕ + E tgϕ = E tgϕ .
R1 + h
R1 + h
ωξ = −
Учитывая,
что
Vξ ≡ VE = v E + R1u cos ϕ ,
получим
R
V&ξ = v&E − 1 v N u sin ϕ ≈ v&E − v N u sin ϕ . Учтем также, что V&η ≡ v& N , V&ζ ≡ v&ζ . ≡ h& .
R2
После подстановки формул в (3.13) и преобразований получим
Wξ = v&Ε − (u sin ϕ + ως )vΝ + ( 2u cos ϕ +
vΕ
)vς ;
R1 + h
Wη = v&Ν + (u sin ϕ + ως )vΕ + u 2 (R1 + h )sin ϕ cosϕ +
vΝ ⋅ vς
R1 + h
;
(3.16)
v N2
v E2
−
− 2uvΕ cos ϕ − u 2 (R1 + h ) cos2 ϕ .
Wς = v&ς −
R2 + h R1 + h
4. Алгоритмы БИНС
Как видно из ранее изложенного, алгоритмы функционирования БИНС
могут отличаться видом системы координат, принятой за основную, видом
уравнений ориентации, используемой формой записи ускорений, избранным
методом численного интегрирования и др. БИНС могут отличаться используемыми гироскопическими измерителями. Поэтому алгоритмы БИНС весьма разнообразны. Рассмотрим некоторые примеры.
4.1. Алгоритм БИНС в инерциальной системе координат
Данный алгоритм реализует метод счисления пути, в первую очередь, в
инерциальной системе координат.
В блоке чувствительных элементов акселерометры и гироскопы могут
быть в минимальном количестве (3 одноосных акселерометра, 3 датчика угловой скорости), а могут иметь избыточную сложную структуру (см. п. 4.6), механизмы автокомпенсации помех. Алгоритмы предварительной обработки сигналов могут содержать элементы автокомпенсации помех или корректировки
измерений по результатам калибровки параметров модели сигналов (см. гл.10).
Для получения сигналов в проекциях на оси связанной системы координат проводится преобразование сигналов чувствительных элементов, их диагностика, в
r
результате которых определяют вектор проекций кажущихся ускорений ab и
r
вектор проекций угловой скорости объекта ωb .
50
Примеры алгоритмов БИНС
Поскольку основной принята инерциальная система координат, необходимо использовать матрицу направляющих косинусов между осями связанной
b и инерциальной i систем координат C ib . Для получения C ib используют
уравнение Пуассона
C& ib = C ibΩib
(4.1)
b ,
где Ωib
b - матрица проекций абсолютной угловой скорости, измеряемых ДУС:
⎡ 0
⎢
Ωib
b = ⎢ ω zc
⎢ − ω yc
⎣
− ω zc
0
ω xc
ω yc ⎤
⎥
− ω xc ⎥ .
0 ⎥⎦
Для решения уравнения необходима матрица начальных значений направляющих косинусов C0ib , которую находят при начальной выставке БИНС.
С помощью полученной матрицы преобразуют проекции ускорений из связанного базиса в инерциальный:
r
r
ai = C ib ab .
Затем, интегрируя абсолютное ускорение
r r r
V&i = ai + g 'i
(4.2)
в инерциальной
системе координат с учетом начального
значения абсолютной
r
r
скорости Vi 0 , получают абсолютную скорость Vi .
r
Интегрируя инерциальную скорость Vi , с учетом начального радиусаr
r
вектора ri 0 , получим радиус-вектор ri .
Для использования формулы (4.2), соответствующей основному уравнению акселерометра (1.14), рассчитывают вектор гравитационного ускорения
r r
g 'i ( ri ) . Для приближенного расчета могут быть использованы формулы (1.12).
Более точные выражения
(X 2 +Y 2)
g ′X
Z2
g ′Y
Z2
g ′Z
(1 − 3μ 2 ) ; g 'Y = −
(1 − 3μ 2 ) ; g ' Z = −
(1 + 3μ
);
g' X = −
r
r
r
r
r
r2
(4.2а)
2
2
g a
Z
g ′ = Э ,02 ( 1 − β 2 ) ; β = 1.437 ⋅ 10−3 , 3μ = 3.27 ⋅ 10 −3 , g Э,0 = 9.81438 м/с2.
r
r
r
Здесь величина радиуса-вектора ri , определяющего положение объекта, связана
с его проекциями соотношением r 2 = X 2 + Y 2 + Z 2 .
От инерциальных декартовых координат можно перейти к сферическим
(см. рис.4.1): широте ϕ , абсолютной долготе Λ и относительной долготе λ
Примеры алгоритмов БИНС
51
Рис.4.1. Системы координат
ϕ=arctg
Z
2
X +Y
2
,
Λ = arctg
Y
,
X
λ = Λ − ut .
Для определения параметров относительного движения (относительной скорости, углов курса, крена и тангажа) необходимо найти матрицу направляющих
косинусов для преобразования инерциальных координат в относительные C gi .
Это можно сделать, пользуясь рис. 4.1.
C gi
ξ
η
ζ
X
− sin Λ
cos Λ sin ϕ
Y
cos Λ
sin Λ sin ϕ
Z
0
.
cosϕ
cos Λ cosϕ
sin Λ cosϕ
sin ϕ
Для определения углов курса, крена и тангажа найдем матрицу направляющих косинусов C gb :
C gb = C gi C ib ,
а затем, глядя на вид матрицы (2.4), сами углы
gb ⎞
gb ⎞
⎛ c31
⎛ c12
⎜
⎜
⎟
ψ = arctg ⎜ gb ⎟, γ = −arctg ⎜ gb ⎟⎟,
⎝ c22 ⎠
⎝ c33 ⎠
⎛
gb
⎜
c32
ϑ = arctg ⎜
⎜ 1 − с gb
32
⎝
( )
2
⎞
⎟
⎟
⎟
⎠
.
52
Примеры алгоритмов БИНС
Рис.4.1. Структурная схема алгоритма БИНС
в инерциальной системе координат
Примеры алгоритмов БИНС
53
Составляющие абсолютной линейной скорости в проекциях на оси сопровождающего базиса находят перепроектированием по формуле
r
r
Vg = C giVi .
Vξ = −V X sin Λ + VY cos Λ,
В скалярном виде Vη = V X cos Λ sin ϕ + VY sin Λ sin ϕ + VZ cosϕ ,
Vζ = V X cos Λ cosϕ + VY sin Λ cosϕ + VZ sin ϕ .
От абсолютных скоростей можно перейти к относительным
v E = Vξ − uRE cosϕ , v N ≡ Vη , v H ≡ Vη .
При решении уравнения Пуассона, как показано в п.2.2, достаточно решать 6 дифференциальных уравнений, а еще три направляющих косинуса находить из соотношений типа (2.9). В итоге, реализация этого алгоритма требует
решения 13 дифференциальных уравнений.
4.2. Алгоритм в географическом сопровождающем базисе
с обобщенным уравнением Пуассона
Поскольку основной принята система координат g, необходимо использовать матрицу направляющих косинусов C gb между осями связанного b и сопровождающего базиса g. Для получения C gb используют уравнение Пуассона,
которое называют обобщенным уравнением [5], поскольку в нем учтено переносное движение
ig gb
C& gb = C gbΩib
(4.3)
b − Ωg C .
Матрицы угловых скоростей представлены в (2.11).
Для решения уравнения необходима матрица начальных значений направляющих косинусов C0gb , которую находят при начальной выставке БИНС.
С помощью полученной матрицы преобразуют проекции ускорений из связанного базиса в сопровождающий:
r
r
a g = C gb ab .
(4.4)
Относительное ускорение находят путем интегрирования уравнения
r r
r r
r r
v&g = ag − ug + ωg × vg + g g .
(
)
Составляющие угловых скоростей находят в соответствии с (1.3), (1.4).
(4.5)
54
Примеры алгоритмов БИНС
Рис.4.2. Структурная схема алгоритма БИНС
в географическом сопровождающем базисе
r
Вектор гравитационного ускорения g ' g = [0, gϕ , g r ]T , где составляющие gϕ , g r
определяют из (1.8),r (1.9). Для интегрирования уравнений необходимо ввести
начальное значение v& g 0 .
Широту и долготу определяют путем интегрирования уравнений
Примеры алгоритмов БИНС
55
vN
vE
.
, λ& =
RN
RE cosϕ
(4.5)
ϕ& =
Радиусы кривизны эллипсоида вычисляют по формулам (1.1), (1.2).
При интегрировании вводят начальные значения широты и долготы ϕ 0 , λ0 .
Углы курса, крена и тангажа находят, как и прежде, из выражений
gb ⎞
gb ⎞
⎛ c31
⎛ c12
⎜
⎟
⎜
ψ = arctg ⎜ gb ⎟, γ = −arctg ⎜ gb ⎟⎟,
⎝ c22 ⎠
⎝ c33 ⎠
⎛
gb
⎜
c32
ϑ = arctg ⎜
⎜ 1 − с gb
32
⎝
( )
2
⎞
⎟
⎟
⎟
⎠
.
При решении уравнения Пуассона, как показано в п.2.2, достаточно решать 6 дифференциальных уравнений, а еще три направляющих косинуса находить из соотношений типа (2.9). В итоге, реализация этого алгоритма требует
решения 12 дифференциальных уравнений.
4.3. Алгоритм с уравнением Родрига-Гамильтона
Отличительной особенностью этого алгоритма является применение вместо кинематического обобщенного уравнения Пуассона уравнения в параметрах
Родрига-Гамильтона (параметрах кватерниона) вида (2.18)
r
r
& gb = 1 Ω ⋅ Λgb .
Λ
2
В нем матрица угловых скоростей имеет вид
⎡ 0
⎢ω − ω
x
ξ
Ω=⎢
⎢ω y − ωη
⎢ω − ω
ζ
⎣ z
− (ω x − ωξ ) − (ω y − ωη ) − (ωz − ωζ )⎤
0
ω z + ωζ
− (ω y + ωη )⎥
⎥.
0
ω x + ωξ ⎥
− (ω z + ωζ )
⎥
ω y + ωη
0
− (ω x + ωξ )
⎦
В результате решения этого дифференциального уравнения находят параметры
Родрига-Гамильтона Λgb = [λ0 λ1 λ2 λ3 ]T .
Получив параметры Родрига-Гамильтона, находят направляющие косинусы C gb в соответствии с формулами (2.17).
В остальном алгоритм аналогичен предыдущему. Отметим, что использование кинематических уравнений в параметрах Родрига-Гамильтона позволяет
56
Примеры алгоритмов БИНС
Рис.4.3. Структурная схема алгоритма с интегрированием уравнения
в параметрах Родрига-Гамильтона в географическом сопровождающем базисе
уменьшить число дифференциальных уравнений, используемых в алгоритме,
до 10. Приведем эти уравнения в скалярной форме:
⎧2λ&0 = − (ω xc − ωξ )λ1 − (ω yc − ωη )λ2 − (ω zc − ωζ )λ3 ,
⎪
&
⎪⎪2λ1 = (ω xc − ωξ )λ0 + (ω zc + ωζ )λ2 − (ω yc − ωη )λ3,
;
⎨ &
⎪2λ2 = (ω yc − ωη )λ0 − (ω zc + ωζ )λ1 + (ω xc − ωξ )λ3
⎪ &
⎪⎩2λ3 = (ω zc − ωζ )λ0 + (ω yc + ωη )λ1 − (ω xc − ωξ )λ2
Примеры алгоритмов БИНС
57
⎧
vΕ
)vς ,
⎪v&Ε = aξ + (u sin ϕ + ως )vΝ − ( 2u cos ϕ +
+
R
h
1
⎪
⎪⎪
vΝ ⋅ vς
&
v
a
u
v
=
−
+
−
(
sin
ϕ
ω
)
,
⎨ Ν
η
ς Ε
R
h
+
1
⎪
⎪
v N2
v E2
⎪v&ς = aζ +
+
+ 2uvΕ cos ϕ + g ;
⎪⎩
R2 + h R1 + h
v
vE
, h& = vζ .
ϕ& = N , λ& =
RN
RE cos ϕ
4.4. Алгоритм с вектором ориентации
В данном алгоритме БИНС для определения ориентации вычисляется
r
Т
вектор ориентации φ = φ x φ y φ z (см. п.2.4). Для этого интегрируются уравнения (2.22)
r
φ⎞ r
dφ r 1 r 1 ⎛ φ
= ω − Ωφ + 2 ⎜1 − ctg ⎟Φ 2ω ,
dt
2
2⎠
φ ⎝ 2
[
⎡ 0
⎡ω x ⎤
⎢
где ω = ⎢ω y ⎥ ; Ω = ⎢ ω z
⎢ ⎥
⎢− ω y
⎢⎣ω z ⎥⎦
⎣
r
]
− ωz
0
ωx
⎡ 0
ωy ⎤
⎥
⎢
− ω x ⎥ ; Φ = ⎢ φz
⎢− φ y
0 ⎥⎦
⎣
− φz
0
φx
φy ⎤
⎥
− φ x ⎥ , или исполь0 ⎥⎦
зуются упрощенные уравнения (2.23), справедливые для достаточно малого шага интегрирования:
r
dφ r 1 r
= ω − Ωφ .
dt
2
Необходимую для последующих вычислений матрицу направляющих косинусов можно определить по точным формулам (2.25) или (2.26), или используя приведенную ниже численную процедуру.
Когда найдены компоненты вектора Эйлера φi , x , φi , y , φi , z , на интервале
интегрирования вычисляется матрица направляющих косинусов ΔСі , описывающая поворот от исходной системы к связанной на этом интервале. Для этого
используется соотношение (с учетом малого угла φ ) [13]
1
1
1
1
ΔC = e Φ = I + Φ + Φ 2 + Φ 3 + ... = I + Φ sin φ + Φ 2 2 (1 − cos φ ) , (4.6)
φ
2
6
φ
Примеры алгоритмов БИНС
58
где I – единичная матрица размера 3 × 3.
Матрицу направляющих косинусов для i–го момента Ci определяют путем перемножения
C i = C i − 1 ΔC i .
(4.7)
Если входной информацией является абсолютная угловая скорость, измеr
ряемая датчиками угловой скорости (вектор ω и соответствующая ему матрица
Ω ) , то вектор Эйлера покажет поворот относительно исходного инерциального
базиса i , а определяемая матрица направляющих косинусов может быть обозначена C ≡ C ib .
gb
После определения матрицы направляющих косинусов C дальнейшая
логика алгоритма БИНС полностью аналогична описанному в п.4.2.
Запишем последовательно уравнения алгоритма БИНС с вектором ориентации:
T r
r
r
ω o = ω − C gb ω g ;
( )
⎧φ&x = ω xo − (−ω zoφ y + ω yoφ z ) / 2;
⎪ &
o
o
o
⎨ φ y = ω y − (ω z φ x − ω x φ z ) / 2;
⎪φ& = ω o − (−ω oφ + ω oφ ) / 2;
z
y x
x y
⎩ z
⎡ 0
⎢
Φ = ⎢ φz
⎢− φ y
⎣
− φz
0
φx
φy ⎤
⎥
− φx ⎥ ;
0 ⎥⎦
1
1
ΔCigb = I + Φ sin φ + Φ2 2 (1 − cos φ ) ;
φ
φ
gb
Cigb = Cigb
−1 ⋅ ΔCi ;
[
r
a g = aξ
aη
aζ
]т = C gb arb ;
⎧
vΕ
)vς ,
⎪v&Ε = aξ + (u sin ϕ + ως )vΝ − ( 2u cos ϕ +
+
R
h
1
⎪
⎪⎪
vΝ ⋅ vς
,
⎨v&Ν = aη − (u sin ϕ + ως )vΕ −
+
R
h
1
⎪
⎪
v N2
v E2
⎪v&ς = aζ +
+
+ 2uvΕ cos ϕ + g ;
⎪⎩
R2 + h R1 + h
Примеры алгоритмов БИНС
59
vN
vE
, λ& =
, h& = vζ .
RN
RE cos ϕ
Отметим, что использование вектора ориентации, так же, как и кинематических уравнений в параметрах Родрига-Гамильтона, позволяет уменьшить
число дифференциальных уравнений, используемых в алгоритме, до 8 – по
сравнению с 12 при использовании уравнений Пуассона (см. п.4.2).
ϕ& =
Рис.4.4. Структурная схема алгоритма с интегрированием вектора ориентации
в географическом сопровождающем базисе
60
Методы и алгоритмы вычислений
4.5. Методы и алгоритмы вычислений
Для реализации алгоритмов БИНС используют бортовые цифровые вычислительные машины (БЦВМ). Как известно, в БЦВМ информация о параметрах непрерывного процесса движения (угловой скорости, ускорении и др.)
должна подаваться в дискретные моменты времени. Для интегрирования дифференциальных уравнений в БЦВМ используют различные численные методы.
При этом разнообразное движение объекта, на котором установлена БИНС,
может вносить особенности в результаты интегрирования. Можно сказать, что
разработка численных алгоритмов – отдельное направление в проектировании
БИНС. Оно отражено в многочисленной учебной и научной литературе. Для
примера отметим [8,9,13,14,16]. В настоящем параграфе мы лишь отметим вопросы, которые необходимо рассматривать при проектировании системы.
Дискретизация сигнала измерителей
Выбор периода квантования или частоты обработки данных диктуется,
как правило, возможностью восстановления с требуемой точностью по дискретной последовательности значений измеряемого непрерывного сигнала. Естественно, чем меньше шаг дискретизации Δt ≡ h , тем ближе дискретная последовательность соответствует непрерывному процессу. Однако уменьшение
Δt ограничивается техническими возможностями аппаратуры, БЦВМ. Поэтому, для реализации в различных фрагментах алгоритма используют различные
Δt .
Наименьший Δtи (такт) первичной информации желательно иметь при
съеме первичной информации, поскольку до ввода в основной алгоритм сигнал
может предварительно обрабатываться, например, сглаживаться. В соответствии с теоремой Котельникова, частота дискретизации f и = 1 Δtи должна, как
минимум вдвое превышать максимальную частоту изменения f max входного
сигнала. В настоящее время технические возможности позволяют реализовать
f и < 3 кГц [13]. Зачастую выходной сигнал с датчиков угловой скорости и акселерометров получают в виде интеграла от измеряемой величины. Это интегрирование выполняется, если на выходе измерителя стоит преобразователь напряжения или тока в частоту (ПНЧ). Количество импульсов сигнала на такте
отсчета соответствует приращению измеряемой величины. Приращение угла
поворота объекта θi ,k +1 на шаге k + 1 относительно соответствующей оси и
приращение "кажущейся" линейной скорости будут соответственно
Δωi , k +1 ≡ θi , k +1 =
t k +1
∫ ωi,k dt;
tk
Δvi , k +1 =
t k +1
∫ v&i ,k dt;
tk
i = x, y , z .
Методы и алгоритмы вычислений
61
При использовании ПНЧ интегрирование производится простейшим методом
прямоугольников.
Если для оцифровки сигналов измерителя используется аналогоцифровой преобразователь (АЦП), для получения приращений могут быть использованы более сложные методы интегрирования, например, метод парабол
(формула Симпсона)
tk +2
Δt
θ k +1 = ∫ ωk dt = и (ωk −1 + 4ωk + ω2 k +1 );
3
t
k
Большую величину должна иметь частота решения так называемой «быстрой» задачи. Такой задачей является решение уравнений ориентации. Для
многих объектов в настоящее время частота решения «быстрой» задачи составляет 100…300 Гц, следовательно, шаг дискретизации составляет 0,01…0,003 с.
Наименьшую величину имеет частота решения "медленных" задач. Это
может быть задача навигации. Обычно она не превышает 10 Гц, т.е. шаг дискретизации имеет порядок Δt м > 0,1 c .
Для конкретной системы все эти такты выбирают, исходя из условий работы и требований технического задания к системе.
На разработку и оптимизацию алгоритмов существенные ограничения
накладывает вид выходной информации датчиков: квантование информации по
уровню может привести к ошибке, существенно превышающей как остальные
ошибки обработки информации, так и инструментальные погрешности измерительных приборов.
Интегрирование дифференциальных уравнений
При интегрировании можно использовать различные методы. В первую
очередь, это одношаговые методы. Они позволяют определить следующую
точку интегральной кривой по информации только об одной предыдущей точке. Одношаговые методы называют методами Рунге-Кутта.
Ниже приведем таблицу, представляющую суть этих методов (табл.4.1.).
Обобщенная формула получения последующего значения переменной
yk +1 на шаге k + 1 по известному значению ее yk на предыдущем шаге k для
методов Рунге-Кутта имеет вид
yk +1 = yk + h ⋅ F (tk , yk , h ) .
(4.8)
Зависимость F (tk , yk , h ) может иметь сложный вид. Именно ее видом отличаются модификации методов Рунге-Кутта (см. табл. 4.1). В таблице
Z (tk , yk ) ≡ y 'k - производная, соответствующая скорости изменения интегрируемой функции. Порядок метода говорит о его точности, определяемой членами порядка h p , где степень p и есть порядок метода [8].
Методы и алгоритмы вычислений
62
Таблица 4.1. Методы Рунге-Кутта
Порядок
Формула метода
метода
1
F=k1
2
F=( k1+ k2)/2
2
3
3
4
4
Вспомогательные величины
k1=Z(tk; yk)
k1=Z(tk; yk);
k2=Z(tk+h; yk+hk1)
F=Z(tk+h/2; yk+hk1/2) k1=Z(tk; yk)
F=( k1+ 4k2+ k3)/6
k1=Z(tk; yk);
k2=Z(tk+h/2; yk+hk1/2);
k3=Z(tk+h; yk+h(2k2-k1))
F=(k1+3k3)/4
k1=Z(tk; yk);
k2=Z(tk +h/3; yk +h k1/3);
k3=Z(tk + 2h/3; yk + 2hk2/3)
F=(k1+2k2+2k3+ k4)/6 k1=Z(tk; yk);
k2=Z(tk +h/2; yk + h k1/2);
k3=Z(tk +h/2; yk + h k2/2);
k4=Z(tk +h; yk+ h k3)
F=(k1+3k2+3k3+k4)/8 k1=Z(tk; yk);
k2=Z(tk +h/3; yk + h k1/3);
k3=Z(tk +2h/3; yk + h (k2- k1/3);
k4=Z(tk +h; yk+ h (k1 - k2+ k3))
Название
метода
Эйлера
модифицированный
Эйлера
Хойне
Рунге-Кутта
Достаточно эффективны многошаговые методы. Многошаговые методы называют еще методами прогноза-коррекции. Это обусловлено тем, что расчеты
по этим методам построены на применении двух последовательных этапов. На
первом на основе информации о предыдущих точках прогнозируется значение
искомой переменной состояния в следующей точке. На втором осуществляется
уточнение ("корректировка") найденного значения по специальной неявной
формуле [8]. В таблице 4.2 для примера приведены формулы методов второго и
третьего порядка. Существуют и методы четвертого порядка.
Таблица 4.2. Формулы методов прогноза-коррекции
Порядок
метода
2
3
Формула прогноза
Формула коррекции
yk +1 = yk −1 + 2h ⋅ Z (tk , yk )
h
yk +1 = yk + [ Z (tk +1 , yk* +1 ) +
2
+ Z (tk , yk )]
h
[23 ⋅ Z (tk , yk ) −
12
− 16 ⋅ Z (tk −1 , yk −1 ) + 5 ⋅ Z (tk − 2 , yk − 2 )]
h
[5 ⋅ Z (tk +1 , y k* +1 ) −
12
+ 8 ⋅ Z (tk , y k ) − Z (tk −1 , y k −1 )]
yk +1 = yk +
( yk* +1 получают при прогнозе на первых шагах)
y k +1 = y k +
Методы и алгоритмы вычислений
63
Канал ориентации
Уг лы ориент ации плат формы, г рад
5
Уравнения Пуассона
Уг ловое движ ение по з акону:
θ
ψ (t) = ψ 0 + ψ '0t + ψ msin(ω ψt + εψ)
θ (t) = θ0 + θ'0t + θmsin(ω t + ε )
θ
θ
φ (t) = φ 0 + φ '0t + φ msin(ω φ t + εφ )
φ
Параметры уг ловог о движ ения :
ψ
0
-----------------------------------------------------ψ0 = 0
0
; θ0 = 0
0
; φ0 = 0
0
0
;
0
0
ψ '0 = 0 /c ; θ'0 = 0 /c ; φ '0 = 0 /c ;
ψm = 5
-5
0
50
x 10
8
6
150
t, c
200
250
300
0
; φm = 5
-1
0
;
-1
0
-1
0
ε ψ = 30 ; ε θ = 90 ; ε φ = 0 ;
------------------------------------------------------
Ошибки Д УС (г рад/час);
Δωx = 0 ; Δωy = 0 ; Δωz = 0 ;
Δψ
intomx = 0; intomy = 0; intomz = 0;
------------------------------------------------------
Δθ
Δφ
2
Ошибки акселеромет ров (м/с ):
4
Δax = 0 ; Δay = 0 ; Δaz = 0 ;
2
intax = 0; intay = 0; intaz = 0;
------------------------------------------------------
0
Ошибки начальной выставки:
-2
0
0
0
δψ 0 = 0 ; δθ0 = 0 ; δφ 0 = 0 ;
-4
Порядок метода интег рирования:
-6
-8
0
; θm = 5
ωψ = 0.1 c ; ωθ = 0.1 c ; ωφ = 0.1 c ;
0
Ошибки определения уг лов, уг л. мин.
-9
10
100
0
ode4 (Runge-Kutta)
50
100
150
t, c
200
250
300
Шаг инт ег рирования [c] , hi = 0.1
===============================
Прог рама BINS-upr;15:32: 07-Jan-2011
Выполнил: Васильковский И. В.
Рис.4.5. Ошибки численного интегрирования
Рис.4.6. Зависимость ошибки определения угла курса от шага
и метода интегрирования
64
Методы и алгоритмы вычислений
Погрешности численного интегрирования уравнений зависят от используемых
уравнений ориентации, от выбранного метода интегрирования, от шага интегрирования. На рис.4.5 приведены графики изменения погрешности интегрирования для 4 типов кинематических уравнений в зависимости от шага (такта) интегрирования [17]. Очевидно, что с уменьшением шага погрешности уменьшаются. Но уменьшение шага ограничено возможностями аппаратуры (возможностями аналого-цифрового преобразователя, возможностями БЦВМ и др.).
Меньшие погрешности имеют место для уравнений в кватернионах и вектора ориентации. Из рис.4.6 можно увидеть, что повышение порядка метода
интегрирования дает повышение точности. Оценка времени вычислений при
моделировании показывает (рис.4.7), что наибольшее время требуется для решения уравнений с вектором ориентации, наименьшее – для уравнений Эйлера
и Пуассона. Следовательно, последние позволяют использовать наименьший
шаг интегрирования. Однако по совокупности оценок, представляется, что наибольший интерес для реализации представляют уравнения в кватернионах или
в параметрах вектора ориентации. Очевидно, что окончательное решение можно принимать после моделирования для конкретных условий, конкретной задачи.
Существуют тестовые движения, при которых оцениваются различные
алгоритмы численного интегрирования. Это могут быть конические движения
или другие движения. Наиболее явно "конические" ошибки проявляются тогда,
когда имеют место гармонические угловые колебания объекта по двум взаимно
перпендикулярным осям с одной и той же частотой и с фазой, сдвинутой на
π / 2 . В этом случае верхняя граница частотного диапазона измерения должна
Рис.4.7. Диаграмма затрат времени
Методы и алгоритмы вычислений
65
быть значительно выше ( в 5-10 раз), чем частота колебаний [22].
Для уравнений ориентации разработаны специальные алгоритмы численного решения уравнений, использующее до 4-х шагов обновления информации
и имеющие 6-й порядок точности [9]. Пример алгоритмов вычислений в БИНС
можно найти в [13].
4.6.
Избыточность измерений
Обычно измерительный блок содержит 3 одноосных акселерометра и 3
датчика угловой скорости. Некоторые измерители устроены так, что они имеют
2 измерительных оси, расположенные взаимноперпендикулярно. Это могут
быть двухосные акселерометры или динамически настраиваемые гироскопы,
твердотельные волновые гироскопы. В этом случае возникает вопрос, как с
пользой использовать возникающую избыточность информации. А иногда специально с целью повышения точности и надежности системы ее строят избыточной, например, ставят вместо трех 6 и более измерителей.
Система, содержащая более 3 измерительных осей с произвольным, но
известным расположением, называется функционально-избыточной.
Избыточность характеризуется разностью l между n установленных измерителей и числом m минимально необходимых в неизбыточном базисе:
l = m − n . Число неизбыточных базисов соответствует количеству сочетаний
n!
.
(4.9)
Cnm =
m! ( n − m)!
Например, из 6 измерителей можно сформировать 20 неизбыточных трехмерных базисов:
6 ⋅ 5⋅ 4 ⋅ 3⋅ 2
C63 =
= 20 .
3 ⋅ 2 ⋅ (6 − 3) ⋅ 2
Рассмотрим простейший пример с минимальной избыточностью, когда имеются 4
измерительных оси.
На рис. 4.8 покажем расположение 2-х
измерителей с взаимноперпендикулярными
осями. Оси измерителей подняты на угол 45
градусов над плоскостью Oxy . Если в качестве измерителей приняты акселерометры, то
выходной сигнал каждого из акселерометров
можно записать в виде
ai = ax cos( x i ) + a y cos( y i ) + az cos(z i ), i = 1,2,3,4 ,
Рис.4.8. Измерительный
«избыточный» блок
где ai - выходной сигнал (кажущееся ускорение) i -го акселерометра, a x , a y , a z - проекции
66
Избыточность измерений
вектора кажущегося ускорения на оси объекта Oxyz . При указанном расположении осей чувствительности акселерометров с углом наклона 45 градусов
a1 = a y cos 45o + a z cos 45o ,
a2 = − a y cos 45o + a z cos 45o ,
o
o
a3 = − a x cos 45 + a z cos 45 ,
(4.10)
a4 = a x cos 45o + a z cos 45o.
r
Если обозначить a = [a x , a y , a z ]T - измеряемый вектор кажущегося ускорения,
r
s = [a1 , a2 , a3 , a4 ]T - вектор выходных сигналов, то соотношение между ними
можно записать в виде
r
r r
(4.11)
s =Ra +e
4 ×1
4 ×3 3×1
4 ×1
где R - матрица направляющих косинусов положения измерительных осей (ее
r
иногда называют юстировочной [12]), e - вектор ошибок измерений. Матрица
R из (4.10) получит вид
1 1⎤
⎡0
⎢
⎥
2 ⎢ 0 − 1 1⎥
R=
.
2 ⎢− 1 0 1⎥
⎢ 1
0 1⎥⎦
⎣
Вектор кажущегося ускорения объекта можно получить методом наименьших
квадратов
)r
−1
r
a = R TR R T s .
(
)
)r
Очевидно, что точность определения a при этом повышается за счет сглаживаr
ния погрешностей e .
При наличии избыточности можно также контролировать отказ какоголибо измерителя.
Поскольку из (4.10) следует
a1 + a2 = 2a z , a3 + a4 = 2a z ,
различие сумм (с заданным допуском) говорит о наличии отказа одного из акселерометров. При определении отказавшего измерителя можно исключить его
из работы и перейти к работе с тремя исправными измерителями1.
1
Методика определения отказов изложена в [21, 23]
Избыточность измерений
67
Из системы с 4-мя измерителями можно получить, в соответствии с (4.9),
4 трехмерных измерительных базиса:
r
s1
r
s2
r
s3
r
s4
= [ a 1 , a 2 , a 3 ]T ,
= [ a 2 , a 3 , a 4 ]T ,
= [ a 1 , a 3 , a 4 ]T ,
= [ a 1 , a 2 , a 4 ]T .
Матрицы направляющих косинусов в этом случае получают вид
⎡0
2⎢
0
R1 =
2 ⎢
⎣⎢ − 1
⎡0
2⎢
−1
R3 =
2 ⎢
⎢⎣ 1
1
−1
0
1
0
0
1⎤
1⎥ ,
⎥
1⎦⎥
1⎤
1⎥ ,
⎥
1⎥⎦
⎡0
2⎢
−1
R2 =
2 ⎢
⎣⎢ 1
⎡0
2⎢
0
R4 =
2 ⎢
⎢⎣1
−1
0
0
1
−1
0
1⎤
1⎥ ,
⎥
1⎦⎥
1⎤
1⎥ .
⎥
1⎥⎦
Оценки вектора кажущегося ускорения можно записать в 4 вариантах:
(
(
(
(
)
−1
r
a1 = R1T R1 R1T [a1 , a2 , a3 ]T ,
−1
r
a 2 = R 2 T R 2 R 2 T [a2 , a3 , a4 ]T ,
−1
r
a 3 = R 3T R 3 R 3T [a1 , a3 , a4 ]T ,
−1
r
a 4 = R 4 T R 4 R 4 T [a1 , a2 , a4 ]T .
)
)
)
(4.12)
Структура избыточного блока может быть разнообразной. Блоки могут
отличаться количеством измерителей, их расположением относительно осей
связанного базиса. Как показано в [23], предпочтительной является конфигурация, когда измерители расположены так, что их оси чувствительности образуют
конус с углом при вершине α = arccos a (1 / 3 ) = 54,7o (рис. 4.9). В этом случае
углы между измерительной осью и осями связанного базиса одинаковы, откуда
следует соотношение
cos2 α + cos2 α + cos2 α = 1 ,
которое и приводит к указнной формуле. Таким образом, 3 измерителя на таком
конусе образуют декартову систему координат.
Но на конфигурацию влияет не только количество измерителей и угол
при вершине конуса α , а также поворот системы относительно связанного ба-
68
Избыточность измерений
зиса вокруг оси конуса на угол β (рис. 4.9). В [23] показано, что наименьшие
случайные погрешности при равноточных измерителях имеют блоки при n=4 c
β = 45o , при n=5 c β = 27o , при n=6 c β = 15o .
Рис.4.9. Разновидности "избыточных" блоков
Для блока с n=5 юстировочная матрица имеет вид
cos β sin α
⎡
⎢ cos( β + 72o ) sin α
⎢
R = ⎢ cos( β + 144o ) sin α
⎢cos( β + 216o ) sin α
⎢
o
⎣⎢cos( β + 288 ) sin α
sin β sin α
sin( β + 72o ) sin α
sin( β + 144o ) sin α
sin( β + 216o ) sin α
sin( β + 288o ) sin α
cos α ⎤
cos α ⎥
⎥
cos α ⎥ .
cos α ⎥⎥
cos α ⎦⎥
Блоки с 5 измерителями и более позволяют не только выявить наличие отказа,
но и определить, какой измеритель отказал. При n=5 выявление отказавшего
измерителя производится путем сравнения информации от 10 ( в соответствии с
(4.9)) неизбыточных базисов.
Уравнения идеальной и реальной работы
69
5. Уравнения идеальной и реальной работы
Алгоритм работы БИНС описывают системой уравнений, которые называют уравнениями идеальной работы. В этих уравнениях отсутствуют какиелибо факторы, учитывающие отклонения работы БИНС от идеальной. Как показано в главе 4, существует достаточно много алгоритмов БИНС. Каждому из
них соответствуют свои уравнения идеальной работы. Рассмотрим примеры таких уравнений.
5.1. Уравнения идеальной работы БИНС в инерциальной
системе координат
Эти уравнения можно заимствовать из гл.4.
Уравнение ориентации Пуассона для определения матрицы направляющих косинусов в матричном виде можем записать из (2.12)
C& ib = C ibΩib
b .
(5.1)
Для организации вычислений могут быть использованы эти уравнения в скалярной форме:
⎧ c&11 = ωzc c21 − ω yc c31 ,
⎪
⎨ c&21 = ω xc c31 − ωzc c11 ,
⎪c& = ω c − ω c ;
yc 11
xc 21
⎩ 31
⎧ c&12 = ω zc c22 − ω yc c32 ,
⎪
⎨ c&22 = ω xc c32 − ωzc c12 ,
⎪c& = ω c − ω c ;
yc 12
xc 22
⎩ 32
⎧c&13 = ωzc c23 − ω yc c33 ,
⎪
⎨ c&23 = ω xc c33 − ω zc c13 ,
⎪c& = ω c − ω c .
yc 13
xc 23
⎩ 33
Очевидно, что для выполнения вычислений необходимо знать начальные значения направляющих косинусов.
Полученные значения cij используют для преобразования кажущихся ускорений, измеренных акселерометрами в связанной с объектом системе координат b , в проекции ускорений в инерциальной системе координат i:
r
r
ai = C ib ab .
В скалярной форме
a X = c11a xc + c12 a yc + c13a zc ,
aY = c21a xc + c22 a yc + c23a zc ,
(5.2)
a Z = c31a xc + c32 a yc + c33a zc .
Для получения составляющих абсолютного ускорения необходимо в соответствии с основным уравнением акселерометра учесть гравитационное ускорение,
Уравнения идеальной и реальной работы
70
которое можно рассчитать по формулам (4.2а)
(X 2 +Y 2)
g ′X
Z2
g ′Y
Z2
g ′Z
(1 − 3μ 2 ) ; g 'Y = −
(1 − 3μ 2 ) ; g ' Z = −
(1 + 3μ
).
g' X = −
r
r
r
r
r
r2
(5.3)
r r r
В результате
V&i = ai + g 'i , а в скалярной форме
V&X = a X + g ' X , V&Y = aY + g 'Y , V&Z = a Z + g ' Z .
Интегрирование абсолютных ускорений дает инерциальные координаты:
t
t
t
0
0
0
X = ∫ V&X dt +V X 0 , Y = ∫ V&Y dt +VY 0 , Z = ∫ V&Z dt +VZ 0 .
Таким образом, задача навигации в инерциальной системе координат решена.
Перейдем к определению параметров в географическом базисе.
От инерциальных координат перейдем к географическим
ϕ=
Z
2
X +Y
2
Λ = arctg
,
Y
,
X
λ = Λ − ut .
Для получения относительной скорости, а также углов курса, крена и тангажа
составим матрицу
C gi X
Y
Z
ξ
η
ζ
− sin Λ
cos Λ sin ϕ
cos Λ
sin Λ sin ϕ
0
.
cosϕ
cos Λ cosϕ
sin Λ cosϕ
sin ϕ
Затем выполним преобразование
C gb = C gi C ib .
В результате получим значения элементов матрицы
C gb
ξ
η
ς
xc
c11 = cos γ cosψ +
+ sin γ sinψ sin ϑ ;
c21 = - cos γ sinψ +
+ sin γ cosψ sin ϑ ;
c31 = − cosϑ sin γ ;
yc
c12 = cosϑ sinψ
c22 = cosϑ cosψ ;
c32 = sin ϑ ;
zc
c13 = cosψ sin γ −
− sinψ cos γ sin ϑ ;
c23 = − sinψ sin γ −
− cos γ cosψ sin ϑ ;
c33 = cosϑ cos γ .
(5.4)
Уравнения идеальной и реальной работы
71
Используя элементы этой матрицы, получим углы ориентации объекта
⎞
⎛
gb ⎞
gb
gb ⎞
⎟
⎜
⎛ c31
⎛ c12
c32
ψ = arctg ⎜⎜ gb ⎟⎟, γ = −arctg ⎜⎜ gb ⎟⎟,
ϑ = arctg ⎜
⎟
2
gb
c
c
⎟
⎜ 1− с
⎝ 22 ⎠
⎝ 33 ⎠
32
⎠
⎝
r
r
gi
и составляющие абсолютной линейной скорости (Vg = C Vi ):
( )
,
Vξ = −V X sin Λ + VY cos Λ ,
Vη = V X cos Λ sin ϕ + VY sin Λ sin ϕ + VZ cosϕ ,
Vζ = V X cos Λ cosϕ + VY sin Λ cosϕ + VZ sin ϕ .
Составляющие относительной скорости
v E = Vξ − uRE cosϕ , v N ≡ Vη , v H ≡ Vη .
5.2. Уравнения реальной работы БИНС в инерциальной
системе координат
Уравнения реальной работы учитывают те факторы, которые изменяют
идеальную работу. Вид уравнений, естественно, зависит от того, насколько детально учитывают эти факторы (возмущения).
Для примера рассмотрим уравнения решения задачи навигации в инерциальной системе координат. Возмущающими факторами пусть будут эквивалентный уход гироскопов Δω , эквивалентная ошибка акселерометров Δa и погрешности начальной выставки в виде ошибок начальных значений направляющих косинусов Δcij 0 .
Реальные значения параметров будем обозначать знаком ∼ . Тогда реальное уравнение Пуассона
~
~ ~
(5.5)
C& ib = C ibΩib .
b
Скалярные реальные уравнения Пуассона получат вид
~
~ c~ − ω
~ c~
⎧ c&11 = ω
zc 21
yc 31
⎪~
~
~
~
~
⎨ c&21 = ω xc c31 − ωzc c11
⎪c~& = ω~ c~ − ω~ c~
yc 11
xc 21
⎩ 31
или в развернутой форме
~
⎧ c&12 = ω~zc c~22 − ω~ yc c~32
⎪~
~ ~
~ ~
⎨ c&22 = ω xc c32 − ωzc c12
⎪c~& = ω~ c~ − ω~ c~
yc 12
xc 22
⎩ 32
~
⎧ c&13 = ω~zc c~23 − ω~ yc c~33
⎪~
~ ~
~ ~
⎨ c&23 = ω xc c33 − ω zc c13
⎪c~& = ω~ c~ − ω~ c~
yc 13
xc 23
⎩ 33
72
Уравнения идеальной и реальной работы
~
⎧c&11 = c&11 + Δc&11 = (ωzc + Δω zc )(c21 + Δc21 ) − (ω yc + Δω yc )(c31 + Δc31 ),
⎪⎪~
⎨c&21 = c&21 + Δc&21 = (ω xc + Δω xc )(c31 + Δc31 ) − (ωzc + Δω zc )(c11 + Δc11 ),
⎪c~& = c& + Δc& = (ω + Δω )(c + Δc ) − (ω + Δω )(c + Δc );
⎪⎩ 31 31
31
yc
yc
11
11
xc
xc
21
21
~
⎧c&12 = c&12 + Δc&12 = (ωzc + Δω zc )(c22 + Δc22 ) − (ω yc + Δω yc )(c32 + Δc32 ),
⎪⎪~
⎨c&22 = c&22 + Δc&22 = (ω xc + Δω xc )(c32 + Δc32 ) − (ω zc + Δω zc )(c12 + Δc12 ),
⎪c~& = c& + Δc& = (ω + Δω )(c + Δc ) − (ω + Δω )(c + Δc );
⎪⎩ 32
32
32
yc
yc
12
12
xc
xc
22
22
~
⎧c&13 = c&13 + Δc&13 = (ω zc + Δω zc )(c23 + Δc23 ) − (ω yc + Δω yc )(c33 + Δc33 ),
⎪⎪~
⎨c&23 = c&23 + Δc&23 = (ω xc + Δω xc )(c33 + Δc33 ) − (ω zc + Δω zc )(c13 + Δc13 ),
⎪c~& = c& + Δc& = (ω + Δω )(c + Δc ) − (ω + Δω )(c + Δc ).
⎪⎩ 33 33
33
yc
yc
13
13
xc
xc
23
23
При решении таких уравнений начальными значениями будут
c~110 = c110 + Δc110 ,
c~120 = c120 + Δc120 ,
c~ = c + Δc ,
130
130
130
c~210 = c210 + Δc210 ,
c~220 = c220 + Δc220 ,
c~ = c + Δc ,
230
230
230
c~310 = c310 + Δc310 ,
c~320 = c320 + Δc320 ,
c~ = c + Δc .
330
330
330
Преобразование кажущихся ускорений представим в виде
~r ~ ~r
ai = C ib ab .
Это уравнение, как и уравнение Пуассона, легко записать в скалярной форме,
используя (5.2).
Далее запишем (5.3) для реальной работы, предполагая, что постоянные
g ′ и μ вычисляют без погрешностей
~
~
~
~
~
~
~
(X 2 +Y 2)⎞
g ′X ⎛
Z2 ⎞ ~
g ′Y ⎛
Z2 ⎞ ~
g ′Z ⎛
~
⎟.
g ' X = − ~ ⎜⎜1 − 3μ ~2 ⎟⎟ ; g 'Y = − ~ ⎜⎜1 − 3μ ~ 2 ⎟⎟ ; g ' Z = − ~ ⎜⎜1 + 3μ
⎟
~
r ⎝
r ⎝
r ⎝
r ⎠
r ⎠
r2
⎠
Затем получим реальные значения составляющих абсолютной линейной
скорости
~r ~r ~r
V&i = ai + g 'i .
Реальные значения инерциальных координат получим из формул
Уравнения идеальной и реальной работы
t
t
t
0
0
0
73
~
~
~
~
~
~
~
~
~
X = ∫ V&X dt +V X 0 , Y = ∫ V&Y dt +VY 0 , Z = ∫ V&Z dt +VZ 0 .
На этом можно, не переходя к определению реальных географических
координат, остановиться. Детализация реальных уравнений зависит от того, какие возмущающие факторы или условия работы мы хотим учитывать.
5.3. Уравнения идеальной работы БИНС в географической
системе координат
Приведем еще раз уравнения, описывающие работу БИНС с использованием обобщенного уравнения Пуассона:.
ig gb
C& gb = C gbΩib
b − Ωg C .
(5.6)
В скалярном виде они записаны в (2.12а).
Преобразование кажущихся ускорений из связанного в сопровождающий базис
соответствует выражению
r
r
a g = C gb ab .
(5.7)
Это уравнение легко представить как 3 скалярных уравнения:
Относительную скорость получим путем интегрирования уравнения
r r
r r
r r
v&g = ag − (ug + ωg ) × vg + gg .
Или из скалярных составляющих
⎧v&Ε = aξ + (u sin ϕ + ως )vΝ − (u cos ϕ + ωη )vς ,
⎪
⎨v&Ν = aη − (u sin ϕ + ως )vΕ + ωξ vς ,
⎪&
⎩vς = aζ − ωξ vΝ + (u cos ϕ + ωη )v E + g.
(5.8)
Широту и долготу получим из уравнений
ϕ& =
vN
vE
, λ& =
,
RN
RE cos ϕ
где радиусы кривизны эллипсоида RN и RE вычисляют по формулам (1.1),
(1.2)
74
Уравнения идеальной и реальной работы
RE (1 − e 2 )
,
RN =
1 − e 2 sin 2 ϕ
RE =
a+h
2
2
1 − e sin ϕ
.
Углы курса, крена и тангажа находят по формулам
⎛
gb
⎜
c32
ϑ = arctg ⎜
⎜ 1 − с gb
32
⎝
⎛ c gb ⎞
⎛ c gb ⎞
⎟, γ = − arctg ⎜ 31 ⎟,
ψ = arctg ⎜⎜ 12
gb ⎟
⎜ c gb ⎟
⎝ c22 ⎠
⎝ 33 ⎠
( )
2
⎞
⎟
⎟
⎟
⎠
.
5.4. Уравнения реальной работы БИНС в географической
системе координат
По аналогии с ранее приведенным примером, уравнения реальной работы
легко записать в виде:
~
~ ~
~ ig ~ gb
C& gb = C gbΩib
b − Ωg C ,
~r
~ ~r
a g = C gb ab .
(5.9)
~r
Реальное кажущееся ускорение в связанном базисе ab отличается от идеальноr
го вследствие ошибки акселерометров Δa :
r
r
~r
a b = a b + Δa .
~r ~r
~r ~r ~r ~r
v&g = ag − ug + ωg × vg + gg
(
~
v~
RN
)
~
v~E
,
RE cosϕ~
ϕ& = ~N , λ& = ~
~
RE (1 − e 2 )
~
,
RN =
1 − e 2 sin 2 ϕ~
~
RE =
~
a+h
1 − e sin ϕ~
2
2
.
Опуская верхние индексы направляющих косинусов, реальные значения
углов курса, крена и тангажа будут
⎞
⎛
~ ⎞
~
~ ⎞
⎟
⎜
⎛
⎛
c
c
c
~
32
ψ~ = arctg ⎜⎜ ~12 ⎟⎟, γ~ = −arctg ⎜⎜ ~31 ⎟⎟,
ϑ = arctg ⎜
⎟ .
2
c
c
~
⎟
⎜ 1− с
⎝ 22 ⎠
⎝ 33 ⎠
32 ⎠
⎝
( )
Уравнения ошибок БИНС
75
6. Уравнения ошибок БИНС
Важнейшей характеристикой измерительной системы является ее точность (или ошибки). Исследования точности системы и решение других задач
можно произвести с помощью уравнений ошибок.
Уравнения ошибок можно получить путем вычитания из уравнений реальной работы уравнений идеальной работы.
Рассмотрим это на примере уравнений (5.5), (5.1):
~
~ ~
ib ib
ΔC& ib = C& ib − C& ib = C ibΩib
b − C Ωb .
(6.1)
Выражения для приборных (реальных) значений
~
C& ib = C& ib + ΔC& ib ,
~
ib
ib
Ωib
b = Ω b + ΔΩ
подставим в уравнение (6.1)
(
)(
)
ib
ΔC& ib = C ib + ΔC ib Ωib
− C ibΩib
b + ΔΩ
b .
После выполнения преобразований получим выражение
ib
ib
ib
ib
ΔC& ib = ΔC ibΩib
b + C ΔΩ + ΔC ΔΩ b .
Поскольку обычно погрешности являются малыми величинами по сравнению с
номинальными идеальными величинами, их произведением (третье слагаемое)
можно пренебречь. Тогда дифференциальное выражение для ошибок вычисления направляющих косинусов получит вид
ib
ib
ΔC& ib ≈ ΔC ibΩib
b + C ΔΩ .
(6.2)
Это выражение можно получить также путем варьирования уравнения
идеальной работы C& ib = C ibΩib
b .
Аналогично, при использовании обобщенного уравнения Пуассона
ig gb
C& gb = C gbΩib
b − Ωg C
получим
( )
( )
( )
& gb
& gb
∂ C& gb
gb ∂ C
ib ∂ C
ib
ig
gb
gb
ib
ig
C
ΔΩ
+
ΔΩig
ΔC& gb ≈
Δ
+
b
g = (Ωb − Ωg )ΔC + C (ΔΩb − ΔΩg ) .
gb
ib
ig
∂C
∂Ωb
∂Ωg
Уравнения ошибок БИНС
76
Ошибки кажущихся ускорений в географическом сопровождающем базисе получим из выражения (5.7)
r
r
r
Δa g ≈ C gb Δab + ΔC gb ab .
Ошибки определения направляющих косинусов можно связать с ошибками определения углового положения связанного базиса следующим образом.
Приборное (вычисленное) значение можно записать
~
~
C gb = BC gb или C gb = ( I + Ψ ′)C gb = C gb + ΔC gb ,
где I - единичная матрица, Ψ′ - матрица углов ошибок,
B = I + Ψ ′, ΔC gb = Ψ ′C gb .
На рис.6.1. показано положение действительного Oxc yc zc и вычисленного базиса O~
xc ~
yc ~
zc , повернутого относительно действительного на углы δ , β , α .
Рис.6.1. Действительный и вычисленный связанные базисы
В предположении малости углов ошибок матрицы B и Ψ ' имеют вид
B
~
x
xc
yc
zc
1
δ
−α
~
yc
~
z
−δ
1
β
α
−β
1
c
c
Ψ'
~
x
xc
yc
zc
0
δ
−α
~
yc
~
z
−δ
0
β
α
−β
0
c
c
.
Уравнения ошибок БИНС
Или можно записать
где Ψ = − Ψ ' . Из (6.3)
77
~
C gb = ( I − Ψ )C gb ,
(6.3)
( )
T
~
Ψ = I − C gb C gb .
Продифференцируем (6.4)
( )
& = −C~& gb C gb
Ψ
T
(6.4)
( )
T
~
− C gb C& gb .
~
Подставляя выражения для C& gb и C& gb из (5.9), (5.6), с учетом матричных операций ( ( A ⋅ B )T = B T ⋅ AT ,
ΩT = − Ω )
(
)( )T − C~ gb (C gbΩibb − Ωigg C gb )T =
~ ~
~ ig ~ gb gb T ~ gb ib gb T ~ gb gb T ig
gb T
= −C gbΩib
b (C ) + Ω g C (C ) + C Ω b (C ) − C (C ) Ω g =
~ ~
~ ig ~ gb gb T ~ gb gb T ig
ib
gb T
)(
)
C
= −C gb (Ωib
−
Ω
+
Ω
b
b
g C (C ) − C (C ) Ω g .
~ ib ~ ig ~ gb gb
& = − C~ gbΩ
C
Ψ
b − Ωg C
~
Подставим вместо C gb выражение (6.3), с учетом
C ⋅ CT = 1,
(
)( )
( )
~
~ ig
ib
gb T
ig
= −( I − Ψ)C gb (Ωib
b − Ωb )(C ) + Ω g ( I − Ψ) − ( I − Ψ)Ω g .
( )
T
~ ib
~
ib
gb T
gb gb T
& = −( I − Ψ)C gb Ω
Ψ
+ Ωig
C
− ( I − Ψ)C gb C gb Ωig
b − Ωb C
g ( I − Ψ)C
g =
Учтем, что
(6.5)
~ ib
~ ig
ib
ig
ig
ΔΩ ib
=
Ω
−
Ω
,
ΔΩ
=
Ω
b
b
b
g
g − Ωg .
( )
gb
После подстановки, пренебрегая произведением малых ΨC gb ΔΩib
b C
( )
& ≈ ΨΩig − Ωig Ψ + ΔΩig − C gb ΔΩib C gb
Ψ
g
g
g
b
T
.
T
и др.,
(6.6)
Выражение (6.6) представим в векторной форме:
r
r
r
r
r
φ& ≈ −(ω gig ×)φ + Δω gig − C gb Δωbib .
r
Здесь вектор углов ошибок φ = [β
вектора
(6.7)
α δ ]T , матричная форма представления
Уравнения ошибок БИНС
78
⎡ 0
φ× = ⎢ δ
⎢
⎢⎣ − α
r
−δ
0
α ⎤
r
− β⎥ , φ× ≡ Ψ,
β
⎥
0 ⎥⎦
r
ωgig × ≡ Ωigg - матрица угловых скоростей сопровождающего базиса,
r
Δω gig × ≡ ΔΩig
g - матрица ошибок вычисления угловых скоростей сопровождающего базиса,
r
Δωbib × ≡ ΔΩib
b - матрица ошибок измерения угловых скоростей , соответствующая ошибкам датчиков угловой скорости.
Этим матрицам соответствуют векторы
r
Δω gig = [ Δωξ
Δωη
Δωζ ]T ,
r
Δωbib = [ Δω xc
Δω yc
Δω zc ]T .
В эквивалентности выражений (6.6) и (6.7) можно убедиться, проделав соответствующие преобразования.
В развернутом виде (6.7) запишем, как
⎡ β& ⎤
⎡ωξ ⎤ ⎡ β ⎤ ⎡ Δωξ ⎤ ⎡ c11 c12
⎢ ⎥
⎢ ⎥ ⎢ ⎥ ⎢
⎥ ⎢
&
=
−
×
+
Δ
α
ω
α
ω
η
η
⎢ ⎥
⎢ ⎥ ⎢ ⎥ ⎢
⎥ − ⎢c21 c22
⎢δ& ⎥
⎢ωζ ⎥ ⎢⎣δ ⎥⎦ ⎢ Δωζ ⎥ ⎢⎣c31 c32
⎣ ⎦
⎣
⎦
⎣ ⎦
c13 ⎤ ⎡ Δω xc ⎤
c23 ⎥ ⋅ ⎢ Δω yc ⎥ .
⎥ ⎢
⎥
c33 ⎥⎦ ⎢⎣ Δω zc ⎥⎦
Тогда в скалярном виде
β& = −ωη δ + ωζ α + Δωξ − c11Δω xc − c12 Δω yc − c13Δω zc ,
α& = −ωζ β + ωξ δ + Δωη − c21Δω xc − c22 Δω yc − c23Δω zc ,
(6.8)
δ& = −ωξ α + ωη β + Δωζ − c31Δω xc − c32 Δω yc − c33Δωzc .
Напомним, что в этих уравнениях Δωξ , Δωη , Δωζ - ошибки вычисления угловой скорости сопровождающего базиса. Исходя из выражений угловых скоростей сопровождающего базиса (1.3), ошибки их вычисления имеют вид
Δωξ = − Δv N / RN + v N RN−2 ΔRN ,
Δ ω η = Δ v E / R E − u sin ϕ ⋅ Δ ϕ ,
(
)
(
)
Δωζ = Δv E tgϕ / RE + v E /( RE cos2 ϕ ) + u cos ϕ Δϕ − v E tgϕ / RE2 ΔRE .
Ошибки вычисления относительной линейной скорости найдем, используя
r
дифференциальные уравнения вычисления v g , например, в виде
Уравнения ошибок БИНС
(
ro
79
)
r
r
r
r
r
v& g = C gb ab − 2u g + ω g × v g + g .
Приборное значение
(
)
~r
~r
~r
~r
~r
~ ~r
v& g = C gb ab − 2u g + ω go × v g + g .
Вычитая,
(
)
(
)
r
r ~r r
r
r
r
~r
~r
~r
~r r
~ ~r
Δv& = v& g − v& g = C gb ab − C gb ab − 2u g + ω go × v g + 2u g + ω go × v g + g − g .
r ~r r
Δab = ab − ab .
~
C gb = ( I − Ψ )C gb ,
Учтем, что
Предположим, что погрешности компенсации переносных и кориолисовых ускорений пренебрежимо малы, а также пренебрежимо малы ошибки вычисления
r
g.
После подстановок, пренебрегая произведениями малых
r
r
r
Δv& = − ΨC gb ab + C gb Δab .
r
r
C gb ab = a g .
Учтем, что
В результате
r
r
r
Δv& = − Ψa g + C gb Δab .
r
В развернутом виде, принимая, что a g = [a E , a N , a H ]T - вектор кажущихся ускорений,
⎡ Δv&E ⎤ ⎡ 0
⎢ Δv& ⎥ = ⎢ − δ
⎢ N⎥ ⎢
⎢⎣ Δv&H ⎥⎦ ⎢⎣ α
δ
0
−β
− α ⎤ ⎡ a E ⎤ ⎡ c11 c12
β ⎥ ⋅ ⎢ a N ⎥ + ⎢c21 c22
⎥ ⎢ ⎥ ⎢
0 ⎥⎦ ⎢⎣a H ⎥⎦ ⎢⎣c31 c32
c13 ⎤ ⎡ Δa xc ⎤
c23 ⎥ ⋅ ⎢ Δa yc ⎥ .
⎥
⎥ ⎢
c33 ⎥⎦ ⎢⎣ Δa zc ⎥⎦
(6.9)
В скалярном виде
Δv&E = δ ⋅ a N − α ⋅ a H + c11Δa xc + c12 Δa yc + c13Δa zc ,
Δv& N = −δ ⋅ a E + β ⋅ a H + c21Δa xc + c22 Δa yc + c23Δa zc ,
(6.10)
Δv&H = α ⋅ a E − β ⋅ a N + c31Δa xc + c32 Δa yc + c33Δa zc .
Ошибки координат
Δϕ& = − Δωξ ,
Δλ& =
1
v
( Δv E + v E tgϕΔϕ − E ΔRE ).
RE cos ϕ
RE
(6.11)
Уравнения ошибок БИНС
80
Ошибки ΔRE , ΔRN можно найти путем варьирования выражений (1.1), (1.2).
ΔRE
(
a + h )e 2 sin 2ϕ
=
Δϕ ,
2
2
1 − e sin ϕ
(
)
ΔRN = ΔRE 1 − e 2 . .
Можно отметить, что уравнения ошибок данной БИНС отличаются от
уравнений ошибок платформенной ИНС тем, что в уравнениях (6.8) сумма последних 3-х слагаемых в каждом уравнении (сумма проекций уходов ДУС) эквивалентна уходу платформы по ее оси. Аналогично, в уравнениях (6.10) сумма
последних 3-х слагаемых в каждом уравнении (сумма проекций ошибок акселерометров) эквивалентна ошибке измерения ускорения по оси платформы.
В заключение приведем пример ошибок БИНС (рис. 6.2), полученный
моделированием по уравнениям (6.8), (6.10), (6.11) при ошибках начального горизонтирования 0,01 град., азимутальной выставки 0,1 град., эквивалентном
уходе датчиков угловой скорости 0,1 град./час, эквивалентной ошибке акселерометров 10-4 м/с2
6
Oшибки определения широты, долготы, угл.мин.(мили) [φ(-), λ (-.-)]
Ошибки БИНС
4
fi0= 0 (град.)
2
β0= 0.01 (град.)
0
α0= 0.01 (град.)
-2
0
δ 0= 0 (град.)
0.5
1
1.5
2
2.5
3
3.5
4
Ошибки определения скорости, м/c [Vn(-), Ve(-.-)]
3
hi0= 45 (град.)
ω dr= 0.02 (град./час)
2
2
da= 0.0001 (м/сек )
1
0
-1
-2
0
2
0.5
1
1.5
2
2.5
3
3.5
4
Ошибки ориентации платформы, угл.мин. [бета(-), альфа(-.-), дельта(...)]
0
-2
-4
-6
0
V= 200 (м/сек)
0.5
1
1.5
2
Время,час
2.5
3
Рис.6.2. Пример ошибок БИНС
3.5
4
Начальная выставка БИНС
81
7. Начальная выставка БИНС
Алгоритмы работы БИНС описываются системой дифференциальных
уравнений, и для их решения необходимо задать начальные условия, которые и
определяются в процессе начальной выставки.
Cуть алгоритма БИНС, так же как и платформенной ИНС, состоит в интегрировании ускорений (дважды) для определения текущих значений линейной скорости и координат объекта. Для интегрирования уравнений необходимо
задать начальные условия – 3 проекции начальной линейной скорости и 3 координаты. БИНС не может автономно определить эти начальные параметры, для
этого нужна внешняя информация. Во многих случаях БИНС включают при
неподвижном относительно Земли объекте. В этом случае проекции линейной
скорости, например, в осях географического сопровождающего трехгранника
равны нулю. А вот координаты места объекта должны быть введены извне, например, от приемника СНС.
Для определения ориентации также необходимо задать начальные углы.
Это начальные условия для дифференциальных уравнений Эйлера. Для уравнений Пуассона необходимо задать начальную матрицу направляющих косинусов. Для уравнений в кватернионах необходимо, соответственно, задать начальный кватернион ориентации.
В отличие от начальной линейной скорости и координат, начальные углы
ориентации могут быть определены автономно самой БИНС, хотя возможно
получить эти углы и извне.
В этой главе рассмотрим начальную выставку БИНС на неподвижном основании. Вопросы выставки БИНС при движении объекта будут рассмотрены в
п.10.4
В бесплатформенных инерциальных навигационных системах начальная
выставка, в основном, производится аналитически. По информации, поступающей от чувствительных элементов БИНС, определяются начальные значения
параметров – углов Эйлера-Крылова, направляющих косинусов, других параметров. Такая выставка является автономной так как не требует привлечения
внешней информации.
Рассмотрим выставку БИНС с
датчиками угловой скорости .
На рис.7.1 показаны Оξηζ - географический сопровождающий трёхгранник , Оxyz - трёхгранник, связаный
с блоком чувствительных элементов
БИНС ( далее просто БИНС ).
Матрица направляющих косинусов между осями Оξηζ и Оxyz соответствует таблице 7.1 .
Чувствительными элементами яв- Рис. 7.1. Начальное положение БИНС
Начальная выставка БИНС
82
ляются акселерометры и, например, датчики угловой скорости (ДУС), измеряющие абсолютную угловую скорость . Это могут быть поплавковые ДУС ,
ДУС на динамически настраиваемых гироскопах , лазерные гироскопы , волоконно - оптические гироскопы, твердотельные волновые гироскопы.
Таблица 7.1. Направляющие косинусы
C gb
ξ
η
ς
x
y
c11 = cos γ 0 cosψ 0 +
+ sin γ 0 sinψ 0 sin ϑ0 ;
c21 = - cos γ 0 sinψ 0 +
+ sin γ 0 cosψ 0 sin ϑ0 ;
c31 = − cosϑ0 sin γ 0 ;
c12 = cosϑ0 sinψ 0
c22 = cosϑ0 cosψ 0 ;
c32 = sin ϑ0 ;
z
c13 = cosψ 0 sin γ 0 −
− sinψ 0 cos γ 0 sin ϑ0 ;
c23 = − sinψ 0 sin γ 0 −
− cos γ 0 cosψ 0 sin ϑ0 ;
c33 = cosϑ0 cos γ 0 .
Акселерометры измеряют проекции вектора ускорения силы тяжести g
gx = gcosϑ0sinγ0 = - gc31 ;
gy = -gsinϑ0 = - gc32 ;
gz = - gcosγ0cosϑ0 = - gc33 .
(7.1)
На неподвижном относительно Земли основании проекции угловой скорости
вращения сопровождающего трёхгранника Οξηζ имеют вид
ωξ = 0 , ωη = u cos ϕ , ωζ = u sin ϕ ,
-5
где u= 7,29⋅ 10 1/с - угловая скорость вращения Земли , ϕ - широта .
Датчики угловой скорости измеряют абсолютные угловые скорости
Отметим , что
ωx = ωη c21 + ωζ c31 ;
ωy = ωη c22 + ωζ c32 ;
ωz = ωη c23 + ωζ c33 .
(7.2)
ωy = ωηcosϑ0 cosψ0 + ωζsinϑ0 .
(7.3)
Для того , чтобы обеспечить начальными условиями алгоритм функционирования БИНС , необходима информация об угловом положении объекта (
углы ψ0 , γ0 , θ0 ) и информация о направляющих косинусах между осями Oxyz и
Oξηζ ( табл. 2.1).
Из выражений (7.1) следует , что
⎛
⎞
ϑ0= arcsin (- gy /g) или ϑ0= - arctg (-gy ⋅ sinγ0 / gx) или ϑ0 = arctg ⎜ c32 ⎟ ;
2 ⎟
⎜
⎝ 1 − с32 ⎠
γ0= - arctg( gx / gz ) , g = g x2 + g y2 + g z2 ;
(7.4)
Начальная выставка БИНС
Тогда из (7.3)
83
c31 = - gx /g , c32 = - gy/g , c33 = - gz /g .
ψ0= arccos (ωy - ωζ⋅sinϑ0)/ωη cosϑ0 .
(7.5)
Однако это определение неоднозначно .
Получив c31 , c32 , c33 из (7.2), можно найти
c21 = (ωx - ωζ⋅c31) ωη , c22 = (ωy - ωζ⋅c32)/ωη , c23 = (ωz - ωζ⋅c33)/ ωη . (7.6)
Получив шесть направляющих косинусов , остальные три можно найти из соотношений :
c11 = c22 c33 - c23 c32 ,
(7.7)
c12 = c23 c31 - c21 c33 ,
c13 = c21 c32 - c22 c31 .
Если известны все направляющие косинусы , то начальное значение курса
можно найти из соотношения
(7.8)
ψ0= arctg (c12 /c22 ) .
Использование выражения (7.8) позволяет устранить неоднозначность
определения ψ0 для разных квадрантов , которая существует в выражении (7.5) .
Структурная схема алгоритма начальной выставки приведена на рис.7.2 .
Если алгоритм ориентации построен на использовании параметров Родрига -Гамильтона , то для его функционирования необходимо получить начальные значения этих параметров . Их можно получить из формул :
λ00 = cos
λ10 = cos
λ20 = cos
λ30 = sin
ψ0
2
ψ0
2
ψ0
2
ψ0
2
cos
sin
2
ϑ0
2
cos
cos
ϑ0
ϑ0
2
ϑ0
2
cos
γ0
2
cos
γ0
sin
γ0
cos
γ0
2
2
2
+ sin
ψ0
2
+ sin
ψ0
− sin
ψ0
− cos
2
2
ψ0
2
sin
ϑ0
cos
ϑ0
sin
2
2
ϑ0
sin
2
ϑ0
2
sin
γ0
sin
γ0
cos
γ0
sin
γ0
2
,
2
2
2
,
,
.
Каким же требованиям должны удовлетворять ЧЭ для того , чтобы обеспечить начальную вставку с приемлемой точностью ?
Чтобы ответить на этот вопрос , необходимо оценить зависимость величины погрешностей выставки от величины погрешностей ЧЭ . Оценим эту зависимость при определении начального положения в азимуте ψ 0 . Для этого
будем варьировать соответствующее выражение.
Пусть
ψ 0 = arctg ( c12 / c22 ) = F1 (c12 , c 22 ) ,
84
Начальная выставка БИНС
Рис.7.2. Структурная схема алгоритма начальной выставки
Начальная выставка БИНС
Δψ 0 =
=
85
δF1
δF
Δc12 + 1 Δc22 =
δc12
δc22
1 / c22
1 / c22
Δ
+
c
12
1 + ( c12 / c22 ) 2
1 + ( c12 / c22 ) 2
(
− c12
) Δc22 .
c22
Из (7.7) выпишем, что c12 = c23c31 − c21c33 = F2 ( c23 , c31 , c21 , c33 );
Δc12 =
δF2
δF
δF
δF
Δc23 + 2 Δc31 + 2 Δc21 + 2 Δc33 =
δc23
δc31
δc21
δc33
= c31Δc23 + c23Δc31 − c33Δc21 − c21Δc33 .
Из (7.6) c22 = (ω y − ωξ c32 ) / ωη = F3 (ω y ,ϕ , c32 ) .
δF3
δF
δF
Δω y + 3 Δϕ + 3 Δc32 =
δω y
δϕ
δa32
ω −ω c
1
1
1
=
Δω y −
c32 Δωξ −
ωξ Δc32 − y 2ξ 32 Δωη .
ωη
ωη
ωη
ωη
Δc22 =
Опуская далее промежуточные выражения ,запишем
Δωη = u cosϕΔϕ ,
Δωη = −u sin ϕΔϕ ;
1
1
1
Δg y ,
Δc33 = − Δg z ,
Δc31 = − Δg x ;
g
g
g
ωξ
1
1
1
Δc23 =
Δω z −
c33Δωξ −
Δc33 − 2 (ω z − ωξ c33 ) Δωη ;
Δc32 = −
ωη
Δc21 =
1
ωη
ωη
Δω x −
1
ωη
ωη
c31Δωξ −
ωη
ωξ
1
Δc31 − 2 (ω x − ωξ c31 ) Δωη
ωη
ωη
Теперь, подставляя погрешности акселерометров Δg x , Δg y , Δgz , погрешности угловой скорости (ДУС) Δω x , Δω y , Δω z , погрешность ввода широты Δϕ , можно получить погрешность измерения начального курса Δψ 0 . .
Оценим в качестве примера погрешность Δψ 0 в частном случае .
Пусть начальное положение обьекта таково , что углы Δγ 0 и Δϑ 0 равны
нулю , имеется начальный курс ψ 0 ≠ 0 .
Тогда из таблицы 7.1 запишем
Начальная выставка БИНС
86
c11 = cosψ 0 ,
c21 = − sinψ 0 ,
c31 = 0,
c12 = sinψ 0 ,
c22 = cosψ 0 ,
c32 = 0,
c13 = 0;
c23 = 0;
c33 = 1
Рассмотрим влияние погрешностей ДУС , т.е положим , что
Δϕ = 0, Δω i ≠ 0, i = x , y , z.
Тогда Δc31 = 0; Δc32 = 0; Δc33 = 0; Δωζ = 0 , Δω η = 0 ;
Δg i = 0,
Δc21 = Δω x / ωη , Δc22 = Δω y / ωη ,
Δc23 = Δω z / ωη , Δc12 = − Δω x c33 / ωη .
Подставляя эти выражения в (7.8), получим
Δψ 0 = −( cosψ 0 Δω x + sinψ 0 Δω y ) / ω η .
(7.10)
Отсюда видно, что погрешность гирокомпасирования зависит от начального курса и от широты, т.к. ωη = u cosϕ .
Рассмотрим зависимость погрешности гирокомпасирования от погрешностей акселерометров, т.е. положим, что Δgi ≠ 0, Δϕ = 0, Δω i = 0, i = x , y , z .
Выражение (7.8) можно записать в виде
Δψ 0 = cosψ 0 Δc12 − sinψ 0 Δc22 .
Здесь Δc12 = − Δc21 + sinψ 0 Δc33 ;
ωζ
1
Δc31 = (tgϕ / g ) Δg x , Δc33 = − Δg z , ;
g
ωη
ωζ
Δc12 = -( tgϕ / g) Δgx − ( sinψ 0 / g) Δgz ,
Δc22 = −
Δc = (tgϕ / g ) Δg y .
ωη 32
Δc21 = −
Тогда получим
Δψ 0 = −[ tgϕ( cosψ 0 Δgx + sinψ 0 Δg y ) + cosψ 0 sinψ 0 Δgz .
(7.11)
Из (7.11) видно, что погрешность также зависит от широты, от начального курса. Элементарный анализ показывает, что на кардинальных курсах (ψ 0 = 0°,
90°, 180°, 270°) последнее слагаемое (с Δgz ) обращается в нуль, выражение в
скобках при tgϕ имеет минимальное значение.
Рассмотрим влияние погрешностей ввода широты Δϕ . В выражении
(7.8) в этом случае Δc21 = 0, Δc31 = 0, Δc33 = 0, Δc12 = 0 . Тогда
Начальная выставка БИНС
Δc22 = −(ω y / ωη2 ) Δωη
вок получим
87
. Но ω y = ω η cosψ 0 , Δω η = −ωζ Δϕ . После подстано-
Δc22 = −tgϕ cosψ 0 Δϕ и
Δψ 0 = sinψ 0 cosψ 0 tgϕΔϕ
(7.12)
На кардинальных курсах погрешность обращается в нуль.
Пример численной оценки погрешностей приведен на рис. 7.3.
Анализ выражений (7.10), (7.11), (7.12) показывает, что погрешности начальной выставки БИНС зависят от широты и имеют минимумы в зависимости
от положения объекта в азимуте. Эти выводы подтверждаются при моделировании погрешностей на ЭВМ. Из последнего графика видно, что при реальных
погрешностях чувствительных элементов желательно, чтобы начальное положение объекта в азимуте было близко к значениям ψ0 = - 45°, 135°. Это соответствует зависимости (7.10) для данного примера, где определяющую роль играет
погрешность датчиков угловой скорости.
1
100
2
-1
-2
-3
1
10
-4
10
-3
а) Погрешность акселерометров, м/с2
-100
-200
-400
-2
-1
0
10
10
10
б) Погрешность ДУС, град./час
5
6
2
0
4
3
-5
угл.мин
угл.мин
3
1
-300
-4
-5
-5
10
2
0
3
угл.мин
угл.мин
0
1
-10
2
3
-2
-15
-4
-20
-6
-200
0
20
в)
40
60
Широта, град.
80
2
0
1
г)
-100
0
100
200
Начальный курс, град.
Рис.7.3. Погрешности начальной выставки Δψ0 - 1, Δϑ0 - 2, Δγ0 - 3:
а) при ϕ =50°, Δωi =0.01°/ч, ψ0=0° ;
б) при ϕ =50°, Δgi =1.e-4g, ψ0=0°;
в) при Δgi =1.e-4g, Δωi =0.01°/ч, ψ0=0°; г) при Δgi =1.e-4g, Δωi =0.01°/ч, ϕ =50°.
88
Демпфирование БИНС
8. Демпфирование БИНС
Как показывает проведенный анализ, погрешности БИНС, в основном,
имеют характер незатухающих колебаний с периодом Шулера. Возникает естественный вопрос, нельзя ли демпфировать эти колебания. В известной литературе рассматривается такая возможность [5,7,15] применительно к платформенным системам. Рассмотрим возможности демпфирования в БИНС.
8.1. Демпфирование с помощью внутренних связей
Рассмотрим северный канал, в котором первые интеграторы охвачены
обратной связью (рис.8.1).
Рис.8.1. Северный канал БИНС с демпфированием
Кажущиеся ускорения, измеряемые акселерометрами, соответствуют
формулам
a xc = g sin ϑ − V&η cosϑ ,
(8.1)
a yc = g cosϑ + V&η sin ϑ .
Отметим, что в результате преобразования кажущихся ускорений в блоке направляющих косинусов и преобразования координат НК-ПК получим кажущееся ускорения в проекциях на оси сопровождающего базиса
aη = V&η .
(8.2)
Охват первых интеграторов отрицательной обратной связью
Запишем уравнения идеальной работы, которые будут отличаться от
Демпфирование БИНС
89
(1.18) - (1.22) тем, что в них учтен охват первых интеграторов обратной связью
с коэффициентом передачи k0 .
t
ϑ& = ω z − Vη / R, ϑ0 = 0; (ϑ = ∫ ϑ&dt +ϑ0 ; )
0
aη = − a xc cosϑ + a yc sin ϑ ,
t
V&η = aη − k0Vη , Vη 0 = 0; (Vη = ∫ (V&η − k0Vη )dt +Vη 0 ;
(8.3)
0
ϕ& =
Vη
R
t
, ϕ 0 = 0; (ϕ = ∫ ϕ&dt +ϕ 0 ; ) .
0
Как ранее было показано, ошибка определения угла поворота объекта тождественна ошибке вычисления положения географического базиса:
Δϑ ≡ β , Δϑ& ≡ β& .
Варьируя первое уравнение (8.1)
β& = Δω z −
ΔVη
R
.
(8.4)
где Δω z - ошибка измерителя угловой скорости.
Варьируя второе уравнение (8.3), с учетом уравнений (8.1),
Δaη = − Δa xc cosϑ + Δa yc sin ϑ + gβ .
(8.5)
где Δa xc , Δa yc - ошибки акселерометров.
Варьируя последующие уравнения системы (8.3), запишем
ΔV&η = Δaη − k0 ΔVη .
Δϕ& =
ΔVη
(8.6)
.
R
Рассмотрим дифференциальные уравнения (8.4), (8.6).
Продифференцировав (8.4), подставляя (8.5), приняв для простоты, что погрешности акселерометров в (8.5) отсутствуют, запишем
β&& = Δω& z −
ΔV&η
,
R
ΔV&η = gβ − k0 ΔVη .
Примем, что Δωz = Const и подставим второе уравнение в первое
Демпфирование БИНС
90
1
R
β&& = − ( gβ − k0 ΔVη ) .
Подставим вместо
ΔVη
R
соответствующее выражение из (8.4), получим в итоге
β&& +
k0 & g
k
β + β = 0 Δω z .
R
R
R
(8.7)
g
= ν 2 - квадрат частоты Шулера. Наличие обратной отрицаR
тельной связи обеспечивает демпфирование шулеровских колебаний ошибки
β . Очевидно, что следствием будет демпфирование всех погрешностей. Постоянный уход гироскопа Δω z приводит к установившейся ошибке, зависящей от
коэффициента передачи обратной связи.
k
β уст = 0 Δω z .
g
Как известно,
Охват первых интеграторов прямой связью
Фрагмент структурной схемы северного канала показан на рис.8.2, где kп
- коэффициент передачи прямой цепи.
Рис.8.2. Охват первого интегратора прямой связью
Уравнения (8.3) идеальной работы в этом случае приобретут вид
t
ϑ& = ω z − V 'η / R, ϑ0 = 0; (ϑ = ∫ ϑ&dt +ϑ0 ; )
0
aη = −a xc cosϑ + a yc sin ϑ ,
t
V& 'η = V&η + kпV&η , Vη 0 = 0; (V 'η = ∫ V&η dt + k пVη );
0
ϕ& =
V 'η
R
t
, ϕ 0 = 0; (ϕ = ∫ ϕ&dt +ϕ 0 ; )
0
Уравнение ошибки (8.4) преобразуется в
β& = Δω z −
ΔV 'η
R
.
(8.8)
Демпфирование БИНС
Вторая производная
ΔV& 'η
&
&
,
β = Δω& z −
R
91
(8.9)
где ΔV& 'η = gβ + kп gβ& . После подстановки в (8.9) ΔV& 'η и преобразований уравнение ошибки
β&& + kпν 2 β& + ν 2 β = Δω& z
(8.10)
Вид уравнения (8.10) показывает, что при охвате первого интегратора обратной
связью ошибка имеет затухающий характер. Установившаяся ошибка может
возникнуть лишь при угловом ускорении ухода, что редко имеет место. Следовательно, эта схема имеет некоторое преимущество перед схемой с отрицательной обратной связью.
8.2. Демпфирование по скорости от внешней системы
Используем для демпфирования данные о скорости внешней системы,
имеющей высокую точность измерения (спутниковая навигационная система,
доплеровские измерители скорости). Структура северного канала для этого ва~
рианта приведена на рис.8.3. Разность приборной скорости Vη и эталонной скорости Vη от внешней системы может быть подана на вход первого интегратора
линейного ускорения с коэффициентом k2 или на выход измерителя угловой
скорости с коэффициентом k1 или одновременно на оба эти корректирующие
входа одновременно.
Рис.8.3. Демпфирование по внешней скорости
Демпфирование БИНС
92
По аналогии с ранее приведенными системами уравнений (8.3), (8.8), запишем уравнения идеальной работы в виде
ϑ& = ω z − Vη / R + k1ΔVη , ϑ0 ≠ 0;
aη = − a xc cosϑ + a yc sin ϑ ,
aζ = a xc sin ϑ + a yc cosϑ ,
(8.11)
V&η = aη + k2 ΔVη , Vη 0 = 0;
ϕ& =
Vη
R
, ϕ 0 = 0;
Уравнения ошибок получим варьированием уравнений идеальной работы с учетом уравнения (8.1):
Δϑ ≡ β ,
β& = Δω z −
ΔVη
+ k1ΔVη ,
R
Δaη = − Δa xc cosϑ + Δa yc sin ϑ + gβ ,
Δaζ = Δa xc sin ϑ + Δa yc cosϑ − V&η β ,
(8.11)
ΔV&η = Δaη + k2 ΔVη ,
Δϕ& =
ΔVη
R
.
Упрощая рассмотрение, пусть Δωz = 0, Δa xc = 0, Δa yc = 0; β 0 ≠ 0 .
Дифференцируя при этих условиях второе уравнение (8.11) и подставляя пятое
уравнение (8.11)
1
1
(8.12)
β&& = ( k1 − ) ΔV&η = ( k1 − )(Δaη + k 2 ΔVη ).
R
R
Учтем, что при Δa xc = 0, Δa yc = 0 из (8.11) Δaη = gβ .
Рассмотрим случай, когда k2 =0. Тогда
g
β&& + ⎛⎜ − k1 ⎞⎟ β = 0.
⎝R
⎠
(8.13)
При этом частота собственных незатухающих колебаний отличается от
g
g
«шулеровской» ν 2 = , так как квадрат частоты - ω02 = − k1 . Таким образом,
R
R
коррекция на выходе измерителя угловой скорости позволяет увеличивать частоту колебаний ( k1 < 0 ) и в итоге ускорять демпфирование.
omz
aeta
V0
omdr
ax
ay
Veta
fi
моделирование
omz
a
V0
dax
1
s
day
tet0
teta
ax
teta
ay
axcos
ay sin
K1
Out3
3
beta
преобразование
Veta
From
[V0]
K2
fi
K4
K5
1
xo s
aetap
Рис.8.4. Схема моделирования северного канала
1/Rz
[V0]
Goto
1
s
2
1/Rz
put
Vetap
dVk
Out2
dVeta
1
fip
d_fi
Out1
93
Демпфирование БИНС
94
Если принять, что k1 = 0 , а k2 ≠ 0 . Тогда, глядя на рис.8.3, запишем следующие
выражения:
β& = −
ΔVη
R
,
t
ΔVη = ∫ (gβ + k2 ΔVη′ )dt ,
0
t
1
β& = − ∫ (gβ + k 2 ΔVη′ )dt ,
R0
1
1
β&& = − (gβ + k2 ΔVη ) = − (gβ − k2 β&R ), откуда получим
R
R
β&& + k2 β& + ν 2 β = 0 .
Таким образом, корректирующий сигнал на вход первого интегратора ускорения позволяет демпфировать колебания.
Поведение системы при различных сочетаниях корректирующих воздействий можно рассмотреть при моделировании, например, по схеме на рис. 8.4.
На рис. 8.5 приведен пример моделирования канала с демпфированием.
Существуют и другие схемы демпфирования [5].
Ошибки БИНС
dfi, угл.мин.
0.4
0.3
da= 0 (м/с2)
aeta = 0 (м/с2)
tet0 = 0.001 (рад.)
0.2
0.1
0
0
2000
4000
6000
8000
10000
dVeta, м/с
1.5
1
0.5
0
beta, угл.мин.
-0.5
0
2000
4000
6000
8000
10000
2000
4000
6000
8000
10000
0.4
0.2
0
0
Время , сек
Рис.8.5. Ошибки при демпфировании
V0= 0.1 (м/с)
dVk= 0.1 (м/с)
omdrn= 0 (град./час)
K1 = 20
K2 = 0.005
K4 = 0
K5 = 0
Коррекция БИНС
95
9. Коррекция БИНС
Как показывают исследования и опыт эксплуатации, основной недостаток
БИНС, как и других инерциальных навигационных систем – нарастание погрешностей со временем. Уменьшение погрешностей за счет совершенствования чувствительных элементов сопряжено с большими трудностями и затратами. Значительный эффект повышения точности достигается при коррекции
БИНС от других навигационных систем, обладающих высокими точностными
характеристиками. Это могут быть доплеровские измерители скорости или лаги, спутниковые навигационные системы, астронавигационные системы, различные обзорно-сравнительные системы. Комплексные системы, включающие
несколько различных систем, называют иногда интегрированными.
Наиболее привлекательным и массовым в настоящее время является интегрирование инерциальной навигационной системы (ИНС) и спутниковой навигационной системы (СНС). Основные особенности ИНС и СНС приведены в
таблице 9.1.
Таблица 9.1. Основные свойства и недостатки СНС и ИНС
Тип
Основные свойства
системы
СНС Высокая точность (порядок 1 м в определении координат и 0,1 м/с в определении скорости ).
Ошибки не имеют тенденции к росту.
Высокочастотный характер ошибок.
ИНС Высокая скорость выдачи информации (до 100 Гц).
Полный набор необходимой информации для управления, включая ориентацию.
Полная автономность.
Неподверженность внешним помехам.
Низкочастотный характер ошибок
(шулеровские колебания, суточные
колебания, нарастание во времени).
Недостатки
Низкая скорость обновления
информации (1-10 Гц).
Отсутствие информации об
ориентации (как правило).
Подверженность помехам.
Неограниченный рост ошибок
во времени.
Необходимость знания модели
гравитационного поля.
Необходимость начальной выставки при запуске и любом перерыве в работе.
При интегрировании ИНС и СНС могут использоваться различные схемы. Простейшей является разомкнутая схема, базирующаяся на известной в
комплексных системах [10] схеме компенсации. Обязательным условием при
использовании такой схемы является разный частотный состав помех систем,
Коррекция БИНС
96
используемых в комплексной системе. Этому условию, как видно из табл.9.1,
полностью отвечают ИНС и СНС.
9.1. Разомкнутая схема комплексирования со спутниковой
навигационной системой
В такой схеме (рис.9.1) чаще всего применяют фильтр Калмана для получения оценок ошибок ИНС. Входным сигналом для фильтра является разность
координат и составляющих скорости, вырабатываемых ИНС и СНС. Эта раз-
Рис.9.1. Разомкнутая схема комплексирования ИНС и СНС
ность соответствует разности ошибок ИНС Δv N , Δv E , Δϕ , Δλ
и СНС
Δv Nc , Δv Ec , Δϕ c , Δλc :
Δv N − Δv Nc , Δv E − Δv Ec , Δϕ − Δϕ c , Δλ − Δλc .
Разность ошибок, проходя через фильтр Калмана, практически очищается от
ошибок СНС. Благодаря возможностям фильтра, на выходе получаем не только
оценки ошибок координат
и скорости, но также оценки ошибок углов ориента) )
ции объекта α , β . Ошибка азимутального канала δ не оценивается. Затем из
реальных выходных сигналов ИНС вычитаем оценки их ошибок и получаем
практически точные значения навигационных параметров, вырабатываемых
ИНС. Для улучшения работы азимутального канала необходимо интегрирование с курсовой системой. Зачастую в качестве такой используют магнитную
систему.
Недостаток разомкнутой схемы в том, что при пропадании сигнала СНС
из-за действия помех, в тоннелях, в тени зданий или деревьев корректирующий
сигнал, а с ним и положительный эффект комплексирования, тотчас же исчезает.
Рассмотрим пример моделирования оценки ошибок в разомкнутой схеме
интегрирования.
При использовании фильтра Калмана пользуются следующей матричной
формой записи линейной модели исследуемой системы
Коррекция БИНС
x& = Ax + Bu + Gw ,
97
(9.1)
где ⎯x - n-мерный вектор состояния системы; ⎯u - r-мерный вектор управления;
⎯w - k-мерный вектор случайных воздействий; A- матрица состояния размерности n×n; B - матрица управления размерности n×r; G - матрица передачи случайных воздействий размерности n×k.
Как правило, наблюдатель не имеет возможности измерять полный вектор состояния системы ⎯x . Измеряемая информация (доступная либо необходимая для решения поставленных задач), определяется следующим уравнением:
y = Hx + v ,
(9.2)
где ⎯y - m -мерный вектор измерения; ⎯v - m-мерный вектор помех измерения;
H - матрица измерения размерности m×n.
Воздействия w и помехи v будем считать гауссовскими случайными процессами типа белого шума с нулевыми математическими ожиданиями
M[w(t)]=0, M[v(t)]=0
и корреляционными матрицами
M[w(t)wТ(τ)]=Q(t)δ(t-τ);
M[v(t)vТ(τ)]=R(t)δ(t-τ),
где Q(t) - симметрическая неотрицательно-определенная (k×k) матрица интенсивности белого шума w(t); R(t) - симметрическая положительно определенная
(m×m) - матрица интенсивности белого шума v(t); δ(t) - дельта - функция Дирака.
Начальное состояние системы x(t0) характеризуется известным математическим
ожиданием
и
корреляционной
матрицей
M x (t 0 ) = x 0
{
[
M [ x (t0 ) − x 0 ] x (t0 ) − x 0T
[
]} = P(t
]
0 , t0
)
. Также предполагается, что начальное
состояние системы, случайные воздействия и помехи измерений взаимно некоррелированы при всех t≥t0 .
Построение фильтра Калмана сводится к определению матрицы коэффициентов усиления K, которая бы обеспечила оптимальную оценку (в смысле
минимума дисперсии ошибки оценивания) вектора состояния.
Дискретный фильтр Калмана
В дискретном фильтре Калмана непрерывной динамической системе (9.1)
и измерениям (9.2) соответствуют записанная в разностной форме дискретная
система и дискретные измерения
98
Коррекция БИНС
xk + 1 = Fk xk + Ψk uk + Γk wk ;
y k = H k xk + vk ,
(9.3)
(9.4)
где подстрочные индексы «k» и «k+1» указывают номер дискретного момента
времени tk и tk+1; шаг дискретизации Δt = tk +1 − tk ;
Fk = exp( A ⋅ Δt ) - переходная матрица системы в момент времени tk, которая
может быть представлена приближенно в виде разложения в ряд
Fk = I + Fk Δt +
1
(Fk Δt )2 + ...
2
;
Γ k ≅ Fk Gk Δt – матрица, определяющая влияние вектора входных шумов wk–1 в
момент времени tk , в первом приближении Γ k ≈ Gk Δt ;
Ψ k ≅ Fk B k Δt – матрица, определяющая влияние вектора управления uk–1 в момент времени tk, в первом приближении Ψ k ≈ Bk Δt ;;
I - единичная матрица;
Матрицы интенсивностей векторных дискретных гауссовских шумов wk системы и vk измерений связаны с соответствующими матрицами непрерывной динамической системы соотношениями:
Qk =
Q ( tk )
,
Δt
Rk =
R( tk )
.
Δt
Для оценки вектора состояния можно использовать алгоритм линейного
дискретного фильтра Калмана :
)
- зададим начальное значение xk 0 оценки вектора переменной состояния, начальное значение предсказанной (априорной) корреляционной матрицы ошибок
(
Pk 0 , матрицы интенсивностей шумов Qk и Rk ;
- получим предсказанное (априорное) значение вектора переменных состояния
(
)
xk + 1 = Fk xk + Buk ,
- рассчитаем значения коэффициентов фильтра
[
(
(
K k = Pk H kT H k Pk H kT + Rk
]
−1
,
- вычислим скорректированные (апостериорное) значение оценки вектора состояния
)
(
(
x k = x k + K k yk − H k x k ,
[
]
- вычислим скорректированную (апостериорную) корреляционную матрицу
Коррекция БИНС
ошибок фильтра
99
)
(
Pk = [I − K k H k ]Pk ,
- рассчитаем значение предсказанной (априорной) корреляционной матрицы
ошибок для нового шага вычислений
(
)
Pk + 1 = Fk Pk FkT + Qk ,
- переходим на новый цикл вычислений.
Для облегчения вычислений можно взять приближенное значение пере)
ходной матрицы F = I + A Δt ; x k - скорректированное (апостериорное) значение
(
оценки вектора состояния, xk - предсказанное (априорное) значение оценки
)
вектора состояния, K – матрица коэффициентов усиления Калмана, Pk - скор(
ректированная (апостериорная) корреляционная матрица ошибок фильтра, Pk предсказанная (априорная) корреляционная матрица ошибок фильтра, R- матрица интенсивностей шумов наблюдения (измерения) v, Qk – матрица интенсивностей входных возмущений w . Начальная матрица Pk обычно содержит
диагональные элементы, соответствующие начальным дисперсиям ошибок соответствующих переменных состояния.
В качестве модели БИНС возьмем ее уравнения ошибок (6.8), (6.10),
(6.11) в виде
β& = −ωηδ + ωζ α − ΔvN / RN − c11Δωxc − c12Δωyc − c13Δωzc ,
α& = −ωζ β + ωξ δ + ΔvE / RE − u sinϕ ⋅ Δϕ − c21Δωxc − c22Δωyc − c23Δωzc ,
(
)
δ& = −ωξα + ωη β + ΔvEtgϕ / RE + vE /(RE cos2 ϕ ) + u cosϕ Δϕ − c31Δωzc − c32Δωxc − c33Δωyc.
Δv&E = δ ⋅ a N − α ⋅ a H + c11Δa xc + c12 Δa yc + c13Δa zc ,
Δv& N = −δ ⋅ a E + β ⋅ a H + c21Δa xc + c22 Δa yc + c23Δa zc ,
(9.5)
Δv&H = α ⋅ a E − β ⋅ a N + c31Δa xc + c32 Δa yc + c33Δa zc .
Δϕ& = Δv N / RN ,
Δλ& ≈
1
( Δv E + v E tgϕΔϕ ).
RE cos ϕ
Эти уравнения необходимо представить в векторно-матричной форме (9.1).
Вектор переменных состояния будет
r
x = [ β , α , δ , Δv E , Δv N , Δv H , Δϕ , Δλ ]T .
Матрица состояния
Коррекция БИНС
100
⎡
⎢ 0
⎢
⎢ − ωζ
⎢
⎢
⎢ ωη
⎢
A= ⎢ 0
⎢a
⎢ H
⎢− a N
⎢
⎢ 0
⎢
⎢ 0
⎢⎣
1
RN
0
0
0
0
− u sin ϕ
0
0
0
0
vE
+ u cos ϕ
RE cos2 ϕ
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
1
RE cos ϕ
0
1
RN
0
0
v E tgϕ
RE cos ϕ
ωζ
− ωη
0
0
ωξ
− ωξ
0
− aH
0
aN
− aE
1
RE
tgϕ
RE
0
aE
−
⎤
0⎥
⎥
0⎥
⎥
⎥
0⎥
⎥
0⎥ .
0⎥
⎥
0⎥
⎥
0⎥
⎥
0⎥
⎥⎦
Если ошибки сигналов измерителей имеют постоянную и случайную составляющие
Δωic = ωдрi + ε ωi ; Δaic = Δai + ε ai , i = x, y , z ,
r
запишем вектор управления u = [ωдрx , ωдрy , ωдрz , Δa x , Δa y , Δa z ]T .
Матрица управления В имеет вид
⎡ − c11
⎢− c
⎢ 21
⎢ − c31
⎢ 0
B=⎢
⎢ 0
⎢
⎢ 0
⎢ 0
⎢
⎣ 0
− c12
− c13
0
0
− c22
− c23
0
0
− c32
− c33
0
0
0
0
0
0
c11 c12
c21 c22
0
0
c31 c32
0
0
0
0
0
0
0
0
0⎤
0⎥
⎥
0⎥
c13 ⎥⎥
.
c23 ⎥
⎥
c33 ⎥
0⎥
⎥
0⎦
Примем, что случайные составляющие являются «белошумными» и могут быть
представлены в виде ε ωi ≈ σ ωi ⋅ w; ε ai ≈ σ ai ⋅ w; i = x, y , z , где w – «белый шум»
единичной интенсивности, σ ωi и σ ai - среднеквадратические ошибки сигналов
ДУСов и акселерометров соответственно.
Матрица возмущения G имеет 8 строк, из которых 2 нижних нулевые, ориентируясь на моделирование в пакете MatLab:
Коррекция БИНС
⎡ − c11σ ωx
⎢− c σ
⎢ 21 ωx
⎢ − c31σ ωx
⎢
0
G=⎢
⎢
0
⎢
0
⎢
⎢
0
⎢
0
⎣
101
− c12σ ωy
− c22σ ωy
− c32σ ωy
− c13σ ωz
− c23σ ωz
− c33σ ωz
0
0
0
0
0
0
0
0
0
0
0
0
c11σ ax
c21σ ax
c31σ ax
c12σ ay
c22σ ay
c32σ ay
0
0
0
0
0
0
0
0
⎤
⎥
⎥
⎥
c13σ az ⎥⎥
.
c23σ az ⎥
⎥
c33σ az ⎥
0 ⎥
⎥
0 ⎦
0
0
0
Вектор измерения y = [Δv E , Δv N , Δϕ , Δλ ]T .
Матрица измерения Н имеет вид
H = [ H1
H 2 ] , где H 1 = 0 ( 4,3)
⎡1
⎢0
H2 = ⎢
⎢0
⎢0
⎣
0 0 0 0⎤
1 0 0 0⎥
⎥.
0 0 1 0⎥
0 0 0 1⎥⎦
Для оценки возможностей интегрированной системы целесообразно проверить
ее наблюдаемость.
Матрица наблюдаемости N имеет вид
⎡ H ⎤
⎢ HA ⎥
⎢
⎥
N = ⎢ HA2 ⎥ ,
⎢ M ⎥
⎢
⎥
n −1
⎢⎣ HA ⎥⎦
(9.6)
где n – порядок системы. Число наблюдаемых переменных равно рангу матрицы N. Ранг матрицы равен наибольшему числу линейно независимых строк
(или столбцов) матрицы. В примере rank(N) равен 7, т.е. одна переменная из 8
не наблюдается.
Матрица интенсивностей шумов возмущений принята в виде
(фильтр Калмана допускает эмпирические настройки матриц Q, R, P0)
2
2
2
σ ay
σ az
Q = 1e10 ⋅ diag[σ ω2x σ ω2y σ ω2z σ ax
1 1] .
Несмотря на то, что в матрице G нижние строки нулевые, интенсивности шума
Коррекция БИНС
102
(элементы 7 и 8) не могут быть нулевыми, иначе можно получить плохую обусловленность матриц фильтра.
Матрица интенсивностей шумов измерений
2
⎡σ ve
0
0
0⎤
⎢
⎥
2
0
σ
0
0
vn
⎥.
R=⎢
2
⎢ 0
0 σϕ 0 ⎥
⎢
⎥
0
0 σ λ2 ⎥⎦
⎢⎣ 0
Начальное значение корреляционной матрицы ошибок P соответствует дисперсиям переменных по диагонали или нулям при неопределенности.
Ниже (рис.9.2, 9.3, 9.4) приведены результаты моделирования данной
схемы при среднеквадратических ошибках ( σ ) спутниковой системы в определении координат – 1 м, скорости -0,05 м/с. Возмущающие воздействия – ошибки начального горизонтирования 0,01 град., ошибка гирокомпасирования – 0,1
град., уход гироскопа – 0,1 град./час, ошибка акселерометра 10-4 м/с2. Из рис.
9.4 видно, что ошибка азимутального канала δ не оценивается. Шум оценивания практически незаметен при оценке ошибок координат, скорости и существенный при оценке ошибок ориентации. При моделировании предполагается,
что систематические составляющие помех (вектор u ) предварительно опреде
лены.
Ошибки координат и их оценки
5
3
dfi, угл.мин
dlam, угл.мин
-3
Ошибки оценивания
2
0
1
-5
0
-1
-10
-15
x 10
оценка
реальные
0
0.5
1
1.5
-2
2
-3
0
10
3
8
2
6
1
4
0
2
-1
0
-2
-2
0
0.5
1
1.5
Время, час
2
x 10
-3
0
0.5
1
1.5
2
1
1.5
Время, час
2
-3
0.5
Рис.9.2. Оценивание ошибок координат
Коррекция БИНС
8
Ошибки скоростей и их оценки
оценка
реальные
6
δ ve, м/с
0.3
0.1
2
0
0
-0.1
0.5
1
1.5
2
2
-0.2
0
0.5
1
1.5
2
1
1.5
Время, час
2
0.2
0
δ vn, м/с
Ошибки оценивания
0.2
4
-2
0
103
0.1
-2
0
-4
-0.1
-6
-8
0
0.5
1
1.5
Время, час
2
-0.2
0
0.5
Рис.9.3. Оценивание ошибок скорости
Ошибки и их оценки
β, угл.мин
4
оценка оср.
реальные
2
0.5
0
0
-2
-0.5
-4
0
0.5
1
1.5
2
α, угл.мин
5
-1
0
0.5
1
1.5
2
0.5
1
1.5
2
1
1.5
Время, час
2
4
2
0
0
-2
-5
0
0.5
1
1.5
2
10
δ, угл.мин
Ошибки оценивания
1
-4
0
5
0
0
-5
-10
0
0.5
1
1.5
Время,час
2
-10
0
0.5
Рис.9.4. Оценивание ошибок углов
Коррекция БИНС
104
Для уменьшения шума измерения можно использовать сглаживание шума
путем осреднения на каком-то интервале. На рис. 9.4 показано, что при сглаживании оценки β в окне из 21 точек (скользящее среднее) величина шума ошибки β уменьшается примерно на порядок.
Для сглаживания оценок могут применяться разные алгоритмы [13].
Обычно, при осреднении полагается, что частота поступления первичной информации должна быть на порядок выше, чем частота обновления информации
на выходе системы.
При осреднении аналоговых сигналов на некотором интервале Т можно воспользоваться теоремой о среднем значении функции f(t):
1 t +T
(9.7)
f (t ) T =
∫ f (t )dt .
T t
Таким образом, сглаживание или осреднение может проводиться простым интегрированием показаний измерителей. Для численного интегрирования могут
применяться все известные методы.
Так, дискретное выражение для формулы (9.7) в самом простом случае
использования метода прямоугольников примет вид суммы:
f ( k ⋅ Δt )
n
1 n
= Σ f ( k ⋅ Δt ), n ⋅ Δt = T
n k =1
где Δt - шаг дискретизации, п — число шагов на интервале осреднения Т.
Часто на практике применяются усредняющие фильтры [12]:
1
фильтр 1-го порядка
,
T0 p + 1
фильтры 2-го порядка:
1
,
с сильным затуханием
(T1 p + 1)(T2 p + 1)
1
с критическим затуханием
,
(T0 p + 1)2
1
.
со слабым затуханием
- 2 2
T0 p + 2T0ζp + 1
Этим динамическим звеньям соответствуют дифференциальные уравнения
формирующих фильтров:
для фильтра 1-го порядка: T0 y& + y = x (t );
для фильтра 2-го порядка: T02 &y& + 2ζT0 p + y = x (t ) ,
где x(t) — измеряемая функция; y(t) — сглаженное значение функции x(t).
Если для осреднения воспользоваться не формулами интегрирования (9.7), а
усредняющими фильтрами, то задача может быть сведена к численному решению дифференциальных уравнений усредняющих фильтров.
Коррекция БИНС
105
Для наиболее простого метода интегрирования — метода Эйлера, общее выражение которого имеет вид
yk +1 = yk + y& k Δt ,
соотношения фильтра 1-го порядка примут следующий вид:
Δt
yk +1 = yk − ( yk − xk ) .
T0
Этот метод интегрирования дает хорошее приближение при малом шаге дискретизации Δt и малом времени осреднения Т.
Для фильтра 2-го порядка дифференциальное уравнение необходимо
привести к форме Коши путем замены переменных y1 = y , y2 = y&1 = y& :
y&1 = y2 ,
y& 2 =
1
( −2ζT0 y2 − y1 + x ),
T02
и к полученной системе уравнений применять известные численные методы
интегрирования [8].
Полученные ошибки вычитают из сигналов БИНС, как показано на
рис.9.1.
9.2. Замкнутая схема комплексирования cо спутниковой
навигационной системой
В такой схеме (рис.9.5), в отличие от предыдущей, оценки ошибок используются для формирования корректирующих воздействий в БИНС.
Рис.9.5. Замкнутая схема комплексирования ИНС и СНС
Корректирующие воздействия могут формироваться различным образом. Можно использовать весь комплекс оценок ошибок, можно использовать часть оценок. Чаще всего используют оценки ошибок скорости. Варианты использования
106
Коррекция БИНС
могут быть разные. На рис. 9.6 показано, что, аналогично схеме на рис.8.3,
можно ввести корректирующий сигнал с коэффициентом k2 на вход интеграторов ускорений, можно ввести корректирующий сигнал с коэффициентом k1 в
r
сигналы датчиков угловой скорости ωb . Можно корректировать сигналы на
выходе соответствующих блоков как по всему комплексу выходных параметров (показано
на рис. 9.6 как вычитание оценок ошибок), так и по его части.
r)
)r
Векторы b и c содержат оценки ошибок гироскопов и акселерометров, определенные при калибровке (см. гл. 10).
Рис.9.6. Алгоритмы коррекции БИНС
В заключение приведем пример моделирования одной схемы корректируемой
системы. На рис. 9.7 показаны ошибки корректируемой БИНС, в которой с помощью фильтра Калмана, используя показания спутниковой системы, используются ранее упомянутые (рис.8.3) корректирующие сигналы с коэффициентами k1 , k2 и с корректирующим сигналом на входе интеграторов широты и долготы.
Δφ ,Δλ , угл. мин.
Коррекция БИНС
107
Ошибки
ИНСКалмана
Похибки
ІНС, фільтр
0.04
Заммкнутасхема
схема
Замкнутая
використанням
ФК:
сз фильтром
Калмана
Δφ
Δλ
0.02
0
-0.02
0
1
2
3
4
5
6
Δ Vξ,Δ Vη, м/c
0.4
7
ΔV
ξ
ΔV
0.2
χ0= 45 (град.)
α X0= 0.01 (град.)
α Y0= 0.01 (град.)
α Z0= 0.1 (град.)
8 Δ V0= 0.1 (м/с)
Δφ 0= 1e-005 (град.)
ωdr= 0.1 (град./час)
2
Δ a= 0.0001 (м/с )
η
0
h= 10 (с.)
Коэффициенты
коррекции
Коефіцієнти корекції:
-0.2
-0.4
0
1
2
3
4
5
6
7
8
2
3
4
5
6
7
8
α X,α Y, угл. мин.
1
0.5
0
αX
αY
-0.5
-1
0
1
Время,час
k1 1= -0.01 - по Δ Vξ
k1 2= -0.01 - по Δ Vη
k2 1= -0.008 - по Δφ
k2 2= -0.01 - по Δλ
k3 1= -1e-005 - по α X
k3 2= 4e-006 - по α Y
---------------------------------------------------------
Δφ,Δλ , угл. мин.
Рис.9.7. Ошибки замкнутой схемы при коррекции по всем оценкам ошибок
Ошибки ИНС
0.04
Δφ
Δλ
0.02
0
-0.02
0
500
1000
1500
Δ V ,Δ V , м/c
ξ
η
0.1
0
α ,α , угл. мин.
X Y
500
1000
k1 = -0.006
2000 1
1500
1
αX
αY
0.5
0
0
h= 1 (с.)
Коэффициенты
Коефіцієнти коррекции
корекції:
-0.2
-0.3
0
χ0= 45 (град.)
α X0= 0.01 (град.)
α Y0= 0.01 (град.)
α Z0= 0.1 (град.)
2000Δ vN= 0.05 (м/с)
Δφ 0= 1e-005 (град.)
ω dr= 0.1 (град./час)
Δ Vξ
2
ΔV
Δ a= 0.0001 (м/с )
η
-0.1
Замкнутая схема c
фильтром I-го порядка:
500
1000
Время,сек.
1500
- по Δ Vξ
- по Δ Vη
k12= -0.02
k21= -0.01 - по
k22= -0.01 - по
k31= -8e-006- по
k32= 4e-006 - по
Δφ
Δλ
αX
αY
2000
---------------------------------------------
Рис.9.8. Ошибки замкнутой схемы с апериодическим фильтром
Коррекция БИНС
108
Использование замкнутой схемы дает возможность использовать для
оценки погрешностей не фильтр Калмана, а более простые фильтры первого и
второго порядка, аналогичные тем, которые используют для сглаживания случайных помех. Такие фильтры позволяют получить оценки ошибок скорости и
координат, но не позволяют получить оценки ошибок угловой информации. Но
оценки ошибок скорости можно использовать и для коррекции угловой информации. На рис.9.8 приведены ошибки корректируемой БИНС, в которой использован апериодический фильтр с постоянной времени Т=10 с. Коррекция
угловых параметров при этом проводится по оценкам ошибок скорости.
Как показывают результаты моделирования, использование как фильтра
Калмана, так и простых фильтров позволяет получить близкие результаты.
При выборе коэффициентов коррекции желательно предварительно оценить пределы выбора исходя из условий устойчивости системы. Это можно
сделать, условно разделив восточный, северный и азимутальный каналы.
9.3. Астрокоррекция БИНС в географической
системе координат
Пусть на объекте установлены 2 астровизира. Рассмотрим, как можно
корректировать информацию БИНС, фиксируя с их помощью положение известных светил.
В режиме начальной выставки необходимо навести астровизиры на выбранные заранее светила. Для этого по данным об экваториальных координатах:
прямом восхождении светила α э и склонении δ э (из астрономических ежегодников) находят горизонтальные координаты светила: высоту h и азимут А. Горизонтальные координаты вычисляют по известным соотношениям [11]
sin h = sin ϕ sin δ э + cosϕ cos δ э cos t , t = S гр − α э + λ ;
tgA =
− cos δ э sin t
.
cosϕ sin δ э − sin ϕ cos δ э cos t
Здесь ϕ , λ - широта и долгота, S гр - гринвичское звездное время (определяют с помощью астрономических ежегодников).
Для выставки астровизиров в соответствии с рассчитанными углами
высоты h и азимутами А в условиях, когда объект может иметь произвольное
положение с углами курса ψ , тангажа ϑ , крена γ необходимо использовать известные зависимости [11].
sin q cos γ + sin γ (cos q sin ϑ − tgh cosϑ )
q~ = arctg
;
cosϑ cos q + tgh sin ϑ
~
h = arcsin(cos γ (sinh cos γ − cosh cos q sin ϑ ) + cosh sin q sin γ .
(9.9)
Коррекция БИНС
109
Эти зависимости следуют из рис.9.9. На нем показано положение объекта
(связанных осей xc yc zc ), заданное углами курса ψ , тангажа ϑ , крена γ относительно сопровождающего трехгранника ξηζ , h – угол истинной высоты свети~
ла, q – курсовой угол светила, q + ψ = A - азимут светила, h и q~ - приборные
значения углов.
В рабочем режиме астровизиры удерживают направление на светило, соответствующее истинным углам h и q. Управление визирами производится по
зависимостям (9.10).
~
sin ϑ cos γ + tgh sin γ
q = arctg
;
~
cos q~ cosϑ + sin ϑ (sin q~ sin γ − tgh cos γ
(9.10)
~
~
~
h = arcsin(cosϑ (sinh cos γ − cosh sin q~ sin γ ) + cosh cos q~ sin ϑ .
Рис.9.9. Углы положения объекта и светила
БИНС, моделирующая географический сопровождающий трехгранник
ξηζ , выдает информацию об углах ψ ,ϑ , γ , как показано ранее, с ошибками
δ , β , α (см. рис. 9.10).
Используя (6.8),(6.10),(6.11), уравнения ошибок можно представить в виде
β& = −ωη δ + ωζ α + Δωξ − ωдξ ,
α& = −ωζ β + ωξ δ + Δωη − ωдη ,
δ& = −ωξ α + ωη β + Δωζ − ωдζ ,
(9.11)
Коррекция БИНС
110
Δωξ = − Δv N / R,
Δωη = Δv E / R − u sin ϕ ⋅ Δϕ ,
vE
1
)
Δ
ϕ
+
tgϕ ⋅ Δv E ;
R
R cos2 ϕ
Δv&E = δ ⋅ a N − α ⋅ a H + Δa E ,
Δv& N = −δ ⋅ a E + β ⋅ a H + Δa N , ,
Δωζ = (u cos ϕ +
Δv&H = α ⋅ a E − β ⋅ a N + Δa H ,
Δϕ& = − Δv N / R,
Δλ& =
1
(Δv E cosϕ + ΔϕvE sin ϕ ).
R cos2 ϕ
Рис.9.10. Ошибки БИНС
В уравнениях ошибок (9.11) эквивалентные скорости ухода гироскопов, а также эквивалентные ошибки акселерометров в проекциях на оси сопровождающего трехгранника ξηζ имеют вид
ωдξ = c11Δω xc + c12 Δω yc + c13Δωzc ,
ωдη = c21Δω yc + c22 Δω xc + c23Δωzc ,
(9.12)
ωдη = c31Δω zc + c32 Δω xc + c33Δω yc ;
Δa E = c11Δa xc + c12 Δa yc + c13Δa zc ,
Δa N = c21Δa yc + c22 Δa xc + c23Δa zc ,
(9.13)
Δa H = c31Δa zc + c32 Δa xc + c33Δa yc ;
При известных эквивалентных ошибках и направляющих косинусах cij
можно, решая системы уравнений (9.12) и (9.13), найти ошибки каждого измерителя БИНС.
Ошибки моделирования базиса ξηζ с помощью БИНС приводят, как показано на рис.9.11, к ошибкам измерения высоты и азимута светила.
По аналогии с (9.9) запишем
sin( A + δ ) cos α + sin α (cos( A + δ )sin β − tgh cos β )
~
;
A = arctg
cos β cos( A + δ ) + tgh sin β
(9.14)
~
h = arcsin(cos α (sinh cos α − cosh cos( A + δ )sin β ) + cosh sin( A + δ )sin α .
~
~
Погрешности измерения Δh = h − h, ΔA = A − A .
Упростим выражения (9.14), разложив их в ряд Тейлора. С учетом малости ошибок БИНС (не более долей градуса), ограничимся только первыми членами ряда
Коррекция БИНС
111
Рис.9.11. Углы направления светила с учетом ошибок БИНС
~
A = A + δ − tgh[ β sin( A + δ ) + α cos( A + δ )],
~
h = h − β cos( A + δ ) + α sin( A + δ ).
(9.15)
Ошибки измерения азимута и высоты из-за ошибок БИНС будут
ΔA = δ − tgh[ β sin( A + δ ) + α cos( A + δ )],
Δh = − β cos( A + δ ) + α sin( A + δ ).
Отметим, что
sin( A + δ ) = sin A cos δ + cos A sin δ ≈ sin A + cos A ⋅ δ ,
cos( A + δ ) = cos A cos δ − sin A sin δ ≈ cos A − sin A ⋅ δ .
Если используем для коррекции 2 светила, получим уравнения измерений
ΔA1 = δ − tgh1[β (sin A1 + δ cos A1 ) + α (cos A1 − δ sin A1 )] ≈ δ − tgh1[β sin A1 + α cos A1 ],
Δh1 = −β (cos A1 − δ sin A1 ) + α (sin A1 + δ cos A1 ) ≈ −β cos A1 + α sin A1.
ΔA2 ≈ δ − tgh1 ( β sin A2 + α cos A2 ),
Δh2 ≈ − β cos A2 + α sin A2 .
(9.16)
Необходимо решить задачу корректирования БИНС по измерениям положения
светил. Для этого оценим погрешности БИНС по проведенным измерениям.
Используем для этого фильтр Калмана.
Подготовим уравнения ошибок и измерений в известной форме
Коррекция БИНС
112
x& = Ax + Bu + Gw ,
y = Hx + v .
Из уравнений ошибок (9.11) запишем матрицу состояния А.
Положим при этом, что погрешности приборов имеют систематическую и
случайную составляющие
Δai = μi + Qi1wi1 , i=E,N,H ;
ωдi = ε i + Qi 2 wi 2 ,
i= ξ ,η , ζ .
(9.17)
К уравнениям ошибок добавим дифференциальные уравнения изменения
постоянных составляющих ошибок акселерометров и датчиков угловой скорости.
μ&i = 0;
ε&i = 0;
Вектор переменных состояния запишем в виде
r
x = ( β , α , δ , Δv E , Δv N , Δv H , Δϕ , Δλ , ε ξ , εη , ε ζ , μ E , μ N , μ H )T .
Вектор управления u и матрица управления В в данном случае нулевые.
Матрица состояния А будет
A12 ⎤
⎡A
A = ⎢ 11
⎥,
⎣ A21 A22 ⎦
где
1
⎡
⎤
ω
ω
−
−
0
0
0
0
0
ζ
η
⎢
⎥
R
⎢
⎥
1
ωξ
− u sin ϕ
0
0 0
0⎥
⎢ − ωζ
R
⎢
⎥
1
vE
⎢ω
− ωξ
0
0 0 u cos ϕ +
0⎥
tgϕ
η
2
⎢
⎥
R
R cos ϕ
⎢ 0
− aH aN
0
0 0
0
0⎥ ,
A11 = ⎢
⎥
−
a
a
0
0
0
0
0
0
⎢ H
⎥
E
⎢− a
aE
0
0
0 0
0
0⎥
N
⎢
⎥
1
⎢ 0
−
0
0
0
0
0
0⎥
⎢
⎥
R
v E tgϕ
1
⎢
⎥
0
0
0 0
0⎥
⎢ 0
R cos ϕ
R cos ϕ
⎦
⎣
A12 = 0 (8,6) ,
A21 = 0 ( 6,8) ,
A22 = 0 ( 6,6).
Коррекция БИНС
113
r
Вектор возмущений - w = w( 6,1) с единичной интенсивностью.
⎡g ⎤
Матрица возмущений G = ⎢ 11 ⎥ ,
⎣ g 21 ⎦
g11 = diag (σ дx , σ дy , σ дz , σ ax , σ ay , σ az ),
g 21 = 0(8,6) .
r
Вектор измерения y = ( ΔA1 , ΔA2 , Δh1 , Δh2 ) . Тогда матрицу измерений запишем в виде
⎡ − tgh1 sin A1
⎢ − tgh sin A
2
2
H =⎢
⎢ − cos A1
⎢ − cos A
⎣
2
− tgh1 cos A1
− tgh2 cos A2
sin A1
sin A2
1 0 0 0 0 0 0 0 0 0 0 0⎤
1 0 0 0 0 0 0 0 0 0 0 0⎥
⎥.
0 0 0 0 0 0 0 0 0 0 0 0⎥
0 0 0 0 0 0 0 0 0 0 0 0⎥⎦
r
r
Вектор шумов измерений v = Cw1 , где
0
0
0 ⎤
⎡σ A1
⎢ 0 σ
0
0 ⎥
2
A
⎥,
C=⎢
0 σ h1 0 ⎥
⎢ 0
⎢ 0
0
0 σ h 2 ⎥⎦
⎣
r
w1 = w( 4,1) с единичной интенсивностью.
Матрица интенсивности возмущений принята единичной
Q = I (14,14 ) .
Матрица интенсивностей шумов измерения принята R = C 2 .
Проверка наблюдаемости по известному критерию определения ранга
матрицы наблюдаемости N (9.6) показывает, что ранг матрицы равен 6, т.е. наблюдаемыми являются лишь 6 переменных из 14.
Как показывают численные эксперименты (вычеркивание строк и столбцов и проверка наблюдаемости после этого), наблюдаемыми являются переменные β , α , δ , Δv E , Δv N , Δϕ .
Добавление еще одного светила не увеличивает наблюдаемости системы.
Для улучшения наблюдаемости необходимо использовать дополнительные эталонные системы, например, высотомер или меры, описанные в п.10.4.
Рассмотрим пример коррекции БИНС с использованием 7 переменных:
β , α , δ , Δv E , Δv N , Δϕ , Δλ (критерий наблюдаемости показывает, что наблюдаемы первые 6 переменных).
Коррекция БИНС
114
Матрица А приобретает вид
⎡
⎢ 0
⎢
⎢ − ωζ
⎢
⎢ω
⎢ η
A= ⎢ 0
⎢
⎢ aH
⎢ 0
⎢
⎢
⎢ 0
⎣
ωζ
− ωη
0
0
ωξ
1
R
− ωξ
0
− aH
0
aN
− aE
1
tgϕ
R
0
0
0
0
0
0
0
1
R cosϕ
−
1
R
0
0
− u sin ϕ
0
A36
0
0
1
−
R
0
0
0
0
v E tgϕ
R cosϕ
⎤
0⎥
⎥
0⎥
⎥
0⎥
⎥
vE
cos
ϕ
,
=
+
.
A
u
36
0⎥
2
R
cos
ϕ
⎥
0⎥
0⎥
⎥
⎥
0⎥
⎦
Матрица H
⎡ − tgh1 sin A1
⎢ − tgh sin A
2
2
H =⎢
⎢ − cos A1
⎢ − cos A
⎣
2
− tgh1 cos A1
− tgh2 cos A2
sin A1
sin A2
1 0 0 0 0⎤
1 0 0 0 0⎥
⎥.
0 0 0 0 0⎥
0 0 0 0 0⎥⎦
Начальные координаты светил А1 = 10 град., h1 = 20 град., А2 = 80 град.,
h1 = 70 град. Учтены известные рекомендации [11] о том, что высота светила не
должна превышать 70 град., а разность азимутов должна стремиться к 90 град.
Погрешности измерения направления на светила (среднеквадратическая ошибка σ ) примем ΔAi = Δhi = 2e −5 рад ( ≈ 4" ) .
Ошибки начальной выставки БИНС относительно горизонта – 0,01 град.
(=0,6 угл. мин.), в азимуте 0,1 град. (=6 угл. мин.).
Эквивалентные погрешности акселерометров σ a = 1e −5 м/с2 , эквивалентные погрешности гироскопов σ ω = 1e −2 град./час.
Горизонтальная скорость движения 60 м/с, начальная широта - 30 град.
Шаг интегрирования 1 сек. При моделировании использован метод Рунге-Кутта
4-го порядка. Результаты моделирования в течение 4 часов приведены на рисунках 9.12.
Анализ графиков показывает, что хорошо оцениваются угловые ошибки и компоненты скорости и хуже – широта и долгота. Обратим внимание на то, ошибка
определения долготы имеет нарастающий характер (это соответствует
Коррекция БИНС
Ошибки углов ориентации
α x, угл.мин
1
α y, угл.мин
0.1
0
0
-0.5
-0.1
-1
0
1
2
3
Погрешность оценивания ФК
0.2
оценка
реальные
0.5
4
-0.2
0
1
2
3
4
0
1
2
3
4
-0.5
0
1
2
Время, час
3
4
1
0.4
0.5
0.2
0
0
-0.5
-0.2
-1
0
1
2
3
4
α z, угл.мин
8
0.5
0
6
0
2
1
2
Время, час
3
4
Ошибки скоростей
0.01
0.005
0
0
-0.005
-1
-2
0
Погрешность оценивания ФК
0.015
оценка
реальные
1
δ vx, м/с
-0.4
7
5
-0.01
1
2
3
4
1
-0.015
0
1
2
3
4
1
2
Время, час
3
4
0.01
0.005
0
δ vy, м/с
115
0
-1
-0.005
-2
-3
0
-0.01
1
2
Время, час
3
4
-0.015
0
Коррекция БИНС
116
8
Ошибки координат
оценка
реальные
φ,угл.мин
6
0.2
4
0
2
-0.2
0
-2
0
λ,угл.мин
Погрешность оценивания ФК
0.4
1
2
3
4
-0.4
0
0.5
0.03
0
0.025
1
2
3
4
1
2
Время, час
3
4
0.02
-0.5
0.015
-1
0.01
-1.5
-2
0
0.005
1
2
Время, час
3
4
0
0
Рис.9.12. Оценивание ошибок БИНС с использованием астровизиров
ненаблюдаемости переменной). Но погрешность оценивания долготы возрастает настолько медленно, что на интервале 4 часов может быть приемлемой.
Если использовать для коррекции еще 2 светила (всего будет 4, аналогично 4 спутникам СНС), то точности оценки возрастают.
Если сравнить оценки систем с астрокоррекцией и спутниковой коррекцией , то видно, что в системе с астрокоррекцией, где в качестве эталона используется «угловая» информация, лучше всего оцениваются углы ошибок, а
затем уже скорости и координаты. В системе со спутниковой коррекцией, где
используется эталонная информация по скорости и координатам, лучше оцениваются скорости и координаты и хуже – ошибки углов. Очевидно, что наилучшие результаты могут быть достигнуты в интегрированной системе, где одновременно используются в качестве эталонных и спутниковая и астро- системы.
Оцененные ошибки можно использовать для коррекции по разомкнутой
схеме (рис. 9.1) или замкнутой схеме (рис. 9.2, 9.3)
Калибровка БИНС
117
10. Калибровка БИНС
Блок чувствительных элементов (БЧЭ) БИНС, или, как его еще называют
– инерциально-измерительный модуль (ИИМ) состоит из акселерометров и гироскопов, каждый из которых имеет свои индивидуальные особенности в виде
коэффициента передачи (масштабного коэффициента), смещения нуля, перекрестных связей и других параметров. Кроме того, при установке чувствительных элементов в блоке в силу инструментальных погрешностей изготовления
возникают несовпадения осей чувствительности элементов с базовыми осями
блока.
Для обеспечения высокой точности БИНС необходимо иметь высокоточную информацию от измерителей, что может быть обеспечено только при учете
указанных особенностей каждого акселерометра и гироскопа.
Для начала составляется адекватная модель выходных сигналов чувствительных элементов, учитывающая их особенности и погрешности, затем производится определение параметров этой модели. В дальнейшем, при штатной работе БИНС, выходные сигналы акселерометров и гироскопов корректируются с
учетом принятой модели, чтобы как можно точнее определять проекции кажущегося ускорения и угловой скорости вдоль базовых осей БИНС.
Определение параметров выходных сигналов измерителей (параметров
их математических моделей) с целью последующего их учета называют калибровкой.
10.1. Модели выходных сигналов инерциальных измерителей
С одной стороны, чем полнее модель учитывает особенности измерительных устройств, тем точнее можно определить измеряемые величины. Однако, чрезмерное усложнение модели приводит к увеличению объема вычислений в вычислителе БИНС, и это неоправданно если модель дополняется малозначимыми составляющими, а тем более если они слабо повторяемы.
В общем случае, зависимость выходных сигналов чувствительных элементов от измеряемых величин является нелинейной, и допускает линеаризацию для ограниченного диапазона измеряемых величин. В ИНС используются
высокоточные измерители, одним из требований к которым является линейность их характеристик в требуемом диапазоне измерений. Требования к нелинейности здесь обычно на уровне сотых долей процента и меньше. Поэтому в
дальнейшем рассмотрим только линейные модели инерциальных измерителей.
10.1.1. Модель выходных сигналов акселерометров
Основываясь на приведенных выше рассуждениях, представим выходные
сигналы трех акселерометров БИНС в виде
Калибровка БИНС
118
U ax = K x a x1 + U ax 0 + nax ;
U ay = K y a y 2 + U ay 0 + nay ;
U az = K z a z 3 + U az 0 + naz ,
(10.1)
где Uax , Uay , Uaz – значения выходных сигналов акселерометров в размерности
напряжения, тока, кода АЦП, частоты или других величин; ax1 , ay2 , az3 –
значения проекций кажущегося ускорения на оси чувствительности акселерометров Ox1, Oy2, Oz3; Kx, Ky, Kz – масштабные коэффициенты (коэффициенты
передачи) каждого из акселерометров; Uax0 , Uay0 , Uaz0 – смещения нулей акселерометров; nax , nay , naz – шумы измерения.
Модель измерений (10.1) можно записать в матричном виде:
⎡U ax ⎤ ⎡ K x
⎢U ⎥ = ⎢ 0
⎢ ay ⎥ ⎢
⎢⎣U az ⎥⎦ ⎢⎣ 0
0
Ky
0
0 ⎤ ⎡ a x1 ⎤ ⎡U ax 0 ⎤ ⎡ nax ⎤
0 ⎥ ⋅ ⎢a y 2 ⎥ + ⎢U ay 0 ⎥ + ⎢nay ⎥
⎥ ⎢ ⎥ ⎢
⎥ ⎢ ⎥
K z ⎥⎦ ⎢⎣ a z 3 ⎥⎦ ⎢⎣U az 0 ⎥⎦ ⎢⎣ naz ⎥⎦
(10.2)
Важно отметить, что модель измерений (10.1) или (10.2) – это модель не
самих чувствительных элементов акселерометров, а модель всего измерительного тракта каждого из них, включающего сам чувствительный элемент акселерометра, а также измерительные, усилительные и преобразующие элементы.
На практике, из-за остаточных погрешностей установки акселерометров в
ИИМ (БЧЭ) БИНС, направления их осей чувствительности Ox1, Oy2, Oz3 не ортогональны, и не совпадают с базовыми осями ИИМ БИНС. Последние привязаны обычно к внешними базовым (или установочным) поверхностям ИИМ.
Обозначим через Oxyz базовую систему координат БИНС. Зададим парами углов αi , βi положение осей чувствительности Ox1, Oy2, Oz3 каждого из акселерометров относительно базовых осей БИНС, как показано на рис.10.1.
Рис.10.1. Углы перекоса осей чувствительности акселерометров
Более подробно задание углов перекоса иллюстрирует рис.10.2.
Калибровка БИНС
119
Рис.10.2. Углы поворота осей чувствительности акселерометров относительно базовой системы координат БИНС
Используя рис.10.2, можно определить проекции кажущегося ускорения
объекта на осей чувствительности Ox1, Oy2, Oz3 акселерометров:
⎡ax1 ⎤ ⎡cosα x cosβ x
− sinα x cosβ x ⎤⎡ax ⎤
sin β x
⎢ ⎥ ⎢
⎢ ⎥
cosα y cosβ y sinα y cosβ y ⎥⎢a y ⎥ .
⎢a y 2 ⎥ = ⎢ − sin β y
⎥
⎢a ⎥ ⎢⎣ sin β z
− sinα z cosβ z cosα z cosβ z ⎥⎦⎢⎣az ⎥⎦
⎣ z3 ⎦
(10.2)
Подставляя выражение (10.2) в (10.1), получим окончательную модель
сигналов измерений акселерометров:
sinβx
− sinα x cosβx ⎤ ⎡ax ⎤ ⎡Uax0 ⎤ ⎡nax ⎤
⎡Uax ⎤ ⎡Kx 0 0 ⎤ ⎡cosαx cosβx
⎢
⎢U ⎥ = ⎢ 0 K
⎥
0 ⋅ − sinβ y
cosα y cosβ y sinα y cosβ y ⎥ ⋅ ⎢ay ⎥ + ⎢Uay0 ⎥ + ⎢nay ⎥
ay
y
⎥ ⎢ ⎥ ⎢
⎢ ⎥ ⎢
⎥ ⎢ ⎥
⎥ ⎢
⎢⎣Uaz ⎥⎦ ⎢⎣ 0 0 Kz ⎥⎦ ⎢⎣ sinβz
− sinα z cosβz cosαz cosβz ⎥⎦ ⎢⎣az ⎥⎦ ⎢⎣Uaz0 ⎥⎦ ⎢⎣naz ⎥⎦
(10.3)
10.1.2. Модель выходных сигналов гироскопов
Рассмотрим наиболее распространенный тип гироскопов, применяемых в
БИНС – датчики угловой скорости (ДУС). Как было обосновано выше, рассмотрим линейную модель выходных сигналов ДУС:
U ωx = Rxω x1 + U ωx 0 + nωx ;
U ωy = R yω y1 + U ωy 0 + nωy ;
U ωz = Rzω z1 + U ωz 0 + nωz ,
(10.4)
где Uωx , Uωy , Uωz – значения выходных сигналов ДУС в размерности напряжения, тока, кода АЦП, частоты или других величин; ωx1 , ωy2 , ωz3 – значения
проекций абсолютной угловой скорости на оси чувствительности ДУС Ox1, Oy2,
Калибровка БИНС
120
Oz3; Rx, Ry, Rz – масштабные коэффициенты каждого ДУС; Uωx0 , Uωy0 , Uωz0 –
смещения нулей ДУС; nωx , nωy , nωz – шумы измерения.
В БИНС применяют гироскопы, построенные на совершенно различных
принципах, соответственно и базовая линейная модель (10.4) может уточняться
за счет специфических составляющих. Так, для большинства типов гироскопов
характерна существенная чувствительность нуля к линейному ускорению движения. В частности, это все электромеханические гироскопы, в том числе поплавковые, ДНГ, в меньшей степени – волоконно-оптические, твердотельные
волновые гироскопы. Для таких гироскопов модель выходных сигналов (10.4)
расширяют, добавляя слагаемые, пропорциональные проекциям ax1 , ay2 , az3 кажущегося ускорения на оси чувствительности гироскопов:
∗
∗
∗
U ωx = Rxω x1 + U ωx 0 + b11
a x1 + b12
a y1 + b13
a z1 + nωx ;
∗
∗
∗
U ωy = R yω y1 + U ωy 0 + b21
a x1 + b22
a y1 + b23
a z1 + nωy ;
(10.5)
∗
∗
∗
U ωz = Rzω z1 + U ωz 0 + b31
a x1 + b32
a y1 + b33
a z1 + nωx ,
где Uωx0 , Uωy0 , Uωz0 – смещения нулей, не зависящие от ускорений; bij∗ – коэффициенты чувствительности к ускорениям (так называемый «дрейф от g»). Для
ряда гироскопов, например, ДНГ в модель (10.5) добавляют слагаемые, пропорциональные произведению ускорений (так называемый «дрейф от gквадрат»).
Также ряд типов гироскопов имеет, например, заметную чувствительность к напряженности магнитного поля, что также требует соответствующего
расширения математической модели.
Запишем модель измерений гироскопов (10.5) в матричном виде:
⎡U ωx ⎤ ⎡ Rx
⎢U ⎥ = ⎢ 0
⎢ ωy ⎥ ⎢
⎢⎣U ωz ⎥⎦ ⎢⎣ 0
0
Ry
0
∗
∗
b12
0 ⎤ ⎡ω x1 ⎤ ⎡U ωx 0 ⎤ ⎡b11
⎢ ∗
∗
b22
0 ⎥ ⋅ ⎢ω y1 ⎥ + ⎢U ωy 0 ⎥ + ⎢b21
⎥ ⎢ ⎥ ⎢
⎥
∗
∗
Rz ⎥⎦ ⎢⎣ω z1 ⎥⎦ ⎢⎣U ωz 0 ⎥⎦ ⎢⎣b31
b32
∗
⎤ ⎡ a x1 ⎤ ⎡nωx ⎤
b13
⎥ ⎢ ⎥
∗ ⎥ ⎢
b23
⎥ ⋅ ⎢a y1 ⎥ + ⎢nωy ⎥ . (10.6)
∗ ⎥
b33
⎦ ⎢⎣ a z1 ⎥⎦ ⎢⎣ nωz ⎥⎦
Снова подчеркнем, что модель измерений (10.5) или (10.6) – это модель
всего измерительного тракта каждого из гироскопов, включающего сам датчик
гироскопа, а также измерительные, усилительные и преобразующие элементы.
Для БИНС необходимо определять угловую скорость вращения вокруг
базовых осей Oxyz ИИМ (БЧЭ) БИНС, а не вокруг измерительных осей гироскопов Ox1, Oy2, Oz3 , которые к тому же не ортогональны, и не совпадают с базовыми осями ИИМ БИНС ввиду остаточных погрешностей установки гироскопов в ИИМ. Аналогично процедуре, описанной в п.10.1.1 для акселерометров, получим связь выходных сигналов гироскопов Uωx , Uωy , Uωz с требуемыми
проекциями ωx , ωy , ωz абсолютной угловой скорости на базовые оси БИНС:
Калибровка БИНС
⎡Uωx ⎤ ⎡ Rx
⎢U ⎥ = ⎢ 0
⎢ ωy ⎥ ⎢
⎢⎣Uωz ⎥⎦ ⎢⎣ 0
0
Ry
0
0 ⎤ ⎡cosγ x cos ρ x
0 ⎥ ⋅ ⎢ − sin ρ y
⎥ ⎢
Rz ⎥⎦ ⎢⎣ sin ρ z
∗
∗
∗
⎡b11
⎤ ⎡cosγ x cos ρ x
b12
b13
⎢ ∗
∗
∗ ⎥ ⎢
+ ⎢b21
b22
b23
⎥ ⋅ ⎢ − sin ρ y
∗
∗
∗ ⎥
⎢b31
b32
b33
⎣
⎦ ⎢⎣ sin ρ z
sin ρ x
cosγ y cos ρ y
− sin γ z cos ρ z
sin ρ x
cosγ y cos ρ y
− sin γ z cos ρ z
121
− sin γ x cos ρ x ⎤ ⎡ωx ⎤ ⎡Uωx0 ⎤
sin γ y cos ρ y ⎥ ⋅ ⎢ω y ⎥ + ⎢Uωy 0 ⎥ +
⎥ ⎢ ⎥ ⎢
⎥
cosγ z cos ρ z ⎥⎦ ⎢⎣ωz ⎥⎦ ⎢⎣Uωz 0 ⎥⎦
− sin γ x cos ρ x ⎤ ⎡ a x ⎤ ⎡nωx ⎤
sin γ y cos ρ y ⎥ ⋅ ⎢a y ⎥ + ⎢nωy ⎥ (10.7)
⎥ ⎢ ⎥ ⎢ ⎥
cosγ z cos ρ z ⎥⎦ ⎢⎣ a z ⎥⎦ ⎢⎣nωz ⎥⎦
где углы γi , ρi определяют перекосы измерительных осей гироскопов относительно базовой системы координат Oxyz ИИМ БИНС аналогично углам перекоса акселерометров αi , βi , показанным на рис.10.2.
10.1.3. Упрощение моделей выходных сигналов инерциальных
измерителей
Полученные модели выходных сигналов акселерометров (10.3) и гироскопов (10.7) пригодны для исследований. Для практического применения в алгоритмах БИНС эти модели содержат избыточное число промежуточных вычислений, которые можно выполнить один раз еще на этапе калибровки. Например, нет смысла определять в качестве параметров углы перекоса αi , βi , γi ,
ρi , а затем в вычислителе БИНС делать сложные тригонометрические вычисления. Можно сразу после калибровки вычислить необходимые тригонометрические функции и в качестве параметров модели использовать готовые коэффициенты. Таким образом, использование моделей (10.3) и (10.7) значительно
упрощается:
⎡U ax ⎤ ⎡ K x
⎢U ⎥ = ⎢ 0
⎢ ay ⎥ ⎢
⎢⎣U az ⎥⎦ ⎢⎣ 0
⎡Uωx ⎤ ⎡Rx
⎢U ⎥ = ⎢ 0
⎢ ωy ⎥ ⎢
⎢⎣Uωz ⎥⎦ ⎢⎣ 0
0
Ry
0
0
Ky
0
0 ⎤ ⎡ μ11
0 ⎥ ⋅ ⎢ μ 21
⎥ ⎢
K z ⎥⎦ ⎢⎣ μ 31
μ12
μ 22
μ 32
μ13 ⎤ ⎡ a x ⎤ ⎡U ax 0 ⎤ ⎡nax ⎤
μ 23 ⎥⎥ ⋅ ⎢⎢a y ⎥⎥ + ⎢⎢U ay 0 ⎥⎥ + ⎢⎢nay ⎥⎥ ; (10.8)
μ 33 ⎥⎦ ⎢⎣ a z ⎥⎦ ⎢⎣U az 0 ⎥⎦ ⎢⎣ naz ⎥⎦
0 ⎤ ⎡λ11 λ12 λ13 ⎤ ⎡ωx ⎤ ⎡Uωx0 ⎤ ⎡b11 b12 b13 ⎤ ⎡a x ⎤ ⎡nωx ⎤
⎢
⎥
0 ⎥ ⋅ ⎢λ21 λ22 λ23 ⎥ ⋅ ⎢ω y ⎥ + ⎢Uωy 0 ⎥ + ⎢b21 b22 b23 ⎥ ⋅ ⎢a y ⎥ + ⎢nωy ⎥ ,
⎥ ⎢
⎥ ⎢ ⎥ ⎢
⎢ ⎥ ⎢ ⎥
⎥
⎢
⎥
⎥
⎢
⎥
⎢
⎥
⎢
⎥
Rz ⎦ ⎣λ31 λ32 λ33 ⎦ ⎣ωz ⎦ ⎣Uωz 0 ⎦ ⎣b31 b32 b33 ⎦ ⎢⎣ a z ⎥⎦ ⎢⎣nωz ⎥⎦
(10.9)
где следующие матрицы вычисляются заранее (вне вычислителя БИНС):
Калибровка БИНС
122
⎡ μ11
⎢μ
⎢ 21
⎢⎣ μ 31
μ12
μ 22
μ 32
⎡ λ11 λ12
⎢λ
λ22
⎢ 21
⎢⎣λ31 λ32
⎡b11 b12
⎢
⎢b21 b22
⎢b31 b32
⎣
μ13 ⎤ ⎡cosα x cos β x
μ 23 ⎥⎥ = ⎢⎢ − sin β y
μ 33 ⎥⎦ ⎢⎣ sin β z
λ13 ⎤ ⎡cos γ x cos ρ x
λ23 ⎥⎥ = ⎢⎢ − sin ρ y
λ33 ⎥⎦ ⎢⎣ sin ρ z
∗
∗
b13 ⎤ ⎡b11
b12
⎥ ⎢ ∗
∗
b23 ⎥ = ⎢b21
b22
∗
∗
b33 ⎥⎦ ⎢⎣b31
b32
sin β x
cosα y cos β y
− sin α z cos β z
sin ρ x
cos γ y cos ρ y
− sin γ z cos ρ z
∗
⎤ ⎡cos γ x cos ρ x
b13
∗ ⎥ ⎢
b23
⎥ ⋅ ⎢ − sin ρ y
∗ ⎥
b33 ⎦ ⎢⎣ sin ρ z
− sin α x cos β x ⎤
sin α y cos β y ⎥ ;
⎥
cos α z cos β z ⎥⎦
− sin γ x cos ρ x ⎤
sin γ y cos ρ y ⎥ ;
⎥
cos γ z cos ρ z ⎥⎦
sin ρ x
cos γ y cos ρ y
− sin γ z cos ρ z
− sin γ x cos ρ x ⎤
sin γ y cos ρ y ⎥ .
⎥
cos γ z cos ρ z ⎥⎦
Следующим шагом на пути уменьшения объема вычислений в БИНС является предварительное перемножение (вне вычислителя БИНС) первых двух
матриц в выражениях (10.8) и (10.9). В результате получаем окончательные модели выходных сигналов акселерометров и гироскопов:
⎡U ax ⎤ ⎡ k11
⎢U ⎥ = ⎢k
⎢ ay ⎥ ⎢ 21
⎢⎣U az ⎥⎦ ⎢⎣ k31
⎡U ωx ⎤ ⎡ r11
⎢U ⎥ = ⎢r
⎢ ωy ⎥ ⎢ 21
⎢⎣U ωz ⎥⎦ ⎢⎣ r31
r12
r22
r32
k12
k 22
k32
k13 ⎤ ⎡ a x ⎤ ⎡U ax 0 ⎤ ⎡nax ⎤
k 23 ⎥ ⋅ ⎢a y ⎥ + ⎢U ay 0 ⎥ + ⎢nay ⎥ ;
⎥ ⎢ ⎥ ⎢
⎥ ⎢ ⎥
k33 ⎥⎦ ⎢⎣ a z ⎥⎦ ⎢⎣U az 0 ⎥⎦ ⎢⎣ naz ⎥⎦
r13 ⎤ ⎡ω x ⎤ ⎡U ωx 0 ⎤ ⎡b11 b12
⎢
r23 ⎥ ⋅ ⎢ω y ⎥ + ⎢U ωy 0 ⎥ + ⎢b21 b22
⎥ ⎢ ⎥ ⎢
⎥
r33 ⎥⎦ ⎢⎣ω z ⎥⎦ ⎢⎣U ωz 0 ⎥⎦ ⎢⎣b31 b32
b13 ⎤ ⎡ a x ⎤ ⎡nωx ⎤
⎥
b23 ⎥ ⋅ ⎢a y ⎥ + ⎢nωy ⎥ ,
⎢ ⎥ ⎢ ⎥
b33 ⎥⎦ ⎢⎣ a z ⎥⎦ ⎢⎣ nωz ⎥⎦
(10.10)
(10.11)
Отметим, что выражения (10.10), (10.11) полностью идентичны исходным
моделям (10.3), (10.7), но намного проще для использования в вычислителе
БИНС. Более того, идентификация параметров моделей измерителей на этапе
калибровки выполняется сразу для моделей в форме (10.10), (10.11) без предварительной оценки углов перекоса и масштабных коэффициентов как это требовалось бы при использовании исходных моделей (10.3), (10.7).
10.2. Калибровка инерциального измерительного модуля
БИНС в производстве
Как отмечалось выше, задачей калибровки БИНС является определение
параметров модели выходных сигналов акселерометров и гироскопов с целью
последующего их учета при вычислении действительных значений кажущегося
ускорения и абсолютной угловой скорости.
Калибровка БИНС
123
Калибровка акселерометров и гироскопов производится в составе инерциального измерительного модуля, то есть после их установки в ИИМ и полного электрического монтажа штатных измерительных цепей. Это имеет следующие преимущества:
1. Достигается строгая взаимная увязка измерительных осей акселерометров и гироскопов посредством их согласования с одними и теми же базовыми осями ИИМ.
2. Калибруются штатные каналы измерения БИНС целиком, а не отдельные
части (чувствительные элементы).
Калибровка ИИМ в производстве выполняется на специальных стендах
путем задания тестовых входных воздействий (ускорений и угловых скоростей)
и измерения выходных сигналов акселерометров и гироскопов. Методик определения параметров акселерометров и гироскопов разработано много, учитывая
к тому же различные виды имеющегося оборудования для калибровки. В частности, при наличии многоосного (как минимум – двухосного) поворотного
стенда, позволяющего позиционировать и вращать с высокой точностью ИИМ,
весь цикл калибровки не требует перестановок ИИМ и выполняется наиболее
быстро. Однако учитывая высокую стоимость прецизионных многоосных поворотных установок (сотни тысяч долларов США), калибровки ИИМ производятся чаще на одноосных поворотных стендах. При этом возникает необходимость перестановки ИИМ на стенде, чтобы задавать тестовые входные воздействия по всем измерительным осям.
Рассмотрим две наиболее известные методики определения параметров –
одну на примере калибровки акселерометров (с формированием пар уравнений
для нахождения каждого параметра), другую – на примере калибровки гироскопов (с обработкой всей совокупности измерений методом наименьших
квадратов).
10.2.1. Калибровка акселерометров
Калибровка акселерометров в диапазоне измерений ±1g производится путем задания известных положении ИИМ относительно вектора ускорения силы
r
тяжести g . При необходимости калибровки ИИМ в более широком диапазоне
ускорений используются специальные центрифуги.
r
Для высокоточного задания положении ИИМ относительно g , наиболее
часто используются прецизионные оптические делительные головки (ОДГ),
обеспечивающие точность поворота ИИМ на уровне 5 – 10 угловых секунд. Для
развязки от внешних вибраций, суточных колебаний зданий и прочих механических воздействий, ОДГ устанавливают на специальном фундаменте, развязанном относительно здания, в котором находится калибровочный участок.
На рис.10.3 показана оптическая делительная головка ОДГЭ-5 на валу 2
которой закреплена базовая площадка 3 с боковыми базами 4. Плоскость базо-
Калибровка БИНС
124
вой площадки параллельна оси вала, а боковые базы установлены перпендикулярно и параллельно оси вращения вала ОДГ.
ИИМ или другое тестируемое устройство должно иметь как минимум две
перпендикулярные базовые поверхности, которые и формируют базовые оси
устройства. ИИМ устанавливается одной своей базой на площадку ОДГ и прижимается другой базой к одной из боковых баз площадки. Это обеспечивает
параллельность одной из осей ИИМ оси вращения ОДГ.
1
2
4
3
Рис.10.3. Оптическая делительная головка ОДГЭ-5
1 – ОДГ; 2 – вал; 3 – базовая площадка; 4 – боковые базы
Для выполнения калибровки ось вращения ОДГ и базовая площадка выставляются в горизонте также с точностью до 5 – 10 угловых секунд с использованием прецизионных жидкостных уровней. Это позволяет с высокой точностью сориентировать
базовые оси ИИМ относительно вектора ускорения силы
r
тяжести g .
Для определения параметров модели (10.10) выходных сигналов акселерометров необходимо по каждой из трех осей ИИМ задавать известные ускорения. Это означает, что каждая из трех осей должна изменять свое положение
r
относительно вектора g . При повороте ИИМ на ОДГ вокруг одной оси эта ось
r
не меняет своего положения относительно вектора g и, следовательно, измеритель с осью чувствительности по этой оси не может быть откалиброван. Поэтому ИИМ переставляют на ОДГ, меняя ось вращения.
Рассмотрим один из самых распространенных вариантов установок и поворотов ИИМ при калибровке на ОДГ, схема которого показана приведена в
табл.10.1.
Калибровка БИНС
125
В фиксированных положениях, задаваемых с помощью ОДГ, ИИМ остается неподвижным относительно Земли, поэтому кажущееся ускорение, измеr r
r r
r r
ряемое акселерометрами (3.13) a = v& + (2u + ω o ) × v − g , в данном случае
r
r
a = −g ,
(10.12)
r
Учитывая, что вектор g направлен вертикально вниз, значения проекций
кажущегося ускорения на оси ИИМ для каждого положения ИИМ также приведены в табл.10.1.
Подставляя значения проекций кажущегося ускорения ax , ay , az из
табл.10.1 в модель выходных сигналов акселерометров (10.10), и перемножая
матрицы, получим значения выходных сигналов акселерометров в каждом из
положений ИИМ, которые также приведены в табл.10.1. Для существенного
уменьшения влияния шумов измерения, сигналы акселерометров осредняются в
течении времени измерения (обычно 30…60 с), поэтому в выражениях в
табл.10.1 шумы измерений nax , nay , naz не учитываются.
Таблица 10.1. Положение осей ИИМ при калибровке на ОДГ
и соответствующие значения ускорений и выходов акселерометров
№
Проекции Выходные сигналы
ускорения
акселерометров
Поворот вокруг оси X
Uax1 = k13 g + Uax0
ax= 0
Положение осей ИИМ
z
a y= 0
Uay1 = k23 g + Uay0
y
az= g
Uaz1 = k33 g + Uaz0
y
ax= 0
Uax2 = k12 g + Uax0
a y= g
Uay2 = k22 g + Uay0
az= 0
Uaz2 = k32 g + Uaz0
ax= 0
Uax3 = –k13 g + Uax0
a y= 0
Uay3 = –k23 g + Uay0
az= –g
Uaz3 = –k33 g + Uaz0
ax= 0
Uax4 = –k12 g + Uax0
a y= –g
Uay4 = –k22 g + Uay0
az= 0
Uaz4 = –k32 g + Uaz0
1
2
z
y
3
z
z
4
y
Поворот вокруг оси Y
Uax5 = k13 g + Uax0
ax= 0
z
Uay5 = k23 g + Uay0
a y= 0
5
x
az= g
Uaz5 = k33 g + Uaz0
Калибровка БИНС
126
ax= –g
Uax6 = –k11 g + Uax0
a y= 0
Uay6 = –k21 g + Uay0
az= 0
Uaz6 = –k31 g + Uaz0
ax= 0
Uax7 = –k13 g + Uax0
a y= 0
Uay7 = –k23 g + Uay0
z
az= –g
Uaz7 = –k33 g + Uaz0
x
ax= g
Uax8 = k11 g + Uax0
a y= 0
Uay8 = k21 g + Uay0
az= 0
Uaz8 = k31 g + Uaz0
z
6
x
x
7
8
z
Согласно выражениям в последней колонке табл.10.1, определяем искомые параметры модели выходных сигналов акселерометров:
k11 = (Uax8 – Uax6) /2g; k12 = (Uax2 – Uax4) /2g; k13 = (Uax1–Uax3 + Uax5 –Uax7) /4g;
k21 = (Uay8 – Uay6) /2g; k22 = (Uay2 – Uay4) /2g; k23 = (Uay1–Uay3 + Uay5 –Uay7) /4g;
k31 = (Uaz8 – Uaz6) /2g; k32 = (Uaz2 – Uaz4) /2g; k33 = (Uaz1–Uaz3 + Uaz5 –Uaz7) /4g;
1 8
1 8
1 8
(10.13)
Uax0 = ∑U axi ; Uay0 = ∑U ayi ; Uaz0 = ∑U azi .
8 i =1
8 i =1
8 i =1
Естественно, для вычисления параметров акселерометров по формулам
r
(10.13) необходимо знать модуль вектора ускорения силы тяжести g в месте
проведения испытаний.
10.2.2. Калибровка гироскопов
Поскольку, согласно модели (10.11), выходные сигналы гироскопов зависят не только от измеряемых проекций угловой скорости ωx , ωy , ωz , но и от ускорений ax , ay , az , то для калибровки гироскопов необходимо задавать тестовые значения как угловой скорости, так и ускорений. В результате калибровка
гироскопов разбивается на два этапа, в одном из которых задается угловая скорость вращения, а на другом – неподвижный ИИМ устанавливается в различr
ных положениях относительно вектора ускорения силы тяжести g – как при
калибровке акселерометров, описанной в п.10.2.1.
На первом этапе определяются коэффициенты rij матрицы масштабных
коэффициентов и перекрестных связей в модели (10.11) путем задания ряда
значений угловой скорости. Для этого ИИМ устанавливают на специальный
поворотный стол, ось вращения которого должна быть выставлена вертикально.
Точность задания и стабилизации угловой скорости вращения поворотных сто-
Калибровка БИНС
127
лов для калибровки гироскопов должна быть не хуже чем точность калибруемых гироскопов. Например, показанный на рис.10.4 прецизионный поворотный
стол известной фирмы Acutronic обеспечивает стабильность скорости вращения
0,001%.
Рис.10.4. Автоматизированный поворотный стол AC1120-V1.0
фирмы Acutronic
Модель выходных сигналов гироскопов (10.11) представим в следующем
виде, не разделяя нули на зависящие и не зависящие от ускорения слагаемые:
⎡U ωx ⎤ ⎡ r11
⎢U ⎥ = ⎢r
⎢ ωy ⎥ ⎢ 21
⎢⎣U ωz ⎥⎦ ⎢⎣r31
r12
r22
r32
r13 ⎤ ⎡ω x ⎤ ⎡U ω∗x 0 ⎤ ⎡nωx ⎤
⎢
⎥
r23 ⎥ ⋅ ⎢ω y ⎥ + ⎢U ω∗y 0 ⎥ + ⎢nωy ⎥ ,
⎥ ⎢ ⎥
⎢ ⎥
r33 ⎥⎦ ⎢⎣ω z ⎥⎦ ⎢⎣U ω∗z 0 ⎥⎦ ⎢⎣ nωz ⎥⎦
(10.14)
где
⎡U ω∗x 0 ⎤ ⎡U ωx 0 ⎤ ⎡b11 b12
⎢ ∗ ⎥ ⎢
⎥ ⎢
⎢U ωy 0 ⎥ = ⎢U ωy 0 ⎥ + ⎢b21 b22
⎢U ω∗z 0 ⎥ ⎢⎣U ωz 0 ⎥⎦ ⎢b31 b32
⎣
⎦
⎣
b13 ⎤ ⎡ a x ⎤
⎥
b23 ⎥ ⋅ ⎢a y ⎥ .
⎢ ⎥
b33 ⎥⎦ ⎢⎣ a z ⎥⎦
При калибровке на поворотном столе задается несколько значений N угловой скорости вращения в требуемом диапазоне измерений гироскопов (ДУС)
для более точного построения статической характеристики. Например, для рабочего диапазона ДУС ±300°/с удобно задать N =7 значений угловой скорости с
шагом 100°/с от –300°/с до +300°/с. В любом случае должно быть задано N ≥ 2
угловых скоростей по каждой оси.
При выполнении каждого измерения во время калибровки выходные сигналы гироскопов осредняются для существенного подавления шумов измерений nωx , nωy , nωz. Поэтому в дальнейшем в уравнениях измерений эти шумы
учитывать не будем. Для i-го измерения согласно (10.14) имеем:
Калибровка БИНС
128
⎡U ωx i ⎤ ⎡ r11 r12
⎢
⎥ ⎢
U
ω
y
i
⎢
⎥ = ⎢r21 r22
⎢U ωz i ⎥ ⎢⎣r31 r32
⎣
⎦
r13 ⎤ ⎡ω x i ⎤ ⎡U ω∗x 0 ⎤
⎥
⎢ ⎥ ⎢
r23 ⎥ ⋅ ⎢ω y i ⎥ + ⎢U ω∗y 0 ⎥ .
⎥
r33 ⎥⎦ ⎢⎣ω z i ⎥⎦ ⎢⎣U ω∗z 0 ⎥⎦
(10.15)
Для дальнейших вычислений удобно объединить 12 искомых параметров
(9 коэффициентов rij и 3 смещения нуля U ω∗x 0 ,U ω∗y 0 ,U ω∗z 0 ) в одну матрицу, преобразовав матричное уравнение (10.15) к виду
⎡U ωx i ⎤ ⎡ r11
⎥ ⎢
⎢
U
ω
y
i
⎥ = ⎢r21
⎢
⎢U ωz i ⎥ ⎢r31
⎦ ⎣
⎣
r12
r22
r32
⎡ω ⎤
r13 U ω∗x 0 ⎤ ⎢ x i ⎥
⎥ ω
r23 U ω∗y 0 ⎥ ⋅ ⎢ y i ⎥ .
⎢ω ⎥
r33 U ω∗z 0 ⎥⎦ ⎢ z i ⎥
⎣ 1 ⎦
(10.16)
При калибровке на поворотном столе задается вращение ИИМ вокруг каждой из трех осей отдельно. Примем, что вначале задается вращение ИИМ вокруг оси X. В этом случае в уравнении (10.16) ωyi = 0, ωzi = 0 и его можно
упростить:
⎡ω ⎤
r13 U ω∗x 0 ⎤ ⎢ x i ⎥ ⎡ r11 U ω∗x 0 ⎤
⎥ 0
⎥ ⎡ω ⎤
⎢
(10.17)
r23 U ω∗y 0 ⎥ ⋅ ⎢ ⎥ = ⎢r21 U ω∗y 0 ⎥ ⋅ ⎢ x i ⎥ .
⎢
⎥
0
1
⎣ ⎦
r32 r33 U ω∗z 0 ⎥⎦ ⎢ ⎥ ⎢⎣r31 U ω∗z 0 ⎥⎦
⎣ 1 ⎦
Всего при калибровке задается N значений угловой скорости ωxi , в
результате получим N измерений выходных сигналов гироскопов (10.17). Объединим эти измерения в одно матричное уравнение:
⎡U ωx i ⎤ ⎡ r11
⎥ ⎢
⎢
U
ω
y
i
⎥ = ⎢r21
⎢
⎢U ωz i ⎥ ⎢r31
⎦ ⎣
⎣
r12
r22
∗
⎡U ωx1 KU ωxN ⎤ ⎡ r11 U ωx 0 ⎤
⎢U KU ⎥ = ⎢r U ∗ ⎥ ⋅ ⎡ω x1 Kω x N ⎤ .
ωyN ⎥
ωy 0 ⎥ ⎢
⎥
⎢ 21
⎢ ωy1
∗ ⎥ ⎣ 1 K 1 ⎦
⎢
⎢⎣U ωz1 KU ωzN ⎥⎦ ⎣ r31 U ωz 0 ⎦
(10.18)
Затем задается вращение ИИМ вокруг оси Y. В этом случае в уравнении
(10.16) ωxi = 0, ωzi = 0, и оно упрощается следующим образом:
⎡U ωx i ⎤ ⎡ r11
⎥ ⎢
⎢
⎢U ωy i ⎥ = ⎢r21
⎢U ωz i ⎥ ⎢r31
⎦ ⎣
⎣
r12
r22
r32
⎡ 0 ⎤
r13 U ω∗x 0 ⎤ ⎢ ⎥ ⎡ r12 U ω∗x 0 ⎤
⎥ ω
⎥ ⎡ω ⎤
⎢
r23 U ω∗y 0 ⎥ ⋅ ⎢ y i ⎥ = ⎢r22 U ω∗y 0 ⎥ ⋅ ⎢ y i ⎥ .
⎢ 0 ⎥
⎣ 1 ⎦
r33 U ω∗z 0 ⎥⎦ ⎢ ⎥ ⎢⎣r32 U ω∗z 0 ⎥⎦
⎣ 1 ⎦
(10.19)
Калибровка БИНС
129
Для N заданных значений угловой скорости ωyi получим N уравнений
(10.19), которые объединим в одно матричное уравнение:
⎡U ωx N +1 KU ωx 2 N ⎤ ⎡ r12 U ω∗x 0 ⎤
⎥ ⎢
⎢
∗ ⎥ ⎡ω y1 Kω y N ⎤
U
U
r
U
K
=
+
ω
ω
ω
y
N
1
y
2
N
22
y0 ⎥ ⋅ ⎢
⎥.
⎥ ⎢
⎢
1
K
1
⎦
⎢U ωz N +1 KU ωz 2 N ⎥ ⎢ r32 U ω∗z 0 ⎥ ⎣
⎦ ⎣
⎣
⎦
(10.20)
Аналогично, при задании вращение ИИМ вокруг оси Z имеем ωxi = 0, ωн =
0, и уравнение (10.16) преобразуется к виду:
⎡U ωx i ⎤ ⎡ r11
⎥ ⎢
⎢
⎢U ωy i ⎥ = ⎢r21
⎢U ωz i ⎥ ⎢r31
⎦ ⎣
⎣
r12
r22
r32
⎡ 0 ⎤
r13 U ω∗x 0 ⎤ ⎢ ⎥ ⎡ r13 U ω∗x 0 ⎤
⎢
⎥ ⎡ω ⎤
⎥ 0
r23 U ω∗y 0 ⎥ ⋅ ⎢ ⎥ = ⎢r23 U ω∗y 0 ⎥ ⋅ ⎢ z i ⎥ .
⎢ω ⎥
⎣ 1 ⎦
r33 U ω∗z 0 ⎥⎦ ⎢ z i ⎥ ⎢⎣r33 U ω∗z 0 ⎥⎦
⎣ 1 ⎦
(10.21)
Объединяя в одно матричное уравнение N уравнений (10.21) для N
заданных значений угловой скорости ωzi получим:
⎡U ωx 2 N +1 KU ωx 3 N ⎤ ⎡ r13 U ω∗x 0 ⎤
⎥ ⎢
⎢
∗ ⎥ ⎡ω z1 Kω zN ⎤
⎢U ωy 2 N +1 KU ωy 3 N ⎥ = ⎢r23 U ωy 0 ⎥ ⋅ ⎢ 1 K 1 ⎥ .
⎦
⎢U ωz 2 N +1 KU ωz 3 N ⎥ ⎢r33 U ω∗z 0 ⎥ ⎣
⎦ ⎣
⎣
⎦
(10.22)
Уравнения (10.18), (10.20), (10.22) являются линейными уравнениями вида
B = X⋅A,
(10.23)
где A и B – известные матрицы (матрица A составлена из заданных значений
угловой скорости ωi, матрица B составлена из измеренных сигналов гироскопов
Uωi); X – неизвестная матрица, состоящая из 6 неизвестных параметров модели
гироскопов (3 коэффициента rij и 3 смещения нуля U ω∗ x 0 ,U ω∗y 0 ,U ω∗z 0 ). При этом
количество уравнений (размер матрицы B) 3⋅N должно быть не меньше числа
неизвестных, что выполняется при задании значений N ≥ 2 угловых скоростей
по каждой измерительной оси ИИМ. На практике задают избыточное число угловых скоростей N > 2 чтобы получить сглаженное по методу наименьших
квадратов (МНК) решение.
Решение матричного уравнения (10.23) по МНК имеет вид
(
(
X = B ⋅A+ = B ⋅ A т ⋅ A ⋅ A т
)
−1
)
−1
,
(10.24)
где матрица A+ = A т ⋅ A ⋅ A т называется псевдообратной.
Если транспонировать уравнение (10.23), то получим общеизвестную
130
Калибровка БИНС
форму записи пакетного МНК [5, 6]
)
X T = (H T H) −1 H T B T ,
где H = A T .
Применяя решение (10.24) к уравнениям (10.18), (10.20), (10.22), определяем искомые параметры модели выходных сигналов гироскопов:
⎡ r11 U ω∗x 0 ⎤ ⎡U ωx1 KU ωx N ⎤ ⎡ ω x1
⎢
⎢
⎥ ⎢
∗ ⎥
⎢r21 U ωy 0 ⎥ = ⎢U ωy1 KU ωy N ⎥ ⋅ ⎢ M
⎢ r31 U ω∗z 0 ⎥ ⎢U ωz1 KU ωz N ⎥ ⎢⎣ω xN
⎦
⎣
⎦ ⎣
1⎤ ⎛
⎡ω
⎜ ⎡ω x1 Kω xN ⎤ ⎢ x1
⎥
M ⋅⎜⎢
⋅ M
⎥ ⎣ 1 K 1 ⎥⎦ ⎢
⎢⎣ω xN
1⎥⎦ ⎜⎝
−1
1⎤ ⎞
⎟
M⎥ ⎟ ;
⎥
1⎥⎦ ⎟⎠
−1
⎡ r12 U ω∗x 0 ⎤ ⎡U ωx N +1 KU ωx 2 N ⎤ ⎡ ω y1
⎢
⎢
⎥ ⎢
∗ ⎥
⎢r22 U ωy 0 ⎥ = ⎢U ωy N +1 KU ωy 2 N ⎥ ⋅ ⎢ M
⎢ r32 U ω∗z 0 ⎥ ⎢U ωz N +1 KU ωz 2 N ⎥ ⎢ω yN
⎦ ⎣
⎣
⎦ ⎣
⎡ ω y1
1⎤ ⎛
⎜
ω
ω
K
⎥ ⎡
yN ⎤ ⎢
M⎥ ⋅ ⎜ ⎢ y1
⋅ M
1 K 1 ⎥⎦ ⎢
⎜
⎣
⎢ω yN
1⎥⎦ ⎜⎝
⎣
1⎤ ⎞
⎥⎟
M⎥ ⎟ ;
⎟
1⎥⎦ ⎟⎠
⎡ r13 U ω∗x 0 ⎤ ⎡U ωx 2 N +1 KU ωx 3 N ⎤ ⎡ ω z1
⎢
⎥ ⎢
⎢
∗ ⎥
⎢r23 U ωy 0 ⎥ = ⎢U ωy 2 N +1 KU ωy 3 N ⎥ ⋅ ⎢ M
⎢r33 U ω∗z 0 ⎥ ⎢U ωz 2 N +1 KU ωz 3 N ⎥ ⎢⎣ω zN
⎦
⎦ ⎣
⎣
1⎤ ⎛
⎡ ω z1
⎜
ω
ω
K
⎡
⎤
zN
M⎥ ⋅ ⎜ ⎢ z1
⋅⎢ M
⎥ ⎣ 1 K 1 ⎥⎦ ⎢
⎢⎣ω zN
1⎥⎦ ⎜⎝
1⎤ ⎞
⎟
M⎥ ⎟ .
⎥
1⎥⎦ ⎟⎠
(10.25)
−1
Отметим, что на практике численные значения задаваемых угловых скоростей вокруг каждой оси ω x1 Kω xN , ω y1 Kω yN , ω z1 Kω zN выбирают одними и
(
)
−1
(три последние маттеми же, поэтому псевдообратную матрицу A т ⋅ A ⋅ A т
ричные произведения в каждом из выражений (10.25)) вычисляют только один
раз.
Отметим, что при вычислении искомых параметров по формулам (10.25),
смещения нулей гироскопов U ω∗x 0 ,U ω∗y 0 ,U ω∗z 0 вычисляются трижды. На самом
деле, несмотря на одно и то же обозначение, это – разные тройки нулей,
поскольку они соответствуют трем различным положениям ИИМ относительно
вектора ускорения силы тяжести при вращении на поворотном столе вокруг
каждой из трех осей. Напомним, нули U ω∗ x 0 ,U ω∗y 0 ,U ω∗z 0 содержат зависящие и не
зависящие от ускорения слагаемые. Выделение этих слагаемых производится
на втором этапе калибровки гироскопов, который выполняется одновременно с
калибровкой акселерометров на ОДГ когда ускорения
по осям ИИМ задаются в
r
виде проекций вектора ускорения силы тяжести g (см. п.10.2.1).
В фиксированных положениях, задаваемых с помощью ОДГ, ИИМ остается неподвижным относительно Земли, поэтому кажущееся ускорение, измеr
r
ряемое акселерометрами, a = − g (10.12), а абсолютная угловая скорость равна
r r
скорости вращения Земли, ω = Ω .
Калибровка БИНС
131
Примем для упрощения, что ось вращения вала ОДГ выставлена на Север. Тогда ИИМ, установленный на площадке ОДГ, вращается относительно
инерциального пространства:
– вокруг вертикальной оси с угловой скоростью ΩВ = u sinϕ ;
– в направлении оси вращения вала ОДГ с угловой скоростью ΩГ = u cosϕ ,
где ΩВ и ΩГ – вертикальная и горизонтальная проекции угловой скорости вращения Земли, ϕ – географическая широта. Если ось вращения вала ОДГ занимает некоторое промежуточное положение в азимуте, то горизонтальную составляющую скорости вращения Земли необходимо разложить на две составляющие – параллельную оси вращения вала ОДГ и перпендикулярную составляющую. Если принять, что ось вращения вала ОДГ выставлена на Север, то
в положениях ИИМ, указанных в табл.10.1, проекции абсолютной угловой скорости ИИМ на его оси будут иметь значения, указанные в табл.10.2.
Подставляя значения проекций абсолютной угловой скорости ωx , ωy , ωz
из табл.10.2 и кажущегося ускорения ax , ay , az из табл.10.1 в модель выходных
сигналов гироскопов (10.11), и перемножая матрицы, получим значения выходных сигналов гироскопов в каждом из положений ИИМ. Соответствующие выражения также приведены в табл.10.2. Для уменьшения влияния шумов измерения, сигналы гироскопов усредняются в течении времени измерения (обычно
30…60 с), поэтому в выражениях в табл.10.1 шумы измерений nωx , nωy , nωz не
учитываются.
Таблица 10.2. Проекции угловой скорости вращения Земли на оси ИИМ для заданных положений осей
ИИМ при калибровке на ОДГ
Положение Проекции скорости враосей ИИМ щения Земли на оси ИИМ
ωx = ΩГ
1
ωy = 0
ωz = ΩВ
ωx = ΩГ
2
ωy = ΩВ
ωz = 0
ωx = ΩГ
3
ωy = 0
ωz = –ΩВ
ωx = ΩГ
4
ωy = –ΩВ
ωz = 0
ωx = 0
5
ωy = ΩГ
ωz = ΩВ
ωx = –ΩВ
6
ωy = ΩГ
ωz = 0
Выходные сигналы гироскопов
Uωx1 = r11 ΩГ + r13 ΩВ+ b13 g + Uωx0
Uωy1 = r21 ΩГ + r23 ΩВ+ b23 g + Uωy0
Uωz1 = r31 ΩГ + r33 ΩВ+ b33 g + Uωz0
Uωx2 = r11 ΩГ + r12 ΩВ+ b12 g + Uωx0
Uωy2 = r21 ΩГ + r22 ΩВ+ b22 g + Uωy0
Uωz2 = r31 ΩГ + r32 ΩВ+ b32 g + Uωz0
Uωx3 = r11 ΩГ – r13 ΩВ – b13 g + Uωx0
Uωy3 = r21 ΩГ – r23 ΩВ – b23 g + Uωy0
Uωz3 = r31 ΩГ – r33 ΩВ – b33 g + Uωz0
Uωx4 = r11 ΩГ – r12 ΩВ – b12 g + Uωx0
Uωy4 = r21 ΩГ – r22 ΩВ – b22 g + Uωy0
Uωz4 = r31 ΩГ – r32 ΩВ – b32 g + Uωz0
Uωx5 = r12 ΩГ + r13 ΩВ + b13 g + Uωx0
Uωy5 = r22 ΩГ + r23 ΩВ + b23 g + Uωy0
Uωz5 = r32 ΩГ + r33 ΩВ +b33 g + Uωz0
Uωx6 = –r11 ΩВ + r12 ΩГ – b11 g + Uωx0
Uωy6 = –r21 ΩВ + r22 ΩГ – b21 g + Uωy0
Uωz6 = –r31 ΩВ + r32 ΩГ – b31 g + Uωz0
Калибровка БИНС
132
7
8
ωx = 0
ωy = ΩГ
ωz = –ΩВ
ωx = ΩВ
ωy = ΩГ
ωz = 0
Uωx7 = r12 ΩГ – r13 ΩВ – b13 g + Uωx0
Uωy7 = r22 ΩГ – r23 ΩВ – b23 g + Uωy0
Uωz7 = r32 ΩГ – r33 ΩВ – b33 g + Uωz0
Uωx8 = r11 ΩВ + r12 ΩГ + b11 g + Uωx0
Uωy8 = r21 ΩВ + r22 ΩГ + b21 g + Uωy0
Uωz8 = r31 ΩВ + r32 ΩГ + b31 g + Uωz0
Согласно выражениям в последней колонке табл.10.2, определяем коэффициенты bij чувствительности к ускорениям и смещения нулей Uωx0 , Uωy0 ,
Uωz0 гироскопов:
b11 = (Uωx8 – Uωx6) /2g – r11 ΩВ /g; b12 = (Uωx2 – Uωx4) /2g – r12 ΩВ /g;
b21 = (Uωy8 – Uωy6) /2g – r21 ΩВ /g; b22 = (Uωy2 – Uωy4) /2g – r22 ΩВ /g;
b31 = (Uωz8 – Uωz6) /2g – r31 ΩВ /g; b32 = (Uωy2 – Uωy4) /2g – r32 ΩВ /g;
b13 = (Uωx1– Uωx3 + Uωx5 – Uωx7 ) /4g – r13 ΩВ /g;
b23 = (Uωy1– Uωy3 + Uωy5 – Uωy7 ) /4g – r23 ΩВ /g;
b33 = (Uωz1– Uωz3 + Uωz5 – Uωz7 ) /4g – r33 ΩВ /g;
(10.26)
8
8
1
1
Uωx0 = ∑U axi – (r11 + r12) ΩГ /2; Uωy0 = ∑U ayi – (r21 + r22) ΩГ /2;
8 i =1
8 i =1
1 8
Uωz0 = ∑U azi – (r31 + r32) ΩГ /2.
8 i =1
где значения коэффициентов rij уже определены на первом этапе калибровки
гиросокпов по формулам (10.25).
После вычисления по формулам (10.25), (10.26) значений всех параметров
принятой модели (10.11) выходных сигналов гироскопов, завершается процедура калибровки гироскопов.
Методики калибровки можно строить, задавая повороты вокруг оси, не
совпадающей ни с одной из осей чувствительности измерителей трехосного
блока.
10.2.3. Температурная калибровка инерциальных измерителей
В БИНС средней и высокой точности все инерциальные измерители (акселерометры и гироскопы) имеют компенсацию температурных погрешностей
параметров модели их выходных сигналов. Как минимум, производится температурная калибровка нулей U ax 0 ,U ay 0 ,U az 0 ,U ωx 0 ,U ωy 0 ,U ωz 0 и масштабных коэффициентов k11, k22, k33, r11, r22, r33 в моделях (10.10), (10.11) выходных сигналов
акселерометров и гироскопов. При этом для измерения текущей температуры
используются индивидуальные термодатчики в каждом из инерциальных измерителей. В некоторых прецизионных гироскопах используется несколько тем-
Калибровка БИНС
133
пературных датчиков для учета неравномерности теплового поля внутри гироскопа. Иногда учитывается еще и направление изменения температуры, чтобы
компенсировать температурный гистерезис.
Другим способом снижения температурных погрешностей является термостабилизация инерциальных измерителей, то есть поддержание постоянной
температуры внутри гироскопов (прежде всего) и акселерометров. Температура
может стабилизироваться не в отдельных измерителях, а внутри ИИМ в целом.
Термостабилизация позволяет обеспечить высокую стабильность параметров
инерциальных измерителей. Часто это более эффективный способ снижения
температурных погрешностей по сравнению с их компенсацией. Однако температурная стабилизация имеет ряд недостатков:
• большое энергопотребление;
• увеличение габаритов;
• увеличение времени готовности БИНС;
• снижение ресурса измерителей.
Последний недостаток связан с тем, что при термостабилизации рабочую
температуру внутри прибора часто поднимают выше максимальной температуры при эксплуатации - чтобы для поддержания температуры использовать только нагревательные элементы, без охладителей. Для равномерного распределения температуры внутри ИИМ используются также вентиляторы.
В БИНС низкой точности, которые работают лишь в составе комплексированных навигационных систем, компенсация температурных погрешностей
датчиков часто не производится, так как температурная калибровка приводит к
существенному увеличению себестоимости БИНС (использование специальных
стендов с термокамерами) и значительному увеличению времени заводских калибровок и испытаний. Это ведет к уменьшению объема выпуска БИНС за календарный период времени, а также к увеличению себестоимости БИНС. В результате нивелируется одно из основных преимуществ БИНС низкой точности
– низкая стоимость. Изменения нулей и масштабных коэффициентов инерциальных измерителей таких БИНС обычно компенсируются в комплексированной системе путем оценивания этих изменений (например, фильтром Калмана –
см. п. 9) и их последующим учетом.
Температурные зависимости параметров моделей выходных сигналов акселерометров и гироскопов (10.10), (10.11) носят сложный характер, так как определяются множеством факторов (материалы, конструкция, способы соединения элементов и т.п.). Детальное исследование этих зависимостей необходимо
для поиска путей снижения температурной чувствительности параметров,
обеспечения стабильности этой чувствительности для возможности ее компенсации. Как уже указывалось, чаще всего производится компенсация температурных изменений нулей и масштабных коэффициентов акселерометров и гироскопов.
При использовании выходных сигналов инерциальных измерителей в
приборах и системах, температурные зависимости их параметров обычно аппроксимируют несложными функциями от температуры. Наиболее часто ис-
Калибровка БИНС
134
пользуют линейную, кусочно-линейную и полиномиальную аппроксимации.
Рассмотрим несколько примеров.
Линейная аппроксимация температурных зависимостей
При линейной аппроксимации, нулевой сигнал U0 и масштабный коэффициент k каждого измерителя (акселерометра или гироскопа) представляются
в виде:
U0 = U0ном + α ⋅ΔT ;
k = kном (1+β ⋅ΔT );
(10.27)
ΔT = T – Tном,
где U0ном , kном – номинальные значения параметров, определенные при номинальной температуре Tном; T – текущая температура, измеренная термодатчиком; α, β – коэффициенты температурной чувствительности нулевого сигнала
и масштабного коэффициента.
Чтобы определить температурные коэффициенты α, β , необходимо выполнить описанные в п. 10.2.1 и п. 10.2.2 процедуры калибровки акселерометров и гироскопов при двух значениях температуры – номинальной Tном и повышенной или пониженной T2. Для этого калибровку акселерометров и гироскопов производят в термокамерах. В результате калибровки получают пары значений нулей U0ном , U02 и масштабных коэффициентов kном , k2 , соответствующие двум значениям температуры Tном и T2. Далее, согласно выражений (10.27)
определяют искомые коэффициенты температурной чувствительности:
ΔT = T2 – Tном;
α=
U 02 − U 0 ном
ΔT
;
β=
(10.28)
k 2 − k ном
.
k ном ⋅ ΔT
Обычно в качестве номинальной температуры выбирают нормальную Tном
= 20°С или Tном = 25°С, а в качестве второй температуры T2 – максимальную
или минимальную в соответствии с требованиям к измерителю или БИНС в целом.
И еще одно важное замечание: на практике термодатчики измерителей не
калибруют, и используют их «сырые» значения в кодах АЦП (или в размерности напряжения, тока, частоты, других величин являющихся непосредственным
выходом термодатчика). Это касается как калибровки, так и решения обратной
задачи – компенсации температурных изменений нулей и масштабных коэффициентов в соответствии с формулами (10.27). То есть во всех формулах используются значения Tном , T , ΔT в кодах АЦП, а не в градусах Цельсия или Кельвина.
Калибровка БИНС
135
Кусочно-линейная аппроксимация температурных зависимостей
Если температурный диапазон работы БИНС довольно широкий, то нелинейность температурной зависимости параметров измерителя становится
сильно заметной и ее линейная аппроксимация уже не обеспечивает приемлемый уровень компенсации температурных погрешностей.
В этом случае часто оставляют линейную аппроксимацию температурных
зависимостей вида (10.27), но с разными значениями коэффициентов температурной чувствительности α, β для температуры меньшей и большей чем номинальная (T < Tном и T > Tном):
U0 = U0ном + α ⋅ΔT ;
k = kном (1+β ⋅ΔT );
⎧α , при ΔT < 0
α =⎨ 1
;
α
T
,
при
Δ
>
0
⎩ 2
ΔT = T – Tном,
(10.29)
⎧ β1 , при ΔT < 0
⎩β 2 , при ΔT > 0
β =⎨
Для определения температурных коэффициентов α1, β1 и α2, β2 необходимо выполнить описанные в п.10.2.1 и п.10.2.2 процедуры калибровки акселерометров и гироскопов уже при трех значениях температуры – номинальной
Tном , минимальной T2 и максимальной T3. В результате калибровки получают
тройки значений нулей U0ном , U02 , U03 и масштабных коэффициентов kном , k2 ,
k3 , соответствующие трем значениям температуры Tном , T2 , T3 при калибровке.
Искомые коэффициенты температурной чувствительности α1, α2, β1, β2
определяют по формулам, аналогичным (10.28):
ΔT2 = T2 – Tном; ΔT3 = T3 – Tном;
U 02 − U 0 ном
U 03 − U 0 ном
α1 =
; α2 =
;
ΔT2
ΔT3
k −k
k −k
β1 = 2 ном ; β 2 = 3 ном
k ном ⋅ ΔT2
k ном ⋅ ΔT3
(10.30)
Иногда весь температурный диапазон работы измерителей разбивают более чем на два участка и также применяют кусочно-линейную аппроксимацию,
но уже с большим количеством отрезков линий. Естественно, при этом приходится вводить дополнительные температуры для калибровки измерителей.
На практике в формулах (10.29), (10.30) используются некалиброванные
«сырые» значения температурных датчиков Tном , T2 , T3 в кодах АЦП, а не в
градусах Цельсия или Кельвина.
Аппроксимация температурных зависимостей полиномами
Недостатком кусочно-линейной аппроксимации является ее «негладкость», проявляющаяся при существенной нелинейности температурной зависимости. В этом случае в районе номинальной температуры Tном , являющейся
136
Калибровка БИНС
общей точкой двух отрезков линий с разным наклоном, появляется явный излом аппроксимирующей линии.
Этого недостатка лишена аппроксимация температурной зависимости полиномом, проходящим через точки температур калибровки. В частности, при
использовании трех температур Tном , T2 , T3 для калибровки, полученные значения нулей U0ном , U02 , U03 и масштабных коэффициентов kном , k2 , k3 можно аппроксимировать полиномом второй степени. Этот метод иногда так и называют
– трехточечный метод температурной калибровки.
При аппроксимации температурных зависимостей нулевого сигнала U0 и
масштабного коэффициента k полиномом второй степени получаем следующие
выражения:
U0 = U0ном +α1 ⋅ΔT + α2 ⋅ΔT2 ; k = kном (1+β1 ⋅ΔT +β2 ⋅ΔT2);
(10.31)
ΔT = T – Tном .
Для определения коэффициентов температурной чувствительности нулевого сигнала α1, α2 и масштабного коэффициента β1, β2 необходимо, так же как
и при кусочно-линейной аппроксимации, выполнить описанные в п.10.2.1 и
п.10.2.2 процедуры калибровки акселерометров и гироскопов при трех значениях температуры – номинальной Tном , минимальной T2 и максимальной T3. В результате калибровки определяют значения нулей U0ном , U02 , U03 и масштабных
коэффициентов kном , k2 , k3 , соответствующие трем значениям температуры
Tном, T2 , T3 при калибровке. Полагая, что эти параметры имеют зависимость от
температуры вида (10.31), получаем следующие системы уравнений для нахождения коэффициентов α1, α2 , β1, β2 :
⎧U 02 = U 0ном + α1 ⋅ ΔT2 + α 2 ⋅ ΔT22 ⎧k 2 = k ном (1 + β1 ⋅ ΔT2 + β 2 ⋅ ΔT22 )
; ⎨
,
⎨
2
2
=
+
⋅
Δ
+
⋅
Δ
=
+
⋅
Δ
+
⋅
Δ
U
U
α
T
α
T
k
k
(
1
β
T
β
T
)
0 ном
1
3
2
3
ном
1
3
2
3
⎩ 03
⎩ 3
(10.32)
где ΔT2 = T2 – Tном; ΔT3 = T3 – Tном.
Каждое из выражений (10.32) – это система из двух линейных уравнений
с двумя неизвестными – α1, α2 в первой системе уравнений и β1, β2 во второй
системе. Решения систем этих уравнений имеют вид:
U 02 − U 0 ном
⎡U 02 − U 0 ном U 03 − U 0 ном ⎤
−
− α 2 ⋅ (T2 − T3 ) ;
⎥ (T2 − T3 ) ; α1 =
ΔT2
ΔT3
ΔT2
⎣
⎦
(10.33)
⎡k − k
k −k ⎤
k −k
β 2 = ⎢ 2 ном − 3 ном ⎥ [k ном (T2 − T3 )]; β1 = 2 ном − β 2 ⋅ (T2 − T3 ) .
k ном ⋅ ΔT2
ΔT3 ⎦
⎣ ΔT2
α2 = ⎢
Калибровка БИНС
137
Общие рекомендации по выбору аппроксимирующих функций
Теоретически, более сложная функция может точнее описать действительную зависимость параметров измерителя от температуры, и, следовательно,
точнее компенсировать эту зависимость. Однако использование сложных аппроксимирующих функций ограничивается следующими недостатками их использования.
1. Как видно из приведенных выше примеров температурной калибровки
параметров измерителей, более сложная аппроксимирующая функция требует
большего количества температур, на которых необходимо проводить полную
калибровку измерителей, что значительно увеличивает время калибровки.
2. Достоверность и повторяемость коэффициентов аппроксимирующей
функции снижается при чрезмерном усложнении этой функции. Например,
весьма вероятно, что использование полинома пятого порядка очень точно аппроксимировало изменение нулевого сигнала от температуры при данной калибровке, но если провести повторную температурную калибровку, то коэффициенты полинома при старших степенях существенно изменятся, а повторятся
лишь первые два-три коэффициента.
3. Усложнение аппроксимирующей функции увеличивает объем вычислений при решении обратной задачи к калибровке – температурной компенсации
изменений параметров измерителей во время их штатной работы.
Порядок аппроксимирующей функции выбирают из двух соображений:
а) исходя из анализа физических процессов в измерителе при изменении температуры;
б) исходя из результатов экспериментальных исследований, когда, начиная с
аппроксимации простейшими функциями (линейной), оценивают точность
температурной компенсации, и, если она неудовлетворительная, постепенно
усложняют эту функцию с учетом указанных выше трех ограничений.
10.3. Использование результатов калибровки инерциальных
измерителей в алгоритме БИНС
После проведения калибровки, описанной в п.10.2, становятся известными значения параметров моделей (10.10), (10.11) выходных сигналов акселерометров и гироскопов. Это позволяет при работе БИНС определять текущие проекции кажущегося ускорения ax , ay , az и абсолютной угловой скорости объекта
ωx , ωy , ωz на базовые оси БИНС, используя выходные сигналы акселерометров
Uax , Uay , Uaz и гироскопов Uωx , Uωy , Uωz . Эти сигналы пересчитываются в соответствии с уравнениями (10.10), (10.11):
⎡ a x ⎤ ⎡ q11
⎢a ⎥ = ⎢q
⎢ y ⎥ ⎢ 21
⎢⎣ a z ⎥⎦ ⎢⎣ q31
q12
q22
q32
q13 ⎤ ⎛ ⎡U ax ⎤ ⎡U ax 0 ⎤ ⎞
⎜
⎟
q23 ⎥ ⋅ ⎜ ⎢U ay ⎥ − ⎢U ay 0 ⎥ ⎟ ;
⎥ ⎢
⎥ ⎢
⎥
q33 ⎥⎦ ⎜⎝ ⎢⎣U az ⎥⎦ ⎢⎣U az 0 ⎥⎦ ⎟⎠
(10.34)
Калибровка БИНС
138
⎡ω x ⎤ ⎡ m11
⎢ω ⎥ = ⎢m
⎢ y ⎥ ⎢ 21
⎢⎣ω z ⎥⎦ ⎢⎣m31
m12
m22
m32
m13 ⎤ ⎛⎜ ⎡U ωx ⎤ ⎡U ωx 0 ⎤ ⎡b11 b12
⎢
m23 ⎥ ⋅ ⎜ ⎢U ωy ⎥ − ⎢U ωy 0 ⎥ − ⎢b21 b22
⎥ ⎜⎢
⎥ ⎢
⎥
⎜
m33 ⎥⎦ ⎢⎣U ωz ⎥⎦ ⎢⎣U ωz 0 ⎥⎦ ⎢⎣b31 b32
⎝
b13 ⎤ ⎡ a x ⎤ ⎞⎟
⎥
b23 ⎥ ⋅ ⎢a y ⎥ ⎟ ,
⎢ ⎥⎟
⎥
b33 ⎦ ⎢⎣ a z ⎥⎦ ⎟
⎠
(10.35)
где матрицы
⎡ q11
⎢q
⎢ 21
⎢⎣ q31
q12
q22
q32
q13 ⎤ ⎡ k11
q23 ⎥ = ⎢k 21
⎥ ⎢
q33 ⎥⎦ ⎢⎣ k31
k12
k 22
k32
−1
k13 ⎤
⎡ m11
⎥
k 23 ; ⎢m21
⎢
⎥
⎢⎣ m31
k 33 ⎥⎦
m12
m22
m32
m13 ⎤ ⎡ r11
m23 ⎥ = ⎢r21
⎥ ⎢
m33 ⎥⎦ ⎢⎣ r31
r12
r22
r32
−1
r13 ⎤
r23 ⎥ (10.36)
⎥
r33 ⎥⎦
вычисляются заранее (сразу после калибровки) и уже в таком виде загружаются
в память вычислителя БИНС.
Отметим, что компенсация шумов измерений nax , nay , naz , nωx , nωy , nωz ,
входящих в модели (10.10), (10.11) выходных сигналов акселерометров и гироскопов, невозможна ввиду случайного характера этих шумов. Использование
фильтров для снижения уровня этих шумов ограничено в БИНС требованиями
по полосе пропускания для акселерометров и гироскопов, которые, в свою очередь, определяются динамикой объекта, на котором устанавливается БИНС.
При компенсации температурных изменений нулей и масштабных коэффициентов измерителей, объем вычислений увеличивается.
Во-первых, в моделях (10.10), (10.11) выходных сигналов акселерометров
и гироскопов выделяют масштабные коэффициенты, то есть используют модели (10.8), (10.9) выходных сигналов измерителей, где Kx, Ky, Kz – масштабные
коэффициенты трех акселерометров, Rx, Ry, Rz – масштабные коэффициенты
трех гироскопов:
⎡U ax ⎤ ⎡ K x
⎢U ⎥ = ⎢ 0
⎢ ay ⎥ ⎢
⎢⎣U az ⎥⎦ ⎢⎣ 0
⎡U ωx ⎤ ⎡ Rx
⎢U ⎥ = ⎢ 0
⎢ ωy ⎥ ⎢
⎢⎣U ωz ⎥⎦ ⎢⎣ 0
0
Ry
0
0
Ky
0
0 ⎤ ⎡ μ11
0 ⎥ ⋅ ⎢ μ 21
⎥ ⎢
K z ⎥⎦ ⎢⎣ μ 31
μ12
μ 22
μ 32
μ13 ⎤ ⎡ a x ⎤ ⎡U ax 0 ⎤ ⎡nax ⎤
μ 23 ⎥⎥ ⋅ ⎢⎢a y ⎥⎥ + ⎢⎢U ay 0 ⎥⎥ + ⎢⎢nay ⎥⎥ ;
μ 33 ⎥⎦ ⎢⎣ a z ⎥⎦ ⎢⎣U az 0 ⎥⎦ ⎢⎣ naz ⎥⎦
0 ⎤ ⎡λ11 λ12 λ13 ⎤ ⎡ω x ⎤ ⎡U ωx0 ⎤ ⎡b11 b12 b13 ⎤ ⎡ax ⎤ ⎡nωx ⎤
⎢
⎥
0 ⎥ ⋅ ⎢λ21 λ22 λ23 ⎥ ⋅ ⎢ω y ⎥ + ⎢U ωy 0 ⎥ + ⎢b21 b22 b23 ⎥ ⋅ ⎢a y ⎥ + ⎢nωy ⎥ ,
⎥ ⎢
⎥ ⎢ ⎥ ⎢
⎢ ⎥ ⎢ ⎥
⎥
Rz ⎥⎦ ⎢⎣λ31 λ32 λ33 ⎥⎦ ⎢⎣ω z ⎥⎦ ⎢⎣U ωz 0 ⎥⎦ ⎢⎣b31 b32 b33 ⎥⎦ ⎢⎣az ⎥⎦ ⎢⎣nωz ⎥⎦
В соответствии с этими уравнениями, проекции кажущегося ускорения ax,
ay , az и абсолютной угловой скорости объекта ωx , ωy , ωz на базовые оси
БИНС определяются по формулам:
Калибровка БИНС
⎡ a x ⎤ ⎡ q11
⎢a ⎥ = ⎢q
⎢ y ⎥ ⎢ 21
⎢⎣ a z ⎥⎦ ⎢⎣ q31
q12
q22
q32
139
q13 ⎤ ⎡Qx (ΔT ) 0 0 ⎤ ⎛ ⎡U ax ⎤ ⎡U ax 0 (ΔT ) ⎤ ⎞
⎜
⎟
q23 ⎥ ⋅ ⎢0 Q y (ΔT ) 0⎥ ⋅ ⎜ ⎢U ay ⎥ − ⎢U ay 0 (ΔT )⎥ ⎟ ;
⎥ ⎢
⎥ ⎢
⎥ ⎢
⎥
⎜
q33 ⎥⎦ ⎢⎣ 0 0 Qz (ΔT ) ⎥⎦ ⎝ ⎢⎣U az ⎥⎦ ⎢⎣U az 0 (ΔT ) ⎥⎦ ⎟⎠
(10.37)
b13 ⎤ ⎡ax ⎤ ⎞⎟
⎥
b23 ⎥ ⋅ ⎢a y ⎥ ⎟ ,
⎢ ⎥⎟
⎥
b33 ⎦ ⎢⎣az ⎥⎦ ⎟
⎠
(10.38)
⎡ωx ⎤ ⎡m11 m12 m13 ⎤ ⎡ M x (ΔT ) 0 0 ⎤ ⎛⎜ ⎡Uωx ⎤ ⎡Uωx0 (ΔT )⎤ ⎡b11 b12
⎥ ⋅ ⎢0 M (ΔT ) 0⎥ ⋅ ⎜ ⎢U ⎥ − ⎢U (ΔT )⎥ − ⎢b b
⎢ω ⎥ = ⎢m m
m
22
23 ⎥ ⎢
y
⎥ ⎢ 21 22
⎥ ⎜ ⎢ ωy ⎥ ⎢ ωy0
⎢ y ⎥ ⎢ 21
⎜
⎢⎣ωz ⎥⎦ ⎢⎣m31 m32 m33 ⎥⎦ ⎢⎣0 0 M z (ΔT )⎥⎦ ⎢⎣Uωz ⎥⎦ ⎢⎣Uωz 0 (ΔT )⎥⎦ ⎢⎣b31 b32
⎝
где матрицы
⎡ q11
⎢q
⎢ 21
⎢⎣ q31
q12
q22
q32
q13 ⎤ ⎡ μ11
q23 ⎥ = ⎢ μ 21
⎥ ⎢
q33 ⎥⎦ ⎢⎣ μ 31
μ12
μ 22
μ 32
−1
μ13 ⎤ ⎡ m11 m12
μ 23 ⎥⎥ ; ⎢⎢m21 m22
μ 33 ⎥⎦ ⎢⎣ m31 m32
m13 ⎤ ⎡ λ11 λ12
m23 ⎥ = ⎢λ21 λ22
⎥ ⎢
m33 ⎥⎦ ⎢⎣λ31 λ32
−1
λ13 ⎤
λ23 ⎥⎥ (10.39)
λ33 ⎥⎦
вычисляются заранее (сразу после калибровки) и в таком виде загружаются в
память вычислителя БИНС.
Нулевые сигналы Uai0(ΔT), Uωj0(ΔT) и коэффициенты Qi(ΔT) и Mj(ΔT), обратные к масштабным коэффициентам Ki, и Rj, рассчитываются в вычислителе
БИНС с учетом текущей температуры Ti , Tj , измеренной в каждом i-м акселерометре и j-м гироскопе:
ΔTi = Ti – Ti ном ;
Uai0(ΔT) = Uai0ном + α i ⋅ΔTi ;
Qi (ΔT ) = K i−1 = [Ki ном (1+βi ⋅ΔTi )]–1;
ΔTj = Tj – Tj ном;
Uωj0(ΔT) = Uωj0ном + α j ⋅ΔTj ;
–1
M j (ΔT ) = R −1
j = [Rj ном (1+βj ⋅ΔTj )] .
(10.40)
Выражения (10.40) записаны на основании линейной модели температурной зависимости коэффициентов вида (10.27) или (10.29). При более сложной
температурной модели, например (10.31), в выражения (10.40) добавляются соответствующие слагаемые.
В памяти вычислителя хранятся номинальные значения нулей акселерометров и гироскопов Uai0ном, Uωj0ном, их масштабных коэффициентов Ki ном, Rj ном,
а также номинальные значения температур Ti ном, Tj ном, и коэффициенты температурной чувствительности α i, α j, βi, βj. Все перечисленные параметры – строго индивидуальные для каждого из акселерометров и гироскопов, определяются
при температурной калибровке как было описано в п.10.2.3.
140
Калибровка БИНС
10.4. Калибровка и довыставка БИНС на подвижном основании
Комплексирование БИНС с навигационными системами других типов позволяет не только повысить точность решения навигационной задачи, как было
показано в разделе 9, но и выполнить калибровку некоторых параметров измерителей, и закончить выставку БИНС в условиях движения объекта.
Выше была рассмотрена калибровка ИИМ БИНС при его производстве.
Такая калибровка позволяет определить параметры моделей выходных сигналов акселерометров и гироскопов (смещения нулей, масштабные коэффициенты и ряд других) на момент изготовления ИИМ. Однако в процессе эксплуатации эти параметры могут заметно изменяться, приводя к существенному увеличению погрешностей БИНС, если не учитывать это изменение. В практике гироскопических систем рассматривают следующие изменения параметров измерителей:
1) изменение значений параметров от пуска к пуску, при их стабильности
в каждом конкретном пуске;
2) изменение параметров (их нестабильность) в пуске во время работы
БИНС.
Изменение основных параметров акселерометров и гироскопов от пуска к
пуску определяется во время начальной выставки ИНС перед началом штатной
работы. С этой целью в платформенных ИНС осуществляется программный
разворот платформы в известные положения, осреднение сигналов измерителей
в каждом положении и последующее вычисление их параметров по методике,
подобной изложенной выше в п.10.2. В бесплатформенных ИНС инерциальные
измерители установлены неподвижно в корпусе, и их повернуть можно только
вместе с объектом, на котором установлена БИНС, но такая возможность
встречается крайне редко. Поэтому определение параметров измерителей, изменяющихся от пуска к пуску, при начальной выставке БИНС очень ограничено.
Для отслеживания изменения параметров измерителей во время работы
БИНС применяются специальные алгоритмы, пример которых рассматривается
в данном разделе. Естественно, эти алгоритмы позволяют также определить и
изменение параметров от пуска к пуску. Такие алгоритмы требуют привлечения
дополнительной информации от навигационных систем и датчиков другого типа (спутниковые навигационные системы, астроориентаторы, доплеровские измерители скорости, лаги и др.) и также могут требовать выполнения специальных маневров объекта.
Вопросы начальной выставки БИНС были рассмотрены в п.7. Такая начальная выставка производится на неподвижном основании. Однако есть целый
ряд объектов, которые по условиям своей работы не позволяют осуществить
начальную выставку на неподвижном основании, например, крылатые ракеты и
управляемые авиабомбы на борту самолетов, или пуск БИНС на морских объ-
Калибровка БИНС
141
ектах в условиях качки. Кроме того, полная начальная выставка БИНС является
достаточно длительным процессом для объектов, от которых требуется малое
время готовности к работе1.
Указанные проблемы требуют решения задачи начальной выставки (или
довыставки) БИНС в условиях движения объекта. Эта задача может быть решена при комплексировании БИНС с навигационными системами других типов,
как рассматривается ниже.
10.4.1. Алгоритм калибровки и довыставки БИНС на подвижном
основании
В качестве параметров измерителей БИНС, подлежащих калибровке во
время работы БИНС, рассмотрим смещения нулей акселерометров Uax0 , Uay0 ,
Uaz0 и гироскопов Uωx0 , Uωy0 , Uωz0 . Представим их в виде:
Uai0 = U ai 0 + ΔUai0;
Uω i0 = U ω i 0 + ΔUω i0, i = x, y, z
(10.41)
где U ai 0 , U ω i 0 – значения нулей, полученные при калибровке в производстве,
ΔUai0, ΔUω i0 – их изменение в процессе эксплуатации. Задачей алгоритма калибровки БИНС во время штатной работы на движущемся объекте является отслеживание изменения нулей измерителей ΔUai0, ΔUω i0 с целью их компенсации при вычислении текущих проекций кажущегося ускорения ax , ay , az (10.34)
и абсолютной угловой скорости объекта ωx , ωy , ωz (10.35).
При подстановке выражений (10.41) в формулы (10.34), (10.35) получим
аддитивные погрешности измерения проекций ускорения и угловой скорости:
⎡ Δa x ⎤ ⎡ q11
⎢Δa ⎥ = ⎢q
⎢ y ⎥ ⎢ 21
⎢⎣ Δa z ⎥⎦ ⎢⎣ q31
q12
q22
q32
q13 ⎤ ⎡ΔU ax 0 ⎤
q23 ⎥ ⋅ ⎢ΔU ay 0 ⎥ ;
⎥
⎥ ⎢
q33 ⎥⎦ ⎢⎣ ΔU az 0 ⎥⎦
(10.42)
⎡ Δω x ⎤ ⎡ m11
⎢Δω ⎥ = ⎢m
⎢ y ⎥ ⎢ 21
⎢⎣ Δω z ⎥⎦ ⎢⎣m31
m12
m22
m32
m13 ⎤ ⎛⎜ ⎡ΔU ωx 0 ⎤ ⎡b11 b12
⎢
m23 ⎥ ⋅ ⎜ ⎢ΔU ωy 0 ⎥ − ⎢b21 b22
⎥ ⎜⎢
⎥
m33 ⎥⎦ ⎜ ⎢⎣ ΔU ωz 0 ⎥⎦ ⎢⎣b31 b32
⎝
b13 ⎤ ⎡ Δa x ⎤ ⎞⎟
⎥
b23 ⎥ ⋅ ⎢Δa y ⎥ ⎟ .
⎢
⎥⎟
⎥
b33 ⎦ ⎢⎣ Δa z ⎥⎦ ⎟
⎠
Представим погрешности измерения ускорений Δai и угловых скоростей
Δωi (i = x, y, z) в виде медленно меняющихся случайных процессов, описываемых дифференциальными уравнениями
dΔai
= −τ ai−1Δai + ξ ai (t ) ;
dt
1
dΔωi
= −τ ω−1i Δωi + ξωi (t ) .
dt
Вопросы выставки на подвижном основании освещены в [23]
(10.43)
Калибровка БИНС
142
Такие случайные процессы называют экспоненциально коррелированными, τ ωi , τ ai − интервалы корреляции случайных процессов; ξai (t), ξω i (t) − порождающие белые шумы.
При формировании погрешностей измерения ускорений и угловых скоростей учтем также шумы измерения акселерометров εαi и гироскопов εωi :
Δaic = Δai + εαi ;
Δωic = Δωi + εωi ,
(i = x, y, z)
(10.44)
Возьмем за основу алгоритм комплексирования БИНС и СНС, рассмотренный в п.9.2, и дополним модель ошибок БИНС (9.5) погрешностями измерения ускорений и угловых скоростей (10.43), (10.44):
β& = −ωηδ + ωζ α − ΔvN / RN − c11Δωxc − c12Δωyc − c13Δωzc ,
α& = −ωζ β + ωξ δ + ΔvE / RE − u sinϕ ⋅ Δϕ − c21Δωxc − c22Δωyc − c23Δωzc ,
(
)
δ& = −ωξα + ωη β + ΔvEtgϕ / RE + vE /(RE cos2 ϕ ) + u cosϕ Δϕ − c31Δωzc − c32Δωxc − c33Δωyc.
Δv&E = δ ⋅ a N − α ⋅ a H + c11Δa xc + c12 Δa yc + c13Δa zc ,
Δv& N = −δ ⋅ a E + β ⋅ a H + c21Δa xc + c22 Δa yc + c23Δa zc ,
Δv&H = α ⋅ a E − β ⋅ a N + c31Δa xc + c32 Δa yc + c33Δa zc .
Δϕ& = Δv N / RN ,
Δλ& ≈
(10.45)
1
( Δv E + v E tgϕΔϕ ).
RE cos ϕ
Δh& = Δv H ,
Δaic = Δai + εαi ,
Δωic = Δωi + εωi ,
dΔai
= −τ ai−1Δai + ξ ai (t ) ,
dt
dΔωi
= −τ ω−1i Δωi + ξωi (t ) , (i = x, y, z).
dt
Представим расширенную модель ошибок БИНС (10.45) в матричном виде (9.1)
x& = Ax + Bu + Gw .
(10.46)
Включим в вектор состояния x (t) следующие ошибки БИНС и ее измерителей (всего 15 компонент):
x = [Δϕ Δλ Δh ΔvE ΔvN ΔvH β α δ Δax Δay Δaz Δωx Δωy Δω z ] . (10.47)
т
Калибровка БИНС
143
Вектор случайных входных воздействий w (t) сформируем в соответствии с уравнениями ошибок (10.45):
[
w = ε ax
ε ay ε az ε ωx ε ωy ε ωz ξ ax ξ ay ξ az ξωx ξωy ξωz ]т .
(10.48)
Формирование вектора управляющих воздействий u для компенсации
погрешностей БИНС описано ниже (см. выражение (10.57) для u и соответствующее значение матрицы коэффициентов B (10.58)).
Матрицы коэффициентов A, G в матричном уравнении (10.46) при векторах x (10.47) и w (10.48) соответствуют модели ошибок БИНС (10.45). Ввиду
большого размера матриц A и G, запишем их в блочном виде (блоки размером
3×3):
⎡ A11
⎢ 0
⎢
A = ⎢ A31
⎢
⎢ 0
⎢⎣ 0
A12
0
A32
0
0
0
0
A23
A33
0
0
C gb
0
− Ta
0
0 ⎤
⎡ 0
⎢C gb
0 ⎥
⎥
⎢
gb
C ⎥; G = ⎢ 0
⎥
⎢
0 ⎥
⎢ 0
⎢⎣ 0
− Tω ⎥⎦
0
0
C gb
0
0
0 0⎤
0 0⎥
⎥
0 0⎥ ,
⎥
I 0⎥
0 I ⎥⎦
(10.49)
где 0, I – нулевая и единичная матрицы, а значения остальных блоков равны
⎡
− aH aN ⎤
RN−1 0⎤
0 0⎤
0
⎡ 0
⎥
⎢
− aE ⎥ ;
0 0⎥; A12 = ⎢( RE cos ϕ ) −1 0 0⎥; A23 = ⎢ a H
0
⎢
⎥
⎥
⎥
⎢
⎢⎣− a N
0 0⎥⎦
0
0 1⎦
aE
0 ⎥⎦
⎣
⎡ 0
0
0 0⎤
− RN−1 0⎤
⎡
⎥
⎢
A31 = ⎢
0 0⎥; A32 = ⎢ RE−1
0
0⎥ ; (10.50)
− u sin ϕ
⎢
⎥
2
−1
⎢ RE−1 tg ϕ
⎢⎣VE ( R cos ϕ ) + u cos ϕ 0 0⎥⎦
0
0⎥⎦
⎣
⎡ 0
A11 = ⎢λ& tg ϕ
⎢
⎢⎣ 0
0
⎡
A33 = ⎢− Ω sin ϕ
⎢
⎢⎣ Ω cos ϕ
Ω sin ϕ
0
VN RN−1
− Ω cos ϕ ⎤
− VN RN−1 ⎥ ;
⎥
⎥⎦
0
Ta = diag ( τ a−11 , τ a−21 , τ a−31 );
Tω = diag ( τ ω−11 , τ ω−12 , τ ω−13 );
Сgb – матрица направляющих косинусов (6.4) перехода от осей объекта к
географическому базису. В выражения (10.50) подставлены выражения для проекций ωξ , ωη , ωζ абсолютной угловой скорости вращения географического сопровождающего трехгранника, и для сокращения выражений использованы
VE
соотношения λ& =
; Ω = λ& + u .
RE cos ϕ
Калибровка БИНС
144
Рассматриваемая задача калибровки инерциальных измерителей во время
штатной работы БИНС сводится к оценке погрешностей измерения ускорений
Δai и угловых скоростей Δωi (i = x, y, z), которые по сути являются смещениями
нулей акселерометров и гироскопов относительно их значений, определенных
при заводской калибровке БИНС.
Для оценивания этих смещений нулей, как и остальных ошибок БИНС,
включенных в вектор состояния x (t) (10.47), используем методику оптимального фильтра Калмана (ОФК), рассмотренную в п.9.1. Вектор измерений y (t) для
ОФК сформируем из разностей между координатами и скоростями объекта, определенными с помощью БИНС и приемника СНС:
⎡ ϕ бинс
⎢ λ
⎢ бинс
⎢ hбинс
y =⎢
⎢ v E бинс
⎢ v N бинс
⎢
⎢⎣v H бинс
− ϕ снс ⎤
− λснс ⎥
⎥
− hснс ⎥
⎥.
− v E снс ⎥
− v N снс ⎥
⎥
− v H снс ⎥⎦
(10.51)
Учитывая, что
ϕбинс = ϕ + Δϕ;
λбинс = λ + Δλ;
hбинс = h + Δh;
vN бинс = vN + ΔvN;
vE бинс = vE + ΔvE;
vH бинс = vH + ΔvH ,
(10.52)
и приняв погрешности приемника СНС в виде гауссовских белых шумов vi (t)
ϕснс = ϕ + v1 (t);
λснс = λ + v2 (t);
hснс = h + v3 (t);
vE снс = vE + v4 (t);
vN снс = vN + v5 (t);
vH снс = vH + v6 (t),
(10.53)
согласно выражения (10.51) получим
⎡ Δϕ − v1 ⎤
⎢ Δλ − v ⎥
2 ⎥
⎢
⎢ Δh − v 3 ⎥
y =⎢
⎥= H x +v,
V
v
Δ
−
E
4
⎥
⎢
⎢ΔV N − v5 ⎥
⎥
⎢
⎢⎣ ΔV В − v 6 ⎥⎦
(10.54)
Калибровка БИНС
145
С учетом выражения (10.47) для вектора состояния ошибок x , матрица
измерения H будет иметь следующий блочный вид:
H = [I 6,6
0 6,9 ] ,
(10.55)
где I6,6 − единичная матрица размером 6×6, 06,9 − нулевая матрица размером
6×9.
В выражении (10.54) v – вектор случайных шумов измерений приемника
т
СНС, v = − [v1 v 2 v 3 v 4 v 5 v 6 ] .
По измеренной части y (t) вектора состояния x (t) и при выполнении условия полной наблюдаемости, ОФК позволяет получить оптимальную оценку
x̂ (t) всего вектора состояния (10.47), то есть оценку всех ошибок БИНС, включая искомые смещения нулей акселерометров Δai и гироскопов Δωi (i = x, y, z).
Если начальная выставка БИНС была произведена грубо, с большими
ошибками в определении начальных координат объекта Δϕ0, Δλ0, Δh0, его скорости ΔvE0, ΔvN0, ΔvH0 , и углов ориентации β0 , α0 , δ0 , то благодаря устойчивости ОФК, эти ошибки при их оценивании будут сведены к минимально возможным значениям, что позволяет завершить процесс начальной выставки
БИНС уже при движении объекта.
Обычно реализуют замкнутую схему комплексирования БИНС и СНС. В
частности, оценки смещений нулей акселерометров Δai и гироскопов Δωi
используются для компенсации этих смещений на выходе акселерометров и
гироскопов.
В штатном режиме работы комплексированной системы можно добиться
бóльшего снижения ошибок БИНС путем использования известного способа
демпфирования колебаний ИНС за счет использования внешней информации о
скорости объекта, в данном варианте – от СНС. Для этого на входы первых интеграторов БИНС подаются оцененные с помощью ОФК погрешности БИНС по
определению скорости Δv̂ N , Δv̂ E , Δv̂ H . В результате вектор линейной скорости
движения объекта Vˆ будет вычисляться в БИНС по модифицированной формуле:
r
r
r
r
r
r
v& g = C gb ab − 2u g + ωgo × v g + g − K d Δvˆ ,
(10.56)
(
)
где Δ v̂ = [ Δv̂ E , Δv̂ N , Δv̂ H ]т ; Kd = diag(kd1, kd2, kd3) – диагональная матрица, составленная из коэффициентов передачи демпфирующих обратных связей.
В результате применения обратных связей по оценкам смещений нулей
акселерометров Δâi и гироскопов Δω̂i , по оценкам ошибок БИНС в определении линейной скорости Δv̂ N , Δv̂ E , Δv̂ H , вектор компенсационных входов u
для БИНС, введенный в уравнение состояния (10.46), будет иметь вид
[
u = Δaˆ x
Δaˆ y
Δaˆ z
Δωˆ x
Δωˆ y
Δωˆ z
ΔvˆE
Δvˆ N
ΔvˆH
]т ,
(10.57)
146
Калибровка БИНС
а соответствующая матица коэффициентов B в уравнении (10.46) будет состоять из блоков размером 3×3:
⎡ 0
⎢− C gb
⎢
B=⎢ 0
⎢
⎢ 0
⎢⎣ 0
0
0
− C gb
0
0
0 ⎤
− Kd ⎥
⎥
0 ⎥.
⎥
0 ⎥
0 ⎥⎦
(10.58)
10.4.2. Анализ возможности калибровки и довыставки БИНС на
подвижном основании
Чтобы с помощью ОФК можно было оценивать все n =15 введенных в
вектор состояния x (t) (10.47) ошибок БИНС по измеренным ошибкам y (t)
(10.54), необходимо, чтобы выполнялось условие полной наблюдаемости системы, описываемой уравнениями (10.46), (10.54). В случае постоянных матриц
A, H для полной наблюдаемости необходимо и достаточно, чтобы блочная матрица N (9.6) (матрица наблюдаемости второго рода) имела полный ранг, т.е.
rank N = n = 15. Если это условие не выполняется, то ранг матрицы N равен
числу переменных состояния системы xi(t) (ошибок БИНС) или их независимых линейных комбинаций, которые можно оценить по измерениям (10.54).
Движения объекта с постоянной скоростью без колебаний
При таком движении объекта элементы матрицы состояния A (10.49) постоянны. В результате анализа матрицы наблюдаемости N было установлено,
что rank N = 13, то есть из 15 ошибок БИНС, включенных в вектор состояния
(2.29), наблюдаемыми являются 13. Исключая отдельные строки и столбцы
матрицы А (10.49), соответствующие определенным компонентам вектора состояния (10.47), и проверяя после этого ранг матрицы наблюдаемости, было установлено, что не наблюдаются ошибка δ определения угловой ориентации вокруг вертикальной оси и дрейф гироскопа Δωz также вокруг вертикальной оси.
Это означает, что при движении объекта с постоянной скоростью без колебаний невозможно провести довыставку БИНС в азимуте и выполнить калибровку вертикального гироскопа (точнее – оценку эквивалентного дрейфа
гироскопов в проекции на вертикальную ось).
Ниже приведены результаты моделирования работы комплексированной
системы, построенной на основе БИНС низкой точности и приемника СНС, работающего в обычном (не дифференциальном) режиме работы. При моделиро-
Калибровка БИНС
147
вании были использованы численные значения параметров БИНС, СНС, и параметры движения объекта, приведенные в табл.10.1.
Таблица 10.1. Численные значения параметров, принятые при моделировании
Параметр, обозначение в формулах
Значение
Параметры движения объекта
45о
Начальная широта, ϕ
30о
Начальная долгота, λ
Высота, h
0м
Скорость движения объекта, v
10 м/с
Курс, К
45о
0о
Тангаж, θ
0о
Крен, γ
Параметры измерителей БИНС и СНС
Дисперсии медленно меняющихся случайных состав0,01
м/с2)2
(
ляющих смещений нулей акселерометров, D ai
2,5
Интервалы корреляции медленно меняющихся случай10000 с
ных смещений нулей акселерометров, τai
Интенсивности порождающих белых шумов вышеука2 D ai
3,2⋅10–9 м2/с5
занных смещений ξ , q =
ai
ξai
τ ai
Интенсивности шумов измерения акселерометров εαi ,
qεai
Дисперсии медленно меняющихся случайных составляющих дрейфов гироскопов, Dωi
0,001
м/с2)2 / Гц
2,5
0,3 о
(
/час)2 =
2,5
(5,8⋅10–7 рад/с)2
(
Интервалы корреляции медленно меняющихся случай2000 с
ных составляющих дрейфов гироскопов, τωi
Интенсивности порождающих белых шумов вышеука2 Dωi
3,4⋅10–16 (рад/с)2/ c
занных дрейфов ξω i , qξωi =
τ ωi
Интенсивности шумов измерения гироскопов εωi, qεωi
Интенсивности погрешностей приемника СНС vi
(i = 1…3) в определении координат объекта при частоте выдачи информации 1 Гц, rvi
Интенсивности погрешностей приемника СНС vi
(i =4…6) в определении скорости объекта при частоте
выдачи информации 1 Гц, rvi
1 о
/час)2 / Гц =
2,5
(2⋅10–6 рад/с)2/ Гц
(
100 м2/ Гц
(
0,1
м/с)2 /Гц
2,5
Калибровка БИНС
148
0.01
0.014
Действит.
Оценка
0.008
Действит.
Оценка
0.012
Δa , м/с2
0.006
y
x
Δa , м/с2
0.01
0.004
0.008
0.006
0.004
0.002
0
0
0.002
100
t, мин
200
Рис.10.5. Случайное смещение нуля бокового акселерометра и его оценка
0.4
0.01
0.3
Δω , o/час
0.006
x
0.004
100
t, мин
0.3
Действит.
Оценка
0.1
0
-0.3
0
100
t, мин
200
300
Рис.10.8. Случайное смещение нуля
бокового гироскопа и его оценка
0.4
Действит.
Оценка
0.2
Действит.
Оценка
0.3
0.2
Δω , o/час
0.1
0
-0.1
z
y
300
-0.2
Действит.
Оценка
200
300
Рис.10.7. Случайное смещение нуля
вертикального акселерометра и его
оценка
Δω , o/час
200
-0.1
0.002
0.1
0
-0.2
-0.1
-0.3
-0.2
-0.4
0
t, мин
0.2
0.008
0
0
100
Рис.10.6. Случайное смещение нуля
продольного акселерометра и его
оценка
0.012
z
Δa , м/с2
0
0
300
100
t, мин
200
300
Рис.10.9. Случайное смещение нуля
продольного гироскопа и его оценка
-0.3
0
100
t, мин
200
300
Рис.10.10. Случайное смещение нуля
вертикального гироскопа и его оценка
Калибровка БИНС
149
Как видно из графиков рис.10.5, рис.10.6, случайные смещения нулей
Δax, Δay горизонтальных акселерометров оцениваются слабо, в отличие от смещения нуля Δaz вертикального акселерометра (рис.10.7). Это отличие связано с
различными условиями работы горизонтальных и вертикального акселерометров. При рассмотренном в данном пункте стационарном движении объекта его
горизонтальные ускорения равны нулю, а вертикальный акселерометр "измеряет" большое ускорение силы тяжести g.
Случайные дрейфы горизонтальных гироскопов БИНС Δωx , Δωy , как
следует из результатов моделирования, показанных на рис.10.8, рис.10.9, оцениваются хорошо, в отличие от дрейфа Δωz гироскопа с вертикальной осью
чувствительности (см. рис.10.10), который практически не оценивается. Это
следует и из анализа наблюдаемости ошибок БИНС, приведенного выше.
Рассмотрим оценивание ошибок определения углов ориентации в БИНС:
2
3
Действит.
Оценка
1
2
1
α , угл.мин
β , угл.мин
0
-1
-2
0
-1
-3
-2
-4
-3
-5
0
50
100
150
t, мин
200
250
-4
0
300
Рис.10.11. Ошибка определения угла
наклона объекта относительно вертикали в направлении "Север-Юг" и ее
оценка
Действит.
Оценка
50
100
150
t, мин
200
250
Рис.10.12. Ошибка определения угла
наклона объекта относительно вертикали в направлении "Запад-Восток"
и ее оценка
50
δ , угл.мин
40
30
20
10
0
-10
0
50
100
300
150
t, мин
Действит.
Оценка
200
250
300
Рис.10.13. Ошибка определения угла курса объекта и ее оценка
150
Калибровка БИНС
Точность оценок ошибок БИНС β, α в определении углов наклона объекта удовлетворительная, как видно из рис.10.11, рис.10.12. Точность оценивания
ошибки БИНС δ в определении угла курса (см. рис.10.13) значительно хуже,
более того, оценка расходится с действительной ошибкой.
Причина этого следующая. Точность оценивания углов ориентации,
внешняя информация о которых в данных СНС отсутствует, ограничена, прежде всего, величиной нескомпенсированных случайных дрейфов гироскопов (Δωi
– Δω̂i ), то есть, в конечном счете, точностью оценок Δω̂i этих дрейфов. Как
было показано выше при анализе наблюдаемости ошибок БИНС, при стационарном движении объекта дрейф гироскопа Δωz вокруг вертикальной оси не
наблюдается. Соответственно, и точность оценивания ошибки БИНС в определении угла курса (см. рис.10.13) хуже, чем ошибок в определении углов отклонения объекта относительно плоскости горизонта (см. рис.10.11, рис.10.12).
Приведенные результаты моделирования подтверждают, что при движении объекта с постоянной скоростью без колебаний невозможно провести довыставку БИНС в азимуте и выполнить калибровку вертикального гироскопа
(точнее – оценку эквивалентного дрейфа гироскопов в проекции на вертикальную ось).
Напомним, что в данном пункте рассматривается комплексирование
БИНС с СНС эти выводы справедливы именно для такого варианта комплексирования. При комплексировании БИНС с угломерными навигационными системами, например, с астроориентатором, оцениваются все три ошибки определения угловой ориентации БИНС β , α , δ (полная довыставка БИНС) и дрейфы
всех трех гироскопов Δωx , Δωy , Δωz , однако появляются проблемы с оценкой
смещений нулей акселерометров Δax , Δay , Δaz .
Улучшение наблюдаемости ошибок БИНС при колебаниях объекта
Маневрирование, колебания объекта, когда изменяются матрица направляющих косинусов Сgb, проекции кажущегося ускорения объекта ai , приводят к
изменению элементов матрицы A (10.49), которые зависят от этих величин. В
этом случае ранг матрицы наблюдаемости повышается и наблюдаемость ошибок БИНС, включенных в вектор состояния, также улучшается.
Для примера рассмотрим движение объекта с постоянной скоростью при
наличии гармонической бортовой качки:
γ = γm sin ωк t,
(10.59)
где γm , ωк – амплитуда и частота качки. Примем для
Ранее было показано, что при стационарном движении объекта ненаблюдаемым является эквивалентный дрейф гироскопов в проекции на вертикальную ось. При качке объекта положение гироскопов БИНС относительно верти-
Калибровка БИНС
151
кали постоянно изменяется, что изменяет и условия наблюдаемости дрейфов
каждого из гироскопов. Кроме того, изменяются и проекции ускорений, измеряемые акселерометрами. В результате должны быть наблюдаемы все ошибки
БИНС, включенные в вектор состояния x (10.47).
При расположении ИИМ БИНС на расстоянии z от центра масс объекта
по нормальной оси, будет иметь место периодическое горизонтальное ускорение, которое будет измеряться акселерометрами БИНС. В проекциях на оси
географической системы координат получим:
aE = z γ&& sin K = – z γm ω к2 sin K sin ωк t;
aN = – z γ&& cos K = z γm ω к2 cos K sin ωк t.
(10.60)
Зададим для примера
γm = 20°;
ωк = 0,1 Гц;
z = 2 м.
(10.61)
На рис.10.14 показан график случайного смещения нуля бокового акселерометра Δax, реагирующего на бортовую качку, и его оценка Δâ x , полученная с
помощью ОФК.
0.01
Действит.
Оценка
x
Δa , м/с2
0.008
0.006
0.004
0.002
0
0
100
t, мин
200
300
Рис.10.14. Случайное смещение нуля бокового акселерометра
и его оценка при бортовой качке объекта
Сравнивая графики на рис.10.14 и рис.10.5, видим существенное улучшение оценки смещение нуля бокового акселерометра при бортовой качке.
Также при бортовой качке становится наблюдаемым случайный дрейф
гироскопа Δωz. Благодаря этому, существенно повысилась точность определения угла курса в комплексированной системе. На рис.10.15 представлены графики ошибки БИНС δ в определении угла курса, и ее оценки с помощью ОФК.
Сравнение с аналогичными графиками на рис.10.13 подтверждает, что ошибка
определения угла курса стала наблюдаемой и показывает существенное увеличение точности определения угла курса при качке по сравнению со стационарным движением объекта. Это позволяет провести довыставку БИНС в азимуте.
Калибровка БИНС
152
50
δ , угл.мин
40
30
20
10
0
0
50
100
150
t, мин
Действит.
Оценка
200
250
300
Рис.10.15. Ошибка определения угла курса объекта и ее оценка
при бортовой качке объекта
В результате проведенных в данном пункте исследований можно сделать
вывод о существенном улучшении возможностей по калибровке акселерометров и гироскопов БИНС и довыставке БИНС при качке объекта по сравнению
со стационарным движением объекта.
При необходимости маневры объекта могут быть заданы специально.
11. Моделирование БИНС
Как любая измерительная система, БИНС имеет методические и инструментальные погрешности. Это основные характеристики измерительной системы, которые подлежат определению и анализу.
Методические погрешности могут иметь место из-за приближенности отдельных компонентов (уравнений) алгоритма функционирования или даже неучета отдельных физических факторов (например, отличия земного геоида от
эллипсоида, аномальности гравитационного поля Земли и др.), приближенностью методов интегрирования дифференциальных уравнений, приближенного
представления отдельных функций, конечной разрядности БЦВМ и округления
вычислений.
Инструментальные погрешности являются следствием погрешностей измерителей ориентации и акселерометров, дискретизации измерительной информации, неточности начальной выставки, неточности установки измерителей
по отношению к базовым осям объекта.
В пособии рассматриваются системы, в которых измерителями параметров ориентации являются датчики угловой скорости.
В первую очередь, необходимо создать модель движения объекта, которая включает указанные параметры движения. Основные виды движения современного объекта следующие:
•
движение с постоянной линейной скоростью относительно Земли,
•
движение с ускорением относительно Земли,
•
качка и рыскание (трехосная качка) или угловые вибрации,
•
орбитальное линейное движение или поступательные вибрации.
Моделирование БИНС
153
Некоторые варианты движения принимают тестовыми, например, коническое движение на качке.
Зададим гармоническую качку в следующем виде:
ψ = ψ m sin ωψ t ,
ϑ = ϑm sin(ωϑ t + εϑ ) ,
γ = γ m sin(ωγ t + ε γ ) .
(11.1)
Здесь ψ m , ϑm , γ m -амплитуды рыскания, тангажа и крена, ωψ , ωϑ , ωγ круговые частоты качки, εϑ , ε γ - сдвиги по фазе.
Компоненты угловой скорости качки запишем в виде
ψ& = ψ mωψ cos ωψ t ,
ϑ& = ϑmωϑ cos(ωϑ t + εϑ ) ,
γ& = γ mωγ cos(ωγ t + ε γ ) . (11.2)
Для перехода от параметров качки к угловым скоростям в связанных с
объектом осям можно использовать уравнения Эйлера (2.1-2.3), преобразованные к следующему виду
bg
bg
bg
ω x = ϑ& cos γ + ωξ C11
+ ωη C12
+ (ωζ + ψ& )C13
,
bg
bg
bg
ω y = γ& + ωξ C21
+ ωη C22
+ (ωζ + ψ& )C23
,
(11.3)
bg
bg
bg
ω z = ϑ& sin γ + ωξ C31
+ ωη C32
+ (ωζ + ψ& )C33
,
где Cijbg находим из (2.4).
Проекции угловой скорости географического сопровождающего базиса в
(11.3) определяют по формулам
ωξ = −
vN
vE
vE
tgϕ . (11.4)
, ωη = u cosϕ +
, ωζ = u sin ϕ +
RN + h
RE + h
RE + h
Здесь RN , RE - главные радиусы кривизны земного эллипсоида в точке нахождения объекта в соответствии с (1.1), (1.2).
Скорость объекта можно задать постоянной, можно задать переменной,
например, учитывая влияние ускорений места установки на качке [20]. Ускорения движения можно задать, например, в соответствии с (3.16)
Wξ = v&Ε − (u sin ϕ + ως )vΝ + ( 2u cosϕ +
vΕ
)vς ;
RE + h
Wη = v&Ν + (u sin ϕ + ως )vΕ + u 2 (RE + h )sin ϕ cos ϕ +
vΝ ⋅ vς
RE + h
;
v N2
v E2
Wς = v&ς −
−
− 2uvΕ cos ϕ − u 2 (RE + h ) cos2 ϕ .
R N + h RE + h
Моделирование БИНС
154
Абсолютную скорость объекта получают путем интегрирования абсолютного
ускорения
t
t
0
0
Vξ = ∫ Wξ dt + Vξ 0 , Vη = ∫ Wη dt + Vη 0 .
Относительную скорость получают, вычитая переносную скорость из-за вращения Земли
v E = Vξ − ( RE + h )u cos ϕ , v N ≡ Vη .
По данным о скорости вычисляют широту и долготу
t
t
vN
vE
ϕ=∫
dt + ϕ 0 , λ = ∫
dt + λ0 .
(
)
cos
R
+
h
R
+
h
ϕ
E
0 N
0
(11.5)
Компоненты модели движения формируют в блоке Модель движения объекта
(рис.11.1).
Для решения задачи навигации используют основное уравнение инерциальной навигации в виде a = W − g ' , где a - вектор кажущегося ускорения, W вектор абсолютного ускорения, g ' - вектор гравитационного ускорения (ускорения силы тяготения). При движении в околоземном пространстве на объект
всегда действует центростремительное ускорение из-за вращения Земли
Wцс = u × (u × R ) , которое в совокупности с гравитационным ускорением создает ускорение силы тяжести
g = g ' −u × ( u × R ) .
(11.5)
Упрощенная модель ускорения силы тяжести в географическом сопровождающем трехграннике может иметь вид:
gξ = 0 ;
gη = 0 ;
⎡ 2h
⎤
gζ = − g 0 (1 + 5,2884 ⋅ 10− 3 sin 2 ϕ ) ⎢1 − (1 − ε 2 sin 2 ϕ )⎥ ,
a
⎣
⎦
где h - барометрическая высота объекта.
При более точном учете гравитационного ускорения необходимо использовать
формулы (1.14).
Основное уравнение инерциальной навигации может быть представлено в
различных формах
в зависимости от того, какую из скоростей мы используем,
r
r
абсолютную V или относительную v , и от того, в проекциях на какую систему
координат мы записываем уравнение.
Моделирование БИНС
155
Основное уравнение инерциальной навигации в географическом базисе g,
r
записанное для вектора относительной скорости v , можно представить из (3.8)
или (3.9) в виде
r r r r r r r
r
r
v& + (u + ω ) × v + u × (u × r ) = C gb a g + g ' g
(11.6)
(в левой части уравнения индексы g опущены), где вектор относительной скороr
сти в базисе g имеет вид v g = [v E , v N , v H ]Т ,
r
вектор скорости вращения Земли u g = [0, u cos ϕ , u sin ϕ ]Т ,
r
вектор абсолютной угловой скорости ωg = [ωξ , ωη , ωζ ]T .
r
В соответствии с уравнением (11.6) формируют кажущееся ускорение a g .
Это производится в блоке АКС.
Для учета погрешностей приборов можно использовать модели из п. 10.1. В результате на выходе блоков ДУС и АКС получим модели сигналов датчиков угловой скорости и акселерометров.
Зависимости, которые описывают модель движения объекта и уравнения
работы БИНС без ошибок, называют иногда уравнениями "идеальной" работы.
Рис.11.1. Схема моделирования ошибок БИНС
Зависимости, которые учитывают возмущающие факторы (ошибки чувствтельных элементов, погрешности численного интегрирования, погрешности начальной выставки и др.) называют уравнениями "реальной" работы.
В вычислителе БИНС моделируются уравнения реальной работы в соответствии с гл. 6. Результатом моделирования будут приборные значения выходных
~
~ ~
параметров ψ~,ϑ , γ~,ϕ~, λ , h , v~ξ , v~η , v~ζ . После вычитания из приборных значений
Моделирование БИНС
156
исходных (заданных) параметров движения, сформированных в блоке Модель
движения объекта, получают значения погрешностей. В этом случае можно моделировать погрешности численных методов вычисления в отсутствие каких
либо возмущающих факторов.
Ошибки системы получают также как разность между решениями уравнений "реальной" работы и "идеальной" работы. Использование решений уравнений "идеальной" работы вместо заданной модели движения позволяет исключить влияние ошибок численного метода интегрирования.
Такую методику моделирования погрешностей БИНС иногда называют
"прямым" моделированием.
Моделирование погрешностей возможно также по уравнениям ошибок
(см. гл. 7.).
Однако "прямое" моделирование представляется более предпочтительным, поскольку сами уравнения ошибок всегда приближенны. Результат моделирования по уравнениям ошибок можно использовать как тестовый при отладке программы. В то же время более надежными тестами можно считать аналитические результаты в получении ошибок для любых частных случаев, которые поддаются аналитическому исследованию.
В качестве учебного примера моделирования в [25] приведена программа,
составленная на языке общеизвестного пакета Матлаб. Можно отметить, что
моделирование в m-файлах более универсально, чем моделирование в Simulink.
Литература
1.
2.
3.
4.
5.
6.
Бромберг П.В., Теория инерциальных систем навигации. - М.: «Наука»,
1979. - 296 с.
Одинцов А.А. Теория и расчет гироскопических приборов. – К.: Вища
школа, 1985. -392 с.
Павловський М.А. Теоретична механіка. – К.: «Техніка», 2002. - 510 с.
Под ред. М.Н.Красильщикова, Г.Г.Себрякова – Управление и наведение
беспилотных маневренных летательных аппаратов на основе современных информационных технологий – М.: Физматлит, 2005. – 280 с.
Кузовков И.Т., Салычев О.С. Инерциальная навигация и оптимальная
фильтрация. - М.: Машиностроение,1982. - 216 с.
Titterton D., Weston J., Strapdown Inertial Navigation Technology -2nd
Edition, Institution of Electrical Engineers ,UK, 2004. - 558 p.
7.
8.
9.
Слив Э.И. Прикладная теория инерциальной навигации. -Л.: Судостроение, 1972. - 128 с.
Лазарєв Ю.Ф., Моделювання на ЕОМ. - К.: Політехніка, 2007. - 290 с.
Панов А.П. Математические основы теории инерциальной ориентации. –
К., 1995. – 278 с.
Моделирование БИНС
157
10. Бобнев М.П., Кривицкий Б.Х., Ярлыков М.С.. Комплексные системы радиоавтоматики. – М.: «Советское радио», 1968. – 118 с.
11. Самотокин Б.Б., Мелешко В.В., Степанковский Ю.В. Навигационные
приборы и системы. - К.: Вища школа, 1986. - 343 с.
12. Ориентация и навигация подвижных объектов: современные информационные технологии / Под ред. Б.С.Алешина, К.К.Веремеенко,
А.И.Черноморского. – М.: ФИЗМАТЛИТ, 2006. – 424с.
13. Анучин О.Н., Емельянцев Г.И. Интегрированные системы ориентации и
навигации морских подвижных объектов. – СПб.: ГНЦ РФ – ЦНИИ
«Электроприбор», 2003. – 390с.
14. Бранец В. Н., Шмыглевский И. П. Введение в теорию бесплатформенных
инерциальных систем. – М.: Наука, 1992. – 280 с.
15. Андреев В.Д. Теория инерциальной навигации. Корректируемые системы.-М.:Наука,1967.-648 с.
16. Лазарєв Ю.Ф., Бобровицька Я.Г. Розроблення і моделювання алгоритмів
безплатформової системи орієнтації. – К.:НТУУ “КПІ”, 2006.–135с.
www.kafpson.kpi.ua
17. Васильківський І.В. Дослідження похибок безплатформової інерціальної
навігаційної системи. Магістерська дисертація – К.: НТУУ “КПІ”, 2010. –
129 с.
18. Селезнев В.П. Навигационные устройства. – М.: Машиностроение, 1974. 600с.
19. Инерциальные навигационные системы морских объектов /Под ред.
Д.П.Лукьянова. – Л.: Судостроение, 1989. -184 с.
20. Ривкин С.С., Берман З.М., Окон И.М. Определение параметров ориентации объекта бесплатформенной инерциальной системой. - СПб: ГНЦ РФ ЦНИИ "Электроприбор", 1996. - 226 с.
21. Епифанов А.А. Избыточные системы управления летательными аппаратами. – М.: Машиностроение, 1978. - 144с.
22. Водичева Л.В., Бельский Л.Н., Маслова О.И., Лукин Н.А. Оптимальное
проектирование прецизионных малогабаритных БИНС для высокоманевренных подвижных объектов. // Вестник Самарского государственного
аэрокосмического университета, №4(20), 2009. – с.186-198
23. Алешкин М.В. Совершенствование схем и алгоритмов предварительной
обработки информации избыточных блоков инерциальных датчиков.
Кандидатская диссертация. – Саратовский ГТУ, 2009. – 121 с.
24. Мелешко В. В. Инерциальные навигационные системы. Начальная выставка. - К.: «Корнийчук», 1999. – 126 с.
25. Мелешко В.В. Руководство к лабораторной работе "Моделирование
БИНС", www.kafpson.kpi.ua
26. Цисарж В.В., Марусик Р.И. Математические методы компьютерной графики. - К.: «Факт», 2004. – 464 с.
ПРИЛОЖЕНИЕ 1
158
ПРИЛОЖЕНИЕ 1.
Сведения из алгебры кватернионов
r
r
r
Кватернион – 4-х мерное комплексное число λ = λ0 + i λ1 + j λ2 + k λ3 , λi параметры кватерниона (они же параметры Родрига-Гамильтона).
Кватернион описывает поворот тела (связанной с ним системы коордиr
нат) относительно оси υ на угол φ (рис.п1.)
r
r
r
r r r
λ0 - скалярная часть кватерниона, i λ1 + j λ2 + k λ3 - векторная часть, где i , j ,k орты связанной системы координат.
Иногда кватернион представляют как вектор, состоящий из скалярной и
векторной части
r
⎡λ ⎤
λ = ⎢ r0 ⎥ , где λ = [λ1 λ2 λ3 ]T .
⎣λ ⎦
r
r
Принимая, что ось υ задается единичным вектором e , единичный кватернион Λ , описывающий поворот, записывают как
φ r φ⎞
⎛
Λ = ⎜ cos , e sin ⎟ ;
2
2⎠
⎝
φ
φ
φ
φ
λ0 = cos , λ1 = l sin , λ2 = m sin , λ3 = n sin .
2
2
2
2
Здесь l, m, n – направляющие косинусы углов α i (i=1,2,3) соответственно
(рис.п1), определяющих положение оси поворота.
Иногда кватернион записывают
r
r
r
в виде
Λ = i λ1 + j λ2 + k λ3 + λ4
r
r
r
или в виде Λ = λ1 + i λ2 + j λ3 + k λ4 .
Кватернион записывают также аналогично вектору, например,
Λ = [λ1 , λ2 , λ3 , λ4 ]T .
Параметры единичного кватерниона
связаны очевидным соотношением
λ20 + λ12 + λ22 + λ23 = 1 .
(п1.1)
Рис.п1. Поворот системы координат Сложение кватернионов выполняется
так же, как сложение векторов. Так, если
q1 = [ w1
x1
y1
z1 ]T ,
q1 + q 2 = [ w1 + w2
q 2 = [ w2
x1 + x2
y1 + y2
x2
z1 + z2 ]T .
y2
z2 ]T ,
ПРИЛОЖЕНИЕ 1
159
Правила кватернионного сложения:
q1 + q 2 = q 2 + q1 ;
(q1 + q 2 ) + q 3 = q1 + (q 2 + q 3 ) ;
q + 0 = 0 + q;
q + (−q) = (−q) + q = 0 .
Если число s - скаляр, то его умножение на кватернион определяется как
→
⎛
⎞
s q = ⎜ sq0 , s q ⎟ .
⎝
⎠
Перемножение кватернионов соответствует сложению двух последовательных
поворотов вокруг соответствующих осей: r = p o q , причем p o q ≠ q o p , т.е перемножение кватернионов некоммутативно, o - знак кватернионного умножения).
Правила перемножения кватернионов
r0 = p0 q0 − p1q1 − p2 q2 − p3q3 ,
r1 = p1q0 + p0 q1 − p3q2 + p2 q3 ,
r2 = p2 q0 + p3q1 + p0 q2 − p1q3 ,
(п.1.1а)
r3 = p3q0 − p2 q1 + p1q2 + p0 q3 .
Правила кватернионного умножения:
(q1 o q 2 ) o q 3 = q1 o (q 2 o q 3 ) ;
1oq = q o1 = q;
q o q −1 = q −1 o q = 1 = (1 0 0 0) ;
( q1 + q 2 ) o q 3 = q 1 o q 3 + q 2 o q 3 .
В случае n поворотов, задаваемых кватернионами Λ1 , Λ 2 ,..., Λ n , формула
сложения поворотов имеет вид
(п1.2)
Λ = Λ n o Λ n −1 o ... o Λ1 .
Эта формула справедлива, когда кватернионы составляющих поворотов
Λ1 , Λ 2 ,..., Λ n заданы своими компонентами в одном и том же базисе. В этом
случае компоненты результирующего кватерниона будут в этом же базисе.
Сопряженный кватернион:
→
q* = (q0 , − q ) = (q0 , − q1i, − q2 j, − q3k ) = (q0 – q1 – q2 − q3 ) .
Квадрат кватернионной нормы:
ПРИЛОЖЕНИЕ 1
160
→
3
→
q = qq* = q0 2 + q • q =| q 2 |= ∑ qi2 .
2
i =0
Модуль кватерниона
q =
3
2
i =0
i
∑q
.
Обратный кватернион
q*
q =
.
q
−1
и для нормированного кватерниона c нормой q = 1 ,
q −1 = q* ,
что значительно проще, чем вычисление обратной матрицы.
Любой скаляр можно представить как кватернион:
q = (s 0 0 0) = (s,0) .
Любой трехмерный вектор можно представить как кватернион:
→
q = (0 q1 q2 q3 ) = (0, q ) .
Кватернионная матрица [q] или ( q×) :
⎡q
[ q] = ⎢ r0
⎣q
⎡ q0
r
⎢
− q T ⎤ ⎢ q1
r ⎥=
q0 I + [q ×]⎦ ⎢ q2
⎢q
⎣ 3
− q1
− q2
q0
q3
− q2
− q3
q0
q1
− q3 ⎤
q2 ⎥
⎥.
− q1 ⎥
q0 ⎥⎦
r
r
В этом выражении I - единичная матрица, [q ×] - вектор q в матричной
форме, Т – знак транспонирования.
Повороту системы b относительно сопровождающего базиса g на углы
курса ψ , тангажа ϑ , крена γ соответствует кватернион Λgb . Этот кватернион
может быть получен перемножением кватернионов поворота на углы ψ ,ϑ , γ .
Учитывая правила перемножения ортов
r
r r
r r r
r r
i 2 = −1,
i × j = − j × i1 = k ,
i1 × i1 = 0,
r r
r r
r r r
r2
k × i = −i × k = j ,
i2 × i2 = 0,
j = −1,
r r r
r r
r r
r2
i3 × i3 = 0;
j × k = −k × j = i ;
k = −1,
ψ r ψ
повороту на угол ψ соответствует кватернион M = cos − k sin (вектор уг2
2
r
r
ловой скорости ψ& направлен против орта k ),
ПРИЛОЖЕНИЕ 1
161
r ϑ
+ i sin ,
2
2
r
γ
γ
повороту на угол γ соответствует кватернион P = cos + j sin .
2
2
Выполнив, в соответствии с (п1.2), перемножение кватернионов
r
r
r
Λgb = P o N o M , получим параметры кватерниона Λgb = λ0 + i λ1 + j λ2 + k λ3
повороту на угол ϑ соответствует кватернион
λ0 = cos
λ1 = cos
ψ
2
ψ
λ2 = cos
λ3 = cos
2
ψ
2
ψ
2
cos
sin
ϑ
2
ϑ
2
cos
sin
cos
ϑ
2
ϑ
2
cos
2
γ
2
sin
sin
γ
γ
2
γ
2
N = cos
+ sin
+ sin
2
ψ
2
− sin
− sin
ψ
ψ
2
ψ
2
ϑ
γ
ϑ
γ
ϑ
γ
sin
cos
sin
cos
ϑ
sin ,
2
2
sin ,
2
2
(п1.3)
cos ,
2
2
ϑ
2
cos
γ
2
.
Эти соотношения используют для перехода от углов Эйлера-Крылова к параметрам Родрига-Гамильтона.
Матрица направляющих косинусов через параметры кватерниона может
быть представлена в следующем виде
C gb
ξ
η
ζ
xc
yc
zc
λ20 + λ12 − λ22 − λ23 2( −λ0λ3 + λ1λ2 ) 2(λ1λ3 + λ0λ2 )
2(λ0λ3 + λ1λ2 ) λ20 − λ12 + λ22 − λ23 2( −λ0λ1 + λ2λ3 )
2(λ1λ3 − λ0λ2 )
2(λ0λ1 + λ2λ3 ) λ20 − λ12 − λ22 + λ23
(п1.4)
Можно, используя (п1.1), записать
λ20 + λ12 − λ22 − λ23 = 2λ20 + 2λ12 − 1,
λ20 − λ12 + λ22 − λ23 = 2λ20 + 2λ22 − 1,
λ20 − λ12 − λ22 + λ23 = 2λ20 + 2λ23 − 1.
В соответствии с (2.4) из матрицы (п1.4)
⎛ c
⎛c ⎞
⎛ c12 ⎞
32
⎟⎟, γ = − arctg ⎜⎜ 31 ⎟⎟, ϑ = arcsin( c32 ) или ϑ = arctg ⎜
2
⎜
⎝ c22 ⎠
⎝ c33 ⎠
⎝ 1 − с32
ψ = arctg ⎜⎜
значит
⎛ 2( −λ0λ3 + λ1λ2 ) ⎞
⎛ 2( λ1λ3 − λ0λ2 ) ⎞
⎟ , γ = −arctg ⎜
⎟
2
2
2
2⎟
⎜ λ2 − λ2 − λ2 + λ2 ⎟ ,
−
+
−
λ
λ
λ
λ
2
3⎠
2
3⎠
⎝ 0 1
⎝ 0 1
ψ = arctg ⎜⎜
⎞
⎟,
⎟
⎠
ПРИЛОЖЕНИЕ 1
162
⎛
⎞
⎟
(п1.5)
⎜ 1 − 4( λ λ + λ λ )2 ⎟
0 1
2 3
⎠
⎝
Параметры кватерниона можно выразить через направляющие косинусы [26]:
1
1
λ0 = ± 1 + c11 + c22 + c33 , λ1 = ±
( c32 − c23 ),
4λ0
2
ϑ = arcsin( 2( λ0λ1 + λ2λ3 )) или ϑ = arctg ⎜
2( λ0λ1 + λ2λ3 )
1
1
( c13 − c31 ), λ3 = ±
( c21 − c12 ).
4λ0
4λ0
Дифференциальное уравнение, описывающее изменение параметров Родрига-Гамильтона в зависимости от проекций абсолютной угловой скоростей
ω x , ω y , ωz и проекций переносной угловой скорости ωξ , ωη , ωζ , может быть записано, как и обобщенное уравнение Пуассона, в виде [13]
λ2 = ±
& gb = 1 (( Λgb ×)ω − (ω ×)Λgb ),
Λ
b
g
2
ω zc ]T - кватернион , соответствующий вектору угловой
где ωb = [0 ω xc ω yc
r
скорости ωb , а ω g = [0 ωξ ωη ωζ ]T - кватернион , соответствующий вектоr
ру угловой скорости ω g , ( Λgb ×) , (ω g ×) - кватернионные матрицы.
В векторно-матричной форме это выражение можно записать, как
⎡λ&0 ⎤ ⎡λ0
⎢&⎥ ⎢
λ
λ
2⋅⎢ 1⎥ = ⎢ 1
⎢λ&2 ⎥ ⎢λ2
⎢& ⎥ ⎢
⎣λ3 ⎦ ⎣λ3
− λ1
− λ2
λ0
λ3
− λ2
− λ3
λ0
λ1
− λ3 ⎤ ⎡ 0 ⎤ ⎡ 0
λ2 ⎥ ⎢ω xc ⎥ ⎢⎢ωξ
⎥⋅⎢
⎥−
− λ1 ⎥ ⎢ω yc ⎥ ⎢ωη
⎢
λ0 ⎥⎦ ⎢⎣ωzc ⎥⎦ ⎣ωζ
− ωξ
− ωη
0
− ωζ
ωζ
− ωη
0
− ωη
− ωζ ⎤ ⎡ ⎡λ0 ⎤ ⎤
ωη ⎥ ⎢ ⎢ λ1 ⎥ ⎥
⎥ ⋅ ⎢⎢ ⎥⎥ ,
− ωξ ⎥ ⎢ ⎢λ2 ⎥ ⎥
0 ⎥⎦ ⎢⎣ ⎢⎣λ3 ⎥⎦ ⎥⎦
откуда можно получить
⎡λ&0 ⎤
⎡ 0
⎢&⎥
⎢ω − ω
ξ
⎢ λ1 ⎥ = 1 ⎢ x
⎢λ&2 ⎥ 2 ⎢ω y − ωη
⎢& ⎥
⎢ω − ω
ζ
⎣ λ3 ⎦
⎣ z
− (ω x − ωξ ) − (ω y − ωη ) − (ω z − ωζ )⎤ ⎡λ0 ⎤
− (ω y + ωη )⎥ ⎢ λ1 ⎥
ω z + ωζ
0
⎥⋅⎢ ⎥.
− (ω z + ωζ )
ω x + ωξ ⎥ ⎢λ2 ⎥
0
⎥ ⎢ ⎥
− (ω x + ωξ )
ω y + ωη
0
⎦ ⎣λ3 ⎦
(п1.6)
Если обозначить матрицу угловых скоростей Ω , то уравнение можно записать в компактной векторно-матричной форме
& gb = 1 Ω ⋅ Λgb .
Λ
2
Если в уравнении (п1.6) пренебречь проекциями переносной угловой скорости, получим уравнение, описывающее изменение кватерниона в инерциальной системе координат.
Точность определения, СКО
Время выставки
1,25
-
0,9
0,9
БИНС-СП
КЛГ
СБКВ-2
ДНГ
БИНС-ТВГ
ТВГ
БИНС-Т
КЛГ
0,5
0,5
0,6
1,0
1,0
3-5
3-5
60
6
6
2-3
1-2
15
33
2-4 2-4
0,5
0,5
1
1
3
4
3
-
5
5
9
10
8
18
15
70
70
180
180
80
60
5 000
15 000
1 000
6 000
10 000
Масса Потребляе
Наработка
мая
системы, мощность,
на отказ,
кг
час
Вт
РПКБ
РПКБ
РПКБ
АВИА
ПРИБОР
МИЭА
Фирма
Обозначения: КЛГ – кольцевой лазерный гироскоп, ДНГ – динамически настраиваемый гироскоп, ТВГ – твердотельный волновой гироскоп,
МИЭА – Московский институт электромеханики и автоматики, РПКБ – Раменское приборостроительное конструкторское бюро
1,85
Режим
Режим
Коорди- Скорость, Курс, Тангаж,
заданного
гирокомпа
Крен
нати, км
м/с
угл. мин угл.
курса, сирования,
мин
мин
мин
БИНС-90
КЛГ
Шифр системы
Тип гироскопа
Технические характеристики некоторых российских БИНС
ПРИЛОЖЕНИЕ 2.
163
Точность определения
Время виставки
1,5
за 1 час
1,8
за 1 час
0,25
за 1 час
Н-423
ЛГ GG-1300
LINS-300
ЛГ
Sigma 40
ЛГ
0,3
0,76
1,0
ИИБ
8,2
Всего 23
25
6
4
8
3⋅ sec ϕ
(ско)
0,5
10
ИИБ
<20 кг
1
(ско)
2
70
150
140
30
20000
2000
3100
Наработ
Масса Потребляе
мая
ка
системы, мощность, на отказ,
кг
Вт
час
4
4
Обозначения: ЛГ –лазерный гироскоп, ВОГ –волоконно-оптический гироскоп
1,8
Режим
Режим
Коорди- Скорость, Курс, Тангаж,
заданного
гирокомпа
Крен
нати, км
м/с
угл. мин угл.
курса, сирования,
мин
мин
мин
SINS-1000
ВОГ
Шифр системы
Тип гироскопа
Технические характеристики некоторых зарубежных БИНС
SAGEM
(Франция)
British
Aerospace
(Великобритания)
Honeywell
(USA)
Vibtel
Industriel
Фирма
164