Исследование алгоритмов навигации и ориентации.

Оглавление
Введение	7
1 Исследование алгоритмов навигации и ориентации	8
1.1	Анализ аппаратной части бесплатформенной  инерционной навигационной системы	8
1.2	Описание модели Земли и системы координат	9
1.3	Описание объекта управления	10
1.4	Алгоритмы навигации и ориентации	11
1.4.1	Навигационный алгоритм	12
1.4.2	Матрица направляющих косинусы	16
1.4.3	Параметры ориентации Родрига-Гамильтона	20
1.5	Алгоритм навигации и ориентации, использующий промежуточные параметры ориентации	23
1.6	Калибровка инерциальных чувствительных элементов с помощью фильтра Калмана	25
2 Моделирование траектории полета и бесплатформенной инерционной навигационной системы	27
2.1	Моделирование траектории полета объекта управления	27
2.2	Моделирование алгоритма ориентации и навигации на основе БИНС	30
3 Программная реализация алгоритма ориентации и навигации	37
4 Безопасность жизнедеятельности	40
4.1	Анализ опасных и вредных производственных факторов	40
4.2	Требования к помещениям для работы с ПЭВМ	41
4.3	Освещение рабочей зоны	42
4.4	Микроклимат рабочей зоны	43
4.5	Меры защиты от поражения электрическим током	44
4.6	Пожарная безопасность	45
4.7	Организация рабочего места	46
4.8	Организация режима труда и отдыха рабочего	48
5 Организационно-экономический раздел	49
5.1	Сетевое планирование	49
5.2	Построение сетевого графика	50
5.3	Расчет параметров событий сетевого графика	51
5.4	Расчет параметров работ сетевого графика	53
5.5	Расчет трудоемкости работ исполнителей	55
5.6	Определение затрат а выполнение работ	56
5.6.1	Материалы и комплектующие изделия	56
5.6.2	Основная заработная плата работ	56
5.6.3	Дополнительная заработная плата исполнителей работ	57
5.6.4	Отчисления на социальные нужды и обеспечение	57
5.6.5	Расходы на электричество	57
5.6.6	Накладные расходы	57
5.6.7	Смета затрат	57
5.7	Экономический эффект	58
Заключение	59
библиографический список	61
Приложение А	63
Приложение Б	65
Приложение В	67
Приложение Г	70

Введение
В настоящие время всё более широкое распространение получают бесплатформенные инерционные навигационные системы (БИНС) в связи с бурным развитием микроэлектроники процессорной техники. Как известно, в БИНС блок чувствительных элементов жестко крепится к корпусу, поэтому задача определения навигационного базиса решается алгоритмически и реализуется в бортовом вычислители математически. Одна из проблем, возникших при разработке БИНС – построение оптимальных алгоритмов работы бортового вычислителя.
Цель выполнения дипломной работы:программная реализация алгоритма ориентации и навигации для БИНС управляемой ракеты-мишени.
Для выполнения цели дипломной работы необходимо решить следующие задачи:
     моделирование траектории полёта ракеты-мишенипо кинематическим уравнениям;
     анализ и выбор алгоритма ориентации и навигации (рассматриваются алгоритмы в параметрах Родрига-Гамильтона, направляющих косинусах);
     моделирование алгоритма ориентации и навигации в программе прикладных пакетов; 
     управление траекторией ЛА по сигналам БИНС;
    Для решения приведенных выше задач выполняется обзор литературы отечественных авторов. В качестве основной книги по вопросам ориентации и навигации БИНС используется «Основы построения бесплатформенных инерциальных навигационных систем» В. В. Матвеева.

1 Исследование алгоритмов навигации и ориентации
Анализ аппаратной части бесплатформенной  инерционной навигационной системы
Бесплатформенная инерциальная навигационная система (БИНС) представляет собой блок чувствительных элементов (БЧЭ) и бортовой вычислитель.
Блок чувствительных элементов представляет собой конструкции состоящую из трех функционально законченных модулей: блока датчиков угловых скоростей, модуля информационного обмена (МИО-БЧЭ) и блока акселерометров БА.
Построенный на лазерных гироскопах БДУС-ЛГ блок датчиков угловых скоростей обеспечивает измерение и выдачу в цифровом виде приращений угла поворота вокруг трех ортогональных осей связанной системы координат (ССК) согласно ГОСТ 20058-80.[1]
Модель информационного обмена считывает данные о параметрах движения объекта с датчиков первичной информации (БДУС-ЛГ и БА), алгоритмически компенсирует неортогональности установки датчиков первичной информации относительно связанной системы координат БЧЭ, формирует массив данных, который по мультиплексному каналу информационного обмена (интерфейс по ГОСТ Р 52070-2003) считывает внешний потребитель. Такт съема информации с БЧЭ не менее 10 мс (100±0,1 Гц).[7]
Построенный на микромеханических акселерометрах блок акселерометров обеспечивает измерение и выдачу в цифровом виде приращений линейной скорости по трем взаимно-перпендикулярным осям ССК.
Выходными параметрами БИНС являются: местоположение (широта ?, долгота ?, высота h), угловая ориентация ( углы рыскания?, тангажа ?, крена ?) и скорость ЛА относительно нормальной земной системы координат (НЗСК) согласно ГОСТ-20058-80. Частота формальных выходных параметров БИНС должна быть не менее 50 Гц.

