CC BY
53
Минлигареев Владимир Тимурович - Институт прикладной геофизики имени академика Е.К. Федорова (ФГБУ «ИПГ»); 129128, г. Москва, ул. Ростокинская, 9; e-mail: metrologeo@mail.ru; тел.: 84991815215; зам. директора по научной работе; д.т.н.; доцент.
Алексеева Александра Валерьевна - e-mail: аleks.seva@gmail.com; тел.: 84991815215; научный сотрудник.
Качановский Юрий Михайлович - e-mail: kachanovsky@ipggeospace.ru; тел.: 84991815215; главный метролог - ведущий научный сотрудник.
Репин Андрей Юрьевич - e-mail:director@ipg.geospace.ru; тел.: 84991878186; директор; д.ф.-м.н.; доцент.
Хотенко Елена Николаевна - e-mail: khotenko@ipg.geospace.ru; тел.: 84991878186; ученый секретарь; к.ф.-м.н.
Minligareev Vladimir Timurovich - Fedorov Institute of Applied Geophysics (FSBI "lag"); e-mail: metrologeo@mail.ru; 9, Rostokinskaya str., Moscow, 129128, Russia; phone: +74991815215; deputy director for science; dr. of eng. sc.; associate professor.
Alekseeva Alexandra Valerievna - e-mail: аleks.seva@gmail.com; phone: +74991815215; researcher worker.
Kachanovsky Yuri Mikhailovich - e-mail: kachanovsky@ipggeospace.ru; phone: +74991815215; chief metrologist - leading researcher.
Repin Andrey Yuryevich - e-mail: director@ipg.geospace.ru; phone: +74991815215; director; dr. of phis.-math. sc.; associate professor.
Khotenko Elena Nikolaevna - e-mail: khotenko@ipg.geospace.ru; phone: +74991878186; scientific secretary; cand. of phis.-math. sc.
УДК 629.056 DOI 10.23683/2311-3103-2019-1-258-270
С.А. Савельев, И.В. Соловьев
АЛГОРИТМ КОМПЛЕКСИРОВАНИЯ УГЛОМЕРНОЙ НАП ГНСС И ИНЕРЦИАЛЬНЫХ ДАТЧИКОВ УГЛОВЫХ СКОРОСТЕЙ ДЛЯ ОЦЕНКИ ОРИЕНТАЦИИ ПОДВИЖНОГО ОБЪЕКТА
Целью работы является повышение точности и помехоустойчивости оценок параметров ориентации и угловой скорости подвижного объекта путем комплексирования информации гироскопических датчиков угловых скоростей (ДУС) и угломерной навигационной аппаратуры потребителя (НАП) глобальных навигационных спутниковых систем (ГНСС). Для достижения этой цели проводится синтез алгоритма комплексной обработки информации угломерной НАП ГНСС и гироскопических ДУС, предназначенного для оценки параметров ориентации (матрицы, кватерниона), вектора погрешностей (дрейфов) гироскопов и вектора угловой скорости объекта. Особенностью представленного алгоритма является то, что задача оценки параметров ориентации и угловой скорости решается в два этапа: на первом этапе производится оценка кватерниона ориентации с помощью точечного алгоритма; далее полученный кватернион используется в качестве эффективного измерения в линейном фильтре Калмана. Измерения угломерной НАП ГНСС, представляющие собой разности фаз радиосигналов навигационных спутников, принятых антеннами, расположенными в различных точках пространства, приводятся к векторной форме, что позволяет использовать в качестве детерминированного алгоритма один из существующих быстрых алгоритмов решения задачи Вахба. Проводится ковариационный анализ точности полученного решения и выводится выражение для ковариационной матрицы ошибок оценки ориентации. Полученный кватернион (матрица) ориентации является оценкой максимального правдоподобия, что позволяет использовать его совместно с со-
ответствующей ковариационной матрицей в качестве эффективного измерения в линейном фильтре Калмана, вектор состояния которого включает в себя кватернион ориентации и вектор ошибок (дрейфов) гироскопов. Такой подход позволяет повысить вычислительную эффективность алгоритма путем отказа от использования расширенного фильтра Калмана, а также повысить его численную устойчивость. Приведенные результаты математического моделирования демонстрируют устойчивую работу алгоритма при воздействии как шумов измерений, обусловленных погрешностями НАП, так и шумов модели динамической системы, обусловленных шумами измерений ДУС и случайным блужданием дрейфов гироскопов ДУС. Результаты моделирования показывают, что предложенный фильтр позволяет существенно уточнять оценки ориентации, полученные с помощью точечного алгоритма. Предложенный алгоритм повышает устойчивость потребителя навигационной информации к сбоям НАП, так как позволяет продолжать оценку параметров ориентации и угловой скорости при временном пропадании информации НАП без существенной потери точности.
Инерциальные датчики угловых скоростей; гироскопы; угломерная навигационная аппаратура потребителей; комплексирование; спутниковые навигационные системы.
S.A. Savelyev, I.V. Solovyev
AN ALGORITHM FOR INFORMATION FUSION OF GNSS/GYROS ATTITUDE SENSORS OF A MOVING VEHILCLE
The purpose of this paper is to increase the accuracy and stability of moving vehicle attitude estimation by GNSS/gyros attitude sensors information fusion. To this end, an algorithm is proposed which integrates carrier phase measurements of multi-antennas GNSS signal receiver and angular rate measurements provided by gyros. The algorithm estimates attitude parameters (direction cosine matrix, quaternion), angular rate vector and vector of gyro biases. The difference of the proposed method from the majority of previously developed methods is that the attitude solution is obtained in two stages: first, attitude quaternion is estimated using a batch algorithm; next, this quaternion is used as an effective measurement in linear Kalman filter. GNSS signal receiver measures carrier phase differences between signals of navigation satellites detected by antennas placed at different points in space. These phase differences are converted to vector form. This makes it possible to use one of the well-known fast algorithms developed for Wahba problem solution. Covariance analysis of this solution is presented and an expression for error covariance matrix is derived. The quaternion (direction cosine matrix) obtained is a maximum likelihood estimate of the attitude. Therefore, this quaternion can be used as an effective measurement in linear Kalman filter. The filter state vector includes attitude quaternion and vector of gyro biases. This approach makes it possible to increase computational efficiency of the algorithm by avoiding the use of extended Kalman filter and to enhance numerical stability. Mathematical simulation results demonstrate stable algorithm performance under the influence of GNSS receiver measurement noise as well as dynamic system model noise caused be gyros measurement noise and rate random walk noise. Simulation results also indicate that the proposed algorithm allows for substantially increased accuracy of attitude estimates in comparison with the accuracy provided by batch algorithm. The proposed algorithm also enhances stability of navigation information user to the GNSS faults by propagating attitude and angular velocity estimates through GNSS signal outage without substantial loss in accuracy.
Inertial angular rates sensors; gyros; multi-antennas GNSS receiver; information fusion; satellite navigation systems.
1. Введение. Задача оценки ориентации подвижного объекта возникает в многочисленных технических приложениях, среди которых можно выделить системы управления беспилотными летательными аппаратами, мобильными роботами, автомобильным и морским транспортом.
Одним из инструментов решения задачи оценки ориентации является угломерная навигационная аппаратура потребителя (НАП) глобальных навигационных спутниковых систем (ГНСС), принцип действия которой основан на измерении
разности фаз радиосигналов навигационных спутников (НС), принятых антеннами, расположенными в различных точках пространства (рис. 1). Соответствующие алгоритмы оценки ориентации изложены в ряде работ российских [1-3] и зарубежных [4-8] авторов.
Р
Рис. 1. Макет аппаратуры спутниковой навигации с функцией пространственного определения углов
Другим распространенным средством оценки ориентации являются инерци-альные датчики угловых скоростей (ДУС). Комплексирование ДУС и угломерной НАП ГНСС позволяет повысить точность и частоту выдачи навигационных определений. Кроме того, комплексное решение включает в себя информацию не только об ориентации объекта, но и о его угловой скорости. Оценка угловой скорости необходима в ряде приложений, например, в системах управления беспилотными летательными аппаратами для обеспечения работы подсистемы угловой стабилизации. Комплексирование ДУС и угломерной НАП позволяет продолжить оценку параметров ориентации при временном отсутствии сигналов НС путем интегрирования показаний ДУС, что повышает устойчивость потребителя навигационной информации к сбоям НАП.
Целью работы является повышение точности и помехоустойчивости оценок параметров ориентации и угловой скорости объекта путем комплексирования информации гироскопических ДУС и угломерной НАП.
Для решения такой задачи были предложены варианты расширенного фильтра Калмана [9-12]. Особенностью предлагаемого здесь алгоритма является то, что рассматриваемая задача решается в два этапа: на первом этапе производится оценка кватерниона ориентации с помощью точечного алгоритма; далее полученный кватернион используется в качестве эффективного измерения для линейного фильтра Калмана. Поскольку этот кватернион рассчитывается совершенно независимо от работы фильтра, появляется возможность контролировать результаты, выдаваемые фильтром.
2. Задача оценки ориентации с помощью угломерной НАП как задача оценки ориентации по векторным измерениям. Введем приборную систему координат АХХХз (ПСК), связанную с плоскостью расположения трех антенных элементов А, Аь А2. Векторы ААх и АА2 (базовые линии) задают направления осей АХ^ и АХ2 ПСК, соответственно; ось АХ3 дополняет систему до правой. Для простоты будем считать, что ПСК является декартовой прямоугольной, | А А2 | = | А А2 | = I (обобщение изложенного далее алгоритма на случай, когда базовые линии задаются произвольными неколлинеарными векторами, не вызывает затруднений).
Считаем, что на объекте имеется тройка гироскопов, оси чувствительности которых совпадают с осями ПСК. Ориентация ПСК относительно системы координат, связанной со строительными осями объекта (связанной системы координат), известна.
Введем инерциальную систему координат Ох1х2х3 (ИСК), начало которой О -геометрический центр Земли, ось Ох3 совпадает с осью вращения Земли, плоскость 0Х]Х2 - экваториальная плоскость, ось Охх направлена в точку весеннего равноденствия, ось Ох2 дополняет систему до правой.
Введем следующие обозначения:
Г , ■ = 1,..., п - единичные векторы, задающие направления на п навигационных спутников (п > 2) в ИСК (ИСК-векторы);
sI■ , ■ = 1,...,п - единичные векторы, задающие направления на п навигационных спутников в ПСК (ПСК-векторы);
= А А; / | А А; | , у = 1, 2 - единичные векторы, задающие направления на антенные элементы Ль Л2 в ПСК: ж" = ( 1 0 0) т, ж" = (0 1 0) т;
г" ] = 1, 2 - единичные векторы, задающие направления на антенные элементы Ль Л2 в ИСК;
A - матрица ориентации ПСК относительно ИСК:
Введем также единичный кватернион ц = (<?1 <?2 <?з <?4 ) т = (Чу <?4) т , соответствующий матрице A:
Требуется, зная векторы г,, sI, ■ = 1,., п (п > 2), оценить ориентацию ПСК относительно ИСК, т.е. матрицу A и (или) кватернион q.
Координаты НС традиционно задаются в гринвичской системе координат (ГСК), связанной с вращающейся Землей. Эти координаты нетрудно перевести в ИСК. Таким образом, координаты , = 1,...п, НС в ИСК и координаты потребителя \потр в ИСК считаем известными. Тогда
Для каждой из базовых линий ЛЛ1 и ЛЛ2 в результате обработки навигационных сигналов, принятых в двух точках, расположенных на концах линии, измеряются разности фаз Дг^ = I с о б 0 ц (выраженные в единицах длины) для каждого ,-го НС, , = 1,...,п, у = 1, 2. Считаем, что в процедуру формирования разности фаз входит процедура разрешения неоднозначности фазовых измерений и компенсация паразитных фазовых набегов в антенно-фидерном и радиочастотном трактах приемника, т.е. полагаем, что в Дг,,- входят только ошибки фазовых измерений в навигационном приемнике. Тогда модель измерений можно представить в виде
8, = Arг■, г, = АТ8г, , = 1,...,п.
Ч = ( Чу <?4) т = ( пб т^ со б^) Т,
<?2 + <?2 + <?2 + <?2 = 1 , единичный вектор п задает направление оси поворота, ф - угол поворота. Матрица А выражается через кватернион q следующим образом:
А (ч) = (<?2 - I Чу I 2) I з хз + 2 ЧуЧт + 2 <?4 [Чу] ,
где 13х3 - единичная матрица, а матрица имеет вид
/О <?з -<?2\
[Чу] = 1 - <?з 0 <?1 I .
V д2 -(?! О 1
г
Лт ■ ■ T
Р0' =~г=с0 s 0 И = ( Г?) Г + 6 рИ' (!)
где 5р,у - ошибки измерений, которые будем считать белым гауссовским шумом с дисперсией с2. Соотношение (1) можно переписать в ПСК:
р у = со s 0у = (ATsf) TATs г + 6 р у = (sf) Т s г + 6 р у, (2)
в силу ортогональности матрицы A.
Поскольку s j = A гь из (2) получаем следующее уравнение, связывающее измерения pjj и кватернион q:
р у = (s?) TA (q) г + 6 р у (3)
Из (2) следует, что с точностью до шумовых слагаемых величины cosö,i и cos0,2 являются направляющими косинусами единичных векторов s,. Таким образом, можно сформировать векторные измерения в ПСК вида
/ cos е£1 \ / рц \
s j = ( с0 s0 j 2 ) = ( р j2 I . (4)
VVl-cos^, -cos^ei2/ VVl - Pii - Pa/ Перед корнем следует выбрать знак «плюс», так как антенны принимают только сигналы спутников, расположенных над плоскостью ХгХ2.
Таким образом, задача свелась к задаче оценки матрицы ориентации одной системы координат относительно другой по векторным измерениям. В 1965 году Г. Вахба сформулировала эту задачу следующим образом [13]. Пусть заданы два множества векторов (sb ..., sn} и {rb...,rn} (n > 2). Требуется найти ортогональную матрицу A с детерминантом +1, которая минимизирует функцию потерь
£(A)=^If= I s j — A г I 2. (5)
где а, , i = 1, ..., n - веса отдельных измерений.
Эта задача известна как «задача Вахба». Отметим, что в приведенной формулировке задачи предполагается, что все измерения были сделаны при одной и той же ориентации системы координат, то есть, как правило, в один и тот же момент времени.
К настоящему времени предложено немало алгоритмов решения задачи Вах-ба: QUEST [14, 15], ESOQ [16], ESOQ2 [17], FOAM [18], ORIENT [19].
В [20] показано, что при равноточных измерениях при выборе весовых коэффициентов в виде а, = 1/n оптимальная матрица ориентации Aopt, минимизирующая критерий (5), является оценкой максимального правдоподобия для матрицы A. Рассмотрим вопрос об ошибке этой оценки ориентации. Обозначим £ = (£ i £ 2 ез ) Т вектор малого поворота, переводящего истинную ориентацию в ориентацию, определяемую оптимальной матицей Aopt. Тогда ковариационная матрица Ree ошибок решения задачи Вахба, по определению, формируется следующим образом:
R вв = < ££Т> (6)
( (...) обозначает математическое ожидание). Можно показать [15], что
R£ в = М- 1< 6 z6 ZT> М- 1 , (7)
где
М = 1 з хз—^Г= is j sT , (8)
6 z = ±£?= 1 (6 sj x г + s j x 6 г) , (9)
5s,, 5r, - ошибки задания векторов s,, r,, i = 1,.,n.
Будем пренебрегать ошибками в задании векторов г,: 5г = 0, ■ = 1,.,п. Кроме того, отметим, что, по определению (6), ковариационная матрица К^ не зависит от кватерниона q; следовательно, при ее вычислении можно принять ц = (0 0 0 1) т и, следовательно, г = Ь ¿, ■ = 1,...,п.
Что касается ошибок 58,-, то из (4) следует:
5 б ; 1 = 5 р ; 1 ,
5 Б ¿2 = 5 Р I 2,
бэгз = --^брл --^-6рг2
с точностью до слагаемых, квадратичных относительно шумов измерений.
Отсюда получаем следующее выражение для ковариационной матрицы
<5^58?):
/ 1 0
<65^т> = а2
1
. __ 42 ,
\ 513 /
(10)
^¡3 ^¡3
Подставляя (10) в (9), находим:
+ + =213 ' (2 + 213 ) '
2 ^¡3 \ ^¡3 /
Б/т Б,-'
$1"? ( 2 + 713 ) » + + 213 , ¡»¿г*5*:
(11)
V БлБгз, 1 4/
Выражение (7), в котором матрица < 5 г ¿5 гт ) вычисляется по формулам (11), а матрица М - по формуле (8), характеризует ошибки всех алгоритмов решения задачи Вахба по измерениям вида (4).
3. Модель измерений ДУС. Будем использовать следующую простую модель ошибок ДУС (обобщение на случай более сложной модели ошибок не встречает принципиальных затруднений):
<0 ' = <о + Ь + у , (12)
где ю'- измерения ДУС, ю - истинная угловая скорость, Ь - дрейф гироскопов, V -белый гауссовский шум измерений ДУС с характеристиками:
<у(0 ) = 0 з х1, < у а + т) у (0) = а?1 з хз5 (т) . Дрейфы гироскопов будем моделировать процессом случайного блуждания
Ь = ыЮ
с характеристиками
<ш (0 ) = 0 з х1,
+ т) ш (0 ) = ст£ I з хз5 (т) .
4. Фильтр Калмана для оценки кватерниона ориентации и вектора гироскопических дрейфов. Использование уравнения (3) в качестве модели измерений влечет за собой необходимость синтеза расширенного фильтра Калмана, поскольку уравнение (3) нелинейно относительно кватерниона q. Однако в разделе 2 показано, что оптимальная в смысле максимума правдоподобия оценка матрицы ориентации ПСК относительно ИСК А^ может быть получена путем решения
задачи Вахба с помощью одного из указанных алгоритмов. Обозначим р соответствующий этой матрице оптимальный кватернион. Таким образом, имеется возможность определять ориентацию объекта в моменты измерений без использования фильтра. Это обстоятельство позволяет существенно упростить алгоритм и повысить его устойчивость. В предлагаемом варианте линейный фильтр Калмана использует в качестве эффективного измерения оптимальный кватернион р.
4.1. Вектор состояния. Введем 7-компонентный вектор состояния х (£) = ( я ( О Ь ( 0 ) т, где я ( 0 = ( ( 0 <72 ( 0 <?з( 0 <?4 ( 0 ) Т - кватернион ориентации, Ь ( 0 = (Ь 1 ( О Ь2( О Ьз( 0 ) т - вектор дрейфов гироскопов ДУС. Оценки этих параметров будем обозначать .
Эволюция кватерниона ориентации определяется кинематическим уравнением
я (0 = ±а (о (0 ) я (0 , (13)
где
0 (л)3 -<А>2
-<*>з 0
— (л)х 0 (л)3
— (л)х -<*>2 -ш3 0
ii(o)) =
Учитывая (12), получаем следующие уравнения динамической системы:
я ( (о '-Ь-у) я ( 0 , (14)
ь ( о = IV ( о , (15)
Применяя операцию математического ожидания к обеим частям уравнений (14)-(15), получаем уравнения прогноза фильтра Калмана:
Я ( о = 1п (а) я ( 0 ,
ь = о з х1, (16)
а = о ' - ь.
4.2. Вектор ошибок. Введем 6-компонентный вектор 5х(0 ошибок оценки состояния системы:
5х (0 = (50 (0 5Ь (0 ) т ; вектор угловой ошибки 50(/) и кватернион ошибки определяются следующим образом:
я (0 = 5 я (0 О я (0 , 5 я (0 = 71=^4 ( 5 0 ? /2 ) .
Вектор ошибки оценки дрейфов гироскопов
5 Ь (0 = Ь (0 - ь (0 .
Знак ® обозначает композицию кватернионов, которая для двух произвольных нормированных кватернионов р и q вычисляется по следующему правилу:
q =
Можно показать, что компоненты вектора ошибок удовлетворяют уравнениям 5 0 (0 = [а (0] 5 0 (0 - 5 Ь (0 - у (0 ,
5Ь ( о = Ь ( 0 - Ь (0 = ш ( 0 . Получаем следующие уравнения эволюции вектора ошибок:
р4 Рз -р2 Pl\ /4i
-Рз р4 Pi р21 / <?2
Vi -Pi р4 Рз 1 1 <?3
~Vi -р2 -Рз р4/ \<?4
5х (0 = Р (О 5х (О + вш (О , р -1зхз\ г _Мзхз 03х3\
Р (С) КО з хз 0 з хз' , ° 10 з хз I з хз' .
Ковариационная матрица Р6>с6(0 ошибок оценок определяется следующим образом:
Р (0 = < 5х (О 5 хт (О ) .
4.3. Синтез дискретного фильтра Калмана. Предположим, что в моменты 4, к = 0, 1, ...., производятся измерения НАП, в результате обработки которых с помощью одного из алгоритмов решения задачи Вахба формируются кватернионы рк.
Введем обозначения: , - априорная и апостериорная оценки вектора состояния; , - априорная и апостериорная оценки угловой скорости; , - априорная и апостериорная оценки ковариационной матрицы.
4.3.1. Прогноз вектора состояния. Поскольку дрейфы гироскопов постоянны на интервале [4ч, 4] в силу уравнения (16), примем допущение о том, что и угловая скорость постоянна на интервале [4ч, 4]. Тогда можно получить точное решение уравнения (13) в виде
ч/с | к - 1 = ( СО 4x4 + (п,- 0 )цс- 1 | 1,
где ф к- 1 = I о)к- 1 | к- 1 I (£к — £к _ ^ - угол поворота, п к _ 1 = т;—1—, - единичный
|<">(с-1|(с-1|
вектор оси поворота.
Уравнение прогноза вектора дрейфов гироскопов имеет вид
Ь к | к - 1 = Ь к- 1 | к- 1.
4.3.2. Прогноз ковариационной матрицы. Уравнение прогноза ковариационной матрицы Р6х6 имеет вид:
Р/с|/с-1 = Ф/с|/с-1Р/с-1|/с-1Ф/с|/с-1 + Qfc-l
где Ф6х6 - переходная матрица, Q6x6 - ковариационная матрица шумов модели динамической системы.
Переходная матрица определяется следующим образом:
ф а, о = р (о ф а, о , ф ^ о = I 6х6. (17)
Поскольку угловая скорость постоянна на промежутке времени [4ч, 4], можно проинтегрировать уравнение (17) на этом промежутке и получить точное выражение для переходной матрицы Фк/к-1:
Фк | к- 1 = Ф(Ск,Ск- 1) = (0зхз Тз хз ) , (18)
\иЗхЗ '3x3 /
где
, (19)
Чзхз = ( I з хз + [п] ^ + [П] 2 т) (^ — ^- 1) . (20)
Отметим, что соотношение (19) представляет собой известную формулу Эйлера, выражающую матрицу поворота через угол поворота и единичный вектор оси поворота. Таким образом, матрица 0 представляет собой матрицу поворота, заданного вектором ю.
Ковариационная матрица погрешностей модели движения дается выражением
Ок = Ф ^к, т) С<2ш(т) СтФ т (£к, т) (¿т , (21)
где
'ЗхЗ иш'ЗхЗу
Выполнив интегрирование, получаем следующее выражение:
где
n _ Л^зхз 03x3 N In n 2 I Г
4 u3x3 aw'3x3/
[учаем следующее i
n _ /Qll Ql2N
Qk~\Ql2 QJ
(22)
Qu = o5d&t%x3 + ^wdAt2 2I3x3 + nnT
/1 _ ф-ЯПф\\
( з 2 ф О ) ,
Л ? А . I Ж 1 —СОБф , г П ф —БШф , т (1 1 —СОБфЧ
<}12 = а^ (^13х3 + [п] ^ + ппт (- -
Q 2 2 = ашсг1 з х з , Д С = Ск - ^ _ 1.
Здесь а2^ = а2/Д С , аЩ^ = аЩ,Д С - интенсивности шумов в дискретном фильтре Калмана.
При малых углах поворота формулы (18)-(22) можно упростить:
0 з хз = I з хз + И ф +1 [п] 2 Ф 2 + О (Фз ) ,
Ч'з хз = I з хз Д С + 1 [п] ФД С + 1 [п] 2ф2 Д С + О (ф з ) ,
Ql 1 = а^Д С21 з хз + а^Д С2 (IIз хз +1 [п]2ф2) + О (ф з ) ,
Ql 2 = - а^Д С (II з хз +1 [п] ф +1 [п] 2ф 2 ) + О (фз ) .
4.3.3. Коррекция вектора состояния и ковариационной матрицы. Если хк/к-1 -прогнозируемый на момент 4 вектор состояния, то компоненты вектора ошибок в
момент 4, по определению, должны удовлетворять следующим соотношениям:
= =
Прогнозируемые на момент 4 компоненты вектора ошибок, по определению, равны
5 я, , к- 1 = (0 ,
Ь к | к _ 1 = 0 з х 1.
Пусть рк - оценка «истинного» кватерниона ориентации полученная в момент 4 с помощью алгоритма решения задачи Вахба. Ошибка полученной оценки определяется следующим образом:
Р к = 5 р к <Е> як,
5 Р к = 7 1 + | 1к | 2/4 (\ ) ,
где ек - дискретный белый гауссовский шум с ковариационной матрицей, определяемой формулой (7).
Кватернион малого поворота
= Рк® (\l\k-i
представляет собой невязку между прогнозируемым и "измеренным" кватернионами. Определим эффективное измерение z как вектор малого поворота, соответствующий кватерниону 5 q к. Тогда
z к = 5 9 к + £к = Н к5хк + Efc,
где
Н к = 0 3 жЗ 0 з ж3 ) ,
5 qk i к = 7гв^( 5 Y2 ) '
В результате уравнения коррекции фильтра Калмана принимают вид:
В к = Н кРк i к - 1Н к + R к = (Ре е) к | к - 1 + ^ Ь К к = Рк| к- 1Н к В - 1
ук = ^ _ Н к5хк | к - 1 = z Ь 5х к | к = 5 хк | к - 1 + Ккук = К kz Ь Рк | к = 0 6x6 _ КкН к)Рк | к - 1.
Здесь v¿ - обновляющая последовательность, Bk (3x3) - ее ковариационная матрица, Kk - матрица (6x3) коэффициентов усиления фильтра Калмана. Матрица Pee (3x3) - подматрица ковариационной матрицы вектора состояния, соответствующая ковариации угловых ошибок, матрица Rk (3x3) выражается соотношением (7). Априорная оценка 5xk/k-1 вектора 5x в силу его определения тождественно равна нулю.
После вычисления вектора 5xk/k вычисляются коррекции к кватерниону ориентации и вектору дрейфов гироскопов:
q к | к = 5 q к | к ® Чк | к - 1,
Ь к i к = Ь к i к - 1 + 5 Ь к i к. Далее вычисляется оценка угловой скорости:
ак i к = aк _ Ь к i к.
5. Результаты моделирования и тестирования алгоритмов. Для моделирования работы алгоритмов зададим кватернион ориентации ПСК относительно ИСК в виде
( ■ 0(0 0(О\Т q ( t) = ( ns in— с o s—) ,
(cos a0 cos 60\
sina0 со s5 0 I , 0 (t) = 0 0 + a)0t + A sin^-, sin 60 /
где углы ao, 50 задают направление оси вращения ПСК относительно ИСК, e(t) -угол поворота, ю0 = const - угловая скорость равномерного вращения ПСК, величины A, T задают, соответственно, амплитуду и период возмущений равномерного вращения. Тогда угловую скорость истинного движения ra(t) в каждый момент времени t можно вычислить аналитически по формулам:
a ( 0 = n ш ( 0 , ш ( 0 = 0 ( 0 = ш о +^с о s^.
Для моделирования использовались следующие значения параметров: a0 = 0°, 5o = 90°, юо = 10 град./с (36 000 град./ч), eo = 0, T = 3600 с (1 час), A = 1°, l = 1 м, с = 0.002, At = 1 с, cv = 4.3633-10~5 рад/с1/2, cw = 4.8480^106 рад/с3/2, b(t = 0) = (100, 150, 200)T град/ч. Предполагалось, что наблюдаются n = 8 НС, направления на которые задаются следующими ИСК-векторами:
г; = (cos a; cos 5; sina;cos5; sin5;)T, i = 1,...,n, a = (О) + 10°, a0 + 10°, a0 - 10°, a0 - 10°, a, - 30°, a, - 30°, a, - 60°, ao - 60°)T, 5i = (50 + 10°, 50 - 10°, 50 + 10°, 50 - 10°, 50 + 30°, 50 - 30°, 50 + 30°, 50 - 30°)T.
В качестве точечного алгоритма использовался алгоритм ORIENT.
На рис. 2 представлены ошибки оценок ориентации, полученные в результате работы алгоритма: ошибки фильтра Калмана (график слева, сплошная линия), ошибки алгоритма ORIENT (график справа, сплошная линия) и оценки этих ошибок, вычисленные по формулам: линия), 3 аод/иуг = 3 /й
= 3 V?!! + 2 + Р3 з, (график слева, пунктирная е £ 1 + Д е £ 2 + Д £ £ 3 (график справа, пунктирная линия).
Рис. 2. Результаты работы алгоритма
Графики показывают, что фильтр существенно уточняет оценки ориентации, полученные с помощью точечного алгоритма. На 5-минутном интервале (с 400-й по 700-ю секунды) моделировалось отключение НАП, т.е. фильтр работал в режиме прогноза. При этом ошибка, накопленная за эти 5 минут, составила около 65 угл. мин.
Заключение. Разработан алгоритм комплексной обработки информации угломерной НАП ГНСС и инерциальных ДУС. Алгоритм обеспечивает оценку параметров ориентации (матрицы или кватерниона), вектора угловой скорости и вектора систематических ошибок гироскопов ДУС. Отличие разработанного алгоритма состоит в том, что задача оценки ориентации решается в два этапа. На первом этапе производится оценка кватерниона ориентации ССК относительно ИСК с помощью точечного алгоритма. На втором этапе этот кватернион используется как эффективное измерение в линейном фильтре Калмана. Такой подход позволяет повысить вычислительную эффективность алгоритма за счет отказа от использования расширенного фильтра Калмана, а также повысить его численную устойчивость путем контроля результатов работы фильтра.
Проведенное математическое моделирование продемонстрировало устойчивую работу алгоритма при воздействии как шумов измерений, обусловленных погрешностями НАП, так и шумов модели динамической системы, обусловленных шумами измерений ДУС и случайным блужданием дрейфов гироскопов ДУС.
Результаты моделирования показывают, что фильтр позволяет уточнять оценки ориентации, полученные с помощью точечного алгоритма. Предложенный алгоритм повышает устойчивость потребителя навигационной информации к сбоям НАП, так как позволяет продолжать оценку параметров ориентации и угловой скорости при временном пропадании информации НАП без существенной потери точности.
БИБЛИОГРАФИЧЕСКИЙ СПИСОК
1. ГЛОНАСС. Принципы построения и функционирования. - 4-е изд. / под ред. А.И. Перова, В.Н. Харисова. - М.: Радиотехника, 2010. - 800 с.
2. Перов А.И., Шатилов А.Ю. Оценивание углов ориентации объекта с использованием спутниковых радионавигационных систем // Радиотехнические тетради. - 2008. - № 37.
- C. 53-56.
3. Поваляев А.А. Определение ориентации объектов по сигналам глобальных навигационных спутниковых систем. - М.: Радиотехника, 2015. - 320 с.
4. Crassidis J.L., Markley F.L. New Algorithm for Attitude Determination Using Global Positioning System Sygnals // AIAA Jpurnal of Guidance, Control, and Dynamics. - Sept.-Oct. 1997. - Vol. 20, No. 5. - P. 891-896.
5. Giorgi G., Buist P.J., Verhagen S., Teunissen P.J.G. GNSS-Based Attitude Determination. Aerospace and Formation Flying // InsideGNSS. - July - August 2011. - P. 62-71.
6. Cohen C.E. Attitude Determination Using GPS // PhD thesis, Stanford University, Department of Aeronautics and Astronautics, 1992.
7. Crassidis J.L., Lightsey E.G., Markley F.L. Efficient and Optimal Attitude Determination Using Recursive Global Positioning System Signal Operations // Journal of Guidance, Control, and Dynamics. - Vol. 22 (2), - P. 193-201.
8. Park F.C., Kim J., Kee C. Geometric Descent Algorithms for Attitude Determination Using GPS // In 14th World Congress of IFAC, P. 557 - 562, Seoul, Korea, 1999.
9. Перов А.И. Синтез комплексного алгоритма фильтрации разностей фаз в инерциально-спутниковой угломерной навигационной аппаратуре // Радиотехника. - 2018. - № 9.
- С. 120-130.
10. Емельянцев Г.И., Степанов А.П. Интегрированные инерциально-спутниковые системы ориентации и навигации / под общей ред. акад. РАН В.Г. Пешехонова. - СПб.: ГНЦ РФ АО «Концерн «ЦНИИ «Электроприбор», 2016. - 394 с.
11. Wang C. Development of a Low-cost GPS-based Attitude Determination System // PhD thesis, University of Calgary, Department of Geomatics Engineering, 2003.
12. Hirokawa R., Ebinuma T. A Low-Cost Tightly Coupled GPS/INS for Small UAVs Augmented with Multiple GPS Antennas // Navigation: Journal of the Institute of Navigation. - 2009.
- Vol. 56, No. 1. - P. 35-44.
13. Wahba G. A Least Squares Estimate of Spacecraft Attitude // SIAM Review. - 1965. - Vol. 7, No. 3. - P. 409.
14. ShusterM.D. Approximate Algorithms for Fast Optimal Attitude Computation // AIAA Paper 78-1249, AIAA Guidance and Control Conference, Palo Alto, CA, August 7-9, 1978.
15. ShusterM.D., Oh S.D. Three-Axis Attitude Determination from Vector Observations // Journal of Guidance and Control. - January-February 1981. - Vol. 4, No. 1. - P. 70-77.
16. Mortari D. ESOQ: A Closed-Form Solution to the Wahba Problem // The Journal of the Astronautical Sciences. - July-September 1997. - Vol. 45, No. 2. - P. 195-204.
17. Mortari D. Second Estimator of the Optimal Quaternion // Journal of Guidance, Control and Dynamics. - 2000. - Vol. 23, No. 5. - P. 885-888.
18. Markley F.L. Attitude Determination Using Vector Observations: a Fast Optimal Matrix Algorithm // The Journal of the Astronautical Sciences. - April-June 1993. - Vol. 41, No. 2. - P. 261-280.
19. Соловьев И.В. Алгоритм "ORIENT" оценки ориентации космического аппарата по астроиз-мерениям // Авиакосмическое приборостроение. - 2012. - № 12. - C. 11-19.
20. Shuster M.D. Maximum Likelihood Estimation of Spacecraft Attitude // Journal of the Astronautical Sciences. - January - March 1989. - Vol. 37, No. 1. - P. 79-88.
REFERENCES
1. GLONASS. Printsipy postroeniya I functsionirovaniya [GLONASS. Principles of Design and Functioning], 4th ed., ed. by A.I. Perova and V.N. Kharisova. Moscow: Radiotekhnika, 2010. 800 p.
2. Perov A.I., Shatilov A.Yu. Otsenivaniye uglov orientatsii ob''ekta s ispolzovaniem sputnikovykh radionavigatsionnykh system [Estimation of object attitude angles with the use of satellite radio navigation systems], Radiotekhnicheskiye tetradi [Notebooks on radio technique], 2008, No. 37, pp. 53-56.
3. Povalyaev A.A. Opredeleniye orientatsii ob''ektov po signalam global'nykh navigatsionnykh sputnikovykh system [Determination of object attitude by global navigation satellite systems signals], Moscow: Radiotekhnika, 2015, 320 p.
4. Crassidis J.L., Markley F.L. New Algorithm for Attitude Determination Using Global Positioning System Sygnals, AIAA Jpurnal of Guidance, Control, and Dynamics, Sept.-Oct. 1997, Vol. 20, No. 5, pp. 891-896.
5. Giorgi G., Buist P.J., Verhagen S., Teunissen P.J.G. GNSS-Based Attitude Determination. Aerospace and Formation Flying, InsideGNSS, July - August 2011, pp. 62-71.
6. Cohen C.E. Attitude Determination Using GPS, PhD thesis, Stanford University, Department of Aeronautics and Astronautics, 1992.
7. Crassidis J.L., Lightsey E.G., Markley F.L. Efficient and Optimal Attitude Determination Using Recursive Global Positioning System Signal Operations, Journal of Guidance, Control, and Dynamics, Vol. 22 (2), pp. 193-201.
8. Park F.C., Kim J., Kee C. Geometric Descent Algorithms for Attitude Determination Using GPS, In 14th World Congress of IFAC, Seoul, Korea, 1999, pp. 557-562,
9. Perov A.I. Sintez kompleksnogo algoritma fil'tratsii raznistey faz v inertsial'no-sputnikovoy uglomernoy navigatsionnoy apparature [Synthesis of an integrated phase difference filtering algorithm in an inertial-satellite attitude navigation apparatus], Radiotekhnika [Radio technique], 2018, No. 9, pp. 120-130.
10. Yemel'yantsev G.I., Stepanov A.P. Integrirovanniye inertsialno-sputnikoviye sistemy orientatsii i navigatsii [Integrated inertial-satellite orientation and navigation systems], ed. by akad. RAN V.G. Peshekhonova. Saint-Petersburg: GNC RF AO "Koncern "CNII "Electropribor", 2016, 394 p.
11. Wang C. Development of a Low-cost GPS-based Attitude Determination System, PhD thesis, University of Calgary, Department of Geomatics Engineering, 2003.
12. Hirokawa R., Ebinuma T. A Low-Cost Tightly Coupled GPS/INS for Small UAVs Augmented with Multiple GPS Antennas, Navigation: Journal of the Institute of Navigation, 2009, Vol. 56, No. 1, pp. 35-44.
13. Wahba G. A Least Squares Estimate of Spacecraft Attitude, SIAMReview, 1965, Vol. 7, No. 3, pp. 409.
14. Shuster M.D. Approximate Algorithms for Fast Optimal Attitude Computation, AIAA Paper 78-1249, AIAA Guidance and Control Conference, Palo Alto, CA, August 7 - 9, 1978.
15. Shuster M.D., Oh S.D. Three-Axis Attitude Determination from Vector Observations, Journal of Guidance and Control, January-February 1981, Vol. 4, No. 1, pp. 70-77.
16. Mortari D. ESOQ: A Closed-Form Solution to the Wahba Problem, The Journal of the Astronautical Sciences, July-September 1997, Vol. 45, No. 2, pp. 195-204.
17. Mortari D. Second Estimator of the Optimal Quaternion, Journal of Guidance, Control and Dynamics, 2000, Vol. 23, No 5, pp. 885-888.
18. Markley F.L. Attitude Determination Using Vector Observations: a Fast Optimal Matrix Algorithm, The Journal of the Astronautical Sciences, April-June 1993, Vol. 41, No. 2, pp. 261-280.
19. SolovyevI.V. Algoritm "ORIENT" otsenki orientatsii kosmicheskogo apparata po astroizmereniyam [ORIENT algorithm for spacecraft attitude estimation from star sensor measurements], Aviakosmicheskoyepriborostroeniye [Aviaspace machine-building], 2012, No. 12, pp. 11-19.
20. Shuster M.D. Maximum Likelihood Estimation of Spacecraft Attitude, Journal of the Astronautical Sciences, January - March 1989, Vol. 37, No. 1, pp. 79-88.
Статью рекомендовал к опубликованию к.т.н. А.В. Бумагин.
Савельев Сергей Аполлинарьевич - Филиал Акционерного общества «Объединенная ракетно-космическая корпорация» - «Научно-исследовательский институт космического приборостроения» (Филиал АО «ОРКК» - «НИИ КП»); e-mail: tmpayer@yandex.com; 111250, г. Москва, ул. Авиамоторная, 53; тел.: +74955088894(66-99); начальник научно-производственного комплекса.
Соловьев Игорь Валерьевич - e-mail: Solovyev_IV@orkkniikp.ru; тел.: +74955088894(67-79); к.т.н.; начальник группы инженеров-исследователей.
Savelyev Sergey Apollinaryevich - Branch office of "The United Rocket and Space Corporation" -"Research Institute for Space Devices"; e-mail: tmpayer@yandex.com; 53, Aviamotornaya street, Moscow, 111250, Russia; phone.: +74955088894(66-99); head of science and production division.
Solovyev Igor Valerievich - e-mail: Solovyev_IV@orkkniikp.ru; phone: +74955088894(67-79); cand. of eng. sc.; head of research group.
