Лекция: Задача пространственной ориентации объекта

(входная информация: — начальное значение кватерниона ориентации из задачи начальной выставки; — от блока гироскопов после предварительной обработки; — значения вектора угловой скорости географического трехгранника — из навигационной задачи; — оценки погрешностей ориентации из фильтровой задачи)

Искомый кватернион, определяющий ориентацию измерительного блока относительно осей, ищем в виде

или

,

где — приращение кватернионов на шаге ;

для дискретного алгоритма получим

, (4.3)

Вычисление приращений кватернионов может быть осуществлено следующим образом.

-формирование приращения вектора Эйлера

Приведем рекуррентный алгоритм задачи ориентации при использовании дискретного алгоритма 4-го порядка [2] для вычисления вектора Эйлера Тпо квазикоординатам и их разностям:

,(4.4)

где ;

— формирование кватерниона

(4.5)

-формирование приращения вектора Эйлера по значениям вектора угловой скорости из задачи преобразования сигналов акселерометров на навигационные оси и первого интегрирования (в соответствии с разложением (4.4)):

;

;

-формирование кватерниона

(4.6)

-формирование кватерниона

; (4.7)

при ;

-коррекция нормы кватерниона

где – норма кватерниона.

-формирование матрицы направляющих косинусов

; (4.8)

-формированиеуглов

;

где — матрица привязки осей ИБ БИИМ к осям объекта.

Из элемента находим выражение для угла тангажа (килевой качки) :

; (4.9)

Элементы и позволяют определить угол крена (бортовой качки) :

. (4.10)

Поскольку модули углов и меньше, то приведенные выше выражения однозначно определяют значения углов килевой и бортовой качек.

Для нахождения соотношения, однозначно определяющего курс, воспользуемся элементами матриц и функцией Matlab atan2:

при,. (4.11)