CC BY
107
УДК 621.396.96
^1:10.15217/1ззп1684-8853.2016.2.89
АНАЛИЗ АЛГОРИТМА КОМПЛЕКСНОЙ ОБРАБОТКИ УГЛОМЕРНОЙ ИНФОРМАЦИИ В НАВИГАЦИОННОЙ СИСТЕМЕ ПОДВИЖНОГО ОБЪЕКТА
А. Ю. Каплина, канд. техн. наук
М. Г. Степанова, доктор техн. наук, профессор
аОАО «Радиоавионика», Санкт-Петербург, РФ
Постановка проблемы: при комплексировании инерциальных датчиков, входящих в состав системы навигации подвижного объекта, с навигационными датчиками других типов: цифровым магнитным компасом, модулем спутниковой навигации, другими приборами, — последние используются в качестве внешнего дополнительного источника информации для выделения и оценки по алгоритму калмановской фильтрации систематических (медленно меняющихся) ошибок инерциальных датчиков с их последующей коррекцией по разомкнутой или замкнутой схеме. В результате точность навигационной системы объекта определяется точностью оценок указанных ошибок и слабо зависит от информационного вклада измерений других датчиков. Цель: разработать и проанализировать алгоритм комплексной обработки угломерной информации в навигационной системе подвижного объекта. Результаты: применительно к угломерному каналу навигационной системы подвижного объекта предложен и проанализирован алгоритм комплексной обработки полного объема измерительной информации, построенный на основе фильтра Калмана. Рассмотрен и обоснован вклад отдельных компонентов алгоритма в общий процесс обработки. Продемонстрирована работа механизма весового суммирования, реализуемая калмановским алгоритмом фильтрации угломерных данных. Описаны варианты практического задания начальных условий для фильтра. Практическая значимость: сформулированный алгоритм комплексной обработки может использоваться как основа специального программного обеспечения навигационной системы подвижных наземных и околоземных объектов. С методической точки зрения полученные результаты полезны молодым специалистам для понимания и освоения прикладных аспектов калмановской фильтрации.
Ключевые слова — комплексная обработка, фильтр Калмана, угломерный канал, навигационная система, подвижный объект.
Введение
Статья опирается на принципы и алгоритмы, изложенные в основополагающих работах [1-4], а также прикладных статьях [5-8]. По замыслу и содержанию статья является обобщением результатов работы [9]. В доступной форме в ней применительно к конкретной прикладной задаче анализируется механизм комплексной весовой обработки данных различных угломерных датчиков, реализуемый алгоритмом калмановской фильтрации.
Объектом рассмотрения является угломерный канал навигационной системы подвижного наземного (околоземного) объекта, распространенный состав датчиков которого включает трехосевой цифровой магнитный компас (ЦМК), трехосевой датчик угловых скоростей (ДУС) и модуль спутниковой навигации (МСН) [5, 10]. Датчики обеспечивают измерение углов ориентации объекта — азимута ак к, тангажа р? к и крена у]?1^ соответствующих угловых скоростей , и путевого угла а£н. В со-
став канала обычно также входит трехосевой ак-селерометрический инклинометр (АИ), определяющий значения начальных (статических) углов ориентации объекта а0, р0, у0, в отсчете от которых проводятся дальнейшие угловые измерения в ходе его движения. Функциональная схема
комплексной обработки приведена на рисунке (обозначения а о, а и раскрываются далее).
Предполагается, что перед комплексной обработкой текущие измерения проходят предварительную обработку, обеспечивающую ввод систематических поправок (в частности, на магнитное склонение), независимое по измеряемым параметрам сглаживание и сжатие результатов и их привязку к единой шкале времени с одинаковым шагом Тк = 1к - tk__l. Считается также, что предварительно проводится высокоточная калибровка ДУС, ЦМК и АИ [10, 11], позволяющая при обработке компенсировать смещения и асимметрию коэффициентов передачи чувствительных осей датчиков.
АИ
„мк
ак
ЦМК РГ '
уГ .
Бортовой компьютер объекта
ДУС
а£н
МСН
^ x о=[&о,[зо,уо ]
:[ак,Рк,Ук]
■ Функциональная схема комплексной обработки
Обоснование алгоритма комплексной обработки
В соответствии с положениями теории оптимальной линейной фильтрации в процессе обработки измерений оценке подлежит текущий вектор состояния линейной динамической системы, описываемой уравнением (динамической моделью) вида
хк = Фхк-1 + +
н. у. Хо = х(0), к = 1,2,...,
(1)
где пй-1 — стандартный векторный гауссов белый шум с нулевым средним и единичной интенсивностью, пд-1 ~ N(0, I); wk-1 — известный вектор управления; Ф, D — переходные матрицы состояния; Г — матрица интенсивности шума Гпд-1.
Измерения описываются уравнением
У к = Нхк + Гу V
У Vк,
(2)
где vk ~ N(0, I) — стандартный векторный шум измерения; Гу — матрица интенсивности шума Гу
у
H — матрица измерения.
В этих условиях оптимальной по минимуму среднего квадрата ошибки оценивания (минимуму ковариационной матрицы ошибок) является рекуррентная процедура (фильтр Калмана)
Хк = ФХк-1 + РкНТ (Г- )-1 (ук - НФХк-1) +
-1
+ Рк Рк/к-
Dwk-1' Н-У-
'к/к-1 = ФРк1фТ + ГГ 'р-к-1 +нТ (Г у ГТ)-1 Н
х о,
'Т.
(3)
к/к-1 Рк/к-1
НТ (НР,
к/к-Н.у.
-1НТ + Г у ГТ
Ро,
-1
НР
к/к-1,
где Pk — ковариационная матрица ошибок оценивания; Pk/k_1 — экстраполированное на шаг значение этой матрицы.
Применительно к рассматриваемой прикладной проблеме алгоритм комплексной обработки угломерной информации решает задачу формирования по данным ЦМК, ДУС и МСН текущей калмановской оценки углов ориентации подвижного объекта.
В основу комплексирования положены следующие соображения (рассмотрение начнем с непрерывного времени г с последующим возвратом к дискретному времени
Между текущими значениями углов и угловых скоростей существует очевидная аналитическая зависимость Аа(г)
= и? (г), ао =а(*о);
¿Р(*)
а* ~иу
¿у(*) Аг
= и>у (г), Ро =р(*о);
= их ^ У о = У(го).
(4)
При известных функциях времени и2(Ь), иу(г), их(Ь) (известном управлении) эти соотношения могли бы быть исходными для составления исчерпывающей динамической модели изменения углов а(г), Р(г), у(г). Однако, поскольку на практике истинные значения угловых скоростей и2(£), иу(г), их(Ь) не известны, в соответствии с фундаментальной теоремой разделения и принципом распределения информации они заменяются на измеренные ДУС значения иДУС (г), и ДУС (г), и>ДУС(г) с последующим корректным учетом присущих им ошибок измерений.
Измеренные значения угловых скоростей представим в виде сумм
IV
дусг^-
(г) = и? (г)+еш г (г); ДУС (г) = иу (г) + еш у (г);
IV.
иДУС (г) = их (г)+еш х (г)
(5)
неизвестных истинных значений иг, и, их и ошибок измерений е№2, еиу, еи!х. В свою очередь, ошибки измерений запишем в виде
еих (г) = аихПих (г), еиу (г) = аиуПиу (г),
еиг (г) = аи Л г (6)
где пиг, пиу, пих — стандартные независимые белые гауссовы шумы, ^ ~ N(0, I); стих = стиу = сти2 = = сти — среднеквадратические ошибки измерения угловых скоростей (полагаются равными для всех трех каналов ДУС).
Соотношения (4) с учетом (5) и (6) можно переписать в виде
аа(г) дус , .ч , .ч
—— = IV? (г) -Стш Пи г (г);
Аг
(7)
® = иДУС (г)-а„ п^ (г);
0(г = иДУС (г) -иПих (г). аг
Переходя в (7) к конечным разностям, получим
ак = ак-1 + ТиДк-1 - Таи Пиг,к-1;
Рк = Рк-1 + ТиДкС1 - Таи Пиу ; (8) „ДУС
у к =у к-1 + Тих,к-1 - Таи Пих,к-1,
где т = - г
Сравнивая (1) и (8), убеждаемся, что они совпадают при введении следующих обозначений:
ак 1 0 0"
хк = Рк , Ф = 0 1 0
У к _ 0 0 1
к-1
,,Дус
"г,к-1
ДУС у,к-1
их,к-1
т 0 0
0 т 0
0 0 т
ПШ2,к-1 ~-Таш 0 0
пк-1 = пшу,к-1 , Г = 0 -Таш 0
пшх,к-1 0 0 -Та
Обозначая
„МК
ак
У к =
РМК уМк
сн
ГУ =
н
МК
^а
0 0 0
1 0 0
0 1 0
0 0 1
1 0 0
„МК „МК
„МК
„у,к
„сн
„а,к
МК
0 0
0
0 М
у 0
0 0 0
Сн
где „Мк, •••, „— стандартные независимые
белые гауссовы шумы угломерных каналов ЦМК
и азимутального канала МСН соответственно; МК сн
ста ..., ста — среднеквадратическая ошибка измерений в каналах, и приходим к уравнению измерения (2).
С учетом принятых обозначений соотношения (3) перепишутся в виде
хк = хк-1 + ркн К (Ук - Нхк-1) +
+ ркPk/1k-lDwk-l, Н-У- хо,
(9)
рк/к-1
= Рк-1 + Q;
гк/к-1
Рк =(-1 + НТ К-1Н )-1:
■рк/к-1нТ (нрк/к-1нт + к)
Н-У- р0,
где
Т2а1 0 0
Q = ГГт = 0 Т2а1 0
0 0 Т2а1
— ковариационная матрица (матрица дисперсий) возмущающего шума;
2МК аа 0 0 0
0 2МК а2 0 0
0 0 2МК ау 0
0 0 0 _2СН аа
К = ГУГУ =
— ковариационная матрица измерительного шума.
После простых промежуточных преобразований
А = Нт К-1 =
Vа 0 0
0 0 1
-,/ 2МК V ар 0 0
0 -,/ 2МК V стт 0
2СН
В = Нт К-1Н = АН =
((
2МК . аа +
V а;
2СН )
а
1 а
0
2МК
Р 0
1 а
0 0
2МК
соотношения (9) принимают окончательный вид хк = хк-1 + ркА (Ук - Нхк-1) +
+ ркрkг/1k-lDwk-l, н-У-хо; рк/к-1 = рк-1 + Ф
рк =(-1 + В )-1 =
(10) (11)
гк/к-1 " рк/к-
!НТ(
нр
к/к-1 Н-У- р0 •
Нт + К
-1
нр,
к/к-1,
(12)
Выражения (10)-(12) могут служить основой моделирования и практической реализации алгоритма комплексной обработки информации угломерного канала навигационной системы подвижного объекта.
Отметим, что здесь симметрическая матрица
а? к а,к Р.- аР,к Р" к ау,к
1 рк = Р, р а,к о2 р,к Р ру,к
Нрк-1/к, уа ,к Р Ур ,к а2 к У,к
есть текущая ковариационная матрица ошибок оценивания, диагональные элементы кото-
2 2 2
рой а. ,,а ,а — дисперсии ошибок оце-
а,к /> Р,к у,к
нок ак, Рк,у к, а Рар,к = Рра,к, Рау,к = руа,к, Рру к = Рур к — моменты взаимной корреляции
(ковариации) ошибок.
Анализ компонентов алгоритма
1. Рассмотрим второе слагаемое в (10). После несложных преобразований получим
РкА(ук -НХк-1) = Рк А
ак - а к-1
рМК - Рк-1 УМК У
у к - у к-1
ак - а к-1
(хмк-
ак а
к-1
СН
ас -сх
к-1
а:
_2МК
_2СН
а
аа
рМК - Рк-1 2МК а2 уМК -у у к у к-1 _2МК
а'
а,к
( МК-(Х ак сх, 2МК аа
СН
Л
к-1
*"к-1
2СН
аа
рМК-р уМК-у
+ р рк рк-1 + р ук у к-1 * * - 2МК аау,к _2МК
сх р,к
р
Р (х,k
Р" к
уа,к
( МК - СН -
ак -ак-1 + ак -ак-1
Л
аР
¡.МК
2МК 2СН
V аа аа у
( МК - СН - А
ак -ак-1 +ак -ак-1
+ а2 Рм "Рк-1 + р Р,к а2МК ру,к
ау
уМК -у Ук ук-1
а2 МК
V
а
_2МК
а
а
2СН
а
+ Р.-
/
ур,к
«МК у МК -
рк -рк-1 +а2 у к -у к-1
.2МК у,к _2МК
аР
ау
(13)
Очевидно, что этим слагаемым обеспечивается весовая обработка невязок (разностей измеренных и оценочных значений углов), при этом веса определяются как дисперсиями ошибок измерений ааМК,...,ааСН, так и текущими дисперсиями
а2а,к, аЩ, а|,к и ковариациями Рар,к, Рау,к, Рру,к
ошибок оценивания (последнее говорит о статической взаимосвязи оценок а к, Рк, у к).
Важно подчеркнуть, что в расчете дисперсий и ковариаций ошибок оценок участвует ковариационная матрица Q измерительных ошибок ДУС [см. соотношение (11)]. Тем самым обеспечивается их корректный учет.
2. Анализ первых слагаемых элементов вектора (13) показывает, что при наличии двух измерений азимута аМК, аСН больший вес в результирующей оценке придается измерению с меньшей
2СН 2МК
погрешностью. Так, при аа <аа в оценке
сн 2СН
превалирует измерение а^ . При аа ^ аа сн
вклад а к является определяющим (измере-
МК
ние ак в результате практически не участвует). И наоборот, при низкой точности (потере данных) азимутального канала МСН (ааСН >> ааМК) основной вклад в результирующую оценку дает азимутальный канал ЦМК (в пределе азимутальный канал модуля при обработке просто отсекается).
3. Особо рассмотрим третье слагаемое в (10). Наличие члена Dwk ^ = [тw Ду-1, Tw Д,уус1,
TwД'yСl ] , представляющего собой вектор парциальных приращений углов на Д-м шаге, вы-
звано необходимостью восполнить в текущей оценке углов Х к _1 вычитаемый из уД вектор приращений Dwk_2, содержащийся в оценке Хк-1 [см. (10) при Д - 1].
В этом состоит реализуемый в статье механизм комплексирования ДУС с другими датчиками углов. Множитель Р кР к11 служит здесь для пересчета слагаемого Dwk-1, обеспечивающего оптимальность предсказанного значения Хк_ 1, в соответствующее слагаемое для оптимальной оценки Хк. (Заметим, что в большинстве работ по комплексной фильтрации этот момент незаслуженно опускается.)
Задание начальных условий
На практике возможны различные варианты задания начальных условий в уравнениях (10) и (12), связанные с тем или иным набором измерительных данных, привлекаемых для их формирования на первом (нескольких первых) шаге поступления и обработки измерений. Простыми и наглядными являются следующие варианты.
Вариант 1. В качестве н. у. хо выбирается первый отсчет показаний ЦМК, т. е.
Х(о1} = [а^К, рМК, уМК ]Т. Дальнейший счет индекса Д проводится со второго измерительного шага. Очевидно, что в этом случае
Р(1) = Ро "
2МК
а
о о
о
а2МК а2
о о
2МК
Вариант 2. Этот вариант отличается от предыдущего тем, что вместо амк используется отсчет а^ (объект движется), т. е.
42) =kCH, PMK, nMK ]T;
P(2) _ P0 "
2CH
'а
0 0
_2MK ст2
0
0 0
_2MK
Вариант 3. Для формирования н. у. используются данные ДУС. Моментом времени t0 полагается время начала очередного маневра поворота объекта относительно углов, соответствующих времени гок окончания предыдущего маневра. Будем считать, что к моменту ¿ок обработка угловых измерений в ходе закончившегося маневра поворота обеспечивает высокую точность оценки начальных углов а ок, р ок, у ок. В этом случае
Х(3) _
-TW^, Рок + Tw^C, уок + Tw«f
Литература
1. Ли Р. Оптимальные оценки, определение характеристик и управление. — М.: Наука, 1966. — 176 с.
2. Ярлыков М. С. Статистическая теория радионавигации. — М.: Радио и связь, 1985. — 344 с.
3. Ярлыков М. С., Миронов М. А. Марковская теория оценивания случайных процессов. — М.: Радио и связь, 1993. — 464 с.
4. Кузовков Н. Т., Салычев О. С. Инерциальная навигация и оптимальная фильтрация. — М.: Машиностроение, 1982. — 216 с.
5. Иванов А. В. Навигационные системы подвижных наземных объектов. Алгоритмы обработки информации в угловом канале // Радиотехника. 2013. № 4. С. 46-54.
6. Каплин А. Ю., Степанов М. Г. Использование автономной навигационной системы высокоточного позиционирования пешехода на местности // Информационно-управляющие системы. 2015. № 6. С. 86-92. ао1:10.15217Д88п1684-8853.2015.6.86
7. Дэвидсон П., Такала Я. Разработка алгоритмов инерциальной навигационной системы с учетом
Т2ст1 0 0
P03) _ Q _ 0 Т2ст2а 0
0 0
Возможны другие, более сложные сочетания начальных измерений при формировании х 0, Pl
о-
Заключение
Данная работа выполнена в предположении постоянства априори известных значений сред-неквадратической ошибки погрешностей изме-
рении ctw, ст;
MK
ch
ста . На практике требуется
более детальное описание погрешностей и расчет текущих значений их среднеквадратических ошибок. Вместе с тем известные результаты теории и практики прецизионных измерений показывают, что возникающие при этом трудности в значительной мере устраняются применением к погрешностям так называемого «гарантирующего подхода» [12].
особенностей походки человека // Гироскопия и навигация. 2013. № 1. С. 86-94.
8. Salychev O. S. Applied Inertial Navigation: Problems and Solutions. — M.: BMSTU Press, 2004. — 306 p.
9. Каплин А. Ю., Степанов М. Г. Модель и алгоритм комплексной обработки информации азимутального канала пешеходной навигационной системы // Изв. вузов. Приборостроение. 2016. Т. 59. № 3. С. 181-188.
10. Иванов Д. С. и др. Калибровка датчиков для определения ориентации малого космического аппарата: препринт. — ИПМ РАН, 2010. № 28. — 30 с.
11. Пат. 2572109 РФ, МПК С1 G01C 17/38. Способ калибровки электронного магнитного компаса/ А. Ю. Каплин (РФ), М. Г. Степанов (РФ), А. Ю. Ве-ревкин (РФ). — № 2014129725/28; заявл. 18.07.14; опубл. 27.12.15, Бюл. № 1. — 2 с.
12. Бахшиян Б. Ц., Назиров Р. Р., Эльясберг П. Е. Определение и коррекция движения (гарантирующий подход). — М.: Наука. Гл. ред. физ.-мат. лит., 1980. — 360 с.
UDC 621.396.96
doi:10.15217/issn1684-8853.2016.2.89
Analysis of Algorithm for Complex Processing of Goniometric Information in a Moving Object Navigation System
Kaplin A. Y.a, PhD, Tech., a.kaplin@list.ru
Stepanov M. G.a, Dr. Sc., Tech., Professor, smg099@mail.ru
aOAO «Radioavionika», 4 B, Troitskii Pr., 190005, Saint-Petersburg, Russian Federation
Introduction: For complex inertial sensors combined in mobile unit navigation systems with navigation sensors of other types (digital magnetic compass, satellite navigation module and other devices), the latter are used as an external auxiliary source of information for the selection and evaluation (using the Kalman filtering algorithm) of systematic (slowly varying) errors of inertial sensors, and their subsequent correction for an open or closed circuit. As a result, the accuracy of the navigation system of the object is determined by the accuracy of the estimates of these errors and weakly depends on the contribution of the measurements from other sensors. Purpose: The goal is to develop and analyze an algorithm for complex processing of goniometric information in the navigation system of a movable object. Results: For the azimuth channel of a movable object navigation system, an algorithm built on the basis of the Kalman filter is proposed which performs complex processing of the total volume of the measurement information. The contribution of individual components into the processing is discussed and substantiated. We have demonstrated the functioning of the weight summation mechanism implemented by the Kalman filtering algorithm for goniometric data. Options have been described for defining the filter initial conditions. Practical relevance: The formulated comprehensive treatment algorithm can be used as a basis for a specific software navigation system of ground-based and near-Earth mobile objects. From the methodological point of view, the results can be useful for young professionals who want to understand and master the applied aspects of Kalman filtering. Keywords — Complex Processing, Kalman Filter, Azimuth Channel, Navigation System, Moving Object.
References
1. Li P. Optimal'nye otsenki, opredelenie kharakteristik i up-ravlenie [Optimal Assessment, Characterization and Management]. Moscow, Nauka Publ., 1966. 176 p. (In Russian).
2. Yarlykov M. S. Statisticheskaia teoriia radionavigatsii [Statistical Theory of Navigation]. Moscow, Radio i sviaz' Publ., 1985. 344 p. (In Russian).
3. Yarlykov M. S, Mironov M. A. Markovskaia teoriia otseniva-niia sluchainykhprotsessov [Markov's Estimation Theory of Stochastic Processes]. Moscow, Radio i sviaz' Publ., 1993. 464 p. (In Russian).
4. Kuzovkov N. T., Salychev O. S. Inertsial'naia navigatsiia i optimal'naia fil'tratsiia [Inertial Navigation and Optimal Filtering]. Moscow, Mashinostroenie Publ., 1982. 216 p. (In Russian).
5. Ivanov A. V. Navigation Systems Moving Ground Targets. Algorithms of Processing of the Information in the Angular Channel. Radiotekhnika, 2013, no. 4, pp. 46-54 (In Russian).
6. Kaplin A. Y., Stepanov M. G. Using Autonomous Navigation System of High-precision Positioning of a Pedestrian on the Ground. Informatsionno-upravliaiushchie sistemy [Information and Control Systems], 2015, no. 6, pp. 86-92 (In Russian). doi:10.15217/issn1684-8853.2015.6.86
7. Devidson P., Takala J. Development of Algorithms of Inertial Navigation System Based on Features of Human Gait. Giroskopiia i navigatsiia, 2013, no. 1, pp. 86-54 (In Russian).
8. Salychev O. S. Applied Inertial Navigation: Problems and Solutions. Moscow, BMSTU Press, 2004. 306 p.
9. Kaplin A. Y., Stepanov M. G. Model and Algorithm of Complex Information Processing Azimuth Channel Pedestrian Navigation System. Izvestiia vuzov. Priborostroenie, 2016, vol. 59, no. 3, pp. 181-188 (In Russian).
10. Ivanov D. S., at al. Kalibrovka datchikov dlia opredeleniia orientatsii malogo kosmicheskogo apparata [The Calibration of Sensors to Determine the Orientation of Small Spacecraft]. Moscow, IPM RAN Publ., 2010, no. 28. 36 p. Preprint (In Russian).
11. Kaplin A. Y., Stepanov M. G., Veryovkin A. Yu., et al. Spo-sob kalibrovki elektronnogo magnitnogo kompasa [A Method of Calibrating an Electronic Magnetic Compass]. Patent RF, no. 2572109, 2014.
12. Bakhshiyan B. Ts., Nazirov R. R., Elyasberg P. E. Opre-delenie i korrektsiia dvizheniia (garantiruiushchii podkhod) [Determination and Motion Compensation (guarantee approach)]. Moscow, Nauka Publ., 1980. 360 p. (In Russian).