Описание модели Земли и системы координат
    Введем прямоугольную систему координат ОXYZв качестве связанной системы координат БЧЭ. Ось OX(продольная ось) направлена по направлению движения объекта и расположена в плоскости симметрии БЧЭ. Ось ОY(нормальная ось) направленна направлена к верхней части БЧЭ и расположена в плоскости симметрии БЧЭ перпендикулярно оси ОX. Ось ОZ(поперечная ось) направлена вправо от плоскости ОXYи расположена перпендикулярно к плоскости ОXY.[4]
    Вычисления производятся в нормальной системе координат (по ГОСТ 20058-80) с вершиной в центре масс объекта, оси которой направлены по сторонам света, точкаО перемещается вместе с объектом, поэтому такая система координат называется так же географическим сопровождающим трехгранником или географической. Ось  выбранной системы направлена по касательной к меридиану на север, ось  - вдоль вертикали места вверх, ось - по касательной к параллели места на восток.
    В качестве модели Земли при навигации в околоземном пространстве принимается эллипсоид вращения, ось симметрии которого совпадает с осью вращения Земли. Данный эллипсоид является простой и достаточно близкой моделью поверхности Земли при соответствующем выборе его параметров. В отечественной науке приняты параметры, полученные Ф. Н. Красовским:
     радиус земного экватора (большая полуось) a = 6 378 245 м;
     малая полуось ( по оси вращения Земли) b = 6 356 863 м;
     квадрат первого эксцентриситета  = 0.006 692.[6]
Движение Земли сводится к равномерному вращению во круг оси эллипсоида при осуществлении автономной инерциальной навигации. Примем размер угловой скорости Земли постоянным и равным U = 15,0507=7,292116. Ускорение силы тяжести = 9.78049 .
Обязательными параметрами при проектировании инерционной системы, считаются параметры гравитационного поля Земли. Примем, что
     вектор действия силы тяжести совпадает с нормальной земной поверхностью;
     модуль ускорения силы тяжести на земном сфероиде вычисляется по формуле Гельмерта-Кассиниса:
		(1.1)
    

Описание объекта управления
В качестве объекта управления примем мишенный ракетный комплекс «Кабан» (96М6М). Мишень 96М6М представляет собой неуправляемую жидкотоплевную ракету, которая по своим летно-техническим характеристикам на нисходящем участке траектории полета имитирует высокоскоростную баллистическую ракету. Мишень состоит из головного обтекателя, приборного отсека и жидкотопливного двигателя. В приборном отсеке размещен приемопередатчик, позволяющий осуществлять контроль траектории полета ракеты наземным радиолокационным комплексом "Кама". Путем изменения угла пуска и скорости полета ракеты-мишени имеется возможность менять траекторию полета и имитировать различные типы баллистических целей.
В рамках ВКР разработаем систему управления мишени по углу атаки и тяге. Основные характеристики ракеты приведем в таблице 1:
Таблица 1.3.1 – Основные характеристики объекта управления
M, кг
Mk, кг
Sm, 
Cy0
Cx0
Cx2
Cy1
Ip, сек
msec,
330
147.6
0.049
0.04
0.01
0.7
0.6
221
31.05
Параметры атмосферы:
, 
Таблица 1.3.2 – Начальные условия
V(0), м/сек
,град
H(0),м
X(0),м
45.0
1.2…1.57
0
0
где
М – масса объекта управления;
Mk – масса топлива;
Sm – площадь миделя корпуса ракеты;
Cy – коэффициент подъемной силы;
Cx – коэффициент лобового сопротивления;
Cx2 – коэффициент лобового сопротивления при ?=0;
Cy1 – производная от коэффициента подъёмной силы по углу атаки при ?=0 ;
Ip– удельный импульс ракетного двигателя;
msec – секундный расход воздуха и горючего (керосина) через двигатель;
V(0) – скорость движения объекта управления в начальный момент времени;
– угол тангажа в начальный момент времени;
H(0) – высота объекта управления в начальный момент времени;
X(0) – координата объекта управления в начальный момент времени;

Алгоритмы навигации и ориентации
Для описания ориентации и местоположения ЛА в задачах приземной навигации будем использовать инерциальную географическую прямоугольную экваториальную систему координат. Начало – центр Земли (О), основная плоскость – плоскость экватора. Географические координаты ракеты-мишени по рассматриваемому алгоритму БИНС, определяющему широту ?, долготу ?, высоту h, углы ориентации курса?, тангажа ?, крена ?, а так же северную, восточную и вертикальную проекции его относительно скорости. Долготу определяют двухгранным углом между плоскостью меридиана, проходящего через точку О, и плоскостью Гринвичского меридиана. Географическая широта определяется углом между нормалью к поверхности сфероида, проходящего через точку местоположения объекта, и плоскостью экватора Земли. Географические координаты точки изображены на рисунке 1.1.[4]

