CC BY
391
УДК 656.6+523
А.С. Конаков, В.В. Шаврин, В.И. Тисленко, А.А. Савин
Сравнительный анализ среднеквадратической погрешности определения координат объекта в бесплатформенной инерциальной навигационной системе при использовании различных алгоритмов нелинейной фильтрации
Приведены результаты статистического анализа среднеквадратических погрешностей определения координат в бесплатформенной инерциальной навигационной системе при решении задачи совместной оценки координат и параметров модели датчиков ускорения и угловой скорости с использованием алгоритмов расширенного фильтра Калмана, сигма-точечного фильтра Калмана, простого и модификацированного алгоритмов кубатурного фильтра Калма-на и алгоритма на основе «фильтра частиц».
Ключевые слова: бесплатформенная инерциальная навигационная система, нелинейное оценивание, фильтр Калмана, фильтр частиц, нелинейная калмановская фильтрация.
В современных интегрированных навигационных системах в ряде случаев возникает необходимость использования автономного режима работы. В частности, автономное решение навигационной задачи может быть получено путем применения бесплатформенной инерциальной навигационной системы (БИНС). Основой современных БИНС является микроэлектромеханическая система (МЭМС), состоящая из датчиков линейных ускорений и угловых скоростей. Она имеет малые габариты, низкие энергопотребление и стоимость, в ней нет ограничений на угловые маневры объекта и возможна работа в любом базисе. Главный недостаток всех инерциальных систем - накопление с течением времени погрешностей оценок координат, скорости и угловой ориентации.
Уравнения сложного движения объекта, необходимые для формирования оценок его координат, скорости и пространственной ориентации, представляют собой систему нелинейных дифференциальных уравнений (СНДУ) первого порядка [1] и определяют СНДУ вектора состояния х (?) в виде
X () =
Гм (?) уЪх1 (?)
у Ъх1 (?) = % (?) + С(0(?))а(?)- 2 [ш з х у(?)]
0 4x1 (?) 2 о(<Ш
(1)
где г (?) - вектор положения объекта; у(?) - вектор линейной скорости объекта в инерциальной системе отсчета (ИСО); 0(?) - кватернион, задающий переход из связанного базиса в навигационный, собственный базис которого - связанный базис; а(?) - вектор кажущегося ускорения объекта в связанном базисе; шз - угловая скорость вращения Земли (7,292115Е-5 рад/с). Матрицы С(0(?)) и й(?) в (1) имеют следующий вид [1]:
С(д(? ))=
Ч02 + ч! - Ч22 - ЧЪ2 2(Ч1Ч2 - 40ЧЪ) 2(Ч1ЧЪ + 4042)
2((2 + 40ЧЪ) Ч02 - Ч12 + Ч22 - ЧЪ2 2(Ч2ЧЪ - 40Чі)
2(Ч1ЧЪ - 4042) 2(ЧЪ + Ч04і) Ч02 -Ч12 - Ч22 + ЧЪ2
о(?) =
0 -ах -а у -а г
ах 0 аг -а у
а У -а г 0 ах
а2 а у -ах 0
где ш(?) =
(?) = [®х ,Ю у , ] - вектор угловой скорости объекта, вычисленный в связанном базисе.
6 ЭЛЕКТРОНИКА, ИЗМЕРИТЕЛЬНАЯ ТЕХНИКА, РАДИОТЕХНИКА И СВЯЗЬ
В БИНС в качестве входных данных при решении СНДУ (1) используются сигналы У (?) , формируемые МЭМС датчиками линейных ускорений а(?) и угловых скоростей ш(?). Эти данные содержат аддитивную случайную и систематическую погрешность и в общем случае имеют неизвестное значение масштабных коэффициентов. Для типовых МЭМС математическую модель наблюдений У (?) определяют в виде [2]
¥(?) =
АЪх1 (?) йЪх1 (?)
п а (?)
пю (?)
(2)
8 да(?) + а(?)+Ь 8юш(?) + ш(?) +Ь
где А (?) - вектор кажущихся ускорений (показания акселерометра); Q(?) - вектор угловых скоростей (показания гироскопа); 8а , 8Ю - диагональные матрицы размером 3x3, с элементами главной диагонали, равными соответствующим масштабным коэффициентам; па (?) , пю (?) - аддитивные
белые гауссовские шумы по ускорению и угловой скорости, некоррелированные между собой и имеющие нулевые средние значения.
Наряду с неточностью знания начальных условий х(0) для СНДУ (1), основная причина нарастания навигационных погрешностей в БИНС - увеличение дисперсии решения (координат и скорости объекта) при интегрировании (1) в связи с наличием смещения и случайного шума [2]. В связи с этим представляет интерес применение теории фильтрации для совместной оценки координат объекта и неизвестных параметров модели датчиков МЭМС.
Задача совместной оценки координат объекта и параметров модели датчиков МЭМС. Необходимо по зашумленным наблюдениям А(?), Q(?) восстановить вектор состояния х(?), эволюция
которого задается СНДУ (1). Для повышения точности в вектор состояния следует включить масштабные коэффициенты и смещения нулей. Их математические модели могут быть определены в виде простых гаусс-марковских процессов с порождающими ДУ первого порядка в виде (время корреляции указанных процессов составляет около нескольких часов)
«а_3х1(?) = —«а_3х1 (?) + П^ (?) ; «Ю_3Х1 (?) = —4_3х1 (?) + П5Ш(?);
Ьа_Ъх1 (?) = —Ьщ_Ъх1 (?) + ПЬа (?) Ьа_Ъх1 (?) =—Ьа_Ъх1 (?) + Ща (?). тЬш тЬа
При этом размерность вектора состояния возрастает до 28 при размерности вектора наблюдения, равной 6. Причем 10 переменных состояния (г, V, 0) не связаны непосредственно с наблюдениями. Этот факт составляет известную сложность при оценивании подобных систем [Ъ, 4].
Анализ матрицы наблюдаемости, составленной для линеаризованных уравнений состояния [5], показал, что ее ранг меньше, чем размерность вектора состояния, что свидетельствует об отсутствии наблюдаемости системы [5]. Однако, несмотря на неустойчивость решения задачи фильтрации, можно ожидать уменьшения скорости возрастания погрешности по сравнению со случаем непосредственного интегрирования (1).
Математическое моделирование задачи фильтрации с оценкой расширенного вектора состояния показывает, что несмотря на отсутствие наблюдаемости и расходимость оценки вектора состояния, применение фильтрации приводит к уменьшению среднеквадратической погрешности (СКП) координат объекта примерно на 20%. Таким образом, в условиях применения только БИНС использование фильтрации координат совместно с оценкой параметров модели датчиков МЭМС целесообразно и это, очевидно, существенно увеличивает требования к вычислительным ресурсам. В случае ком -плексирования БИНС с другими системами применение фильтрации, очевидно, нецелесообразно из-за усложнения структуры получаемых алгоритмов.
Поскольку уравнения состояния нелинейны и не существует строго оптимального нелинейного алгоритма оценивания (подобного знаменитому фильтру Калмана для линейных систем), то практический интерес представляет поиск квазиоптимального нелинейного фильтра, который обеспечивает меньшую величину СКП оценок координат объекта. При этом, очевидно, следует достигнуть приемлемого компромисса между необходимой точностью и доступными вычислительными ресурсами. Таким образом, критерий для выбора квазиоптимального алгоритма нелинейной фильтрации можно определить в виде минимальной величины СКП при заданном времени выполнения вычислений на текущем временном шаге.
Существует два основных способа аппроксимации байесовского фильтра в нелинейной задаче [3, 4]. Первый использует прямую локальную аппроксимацию нелинейных функций - правых частей в (1) и (2). Второй основан на косвенной глобальной аппроксимации апостериорной плотности вероятностей вектора состояния при заданных наблюдениях.
По первому способу реализуют алгоритмы расширенного фильтра Калмана (EKF - extended Kalman filter) [6], сигма-точечного фильтра Калмана (UKF - unscented Kalman filter) [7], кубатурного фильтра Калмана (CKF - cubature Kalman filter) [3, 4], модификацированного кубатурного фильтра Калмана с использованием квадратного корня (SCKF - square-root cubature Kalman filter) [4]. Второй подход реализуется в различных модификациях алгоритма, известного как «фильтр частиц» (PF -particular filter) [8].
Алгоритм EKF предполагает разложение нелинейных функций в уравнениях состояния и наблюдений в ряд Тейлора с ограничением линейными членами и последующим применением линейного фильтра Калмана [6]. В алгоритме UKF текущие значения оценок экстраполяции состояния и их ковариационных матриц формируются в виде выборочных оценок на конечном множестве сигма-точек, образуемых на каждом шаге вблизи текущего значения оценки состояния [7]. Алгоритм CKF основан на применении сферически радиального способа вычисления объема, позволяющего численно вычислить многомерные интегралы, которые возникают в задачах байесовской квазиоп-тимальной фильтрации [3, 4]. При вычислении необходимо перейти к сферическим координатам, разбить интеграл на два и выполнить вычисления, применяя принципы теории суперсимметрии и квадратурного метода Гаусса-Лагерра [4]. Алгоритм «фильтра частиц» основан на использовании рекурсивной процедуры при вычислении многомерного интеграла для апостериорного среднего вектора состояния методом Монте Карло. В итоге оценка вектора состояния представляет собой взвешенную сумму «частиц - точек», выбираемых из подходящего распределения вероятностей [8].
В рамках данного исследования было проведено статистическое моделирование для двух моделей движения: равноускоренное движение с кажущимся ускорением по двум осям, равным 1 м/с2, движение по окружности с постоянной угловой скоростью 0,15 рад/с и нормальным постоянным ускорением 1 м/с2. Параметры моделирования были заданы соответствующими микроэлектромеха-ническому датчику фирмы Analog Devices марки ADIS16354 (табл. 1). Параметры начальных условий (НУ) выбраны в расчете на возможность их получения от СРНС (GPS и/или ГЛОНАСС). Моделируемое время работы - 60 с. Усреднение проводилось по 100 реализациям. Частота поступления данных составляла 1 Гц.
Таблица 1
Параметры моделирования
СКО акселерометра, м/с2 0,00170
СКО гироскопа, рад/с 0,00375
Масштабный коэффициент акселерометра 0,01200
Масштабный коэффициент гироскопа 0,00200
Дрейф нуля акселерометра, м/с2 -0,02000
Дрейф нуля гироскопа, рад/с -0,00250
СКО масштабного коэффициента акселерометра 0,00200
СКО масштабного коэффициента гироскопа 0,00020
СКО дрейфа нуля акселерометра, м/с2 0,03000
СКО дрейфа нуля гироскопа, рад/с 0,00300
В результате математического моделирования получена зависимость СКП координат от времени для различных квазиоптимальных алгоритмов нелинейной фильтрации. Сравнение алгоритмов выполнялось по величине СКП на 60-й секунде с учетом времени, необходимого для формирования оценок.
На рис. 1 приведены величины нормированного машинного времени выполнения вычислений на текущем временном интервале для пяти квазиоптимальных алгоритмов фильтрации координат объекта. Нормирование времени выполнено относительно времени вычислений для алгоритма «фильтра частиц» как наиболее затратного по объему вычислений.
EKF UKF CKF SCKF PF EKF UKF CKF SCKF PF
■ прямолинейное движение
■ криволинейное движение
а б
Рис. 1. Зависимость нормированного времени работы от применяемого алгоритма оценивания (а), СКО погрешности определения местоположения в метрах при использовании различных алгоритмов нелинейного оценивания при равноускоренном и вращательном движении (б)
Для верификации полученных данных был проведен эксперимент (табл. 2). В эксперименте использовался микро электромеханический датчик Analog Devices ADIS16354. Начальные условия задавались с той же точностью, что и при моделировании.
Таблица 2
Параметры эксперимента
СКО акселерометра, м/с2 0,00172
СКО гироскопа, рад/с 0,00310
Масштабный коэффициент акселерометра 0,01170
Масштабный коэффициент гироскопа 0,00190
Дрейф нуля акселерометра, м/с2 -0,01950
Дрейф нуля гироскопа, рад/с -0,00242
СКО масштабного коэффициента акселерометра 0,00183
СКО масштабного коэффициента гироскопа 0,00019
СКО дрейфа нуля акселерометра, м/с2 0,02800
СКО дрейфа нуля гироскопа, рад/с 0,0029б
Методика эксперимента состояла в том, что данные акселерометра и гироскопа в течение 60 с с интервалом времени 1 с поступали в вычислитель для формирования оценок координат, скорости, угловой ориентации, масштабных коэффициентов и смещений нуля. Статистическое усреднение выполнялось по 50 независимым реализациям. Результаты по СКО погрешности местоположения приведены в табл. 3. Оценки параметров модели датчиков МЭМС не расходятся, но улучшение их СКО незначительно и составляет единицы процентов, что ожидаемо.
Таблица 3
Результаты эксперимента
Алгоритм нелинейной фильтрации EKF UKF CKF SCKF MPF
ЖЭ ошибки определения местоположения, м 33,9 32,1 28,0 32,5 28,5
Заключение. В результате проведенного исследования можно утверждать, что все фильтры, основанные на прямой локальной аппроксимации байесовского фильтра, требуют одинаковых вычислительных ресурсов, затрачивая на один шаг практически равное время. Точность определения ко -ординат объекта данными фильтрами приближенно одинакова. Наилучший результат при моделировании получен при использовании алгоритма 8СКЕ Также этот алгоритм обладает большей устойчивостью, чем алгоритмы СКБ и ИКБ (в нем искусственно обеспечивается положительная определенность ковариационной матрицы за счет применения рЯ-разложения, тогда как в СКБ и ИКБ применяется разложение Холецкого [3, 4, 7]). Низкая точность при моделировании связана с тем, что гироскоп не был откалиброван, и, как следствие, начальные данные о величине дрейфа нуля и масштабном коэффициенте имели низкую точность.
Низкая точность фильтра частиц связана с использованием малого числа частиц (#=100, типичное значение составляет от 1000 до 10000). При использовании фильтра частиц существует возможность увеличения точности за счет применения более мощного вычислителя.
Литература
1. Исследование статистических свойств оценок координат в бесплатформенной инерциальной навигационной системе с использованием кватернионного метода преобразования базисов / А.С. Конаков, В.В. Шаврин, А.А. Савин, В.И. Тисленко // Доклады Томского государственного университета систем управления и радиоэлектроники. - 2011. - № 2 (24), ч. 1. - С. 49-53.
2. Первачев С.В. Радиоавтоматика. - М.: Радио и связь, 1982. - 620 с.
3. Arasaratnam I. Cubature Kalman Filters / I. Arasaratnam, S. Haykin // IEEE Transactions on Automatic Control. - 2009. - Vol. 54. - P. 1254-1269.
4. Arasaratnam I. Square root quadrature Kalman filtering / I. Arasaratnam, S. Haykin // IEEE Transactions Signal Process. - 2008. - Vol. 56. - P. 2589-2593.
5. Квакернаак Х. Линейные оптимальные системы управления / Х. Квакернаак, Р Сиван. - М.: Мир, 1977. - 638 с.
6. Сейдж Э.П. Теория оценивания и ее применение в связи и управлении / Э.П. Сейдж, Дж. Мелс. - М.: Связь, 1976. - 496 с.
7. Julier S.J. A new method for nonlinear transformation of means and covariance’s in filters and estimators / S.J. Julier, J.K. Ulhmann // IEEE Transactions on Automatic Control. - 2000. - Vol. 45. - P. 472478.
8. Andrieu C. Particle filtering for partially observed Gaussian state space models / C. Andrieu, A. Doucet // Journal of the Royal Statistical Society. - 2002. - Vol. 64, № 4. - P. 827-8336.
Конаков Алексей Сергеевич
Студент каф. радиотехнических систем ТУСУРа
Тел.: 8-(382-2) 41-36-70
Эл. почта: aleksey.konakov@gmail.com
Шаврин Вячеслав Владимирович
Магистрант каф. радиотехнических систем ТУСУРа
Тел.: 8-(382-2) 41-36-70
Эл. почта: svv281088@sibmail.com
Тисленко Владимир Ильич
Д-р техн. наук, профессор каф. радиотехнических систем ТУСУРа
Тел.: 8-(382-2) 41-36-70
Эл. почта: wolar1491@yandex.ru
Савин Александр Александрович
Канд. техн. наук, доцент каф. радиотехнических систем ТУСУРа Тел.: 8-(382-2) 41-36-70 Эл. почта: saasavin@mail.ru
Konakov A.S., Shavrin V.V., Tislenko V.I., Savin A.A.
Comparative analysis of a standard error of determining the coordinates of the object in the strapdown inertial navigation system using different algorithms for nonlinear filtering
The results of statistical analysis of mean-square error in determining the coordinates of strapdown inertial navigation system to solve the problem of joint estimation of the coordinates and parameters of the model acceleration sensors and angular velocity using the extended Kalman filter algorithm, the sigma point Kalman filter, a simple and modifikatsirovannogo cubature Kalman filter algorithm and the algorithm based on the «particle filter».
Keywords: strapdown inertial navigation system, nonlinear estimation, Kalman filter, particle filter, nonlinear Kalman filtering.
