Сравнение методов построения фильтра Калмана для инерциального измерительного модуля

Аннотация

The aim of the study is to select the Kalman filter synthesis method for control problems. The first method is based on an extension of the filter state vector. The second method is based on the information distribution principle. Mathematic models for inertial measurement unit with both filters are considered. The results of filter measurement experiments are performed.

В настоящее время микромеханические измерительные системы находят широкое применение для задач навигации и управления. Особенно эти системы актуальны для малых необитаемых морских подвижных объектов, которые, ввиду малых масс и габаритов, не предназначены для размещения высокоточных и громоздких навигационных средств измерения.

Объектом исследования в данном докладе является измерительный модуль MPU6050, в состав которого входит микромеханические гироскоп и акселерометр. Измерение проекций линейного ускорения по трем осям позволяет вычислять углы крена и дифферента модуля, гироскоп обеспечивает измерение соответствующих угловых скоростей. Однако микромеханические чувствительные элементы отличаются довольно низкой точностью и помехозащищенностью, поэтому для получения приемлемых оценок параметров движения целесообразно использовать алгоритмы калмановской фильтрации.

Один из методов получения оценок углов и угловых скоростей основан на построении фильтра Калмана для модели двойного интегратора [1] и предполагает дальнейшее увеличение порядка модели для учета внешних возмущений. Другой метод, использующий принцип распределения информации, позволяет получить математическую модель и фильтр Калмана в упрощенном виде. Целью работы является сравнение этих методов для измерения угловых перемещений модуля MPU6050.

Внешний вид измерительного модуля представлен на рис. 1. В состав модуля интегрирован трехосный акселерометр, трехосный гироскоп и цифровой процессор для первичной обработки измерений. Обмен данных поддерживается интерфейсной шиной I2C. Встроенные в модуль аналоговоцифровые преобразователи обеспечивают измерения на выходе модуля в цифровом виде. Модуль программно может быть настроен на разные диапазоны измерения угловой скорости (±250, ±500, ±1000, и ±2000 градусов в секунду) и линейного ускорения (±2, ±4, ±8 и ±16 g). Погрешности измерений приведены в спецификации модуля [2].

Углы крена (θ) и дифферента (ψ) модуля могут быть получены по данным акселерометра. В модуле MPU6050 предусмотрен аналоговый вход для внешнего компаса, что позволит обеспечить измерение ориентации по трем углам Эйлера. Микромеханические гироскопы измеряют угловые скорости по трем осям.

Математическая модель измерения углов и углов скоростей с помощью модуля имеет вид дискретной линейной модели:

где x_k, y_k – векторы состояния и измерения модуля; A_d, B_d, C_d – матрицы состояния, входа и выхода модели; w_k, v_k – векторы возмущения и шумов измерения, представляющие собой случайные процессы типа «гауссовский белый шум» с ковариациями соответственно R, Q.

Вычисление фильтрованных оценок представляет итерационную процедуру, включающую:

1) вычисление априорных оценок и априорной матрицы ковариации ошибки:

где x^-_k – вектор оценок переменных состояния модели; P_k – априорная матрица ковариации ошибки оценивания;

2) вычисление коэффициентов фильтра Калмана:

3) вычисление скорректированных оценок и матрицы ковариации ошибки:

где I – единичная матрица.

Математическую модель для измерения (1) можно выбирать по‑разному. Типовой моделью для измерения угла и угловой скорости является модель двойного интегратора. Однако при этом необходимо ввести дополнительное уравнение, моделирующее возмущение модуля на основе белого шума, проходящего через инерционное звено [3]. Уравнение состояния из модели (1) принимает вид:

где μ_k – винеровский процесс, определяемый гауссовым белым шумом с малой или медленно меняющейся ковариацией. Модель измерения соответственно имеет вид

Итерационные выражения для пошагового вычисления коэффициентов фильтра и матриц ковариации ошибки имеют довольно громоздкий вид и не приведены в тексте. Однако модель фильтра можно существенно упростить на основе принципа распределения информации [4]. Этот принцип может быть применен при наличии дополнительной информации о погрешностях средств измерения.

