Адаптивный корректор углов ориентации для БИНС
ADAPTIVE ATTITUDE ANGLE CORRECTOR FOR STRAPDOWN INERTIAL NAVIGATION SYSTEM

Изобретение относится к области бесплатформенных инерциальных систем навигации (БИНС), комплексированных с приемником спутниковой навигационной системы (СНС). Задачей изобретения является повышение точности бесплатформенной инерциальной системы путем создания способа непрерывной коррекции инерциальной навигационной системы (ИНС).
x,
y,
z
Классическим алгоритмом вычисления углов ориентации является пересчет показаний ДУС (проекций абсолютной угловой скорости ω ω ω) в угловые скорости  с последующим их интегрированием. Недостатком такой системы является накапливаемая во времени погрешность и, как следствие, ограниченное время работы. Для устранения указанного недостатка в систему необходимо вводить дополнительную информацию, характеризующую угловую ориентацию летательного аппарата (ЛА). В качестве источника такой информации обычно используют спутниковую навигационную систему. В распространенном способе комплексирования при помощи статистического фильтра оцениваются погрешности навигационных параметров и инструментальных погрешностей инерциальных датчиков. По результатам оценок косвенным образом оценивают ошибки текущих измерений углов пространственной ориентации ЛА и корректируют углы. Для обеспечения требуемой точности при этом необходимо оценивать свыше двадцати параметров, требующих больших вычислительных мощностей. Определение углов пространственной ориентации путем измерении фазового сдвига между принятыми сигналами от каждого космического аппарата требует установку на ЛА нескольких антенн и линий приема, и обработки сигналов, что для малогабаритных ЛА не всегда возможно.
Известно устройство «Бесплатформенная инерциальная курсовертикаль». Патент на изобретение №2249791, опубл. 20.12.2004, с контуром коррекции, который содержит трехканальный блок датчиков угловых скоростей, трехканальный блок датчиков линейных ускорений, формирователь производных от углов ориентации, блок коррекции, блок интеграторов, блок вычисления наблюдаемой вертикали, блок вычисления ошибок курсовертикали, фильтр, блок выставки курса.
Устройство работает следующим образом. Угловые скорости, измеренные трехканальным блоком ДУС и преобразованные в производные от углов ориентации, содержат ошибки, обусловленные систематическими и случайными погрешностями измерений. Предполагается, что при интегрировании угловых скоростей ошибка не накапливается из-за вычитания постоянных составляющих ошибки. Крен и тангаж корректируются блоком коррекции с использованием сигналов акселерометров. Курс корректируется блоком коррекции с использованием блока выставки курса. Ошибки курсовертикали компенсируются в блоке коррекции, проходя через фильтр высоких частот.
Недостаток заключается в том, что при маневрировании ЛА моменты времени, когда оценки крена и тангажа обладают достаточной точностью, могут возникать недопустимо редко из-за наличия в сигналах акселерометров медленно меняющихся и быстро меняющихся линейных и поворотных ускорений. Это может привести к возникновению значительных погрешностей в показаниях крена и тангажа.
Наиболее близким является патент №2564379, «Бесплатформенная инерциальная курсовертикаль», МПК G01C 21/16, опубликовано: 27.09.2015 Бюл. № 27, принятый нами за прототип.
Устройство содержит трехкомпонентный блок датчиков угловых скоростей, трех-компонентный блок датчиков линейных ускорений, корректор курса, вычислительный блок, блок формирования матрицы направляющих косинусов, интегрирующий блок, фильтр Калмана и блок формирования функций измерений, соединенных между собой соответствующим образом. Устройство обеспечивает адаптивную (маятниковую) коррекцию БИНС, реализуемую посредством фильтра Калмана, в котором коэффициент усиления изменяется сучетом текущих значений модулей перегрузки и угловой скорости в блоке формирования функций измерений. При этом могут быть использованы датчики ДУС и ДЛУ средней и низкой точности, в том числе микромеханического типа.
В случае длительного активного маневрирования накапливаемые ошибки измерения углов ориентации из-за погрешностей датчиков угловых скоростей будут интенсивно возрастать. Особенно это будет заметно в случае применения грубых датчиков.
Цель работы - повышение точности коррекции БИНС по углам крена, тангажа и обеспечение измерения курса с требуемой точностью во всем диапазоне полета.
Для достижении указанной цели предлагаемый Адаптивный корректор углов ориентации для БИНС, содержащий блок датчиков угловых скоростей, блок формирования кватернионов, блок датчиков линейных ускорений, фильтр Калмана, дополнительно содержит блок определения углов ориентации, одноантенный приемник спутниковой навигации, дифференцирующее устройство, блок формирования невязки и блок преобразования координат, выход блока датчиков угловых скоростей подключен к второму входу блока формирования кватернионов, к выходу которого подключен вход блока определения углов ориентации, к выходу блока датчиков угловых скоростей подключены также вторые входы блока формирования матрицы шумов системы, блока формирования матрицы шумов измерений и пятый вход блока формирования невязки, к выходу блока датчиков линейных ускорений подключены первый вход блока формирования матрицы шумов системы, четвертый вход фильтра Калмана, первый вход блока формирования матрицы шумов измерений и четвертый вход блока формирования невязки, пятый вход фильтра Калмана соединен с выходом блока формирования матрицы шумов системы, а третий вход соединен с выходом блока формирования матрицы шумов измерений, к первому выходу одноантенного приемника спутниковой навигации подключен второй вход блока преобразования координат, второй выход одноантенного приемника спутниковой навигации соединен с входом дифференцирующего устройства, выход которого соединен с первым входом блока преобразования координат, к первому и второму выходам блока преобразования координат подключены третий и второй входы блока формирования невязки соответственно, второй вход фильтра Калмана соединен с выходом блока формирования невязки, первый выход фильтра Калмана соединен с первым входом блока формирования кватернионов, а второй выход соединен с шестым входом блока формирования невязки, выход блока определения углов ориентации соединен с первым входом фильтра Калмана и с первым входом блока формирования невязки.
На фиг. 1 представлена структурная схема корректора углов ориентации для БИНС
На фиг.2 , фиг.3 и фиг.4 представлены сравнительные результаты моделирования тангажа, крена и курса, их идеальных значений (красная линия), с прототипом (зеленая линия) и с предложенным способом коррекции по сигналам СНС (синяя линия).
Корректор углов ориентации для БИНС от СНС состоит из: 1. Блок датчиков угловых скоростей; 2. Блок формирования кватернионов; 3. Блок определения углов ориентации; 4. Блок датчиков линейных ускорений; 5. Блок формирования матрицы шумов системы (Q); 6. Фильтр Калмана; 7. Одноантенный приемник спутниковой навигационной системы; 8. Блок формирования матрицы шумов измерений (R); 9. Дифференцирующее устройство; 10. Блок формирования невязки; 11. Блок преобразования координат.
Суть работы устройства излагается ниже.
В рассматриваемом корректоре используются две модели ориентации. Модель 1 предназначена для учета изменения ориентации аппарата на шагах дискретизации измерений гироскопов. Она является динамической, детерминированной и представлена в параметрах Родрига-Гамильтона.
i
Модель 2 предназначена для коррекции модели 1. Вектор состояния модели 2 учитывает три компоненты: крен, тангаж и курс, которые подлежат оцениванию по текущим значениям сигналов акселерометров. Модель 2 является стохастической и статической, поскольку ее состояние учитывается для текущего момента дискретного времени t, Соотношения для модели 2 имеют вид (1).
По измерениям блока датчиков ДУС 1 и ДЛУ 4 оценивается вектор состояния:
Здесь ϑ - тангаж, γ - крен, φ - курс, i - номер дискретного момента времени измерений датчиков.
Изменение крена, тангажа и курса описывают с помощью кватерниона ориентации в блоке 2 формирования кватернионов, для которого начальное значение курса равно начальному значению выставленного курса ЛА.
Расчет кватерниона (2) по вектору (1), необходимый для определения его начального значения и уточнения на каждом шаге коррекции, выполняют с помощью известных соотношений:
Вычисление кватерниона (2) сопровождается его нормированием.
После вычисления (4) исходный кватернион q заменяют на нормированный кватернион .
В модели 1 по кватерниону ориентации определяют матрицу поворота А(3,3) в блоке 3 определения углов ориентации, через которую определяют углы крена, тангажа и курса:
Крен:
a
а
γ=-Arctg((3,2)/(2,2)) в диапазоне ±180 градусов,
Тангаж:
Курс:
a
а
ψ=-Arctg((1,3)/(1,1)) в диапазоне 0-360 градусов.
При счислении ориентации используется кватернион угловых скоростей:
x
y
z
где ω, ω, ω - измерения, поступающие с блока датчиков угловых скоростей [гад/с].
Определение текущего кватерниона ориентации выполняется с помощью одношагового алгоритма, имеющего вид:
где Δt - шаг дискретизации измерений по времени,  - операция произведения кватернионов.
Вектора состояния адаптивного фильтр Калмана представлен в выражении (1). Вектор наблюдений, обозначаемый далее Z, содержит измерения акселерометров, поступающих с блока 4 датчиков линейных ускорений. Вектор наблюдений, обозначаемый далее  определяется параметрами полета ЛА через функции наблюдений в блоке формирования невязки.
i
Здесь ν - вектор ошибок измерений с заданной ковариационной матрицей R.
x
y
z
Функции наблюдений ƒ, ƒ, ƒ определяют связь измерений перегрузок с параметрами полета. Точные соотношения для этих функций имеют вид:
x
y
z
Здесь V, V, V - проекции вектора земной скорости на связанные оси ЛА, которые определяют через матрицу направляющих косинусов A(3,3) согласно выражению (5) по сигналам приемника СНС:
N
U
E
где V, V, V - проекции вектора земной скорости, полученные от приемников СНС;
g - ускорение силы тяжести;
Для нахождения проекций ускорений  необходимо решить систему дифференциальных уравнений. Учитывая то, что изменение скорости ЛА относится к длиннопериодическому движению, производная скорости выделяется из оценки скорости с помощью сглаживающего фильтра нижних частот в блоке 9.
Здесь Т - постоянная времени фильтра нижних частот.
С учетом (12) матрица Якоби вектора наблюдений (10) имеет вид.
Возможные большие рассогласования наблюдений  в режимах маневрирования учитывается с помощью включения в модель динамики вектора состояния (1) дополнительных возмущений, уровень которых определяется мерой отличия модуля перегрузки от единицы и модуля угловой скорости от нуля в блоке формирования матрицы шумов системы 5 и в блоке формирования матрицы шумов измерений 8.
Для этого текущий вектор состояния (1) рассчитывается по кватерниону (2) с учетом (4), (5), (6), после чего полагается, что он известен с точностью до указанных дополнительных возмущений. При этом уравнения объекта принимаются в виде:
iq
i
i
Здесь x - вектор, в котором компоненты крена, тангажа и курса рассчитаны по соотношениям (6), а компонента скорости принимается равной ее априорному значению на момент текущих измерений; w - вектор возмущений с переменной ковариационной матрицей Q, которая определяется в блоке формирования матрицы погрешностей системы 5:
В соответствии с (14) матрица дискретной модели объекта, используемая в алгоритме фильтра Калмана для прогноза ковариационной матрицы ошибок оценивания, принимается единичной.
Адаптивный алгоритм фильтра Калмана имеет следующий вид (блок 6):
i
i
Отличия ускорений, создаваемым ЛА при маневрах, от ускорений, получаемых путем дифференцирования скоростей от приемников СНС, учитывают с помощью включения в них вектора ошибок измерений ν. При этом дисперсии  в ковариационной матрице R ставятся в соответствие со степенью выдерживания постоянной ориентации на шаге дискретизации измерений Δt, а также с уровнем влияния ускорений, создаваемых летательным аппаратом. Малое влияние этих факторов выражается двумя условиями.
Первое условие определяет выдерживание постоянной ориентации
Данное условие выдерживается относительно просто при задании достаточно большой частоты регистрации сигналов гироскопов.
Второе условие определяет отсутствие ускорений, создаваемых летательным аппаратом. При этом модуль ускорений, измеряемых акселерометрами, равен ускорению свободного падения.
При точных акселерометрах и выполнении условий (18), (19) имеет место идеальный случай коррекции, в котором ошибки измерений в модели наблюдений (10) должны быть равны нулю, то есть . Чем хуже выполняются условия (18), (19), тем больше должны быть дисперсии . В предлагаемом алгоритме зависимость дисперсий  от модуля перегрузки задается функцией модуля перегрузки, формируемой по правилу: чем больше модуль перегрузки отличается от единицы, тем больше дисперсии ошибок измерений.
В реальном процессе полета условия (18), (19) выполняются в той или иной степени приближенно. При этом для достижения желаемой точности ориентации, одного отсчета измерений акселерометров недостаточно. Поэтому оценки крена, тангажа и курса уточняются рекуррентно по множеству измерений. Для пересчета оценок на очередной момент дискретного времени используется модель 1. При этом на каждом шаге Δt апостериорные математические ожидания крена и тангажа пересчитываются в априорные математические ожидания с помощью модели 1, а оценка скорости замораживается. Для выдерживания правильного темпа списывания ковариаций ошибок оценивания, дисперсии возмущений  также задаются функциями модуля перегрузки, формируемыми по аналогичному правилу: чем больше модуль перегрузки отличается от единицы, тем больше дисперсии возмущений.
Дисперсии возмущений в ковариационной матрице (16) в блоке формирования матрицы погрешностей системы -7 задаются нелинейными функциями модуля перегрузки и модуля угловой скорости.
Суть данных функций заключается в том, что чем больше модуль перегрузки отличается от единицы и модуль угловой скорости от нуля, тем больше должны быть СКО возмущений, действующих на модель (16). Для этого функции (22) задаются в кусочно-линейном виде, а узлы их интерполяции определяются путем настройки на обучающем множестве характерных процессов динамики ЛА. Настройка функций (22) осуществляется отдельно, для каждого типа ЛА.
Таким образом, с помощью предлагаемого устройство решается задача определения вектора  по наблюдениям (10) с учетом одношагового алгоритма ориентации (8). Получаемый при этом вектор (1) на каждом шаге пересчитываются в кватернион (2) в соответствии с соотношениями (3).
Расчеты показывают, что устройство работает во всем диапазоне изменения углов тангажа, крена и курса.
Техническим результатом использования изобретения является повышение точности коррекции углов тангажа, крена и курса в условиях маневрирования в полете, позволяет использовать датчики ДУС средней и низкой точности, в том числе микромеханического типа.
Заявляемое устройство является реализуемым и может быть использовано на всех типах ЛА. В качестве датчиков угловых скоростей могут быть использованы микромеханические гироскопические датчики, при этом фильтр Калмана, блок формирования невязки матрицы погрешностей системы и блок преобразований координат могут быть реализованы на стандартных элементах вычислительной техники.