CC BY
4Труды МАИ. Выпуск № 96
http://trudymai.ru/
УДК 629.783:527
Комплексирование навигационного приемника и акселерометров для оценки координат и ориентации высокодинамичных объектов
1 Л 1 * А Л А Л Л
Вовасов В.Е. , Бетанов В.В. , Турлыков П.Ю.
1 Компания «Российские космические системы», ул. Авиамоторная, 53, Москва, 111250, Россия Московский авиационный институт (национальный исследовательский университет), МАИ, Волоколамское шоссе, 4, Москва, A-80, ГСП-3, 125993,Россия
*e-mail: vovasov@list.ru **e-mail: vlavab@mail.ru ***e-mail: turlykov78@mail.ru
Аннотация
В статье рассмотрено построение фильтра калмановского типа (ФКТ), позволяющего получать оценки позиционирования и ориентации управляемого высокодинамичного объекта, использующего измерения навигационного приемника ГНСС и блока акселерометров, расположенных на вращающейся рамке.
Показано, что в этом случае, построение ФКТ не вызывает математических трудностей, применяемый адаптивный элемент ФКТ позволяет отслеживать курсовые маневры объекта, а вращение рамки позволяет производить оценку ориентации даже при прямолинейном и равномерном движении центра рамки.
Ключевые слова: Навигационная аппаратура потребителя глобальной навигационной спутниковой системы, бесплатформенная инерционная навигационная система, фильтр калмановского типа, акселерометр.
Введение
Одним из эффективных методов повышения помехоустойчивости является использование интегрированных инерциально-спутниковых систем. Комплексирование НАП ГНСС с ИНС призвано устранить имеющиеся недостатки каждой системы в отдельности. Объединение информации от НАП ГНСС и БИНС теоретически можно осуществить по измеренным сигналам приемника и измерениям акселерометров и гироскопов БИНС. Однако такое объединение весьма сложное и его реализация в настоящее время затруднительна. На практике для интеграции используют НАП и БИНС, разработанные для автономной работы, но их дополняют входами, позволяющими учесть результаты комплексной обработки. В этих условиях статистическая обработка информации оказывается децентрализованной, в частности в НАП сохраняется деление обработки на первичную и вторичную, а вне НАП осуществляется комплексная обработка. При децентрализованной обработке результаты высшего уровня недоступны низшему, и из-за этого несколько снижается точность обработки.
Обычно, комплексирование предполагает совместную обработку псевдо дальностей, псевдо фаз и псевдо скоростей, получаемых приемником ГНСС и вектора угловых скоростей и ускорений, получаемых ИНС с помощью нестационарного фильтра Калмана, который в дальнейшем будем называть
фильтром калмановского типа (ФКТ) [6]. (Проблемам исследования позиционирования и ориентации объектов с помощью навигационных приемников посвящены работы [7, 8, 9]).
Высокодинамичный объект характеризуется наличием рывков при его движении. Под рывком обычно понимают скачкообразное изменение третьей производной координаты по времени. Для обеспечения этого требования необходимо использовать навигационный приемник, имеющий следящую систему за фазой сигнала (ССФ) с астатизмом третьего порядка [1]. Для слежения за задержкой достаточно иметь навигационный приемник, использующий в схеме слежения за задержкой огибающей сигнала (ССЗ) традиционный фильтр второго порядка [1].
Так как точная модель для интегрирования уравнений ориентации основана на модели предложенной Бортцем, то это затрудняет применение ФКТ, так как требует достаточно сложной записи производных в аналитическом виде. Вектор состояния фильтра должен включать инструментальные погрешности акселерометров, а именно скорость дрейфа нуля и погрешность масштабного коэффициента каждого, что расширяет вектор состояния, а значит, снижает потенциальную точность. Кроме этого, использование скоростных гироскопов требует априорной выставки ориентации.
В связи с этим предлагается не использовать гироскопы, а блок акселерометров расположить в фазовом центре антенны навигационного приемника. При этом производить вращение рамки с установленной на ней антенной и акселерометрами заданным образом. Это необходимо для определения
ориентации в случае движения объекта прямолинейно и равномерно, т. е. когда измерения акселерометров близки к нулю. Комплексирование указанных устройств, каждое из которых имеет возможность определять позиционирование, дает системе новую возможность, т. е. определять ориентацию. Рассмотрим решение задачи позиционирование и ориентации с помощью указанных устройств.
На вход вторичной обработки поступают измерения ПД и ПС от каждого видимого спутника с темпом Т = 0,1 с и ускорения п измеренные акселерометрами с темпом At = 0,01 с.
Для решения поставленной задачи используется ФКТ [3] с адаптивным элементом [4]. Блок-схема ФКТ представлена на рис. 1.
Рис. 1. Блок-схема ФКТ Экстраполятор включает в себя уравнение экстраполяции вектора состояния
и матрицы ковариации.
Корректор включает в себя уравнение коррекции вектора состояния,
уравнение коррекции матрицы ковариации, адаптивный элемент и алгоритм
отбраковки измерений.
Пусть хг/ означает оценку вектора хг, полученную на основании измерений,
проводившихся вплоть до момента времени у. Пусть Р\/; обозначает матрицу
ковариаций ошибок, связанную с оценкой хг/ .
Тогда уравнения ФКТ запишем в следующем виде: Уравнение экстраполяции вектора состояния
= %/ + /(%/) '■ ■& + Д%/) * /(%/) * А*2 / 2 + 5/, (1)
Е % } = 0, (2)
Е {Ъ1Т } = , (3)
/(%,) =(4)
Зх
где & — интервал дискретизации; Qi — матрица шума модели. Если в качестве измеряемого ввести вектор ^, то связь оцениваемых и измеряемых параметров можно записать в виде
21 = Ц (X ) + Е- , (7)
где i — обозначает дискретный момент времени, в который производятся измерения.
Предполагаем, что
Е } = Е {ег } = 0, (8)
где И — матрица интенсивности шума измерений.
Уравнение коррекции вектора состояния
Хг+1/г+1 = Хг+Уг + 1^+1 " (9)
Щ+1 = Р+уН1№+У1ИТ1+1 + Кг+1)"1, (10)
где Щ+1 — матрица коэффициентов усиления фильтра, а
дк
Иг+1 = я дх
х=хк+1/к
(11)
Уравнение экстраполяции матрицы ковариаций
Р+1/г = ФРФ + Огг ■ (12)
Определим переходную матрицу — Фг следующим образом
Ф г=1 + А(хг/г)-&. (13)
Уравнение коррекции матрицы ковариаций
Р +1/г +1 = (I - »г+Иг+1)Р+1/г ■ (14)
Для предотвращения расходимости ФКТ введен адаптивный элемент в виде пересчета в каждый момент времени — I матрицы Qг
а+1 = Опш + 0,1 ■ а + 0,7• ¿шё{Щ+1[1/+1 - Кх1+т)\}Т- Кх1+1/1)]}. (15)
Значение Qmln — выбирается экспериментальным путем.
Алгоритм отбраковки измерений можно записать в следующем виде
- Щ+ш)? < 9 • (16)
Если измерения не удовлетворяют приведенному неравенству, то отбрасываются.
Для обеспечения высокой точности фильтрации оцениваемых параметров, а именно такие требуемые оценки координат и скоростей, фильтр должен иметь минимально возможную матрицу шума модели. В этом случае, в связи
с возможными неучтенными изменениями ускорений, будут возникать переходные процессы, которые могут привести к срыву слежения за оцениваемыми параметрами объекта. Для того чтобы это не случилось, калмановский фильтр должен включать в себя описанный адаптивный элемент.
Измерения навигационного приемника производятся в гринвичской системе координат ECEF (Earth Centered Earth Fixed), совпадающей с WGS-84 для GPS и ПЗ-90.02 для ГЛОНАСС. Измерения инерциальных датчиков обычно производятся в местной системе координат NHE (North, Height, East). Начало этой системы совмещено с началом системы координат объекта. Первая ось N системы NHE направлена по местному меридиану на север, вторая ось H ориентирована по местной вертикали вверх и третья ось E ориентирована на восток. Для определения ориентации необходимо введение системы координат BFS (Body Fixed System), связанной с объектом. Первая ось крена r (roll) этой системы ориентирована вдоль оси объекта по ходу его движения, вторая ось курса h (heading) направлена вверх и третья ось тангажа p (pith) ориентирована направо относительно оси объекта. Углы r, h, p для краткости будем называть углами Эйлера.
Обозначим как хj, у j, Zj, Xj, у j, zj — координаты и составляющие вектора
скорости у-ого навигационного спутника; х, у, z, х, у, z, х, у, z — координаты, составляющие вектора скорости и ускорения центра вращения рамки в системе координат ECEF, с установленной на ней антенной НАП и акселерометрами.
Известно, что навигационное решение приемника по измеренным ПД и ПС подразумевает определение смещение шкалы времени приемника относительно
систем ГЛОНАСС ( смещение шкалы времени умноженное на скорость света — А) и скорости смещения этих шкал (скорость смещения шкал времени умноженная на скорость света — А). Так как величины смещения шкал изменяются медленно и вероятностное поведения их известно, то желательно и их ввести в вектор состояния.
Считаем, что основное время работы ФКТ производится в отсутствие рывков. Таким образом получим один из блоков оцениваемого вектора или вектора состояния ФКТ, включающий 11 параметров в виде
X = (х, у, г, А, х, у, ¿, А, х, у, '¿У
(17)
Ориентацию объекта определяют обычно относительно местной системы координат МИБ [5]. Будем считать, что основное время объект вращается с постоянным угловой скоростью. Таким образом получим второй блок вектора состояния ФКТ, включающий 6 параметров в виде
У =
г, р, И, г, р, ¡г
т
(18)
Вектор представляет углы Эйлера и их производные. В этом случае вектор состояния ФКТ может быть представлен в виде
х,
г н
х, у, А, х, у, ¿, А, х, у, '¿, г, р, /г, г, р, к
(19)
а как блочный следующим образом
Х1 /г =
У
(20)
Будем считать, что изменение скорости смещения шкал времени полагаются
винеровскими процессами с малой скоростью дрейфа, которое характеризуется
двухсторонней спектральной плотностью формирующего шума ■ Переход
к моделям смещения часов в дискретном времени получается в результате интегрирования соответствующих непрерывных уравнений на интервале времени дискретизации. В нашем случае время дискретизации для вторичной обработки будет соответствовать интервалу Л?.
(21) (22)
Таким образом, вектор формирующих шумов
4 к =
^Л, к
(23)
представляет из себя БГШ с матрицей дисперсий
М
4к ■
= 0;= с2 • N
(Л?)3 (Л?)2
(Л? )2
2
(Л?)
(24)
Выбор вектора измерений
Возможность учета измерений от недостаточного количества спутников для формирования навигационного решения приводит к тому, что в качестве измерений необходимо включать псевдо дальности — /)/ и псевдо скорости — /)/ от спутников сигнал от которых искажен минимально. Таким образом
(25)
где 3 — число таких спутников.
Выражение для модели псевдо дальностей и псевдо скоростей приведены
в [2].
Антенна и блок акселерометров устанавливаются на вращающейся с частотой = 1Гц раме, ориентацию которой необходимо определять. Радиус вращения Я = 1м.
Пусть перед началом вращения рамки фазовый центр антенны приемника и блока акселерометров образует вектор, проекции х^, уБ>Е8, на оси системы координат BFS [5], связанной с объектом, находятся заранее путем юстировки с помощью обычных (не спутниковых) измерительных средств. Тогда координаты антенны и акселерометров в системе координат ECEF будут выражаться следующим образом
хА х хА Я • соб(2 • п • / • t)
УА = у + N ' N ' * А уБЕБ + Я • бт(2 • п г • 0
2А 2 2А 2беб 0
^-»лг матрица преобразования из системы координат ПЗ-90.02 в систему координат КИЕ
Т
е ^ n
-собХ-бшф -бшХ- бшф собф соб Х- соб ф бш Х- соб ф Бт ф - бш Х соб Х 0
(27)
Х — долгота, ф — широта ускорения центра вращения рамки с навигационным приемником и акселерометрами,
Тв ^^ матрица преобразования из системы координат ББ8 в КИЕ и приведена
ниже.
В системе координат ECEF ускорение фазового центра антенны навигационного приемника с учетом (26) будет равно
(28)
при этом считается, что за время отсчета акселерометров А? = 0,01 с матрицы ТЕ, -Твявляются константами.
Ускорения, полученные акселерометрами, связаны с ускорениями в системе координат ЕСЕБ следующим образом
хА X - Я - соб(2 - п - / - г)
Ул = у + Те\N ■ ТВ^N ■ (2 - п - /)2 - - Я - бш(2-п-/-г)
*А а 0
п
п
п
п
ГТ1-1 гр
ТВ ^ N - ТЕ ^ N
хА Уа
'¿А
(29)
Отсюда измерения акселерометров можно записать в виде
п
«
гр — \ гр
ТВ ^ N ' ТЕ ^ N
+ (2 ' 71 • /)
N ' ТЕ^N
+ (2-п-/У
—Я • соб(2 -л- f • г) —Я • бш(2-л-f • г) 0
-Я • соб(2 • л • f • Аг • ¡) -Я • Бт(2 • л • f •Аг • ¡)
0
Таким образом, вектор измерений для моментов не кратных T представляется
в виде
п
(31)
а для моментов кратных T как блочный в виде
г
(32)
Описание векторов и матриц ФКТ
Определим элементы вектора к (X+1/1). Для моментов не кратных T этот
вектор представляется в виде к (х1+1/1) = кп , а для моментов кратных T как блочный
в виде
к (х+1//) =
2
(33)
где
к2 =
Д
J
Д
д
в
з
(34)
п
и
где
»у_ ЯУ +А
Я,- =у1 (х - )2 + (у — у у )2 + (* - ^)
<2 , / \2 + (* —
А
(35)
(36)
(37)
(■X -ху)-(х- Ху) + (у -Уу)-(У- у у) + ¿у)
^{х-Ху)2+{у-уу)2+{2-2у)2
(38)
ТБ-N ' ТЕ^N
+ (2 • 7Г • /У
Я ■ соб(2 ■ л ■ f ■Мг ■ О Я ■ бш(2 ■ л ■ f ■ М ■ I)
0
а11 а12 а13 X —Я ■ соб(2 ■ л ■ f ■М ■ г)
а21 а22 а23 у + (2■л^ f)2 ■ —Я ■ бш(2 ■ л ■ f ■М ■ г)
а31 а32 а33 г 0
ап - х + ап ■ у + а13 - г а21-х+а22-у + а23-г а31-х+а32-у + а33-2
+ (2 • 7Г • /)
0
я ■ соб(2 ■ л ■ f ■ М ■ г) Я ■ бш(2 ■ л ■ f ■ &■()
(39)
тб ^^ _ Т3Т2Т1>
(40)
Т =
1 0 0
0 СОБ(Г) Бт(г) 0 — Бт(г) соб(г )
, Т2 _
соб(р) бт(р) 0 — бт( р) соб( р) 0 0 0 1
, Т3 =
СОБ(^) 0 — ) 0 1 0 Бт(^) 0 соб(^)
гр— 1 _ гр— — 1— 1
ТБ_ Т1 Т2 Т3
(41)
Т1—1 _
10 0 0 СОБ(Г ) — Бт(г) 0 Бт(г) соб(г)
Т—1 _ Т2 _
соб( р) — Бт( р) 0 Бт(р) соб(р) 0 0 0 1
Т—1 _ Т3 _
соб(^) 0 бт(^) 0 1 0 — ) 0 соб(^ )
2
3
2
Определим элементы матрицы Нг.
Для моментов не кратных Т эта матрица представляется в виде Щ =
а для моментов кратных Т в виде Щ =
где
Щ 0
Н2 Щ з
Ж
дХ
Их =
дО дО дО
дх ду дх
дв2 до2 дОг
дх ду дх
ЯЪ дО
дх ду дх
дЬх дЬх дО
дх ду дх
дЬ2 дЬ2 до
дх ду дх
д^ до
дх ду дх
во, (х - х, )
дх я, '
(х
0
0 0 0 0 0 0
1 о о
0
дх
ду
ду
дх
0 0 0
0 0 0
дО дО ад
дх ду ы
до до до
дх ду дх
до до до
дх ду дх
_(У - у,)
я, ' дх
_(.У -у* до
я, ' дх
к, (х ~х])
0000 10 0 0 10 0 0
10 0 0
я,-
я,
я я
' 0 н
Н
дК
дХ
(У-Уу) (У-У^
ду ь ь
(¿-¿у) ь
д2 ь ь ь '
0 0 0 0 0 0 0 0 а11 а12 а13
0 0 0 0 0 0 0 0 а21 а22 а23
0 0 0 0 0 0 0 0 а31 а32 а33
Н,
ЭК
дГ
Ьц Ь12 Ь13 0 0 0
Ь21 Ь22 Ь23 0 0 0
Ь31 Ь32 Ь33 0 0 0
где
11
21
31
дТ
-1
5 ^ N
дг
■ Т
Е ^ N
Зс Ь12
у Ь22
Ь32
дТ,
-1
В ^ N
др
■ Т
Е ^ N
Зс Ь13
у Ь23
Ь33
дТ
-1
В ^ N
дК
■ Т
Е ^ N
X
У
дТ
-1
в ^ n
д г
дТ1 1 Т-1Т-1 д г
дТ1-1 _ дг
0 0 0 0 - Бт(г) - СОБ(Г) 0 соб(г ) - Бт(г)
дТ
дТв1 N - Т -1 дТ2 1 Т -1 _2
др 1 др 3 др
-1
- Бт( р) - соб( р) 0 соб( р) - Бт( р) 0 0 0 0
_ т-1 т-1 дТ3 1 - Т1 ■ Т2
дК
дК
дТ
-1
3 _
дК
-вт(к) 0 соб(к) 0 0 0 -сов(к) 0 - бт(к)
Определим вектор /(%, )
)= х,у, ¿, А, Зс, у, '¿, 0,0,0,0, г, р, К О, О, О
(42)
A( x / i ) = f
ox
О О О О i О О О О О О О О О О О О
О О О О О i О О О О О О О О О О О
О О О О О О i О О О О О О О О О О
О О О О О О О i О О О О О О О О О
О О О О О О О О i О О О О О О О О
О О О О О О О О О i О О О О О О О
О О О О О О О О О О i О О О О О О
О О О О О О О О О О О О О О О О О
О О О О О О О О О О О О О О О О О
О О О О О О О О О О О О О О О О О
О О О О О О О О О О О О О О О О О
О О О О О О О О О О О О О О О О О
О О О О О О О О О О О О О О О О О
О О О О О О О О О О О О О О О О О
О О О О О О О О О О О О О О i О О
О О О О О О О О О О О О О О О i О
О О О О О О О О О О О О О О О О i
ômin
s О О О О О О О О О О О О О О О О
О s О О О О О О О О О О О О О О О
О О s О О О О О О О О О О О О О О
О О О qii О О О qi2 О О О О О О О О О
О О О О g О О О О О О О О О О О О
О О О О О g О О О О О О О О О О О
О О О О О О g О О О О О О О О О О
О О О ?2i О О О q22 О О О О О О О О О
О О О О О О О О к О О О О О О О О
О О О О О О О О О к О О О О О О О
О О О О О О О О О О к О О О О О О
О О О О О О О О О О О l О О О О О
О О О О О О О О О О О О l О О О О
О О О О О О О О О О О О О l О О О
О О О О О О О О О О О О О О m О О
О О О О О О О О О О О О О О О m О
О О О О О О О О О О О О О О О О m
Параметры q11, q12, q21, q22 задаем с учетом (24) следующим образом:
(А?)3 (А? )2
_ с2^ Цр, q2l _ ql2 _ с2-%■ , q22 _ с2* ^ '(А?), а ^та^ью из
2
практических соображений 5 _ 0,0001м2, ё _ 0,000001 (м/с)2, к _ 0,000001 (м/с2)2, I _ 0,000004 (рад)2, т _ 0,0000004 (рад/с)2.
В соответствии с введенным вектором измерений — * представим матрицу N для моментов не кратных Т в виде Ni _ |пп|, а для моментов кратных Т как
блочную в виде N _
п00 0
0 п
11
п11 _
аП 0
0 0
0
аП 0
0 а2
а2 _ 0,0001 (м/с2)2
дисперсия шумов измерений
акселерометров.
п
00
а» 0
0 а
00 00 00
00
0 0 0 0 0 0
а
2
о о
о .2
а
о
о о
а
Б
0 0 0
о о
о о о
а
Б
а» _ 10 (м)2
2 2
дисперсия шумов измерений псевдо дальностей, а^ =0,01 (м/с)
дисперсия шумов измерений псевдо скоростей навигационным приемником
с темпом Т _ 0,1 с.
2
2
2
На рис. 2 представлены результаты исследования работы ФКТ по оценке углов ориентации, а именно И — курсового угла, методом моделирования. Причем углы крена и тангажа задавались равными по 10 градусов. Ряд 1 — задаваемое изменение угла курса во времени (приведено 39 отсчетов через 0,1 секунды). Ряд 2 — оценка курсового угла ФКТ.
Рис. 2. Результаты исследования работы ФКТ по оценке курсового угла
Выводы
1. Комплексированный указанным способом навигационный приемник позволяет наряду с оценкой координат и вектора скорости оценивать ориентацию рамы с установленной на ней антенной и акселерометрами.
2. Построение ФКТ при данном виде комплексирования не вызывает математических трудностей.
3. Адаптивный элемент ФКТ позволяет отслеживать курсовые маневры объекта, но при этом возникают переходные процессы, приводящие к ошибкам оценки курсового угла.
4. Вращение рамки позволяет производить оценку ориентации даже при прямолинейном и равномерном движении центра рамки.
Библиографический список
1. Перов А.И., Харисова В.Н. ГЛОНАСС. Принципы построения и функционирования - М: Радиотехника, 2010. - 800 с.
2. Поваляев А.А. Спутниковые радионавигационные системы: время, показания часов, формирование измерений и определение относительных координат. - М.: Радиотехника, 2008. - 328 с.
3. Сейдж Э., Мелс Дж. Теория оценивания и её применение в связи и управлении. -М.: Связь, 1976. - 496 с.
4. Вовасов В.Е., Бетанов В.В., Степанников В.М. Аналитическая оценка установившихся значений оцениваемых параметров и расчет элемента адаптации фильтров калмановского типа // Телекоммуникации. 2014. № 3. С. 2-8.
5. Поваляев А.А., Вейцель В.А., Мазепа Р.Б. Глобальные спутниковые системы синхронизации и управления в околоземном пространстве. - М.: Вузовская книга, 2012. - 188 с.
6. Вовасов В.Е., Ступак Г.Г., Бетанов В.В. Оценка ориентации высокодинамичного объекта с помощью фильтра калмановского типа, использующего измерения навигационных приемников сигналов глобальных навигационных спутниковых систем // Известия РАРАН. 2013. № 1 (76). С. 33-43.
7. Репин А.И., Меркишин Г.В., Попова Л.В. К вопросу об оптимизации параметров и структуры системы начальной ориентации навигационных систем в азимуте // Труды МАИ. 2010. № 39. URL: http://trudymai.ru/published.php?ID=14815
8. Борискин А.Д. Вероятностный и структурный метод сжатия данных глобальных навигационных спутниковых систем // Труды МАИ. 2010. № 41. URL: http: //trudymai .ru/published.php?ID=23765
9. Подкорытов А.Н. Высокоточное местоопределение в абсолютном режиме в ГНСС с использованием разрешения целочисленной неоднозначности псевдофазовых измерений // Труды МАИ. 2012. № 59. URL: http: //trudymai .ru/published.php?ID=34845