Рисунок 1.1 – Географические координаты точки
Алгоритм ориентации и навигации БИНС структурно можно разделить на две взаимозависимые части: алгоритм определения параметров ориентации, вычисляющий матрицы ориентации чувствительных элементов в пространстве и углы ориентации ЛА, и навигационный алгоритм, определяющий проекции скоростей и координаты. Обобщенная функциональная схема ориентации и навигации БИНС изображена на рисунке 1.2.[4]

Рисунок 1.2 – Обобщенная функциональная схема ориентации и навигации БИНС [1]
При синтезе алгоритмического обеспечения используют различные кинематические параметры: матрицы направляющих косинусов, параметры Родрига-Гамильтона – кватернионы, вектор Эйлера. 
Будем рассматривать только навигационный алгоритм и алгоритм ориентации для направляющих косинусов и кватернионах, так как наш объект управления во время полета может отробатывать углы более 90°, а углы Эйлера в данной ситуации бесполезны. [8]
Навигационный алгоритм
Пусть точкаОперемещается с линейной скоростью V относительно поверхности Земли. Обозначим проекции вектора Vна оси  следующим образом: . При движении ЛА вдоль параллели, широта ? остается неизменной, а географическая система координат поворачивается вокруг оси вращения Земли с угловой скоростью . Иллюстрация угловых скоростей  и  изображены на рисунка 1.3 (а) и 1.3 (б).

Рисунок 1.3 – Иллюстрация угловых скоростей  (а) и  (б)
Проекции вектора линейной скорости на оси географической системы координат имеют вид:
	,	(1.2)
где  - горизонтальная составляющая скорости ЛА; К – курсовой угол, т.е. угол между вектором  и направлением на север.
Абсолютная угловая скорость трехгранника скалывается из переносной угловой скорости, обусловленной вращением Земли вокруг своей оси, и относительной скорости вращения трехгранника вследствие перемещения объекта вдоль земной поверхности. Вектор угловой скорости Земли Uимеет следующие проекции на оси географической системы координат:
		(1.3)
где , - горизонтальная и вертикальная составляющая угловой скорости Земли.
Вертикальная составляющая угловой скорости Земли  приводит к вращению плоскости горизонта против часовой стрелки вокруг местной вертикали, если наблюдать за вращением Земли с положительного направления . Горизонтальная составляющая уголовной скорости Земли приводит к вращению плоскости горизонта, если наблюдать вращение с положительного направления оси , то западная часть плоскости будет подниматься, а восточная часть плоскости горизонта – опускаться.
При движении объекта вдоль параллели, географическая система координат поворачивается вокруг оси вращения Земли со скоростью , а широта ? остается неизменной.
	? ?=V_(Z_g )/(R_1 cos?),	(1.4)
где ? R?_1=a/?(1-e^2 ?sin?^2 ?)   - радиус кривизны земного эллипсоида [3].
Перемещая объект в плоскости меридиана, тогда долгота останется неизменной, а приращение широты в единицу времени можно считать угловую скорость вращения трехгранника вокруг оси (рисунок 1.3, б).
Ориентация угловой скорости направлена в отрицательную сторону оси , т.е. на запад, а модуль его равен
	,	(1.5)
где  - радиус кривизны земного эллипсоида.[3]
Тогда, исходя из рисунка 1.1 и 1.3, проекции абсолютной угловой скорости географического трехгранника на его оси равны
		(1.6)
Подставим правые части соотношений 1.4 и 1.5 в 1.6, получим
		(1.7)
Абсолютное ускорение движущейся вершины трехгранника на его ребра складывается из следующих составляющих:
		(1.8)
где  - переносное ускорение, - относительное ускорение, - кориолисово ускорение.
Теперь найдем каждую составляющую абсолютного ускорения.
     - кориолисово ускорение вызвано переносной угловой скоростью вращения Земли и линейной относительной скоростью объекта управления и выражается векторным произведением
		,	(1.9)
где i,j,k– орты осей системы координат .[10]
Вычисляя определитель 1.9, находим проекции кориолисова ускорения на оси :
		?(a_(X_g)^c=2UV_(Z_g ) sin?;@a_(Y_g)^c=-2UV_(Z_g ) cos?;@a_(Z_g)^c=2(V_(Y_g ) Ucos?-V_(X_g ) Usin?).)	(1.10)
     - относительное ускорениевызвано изменением относительной линейной скорости и движением объекта вдоль сферической поверхности Земли с относительной угловой скоростью ?^'=? ?cos?i+? ?sin?j-? ?k:
		a^r=V ?^r+?^'?V^r=V ?^r+|?(i&j&k@V_(Z_g )/R_1 &V_(Z_g )/R_1  tg?&-V_(X_g )/R_2 @V_(X_g )&V_(Y_g )&V_(Z_g ) )|.	(1.11)