Известно, что в модуле MPU6050 измерения акселерометра имеют существенные выбросы и искажения, обусловленные действием боковых ускорений. Для микромеханических гироскопов модуля характерен дрейф, приводящий к ошибкам при интегрировании, однако краткосрочные измерения угловой являются достаточно точными. В этом случае можно использовать измерения угловой скорости гироскопом в математической модели (1) в качестве возмущений, корректируя по ним показания акселерометра. При этом в вектор состояния целесообразно внести угловую скорость дрейфа гироскопа ω_xb. Тогда дискретная математическая модель состояния принимает вид [5]:

Уравнение измерения также изменяется:

В этом случае уравнения (2)–(4) существенно упрощаются, для коэффициентов фильтра и параметров модели можно составить итерационные выражения:

Расчет фильтра с моделью измерения (6) рассчитывается быстрее, чем с моделью (5), настраивается двумя параметрами Q_ω и R_θ , которые могут быть выбраны исходя из характеристик модуля.

Результаты испытаний измерительного модуля без обработки фильтром приведены на рис. 2 (для неподвижного состояния модуля). Из рис. 2 видно, что отдельные выбросы измерения акселерометром достигают 1.5°, а гироскоп имеет дрейф (угловая скорость составляет примерно 0.005 °/с).

В качестве исходных параметров для фильтра Калмана, согласно настройкам модуля MPU6050 были выбраны следующие ковариации:

Q_ω=0.05; R_θ= 0.05; R_ω=0.03

На рис. 3 приведены результаты работы фильтра Калмана 2‑го порядка при повороте модуля вдоль оси x. Из рисунка видно, что фильтр обеспечивает сглаженную оценку угла, с точностью до 0.2°. На рис. 4 сведены результаты работы двух фильтров на основе моделей (5) и (6).

Из рис. 4 видно, что расширенный фильтр Калмана позволяет получить более качественные оценки угла поворота модуля. Наличие дополнительного коэффициента обратной связи позволяет повысить быстродействие фильтра, в результате уменьшается запаздывание сигнала оценки. Однако расширенный фильтр требует более тщательной настройки

Из результатов работы видно, что оба фильтра Калмана обеспечивают достаточно стабильную точность восстановления угловой ориентации модуля. Фильтр, построенный на основе модели (6) отличается простотой реализации. Фильтр, построенный на основе модели (5) имеет лучшую точность оценивания, кроме того, наличие дополнительных коэффициентов для настройки открывает дополнительные возможности для отказоустойчивой адаптации фильтра [1, 3].

Таким образом, модуль MPU6050 с алгоритмами калмановской фильтрации может быть использован для измерения угловых параметров движения и решения задач стабилизации крена и дифферента, а при наличии внешнего измерителя направления – стабилизации угла курса

Список использованной литературы

1. Хабаров С.П., Коренев А.С., Амбросовский В.М. Отказоустойчивый адаптивный к внешним возмущениям фильтр Калмана // Морская радиоэлектроника–СПб .:Отраслевые журналы ,2015. № 3, c. 20–23.
2. MPU‑6000 and MPU‑6050 Product Specification. [Электронный ресурс]. – Режим доступа: www.cdiweb.com/datasheets/invensense/MPU6050_DataSheet_V3%204.pdf
3. В.М. Амбросовский, А.С. Коренев, Ю.А. Лукомский, А.Г. Шпекторов. Координированное управление морскими подвижными объектами. СПб.: Изд‑во СПбГЭТУ «ЛЭТИ», 2016. 128 с.
4. Степанов О.А. Применение теории нелинейной фильтрации в задачах обработки навигационной информации. СПБ: ГНЦ РФ – ЦНИИ «Электроприбор», 1998. С. 370.
5. Brandon McCarron. Low‑Cost IMU Implementation via Sensor Fusion Algorithms in the Arduino Environment [Электронный ресурс]. – Режим доступа: http://digitalcommons.calpoly.edu/cgi/viewcontent.cgi?article=1114&context=aerosp