Использование фильтра калмана в задаче определения ориентации тела, подвешенного на струне руководство по лабораторной работе

Федеральное агентство по образованию

Государственное образовательное учреждение

высшего профессионального образования

Московский физико-технический институт

(государственный университет)

Руководство по лабораторной работе

УДК 629.78, 531.38

В рамках Инновационной Образовательной Программы «Наукоемкие технологии и экономика инноваций» создается учебный процесс, основными целями которого являются развитие у студентов системного подхода к решению задач исследования движения сложных информационно-механических систем и повышение эффективности учебного процесса при изучении различных разделов теоретической механики и прикладной математики. В Руководстве изложены элементы теории адаптивного расширенного фильтра Калмана. На примере тела, подвешенного на струне, демонстрируется применение фильтра Калмана в задаче определения ориентации по измерениям гироскопа. Приведены методики проведения измерений и обработки полученных данных.

УДК 531.38: 629.78

© Московский физико-технический институт

(государственный университет), 2008

Содержание

Введение 4

1. Фильтр Калмана 5

1.1 Линейная задача. Основные понятия 5

1.2 Коэффициент обратной связи 7

1.3 Коррекция значения ковариационной матрицы ошибки 8

1.4 Фильтр Калмана для нелинейных систем 9

1.5 Использование фильтра Калмана 10

1.6 Адаптивный фильтр. Метод “Уточнение ковариации” 11

2. Применение фильтра Калмана 13

2.1 Перманентное вращение 13

2.2 Крутильные колебания 14

3. Макет 17

3.1 Схема макета 17

3.2 Волоконно-оптический гироскоп VG-910D 18

3.3 Интегральный гироскоп на базе iMEMS технологии 19

4.1 Работа программы 21

4.2 Интерфейс 22

4.3 Управление ПО 23

Задача определения ориентации тела заключается в нахождении кватерниона или матрицы перехода из неподвижной системы координат в систему координат, жестко связанную с телом. В настоящей лабораторной работе рассмотрим достаточно простую механическую систему – подвешенное на струне тело вращается вокруг вертикальной оси, другими движениями тела пренебрегаем. Задача определения ориентации такого тела – это задача нахождения угла поворота системы координат, жестко связанной с телом, относительно некоторой неподвижной системы координат. Решать эту задачу будем при условии, что известна угловая скорость вращения тела, которую с известной частотой выдаёт датчик угловой скорости. Решить такую задачу можно просто интегрированием кинематического уравнения , но реальные датчики угловой скорости выдают значение скорости с некоторой ошибкой. Таким образом, при интегрировании кинематического уравнения достаточно быстро накапливается ошибка определения ориентации.

В зависимости от предположений относительно характера и свойств помех предлагаются те или иные способы решения этой проблемы. Например, метод наименьших квадратов, который представляет собой фактически усреднение данных наблюдения, но при динамической оценке состояния системы он не очень удобен, так как требует накопление и обработки всех предыдущих измерений. Поэтому в этой лабораторной работе подробно будет рассмотрен фильтр Калмана – последовательный рекурсивный алгоритм оценки, использующий принятую модель динамической системы для получения оценки состояния системы.

При выполнении настоящей лабораторной работы преследуются следующие цели:

• 
  на примере тела, подвешенного на струне, получить навыки работы с фильтром Калмана;
  
• 
  провести сравнительный анализ двух фильтров Калмана, основанных на разных моделях движения;
  
• 
  ознакомиться с принципом работы современных датчиков угловой скорости;
  
• 
  получить навыки работы в среде Matlab.
  

Фильтр Калмана – последовательный рекурсивный алгоритм, использующий принятую модель динамической системы для получения оценки, которая может быть существенно скорректирована в результате анализа каждой новой выборки измерений во временной последовательности. Этот алгоритм находит применение в процессе управления многими сложными динамическими системами, например, непрерывными производственными процессами, самолетами, кораблями и космическими аппаратами. При управлении динамической системой, прежде всего, необходимо полностью знать её фазовое состояние в каждый момент времени. Но измерение всех переменных, которыми необходимо управлять, не всегда возможно, и в этих случаях фильтр Калмана является тем средством, которое позволяет восстановить недостающую информацию посредством имеющихся неточных (зашумленных) измерений.

Предполагаем, что стохастическая система может быть описана моделями динамики и измерений (для непрерывного и дискретного случаев), представленными в Табл. 1.

Модель	Непрерывное время	Дискретное время
Система
Измерения
Шум системы
Шум измерений

Задача фильтрации состоит в том, чтобы найти оценку вектора состояния системы , которую мы будем обозначать , являющуюся функцией измерений … и которая минимизирует средне квадратичную ошибку

где M – симметричная положительно-определенная матрица.

Ф
ильтр Калмана работает по системе прогноз-коррекция. Допустим, что в момент времени получена оценка вектора состояния системы и теперь необходимо получить оценку в момент . Для этого строим прогноз оценки , базируясь на , получаем измерения и далее корректируем оценку в момент , базируясь на прогнозе и измерениях, и получаем окончательную оценку вектора состояния . называется априори оценка, называется апостериори оценка.

Рис. 1 Принцип работы фильтра Калмана

Второй момент случайного процесса может быть описан в терминах ковариационной матрицы

Матрица P(t) называется ковариационной матрицей ошибки оценки вектора состояния (далее – ковариационная матрица ошибки).

Очевидно, что для получения прогнозируемой оценки необходимо интегрировать модельное динамическое уравнение с начальным условием . А каким образом мы можем получить прогнозируемую оценку ковариационной матрицы ошибки? Для того чтобы ответить на этот вопрос рассмотрим непрерывную модель системы