Из (1.11) найдем проекции относительного ускорения на оси географической системы координат:
	?(a_(X_g)^r=V ?_(X_g )+?V^2?_(Z_g )/R_1  tg?+(V_(X_g ) V_(Y_g ))/R_2 ,@a_(Y_g)^r=V ?_(Y_g )-?V^2?_(Z_g )/R_1 -?V^2?_(X_g )/R_2 ,@a_(Z_g)^r=V ?_(Z_g )+(V_(Z_g ) V_(Y_g ))/R_1 -(V_(X_g ) V_(Z_g ))/R_1  tg?.)	(1.12)
      - переносное ускорениевызвано угловой скоростью вращения Земли, определяется:
		a^e=U?(U?R).	(1.13)
    Тогда
    		?(a_(X_g)^e=U^2 Rsin?cos?;@a_(Y_g)^e=?-U?^2 R?cos?^2 ?;@a_(Z_g)^e=0.)	(1.14)
    Если сложить вектор переносного ускорения a^e и вектор гравитационного поля земли g, то получится вектор (0;g;0).[10]
    Для получения проекций вектора кажущегося ускорения вершины трехгранника  на его оси, сложим соответствующие компоненты вектора ускорения силы тяжести с соответствующими кориолисова (1.10) и относительного (1.12) ускорений, кажущееся ускорение,которого измеряют акселерометры:
    		?(n_(X_g )=V ?_(X_g )+?V^2?_(Z_g )/R_1  tg?+(V_(X_g ) V_(Y_g ))/R_2 +2UV_(Z_g ) sin?;@n_(Y_g )=V ?_(Y_g )-?V^2?_(Z_g )/R_1 -?V^2?_(X_g )/R_2 -2UV_(Z_g ) cos?+g;@n_(Z_g )=V ?_(Z_g )+(V_(Z_g ) V_(Y_g ))/R_1 -(V_(X_g ) V_(Z_g ))/R_1  tg?+2(V_(Y_g ) Ucos?-V_(X_g ) Usin?).)	(1.15)
    Ускорения Кориолиса и ускорение, связанные с криволинейностью движения ЛА вдоль сферической поверхности Земли, должны быть скомпенсированы, так как при навигации вдоль поверхности Земли, нужно что бы не вход первых интегралов подавались только ускорения относительного движения . Из выражения (1.15) можно сделать вывод, что «вредные» (компенсирующие) составляющие ускорения равны:
    		?(?a^k?_(X_g )=?V^2?_(Z_g )/R_1  tg?+(V_(X_g ) V_(Y_g ))/R_2 +2UV_(Z_g ) sin?;@?a^k?_(Y_g )=-?V^2?_(Z_g )/R_1 -?V^2?_(X_g )/R_2 -2UV_(Z_g ) cos?+g;@?a^k?_(Z_g )=(V_(Z_g ) V_(Y_g ))/R_1 -(V_(X_g ) V_(Z_g ))/R_1  tg?+2(V_(Y_g ) Ucos?-V_(X_g ) Usin?).)	(1.16)
    Вычтем из (1.15) уравнение (1.16), получим:
    			?(n_(X_g )-?a^k?_(X_g )=V ?_(X_g );@n_(Y_g )-?a^k?_(Y_g )=V ?_(Y_g );@n_(Z_g )-?a^k?_(Z_g )=V ?_(Z_g ),)	(1.17)
где n_(X_g ),n_(Y_g ),n_(Z_g )–проекции вектора кажущегося ускорения на оси географической системы координат.
    С учетом ввода начальных скоростей V_(X_g ) ?(t_0 ),V?_(Y_g ) (t_0),V_(Z_g ) (t_0)составляющие относительной скорости движения ЛА образуется после интегрирования ускорений (1.17):
    		?(?V_(X_g )=V?_(X_g ) (t_0 )+?_(t_0)^t??(n_(X_g )-?a^k?_(X_g ) )dt=V_(X_g ) (t_0 )+?_(t_0)^t?(V ?_(X_g ) )dt?,@V_(Y_g )=V_(Y_g ) (t_0 )+?_(t_0)^t??(n_(Y_g )-?a^k?_(Y_g ) )dt=V_(Y_g ) (t_0 )+?_(t_0)^t??(V ?_(Y_g ) )dt,??@V_(Z_g ) ?=V?_(Z_g ) (t_0)+?_(t_0)^t??(n_(Z_g )-?a^k?_(Z_g ) )dt?=V?_(Z_g ) (t_0)+?_(t_0)^t??(V ?_(Z_g ) )dt.??)	(1.18)
    С учетом начальных значений высоты и координат h(t_0 ), ?(t_0 ),?(t_0)cпомощью вторичного интегрирования определяются координаты  местоположения ЛА:
    			?(?=?(t_0 )+?_(t_0)^t??(V_(X_g )/R_2 )dt,?@?=?(t_0 )+?_(t_0)^t??(V_(Z_g )/(R_1 cos?))dt,?@h=h(t_0 )+?_(t_0)^t??(V_(Y_g ) )dt.?)	(1.19)
    Следовательно, выражения (1.18) и (1.19) реализуют навигационный алгоритм БИНС, определяющий текущие координаты местоположения объекта и его скорость.
    
    Матрица направляющих косинусы
Рассмотрим решение задачи определения углов ориентации ЛА относительно географической системы координат. Определим углы с помощью взаимного положения связанной и географической системы координат изображенной на рисунке 1.4.