,

описанную в Табл 1. Решение этого уравнения с начальным условием x(t₀) и переходной матрицей состояния Ф(t, t₀) записывается в виде

Математическое ожидание этой величины –

Подставляя (2) в выражение (1), производя некоторые преобразования и вычисляя первую производную функции P(t), в результате получим

Это матричное дифференциальное уравнение Риккати [1].

Итак, получили уравнение продолжения ковариационной матрицы ошибки. Путем интегрирования уравнения (3) с начальным условием получим прогнозируемую оценку ковариационной матрицы ошибки .

1.2 Коэффициент обратной связи

Предположим, что измерения были сделаны в момент времени и эта информация используется для оценки вектора состояния х стохастической системы в момент времени .

Предполагается также, что измерения линейно зависят от вектора состояния и описываются уравнением

. (4)

Оценка базируется на наблюдениях (или измерениях) . Таким образом, она является функцией априори оценки и измерении и может быть записана в следующем виде:

Матрицы и пока неизвестны и их значения будут определяться таким образом, чтобы новая оценка удовлетворяли принципу ортогональности, условия которого могут быть записаны в виде

Подставляя в уравнение (6) значения и из выражений (1) и (5) и учитывая тот факт, что случайные последовательности и некоррелированы, после некоторых преобразований [1], выражаем коэффициент обратной связи в следующем виде:

Этот коэффициент является функцией от априори значения ковариационной матрицы ошибки.

Теперь выведем формулу для апостериори значения ковариационной ма­трицы ошибки (для корректировки ковариационной матрицы ошибки). По определению она записывается в виде

где

Подставляя (7) в (5), получим

Вычтем из обеих частей выражения (11) и подставим в него значение в соответствии с (4). В результате получим

или с учетом (10)

Подставляя уравнение (12) в уравнение (9) и принимая во внимание, что получим

Большинство динамических систем и сенсоров не являются линейными. Но методы фильтрации, разработанные для линейных систем, могут быть применены и для нелинейных систем. В этом разделе рассматриваются методы, которые позволят перенести методологию фильтрации Калмана на нелинейные задачи.

Предположим, что непрерывная или дискретная стохастическая система может быть представлена нелинейным динамическим уравнением и модельным уравнением, описывающим измерения (Табл. 2).

Табл. 2. Нелинейные модели динамики и измерений

Модель	Непрерывное время	Дискретное время
Система Измерения

Обозначим символом малое отклонение от оцениваемой траектории

Тогда в окрестности может быть представлена в виде

Таким образом, получаем

где

В свою очередь, измерения могут быть представлены в виде разложения в ряд Тейлора в окрестности следующим образом:

и

.

Слагаемые выше первого порядка малости здесь опущены. Если в разложении мы пренебрегаем членами высокого порядка, то возмущение может быть представлено так

,

где

В случае непрерывной системы матрицы частных производных имеют следующий вид

Подведем некоторый итог всего вышеизложенного на примере непрерывно-дискретной системы (непрерывная динамическая система с дискретными измерениями), которая и описывает большинство реальных задач. Такая система записывается следующим образом:

Допустим, что в момент времени t_k-1(+) получены апостериори значения и , то есть на шаге t_k-1(+) задача фильтрации выполнена и теперь необходимо определить и .

Априори значения оценок вектора состояния и ковариационной матрицы ошибки и можно получить путем интегрирования модельного уравнения и уравнения типа Риккати

,

с начальными условиями и . Так как рассматриваем нелинейную задачу, то для определения матриц воспользуемся уравнением (16). Выполнив данную процедуру, получим и .

Далее, находим линеаризованную матрицу чувствительности согласно уравнению (15).

Определяем коэффициент обратной связи , используя уравнение (8).

Находим апостериори значения и по формулам (11) и (13).

1.6 Проблема адаптивного фильтра. Метод “Уточнение ковариации”

Одной из основных проблем, с которой сталкиваются при работе с фильтром Калмана, является то, что как модель самой системы, так и модель измерений неидеальны. Статистические свойства шумов, а именно, их ковариационные матрицы являются теми параметрами, от которых зависит точность оценивания вектора состояния системы, то есть точность работы фильтра.

Обычно начальные значения ковариационных матриц шума измерений и шума процесса выбираются путём анализа некоторых эмпирических данных или путём моделирования различных ситуаций и далее считается постоянной. Если в процессе использования этих данных выясняется, что работа фильтра неудовлетворительна, то требуется новая настройка ковариационных матриц шумов. Появляется задача адаптивной фильтрации – параллельно с оценкой вектора состояния системы оценивать статистические свойства шумов процесса и измерений (ковариационные матрицы) с целью повышения точности работы фильтра. Один из методов такой оценки – метод “Уточнение ковариации”.

Этот подход базируется на идее, что ковариационная матрица шума процесса равна ковариационной матрице ошибки измерения, то есть

Введём величину, называемую прогнозируемая разность ошибки измерения

Для придания алгоритму большей статистической значимости будем рассматривать выборку из N, сечение случайного процесса

Аналогичный подход можно использовать для оценки ковариационной матрицы шума системы. Исходя из здравого смысла, его можно аппроксимировать с помощью оценок вектора состояния , полученных на двух последовательных шагах

Оценку ковариационной матрицы шума процесса можно записать с использованием N-выборочного среднего в виде

Общую схему для оценки ковариационной матрицы шума процесса и шума измерений можно представить в виде табл. 3.