Рисунок 1.4 –Взаимное положение связанной и географической системы координат [1]
Введем в рассмотрение трехгранник OXYZ,оси которого направлены вдоль строительных осей ЛА. Ось OX направлена к носу ЛА и совпадает с его поперечной осью, ось OY направлена вверх и перпендикулярна плоскости крыльев, ось OZнаправлена в сторону правого крыла и совпадает с поперечной осью ЛА.[4]
Связанная система координат (строительные оси ЛА) определяется относительно географического трехгранника тремя углами, которые называются углами рыскания (курса), тангажа и крена. Углы ориентации ?,?,?называют самолетными углами или углами Эйлера – Крылова. Угол курса ? – угол, отсчитываемый в горизонтальной плоскости OX_g Z_gот оси OX_g против хода часовой стрелки до проекции продольной оси объекта на эту плоскость. Угол тангажа ? – угол, отсчитываемый в вертикальной плоскости между осью OXи ее проекцией на горизонтальную плоскость. Угол крена ? – угол, отсчитываемый в поперечной плоскости между осью OZ и линией пересечения этой плоскости с плоскостью горизонта. Определим матрицы направляющих косинусов при переходе от OX_g Y_g Z_g(географической системы координат) к связанной OXYZ.
Изначально трехгранники OX_g Y_g Z_gи OXYZ совпадали. Далее произошли три последовательных поворота, опишем эти повороты:
     поворот трехгранника OXYZ вокруг оси OY_g на угол ? против хода часовой стрелки, если заменять за этим поворотом с положения конца OY_g. После первого поворота трехгранник OXYZзанимает промежуточное положение OX’Y’Z’.
     поворот на угол ?происходит относительно промежуточной оси OZ’ против хода часовой стрелки.После второго поворота трехгранник OX’Y’Z’занимает промежуточное положение OX’’Y’’Z’’.
     поворот на угол ? вокруг продольной оси ЛА, оси OX’’.
Полная цепочка преобразований выглядит так:

Стрелка в этой записи показывает, из какого положения в какое переходит трехгранник. Над стрелкой показаны совпадающие оси, вокруг которых совершается конечный поворот, а под стрелкой – угол поворота.
	[XYZ]=A_? [X^'' Y^'' Z^'' ]=A_? A_? [X^' Y^' Z^' ]=A_? A_? A_? [X_g Y_g Z_g ].	(1.20)
Матрица А – исходная матрица преобразования от трехгранника OX_g Y_g Z_g к OXYZ, примем, что ?A=A?_? A_? A_?
	A=??(cos?cos?&sin?&-cos?sin?@-cos?cos?sin?+sin?sin?&cos?cos?&cos?sin?sin?+sin?cos?@sin?cos?sin?+cos?sin?&-sin?cos?&-sin?sin?sin?+cos?cos?)?.	(1.21)
    Наш объект управления – ракета-мишень, она является беспилотным летательным аппаратом, в алгоритмах именно таких ЛА навигация осуществляется преобразованием информации из связанного трехгранника в географическую систему координат, то рассмотрим обратную матрицу
    [X_g Y_g Z_g ]=A^(-1) [XYZ].
    Так как матрицы направляющих косинусов являются ортогональными, то для них выполняется условие:
    	A^(-1)=A^T.	(1.22)
    Пусть матрица ?C=A?^T, которая из выражения (1.21) имеет вид:
    	С=??(cos?cos?&-cos?cos?sin?+sin?sin?&sin?cos?sin?+cos?sin?@sin?&cos?cos?&-sin?cos?@-cos?sin?&cos?sin?sin?+sin?cos?&-sin?sin?sin?+cos?cos?)?.	(1.23)
    Следовательно матрицаС осуществляет переход от связанного трехгранника OXYZк  географическому ?OX?_g Y_g Z_g.
    [X_g Y_g Z_g ]=C[XYZ].
    Пусть матрица (1.23) известна, тогда углы рыскания, тангажа и крена определяются через ее элементы:
    		?(?=arctg(-c_31/c_11 );@?=arctgc_21;@?=arctg(-c_23/c_22 ).)	(1.24)
    
    Для получения матрицы C необходимо решить обобщенное уравнение Пуассона:
    		C ?=C[?]-[?_g ]C;	(1.25)
где [?]=[?(0&?-??_z&?_y@?_z&0&?-??_x@?-??_y&?_x&0)] – кососимметрическая матрица, соответствующая проекциям вектора абсолютной угловой скорости связанного трехгранника на свои оси, [?_g ]=[?(0&?-??_(Z_g )&?_(Y_g )@?_(Z_g )&0&?-??_(X_g )@?-??_(Y_g )&?_(X_g )&0)]–кососимметрическая матрица, составленная из проекций вектора абсолютной угловой скорости географического трехгранника на свои оси.[4]
    На рисунке 1.5 покажем блок-схему алгоритма БИНС с использованием обобщенного уравнения Пуассона:
    
    Рисунок 1.5 – БИНС, основанная на решении обобщенного уравнения Пуассона в направляющих косинусах
    Описание работы алгоритма:
Блок акселерометров передает проекции вектора кажущегося ускоренияnX, nY, nZ на ребра связанного трехгранника. Выполняется пересчет проекций кажущегося ускорения n_(X_g ),n_(Y_g ),n_(Z_g ) на оси географической системы координат, с помощью матрицы С. Из составляющихn_(X_g ),n_(Y_g ),n_(Z_g ) исключаются компенсирующие ускорения ?a^k?_(X_g ),?a^k?_(Y_g ),?a^k?_(Z_g ), в результате чего образуются ускорения относительногодвижения ЛА. Двукратное интегрированиеV ?_(X_g ),V ?_(Y_g ),V ?_(Z_g ) помогает определить координаты местоположения и скорости объекта управления. Поинформации об угловых скоростях ? ?, ? ?, широте ? и угловой скорости ЗемлиU высчитываются проекции вектора абсолютной угловой скоростигеографического трехгранника на свои оси. Из составляющихугловой скорости ?_(X_g ),?_(Y_g ),?_(Z_g )формируются вектор ?g и кососимметрическая матрица[?g].
Три ДУС определяют проекции вектора абсолютной угловой скорости ЛА?x, ?y, ?z на ребра связанного трехгранника, из которых формируются вектор ? икососимметрическая матрица[?]. Для получения матрицы C необходимоинтегрировать обобщенное уравнение Пуассона, в состав которого входяткососимметрические  матрицы [?], [?g]. С помощью компонентов найденной матрицы Cрассчитываются углы рыскания ?, тангажа ?, крена ?.
Достоинством алгоритма является то, что уравнения БИНС, записанные с использованием матрицы направляющих косинусов, - линейны, определены для любых углов рыскания, тангажа и крена. К недостаткам можно отнести необходимость решать уравнения Пуассона, имеющие девятый порядок.[1]
    Параметры ориентации Родрига-Гамильтона
Как известно в теории конечных поворотов доказывается, что трехгранник можно перевести из одного положения в другое можно с помощью одного лишь поворота вокруг некоторой оси (ось конечного поворота).[8]
Конечный поворот может быть задан четырьмя параметрами l, m, n,?, которые характеризуют направление оси и угол поворота, изображенного на рисунке 1.6. Первые три параметра – это проекции единичного вектора, направленного вдоль оси конечного поворота. Данным параметрам в соответствие ставятся четыре числа, которые называются параметрами Родрига-Гамильтона:
     	?(p_0=cos ?/2;@p_1=lsin ?/2;@?(p_2=msin ?/2;@p_3=nsin ?/2.))		(1.26)
     
     Рисунок 1.6 – Параметры ориентации Родрига-Гамильтона
Рассмотрим алгоритм построения БИНС с использованием кватернионов. Необходимо выполнить переход от инерционного трехгранника ?OX?_и Y_и Z_и к географическому ?OX?_g Y_g Z_g и от географического к связанному OXYZ. Первый переход обозначим посредством кватерниона K, второй – через ?. Результат перехода от инерциального трехгранника к географическому обозначим через кватернион M:
     	M=K??.	(1.27)
Из кватерниона ?, являющегося аналогом матрицы направляющих косинусовА, можно получить информацию о положении связанного трехгранника относительно географического в каждый момент времени. Именно эта информация необходима для реализации алгоритмов БИНС. Зная информацию об кватернионе ? можно пересчитать кажущиеся ускорения ЛА, измеренное в связанных с объектом управления осях, в географическую систему координат и определить параметры ориентации ?, ?, ?. Указанные параметры ориентации можно выразить из параметров Родрига-Гамильтона следующим образом:
     ?(?=arctg((2?_1 ?_3-2?_0 ?_2)/(2?_1^2+2?_0^2-1));@?=arcsin?(2?_1 ?_2+2?_0 ?_3 );@?=arctg(-(2?_2 ?_3-2?_0 ?_1)/(2?_0^2+2?_2^2-1)).)	(1.28)
Кватернион ? вычисляется с помощью кватернионного дифференциального кинематического уравнение, оно же является обобщенным уравнением Пуассона:
     2? ?=???-?_g??+?(1+???),	(1.29)
здесь ?(1+???) – корректирующий член для автоматической коррекции нормы кватерниона.[3]
Так как кватернион ? характеризует преобразование географическоготрехгранника в связанный, а в алгоритмах БИНС осуществляется обратныйпересчет информации из связанной системы координат в географическую, то иоперация преобразования гиперкомплексного отображения Ng векторакажущегося ускорения ng также будет обратной:
    	N_g=??N?? ?,	(1.30)
где N_g,N – отображение вектора кажущегося ускорения, заданного в географической и связанной системе координат.[8]
    Схема алгоритма БИНС, основанная на решении уравнения (1.29) представлена на рисунке 1.7:

Рисунок 1.7 – БИНС, основанная на решении одного кинематического уравнения в кватернионах
Описание работы алгоритма:
Блок акселерометров определяет проекции вектора кажущегося ускорения nX, nY, nZ на ребра связанного трехгранника. Пересчет компонент вектора кажущегося ускорения из связанного трехгранника в географический осуществляется с помощью кватерниона ?. В результате пересчета образуются проекции кажущегося ускорения n_(X_g ),n_(Y_g ),n_(Z_g )на оси географической системы координат. Из этих составляющих n_(X_g ),n_(Y_g ),n_(Z_g )исключаются компенсирующие ускорения ?a^k?_(X_g ),?a^k?_(Y_g ),?a^k?_(Z_g ), после  чего образуются ускорения относительного движения ЛА V ?_(X_g ),V ?_(Y_g ),V ?_(Z_g ).После двукратногоинтегрирования ускорений V ?_(X_g ),V ?_(Y_g ),V ?_(Z_g )позволяем координаты местоположения и скорости объекта. По информации об угловых скоростях ? ?, ? ?,  широте ? и угловой скорости Земли U вычисляются проекции вектора абсолютной угловой скорости географического трехгранника на свои оси?_(X_g ),?_(Y_g ),?_(Z_g ). Из составляющихугловой скорости формируются отображение ?g и кватернионнаяматрица [?g].
Три ДУС измеряют проекции вектора абсолютной угловой скорости ЛА?x, ?y, ?z на ребра связанного трехгранника, из которых формируютсяотображение ? и кватеринионная матрица[?]. Для получения кватерниона ?необходимо интегрировать кватернионное дифференциальное кинематическоеуравнение Эйлера, в состав которого входят кватернионные матрицы [?], [?g] икорректирующий член для коррекции нормы кватерниона. С помощью найденныхпараметров Родрига-Гамильтона рассчитываются углы рыскания ?, тангажа ?,крена ?.[4]
Преимуществом является то, что кинематические уравнения в параметрах Родрига-Гамильтона – линейные, имеют четвертый порядок и определены для любых углов рыскания, тангажа и крена.[1]
   Алгоритм навигации и ориентации, использующий промежуточные параметры ориентации
Уравнения навигационного алгоритма идентичны уравнениям, описанным в пункте 1.4.1.
В алгоритме ориентации промежуточные параметры вычисляются с помощью матрицы направляющих косинусов, который основан на решении обобщенного уравнения Пуассона:[4]
    		C ?=C[?]-[?_g ]C.	(1.31)
    Пренебрегая вектором угловой скорости географического трехгранника ?_Xg=?_Yg=?_Zg=0. Тогда уравнение (1.31) примет вид
    		C ?=C[?].	(1.32)
    Сформируем косометрическую матрицу [?]
    		[?]=[?(0&?-??_z&?_y@?_z&0&?-??_x@?-??_y&?_x&0)]	(1.33)
Матрица пересчета сигналов, полученных с акселерометров, из связанной в географическую систему координат для использования в навигационном алгоритме вычисляются:
    	С=??(cos?cos?&-cos?cos?sin?+sin?sin?&sin?cos?sin?+cos?sin?@sin?&cos?cos?&-sin?cos?@-cos?sin?&cos?sin?sin?+sin?cos?&-sin?sin?sin?+cos?cos?)?.	(1.34)
    Следовательно матрицаС осуществляет переход от связанного трехгранника OXYZк  географическому ?OX?_g Y_g Z_g.
    		[X_g Y_g Z_g ]=C[XYZ].	(1.35)
    Пусть матрица (1.36) известна, тогда углы рыскания, тангажа и крена определяются через ее элементы:
	?(?=arctg(-c_31/c_11 );@?=arctgc_21;@?=arctg(-c_23/c_22 ).)	(1.36)
Так как рассматриваемый объект совершает плоское движение, то для определения параметров движения необходимо использовать два акселерометра и один датчик угловой скорости.

Рисунок 1.9 – Одноканальный случай БИНС
Для такого вида БИНС матрица С будет иметь следующий вид:
С=??(cos?&-sin?&0@sin?&cos?&0@0&0&1)?
Исходя из вышесказанного, получим, что переход от связанной системы координат в географическую для ускорений будет выглядеть так
?(a_y0=a_y cos?-a_x sin?;@a_x0=a_y sin?+a_x cos?.)

   Калибровка инерциальных чувствительных элементов с помощью фильтра Калмана
По своему назначению любой фильтр обязан подавлять помехи и наименьшими искажениями пропускать полезный сигнал. То есть фильтр дает оценку полезного сигнала. Структурная схема работы фильтра изображена на рисунке 1.9.[4]

Рисунок 1.9 – Структурная схема работы любого фильтра
Через x(t) обозначим полезный сигнал, зашумленной помехой?(t), которая складывается с x(t) и в результате чего образуется входной сигнал фильтра z(t). На основе сигнала z(t), «засоренного» помехой?(t), фильтр должен дать оценку x ?(t)полезного сообщения x(t). Под оценкой понимается приближенное значение полезного сообщения. Очевидно, чем ближе оценка к полезному сигналу, тем качественнее работа фильтра. Если фильтр удовлетворяет определенному критерию качества (доставляет экстремум заранее заданному функционалу), то он называется оптимальным.[4]
Погрешность чувствительных элементов представляется в виде:
	{?(?_c (k+1)=?_c (k),@z(k+1)=?_c (k+1)+?(k+1),)?	(1.37)
где ?_c (k+1) - систематическая составляющая, ?(k+1) – белошумная составляющая.
Уравнения оптимального фильтра для нашей модели имеют вид:
	P(k+1?k)=P(k?k), P(0?0)=P_0=1[?ед?^2 ];	(1.38)
	K(k+1)=(P(k+1?k))/(P(k+1?k)+R(k+1) );	(1.39)
	P(k+1?k+1)=[1-K(k+1)]P(k+1?k);	(1.40)
	x ?(k+1?k+1)=x ?(k?k)+K(k+1)[z(k+1)-x ?(k?k)], x ?(0?0)=0.	(1.41)
Данные уравнения, как правило, позволяют достичь необходимого компромисса между требуемой точностью и вычислительными затратами в реальных системах навигации. Кроме того, коррелировaнность шумов объектa и нaблюдaтеляповышaет точность фильтрaции, тaккaк в этом случaе в нaблюденияхинформaция о фильтруемом процессе содержится не только в первом слaгaемом, но и во втором, т. е. в шуме.[5]
Заметим, что в нашей программной реализации траектория полета ракеты моделируется в прикладном пакете без задания такого вида помех, поэтому использование фильтра Калмана является не рациональным и громоздким. Так же задача отработки шумов и помех в рамках ВКР передо мной не ставилась.

Выводы к разделу
В ходе анализа в качестве основных были исследованы алгоритмы ориентации и навигации в направляющих косинусах и в параметрах Родрига-Гамильтона. Преимущество данных алгоритмов является то, что уравнения БИНС, записанные с помощью этих кинематических параметров, - линейны, определены для любых углов рыскания, тангажа и крена. Размерность системы кинематических дифференциальных уравнений ориентации в параметрах Родрига-Гамильтона существенно меньше (на пять единиц) размерности системы кинематических уравнений Пуассона, что делает ее более привлекательной для аналитического изучения задачи определения ориентации объекта с помощью БИНС, так же и для ее численного решения на борту движущегося объекта управления.
Из технических характеристик объекта управления выходит, что по полетному заданию дальность полета ракеты должна быть не более 400 км, а движение считается плоскопараллельным, то в модели управления не будет учитываться кривизна земной поверхности и скорость вращения Земли. Тогда стоит сделать переход от географической системы координат к инерционной системе координат.


2 Моделирование траектории полета и бесплатформенной инерционной навигационной системы
Моделирование траектории полета объекта управления
В рамках дипломной работы поставлена задача программной реализации траектории полета ракеты-мишени. Для решения данной задачи, для начала по уравнениям плоскопараллельного движения построим эталонную траекторию ракеты, затем пропишем программу для БИНСа, такую, что бы траектория ракеты была неизменной.
    В данной работе объектом управления является ракета-мишень 36М6М «Кабан». Характеристики данного ракетного комплекса были описаны ранее в п. 1.3.
    Для реализации полета ЛА в прикладном пакете Simulinkпостроим траекторию движения объекта управления по следующим формулам:[2]
	dV/dt=1/m(Pcos?-X-mgsin?);	(2.1)
	d?/dt=1/mV(Psin?+Y-mgcos?);	(2.2)
	(dx_0)/dt=Vcos?;	(2.3)
	(dy_0)/dt=dH/dt=Vsin?;	(2.4)
 	dm/dt=?-m?_sec;	(2.5)
 	?=?-?;	(2.6)
 	P=g_0 m_sec I_p;	(2.7)
 	X=C_x (?)(?V^2)/2 S_m;	(2.8)
 	Y=C_y (?)(?V^2)/2 S_m;	(2.9)
 	?=?_0 e^(-?H(t));	(2.10)
 	C_x (?)=C_x0+C_x2 ?^2;	(2.11)
 	C_y (?)=C_y0+C_y1 ?.	(2.12)
где m – масса объекта управления;
Sm – площадь миделя корпуса ракеты;
Cy – коэффициент подъемной силы;
Cx – коэффициент лобового сопротивления;
Cx2 – коэффициент лобового сопротивления при ?=0;
Cy1 – производная от коэффициента подъёмной силы по углу атаки при ?=0 ;
Ip– удельный импульс ракетного двигателя;
msec – секундный расход воздуха и горючего (керосина) через двигатель;
V – скорость движения объекта управления;
– угол тангажа;
H – высота объекта управления;
X– сила лобового сопротивления;
Y – подъемная сила;
x0, y0– координаты объекта управления в географической системе координат;
? – угол атаки (угол между вектором скорости и продольной осью объекта управления);
Данные уравнения выведены из проекций векторовизброженных на рисунке 2.1:
Рисунок 2.1 – Проекции векторов силы на системы координат
Структурная схема моделирования представлена в приложенииA.
В случае с ракетой-мишенью, не важна цель попадания, важна траектория полёта, она должна быть более витиеватой, что бы имелась сложность сбить данную ракету. По сути, объект управления сам является целью для попадания другой ракеты. 
Управление движением объекта проводится по двум параметрам: по силе тяге Pи углу атаки ?. Графики изменения параметров управления представлены на рисунке 2.2.
    
Рисунок 2.2 – Графики изменения угла атаки и тяги двигателя
Результаты моделирования уравнений (2.1) – (2.12) в прикладном пакете программ Simulinkпредставлены на рисунке 2.3, 2.4, 2.5:


Рисунок .......................