Табл. 3 Уточнение ковариации для оценки R

	Действия	Оценка R	Оценка Q
1	Начальные условия	j=1,	j=1,
2	Аппроксимация шума
3	Вычисление
4	j=N	Подставляем в фильтр	Подставляем в фильтр
5	j	Увеличиваем j на 1 и переходим к шагу 2

Теперь используем фильтр Калмана применительно к нашей задаче определения ориентации тела, подвешенного на струне, учитывая, что имеются дискретные измерения угловой скорости, поступающие от датчика. Рассмотрим две модели.

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

Вектор состояния и динамическое уравнение в нашем случае

Уравнение можно записать по другому:

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

Проинтегрируем уравнение (18), получим:

Пусть ковариационная матрица ошибки имеет следующие элементы:

Тогда матричное уравнение Риккати (3) с начальными условиями

Коэффициент обратной связи, согласно (8)

Тогда апостериори оценка вектора состояния и матрицы ковариации из (11) и (13) соответственно имеют вид

Таким образом, получили все формулы, необходимые для работы фильтра Калмана, основанного на модели перманентного вращения.

Во второй модели предполагаем, что тело совершает малые одномерные крутильные колебания относительно вертикальной оси. Здесь используется [2] тот факт, что при малом погонном угле закрутки механический момент, действующий со стороны струны, пропорционален углу отклонения от положения равновесия

где R – радиус упругой струны, l – её длина, G – модуль сдвига, f – модуль кручения. Погонный угол закрутки α определяется выражением

где k - число оборотов нити, l - ее длина.

Таким образом, уравнение движения системы выглядит следующим образом [3]:

. (20)

Здесь I – момент инерции тела относительно оси вращения, - угол поворота тела относительно положения равновесия. Обозначим

Вектор состояния и динамическое уравнение в этом случае имеют вид

или

Модель измерения

Проинтегрируем уравнение (21):

Матричное уравнение Риккати (3) с начальными условиями

Коэффициент обратной связи согласно (8)

Тогда апостериори оценка вектора состояния и матрицы ковариации из (11) и (13)

Макет представляет собой автономный прибор, включающий в себя систему электропитания, систему определения ориентации и систему связи. Схема макета изображена на рис.2, сам макет – на рис.3.

Система электропитания включает в себя два аккумулятора и преобразователь напряжения. Преобразователь из 17 вольт от аккумуляторов формирует 5 вольт, необходимых для питания бортового компьютера и микромеханического гироскопа iMEMS ADIS16100.

Система определения ориентации состоит из бортового компьютера JRex CE (далее БК), оптоволоконного гироскопа VG-910D и микромеханического гироскопа iMEMS ADIS16100. БK производит считывание информации об угловой скорости с одного из гироскопов, обрабатывает её с помощью программного обеспечения (далее ПО) в режиме реального времени и сохраняет результаты в файл. Передача информации производится через USB порты БК, используется разветвитель USB-Hub.

Система связи состоит из устройства Wi-Fi и БК. Устройство Wi-Fi позволяет организовать доступ к БК через удалённый рабочий стол. Управление ПО происходит со стационарного компьютера через удалённый рабочий стол БК.

Остановимся подробнее на датчиках угловой скорости.

VG-910D – это одноосный прецизионный датчик вращения, выполненный по волоконно-оптической технологии, производства компании ФИЗОПТИКА [4]. Его функционирование основано на релятивистском эффекте Саньяка. Датчик содержит волоконно-оптический преобразователь и блок электроники, реализующий современные алгоритмы обработки аналоговых сигналов.

VG-910D предназначен для использования в различных областях техники с целью измерения угловой скорости вращения. Выходной цифровой сигнал пропорционален угловой скорости с некоторым масштабным коэффициентом. Диапазон измерений град/сек, цифровой выход угловой скорости – 24-х разрядное число в дополнительном коде, входное напряжение – 5V, прибор может питаться через USB кабель. Датчик весит 130 грамм при габаритах 80мм19мм. Частота выходных данных – 300Гц.

VG-910D состоит из двух основных узлов (рис. 4):

• 
  волоконно-оптический преобразователь – кольцевой интерферометр, содержащий волоконный контур (катушка), сварные волоконно-оптические ответвители, волоконно-кристаллический поляризатор, пьезокерамический модулятор и светодиодный излучательный модуль;
  
• 
  плата обрабатывающей электроники с аналого-цифровым преобразователем.
  

ADIS16100 – интегральный гироскоп на базе технологии iMEMS (integrated Micro Electro Mechanical System), устройство фирмы Analog Devices, интегрирующее на одном кремниевом кристалле датчик угловой скорости и электронику, обеспечивающую формирование и предварительную обработку сигнала [5].

Несмотря на меньшую в сравнении с прочими гироскопами точность, микромеханические гироскопы iMEMS обладают целым рядом уникальных достоинств. Прежде всего – это малые габариты и масса. Датчик угловой скорости ADIS16100 выпускается в миниатюрном корпусе размером 773мм, вес такого прибора не превышает 0,5 г. Диапазон измерения датчика град/c, цифровой выход угловой скорости – 12-ти разрядное число. Частота выходных данных – 1400Гц.

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

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

Программное обеспечение (ПО) – это программа, работающая на БК макета, осуществляющая считывание информации с датчика угловой скорости, её обработку и сохранение результатов вычислений в файл. Программа написана в среде C++ Builder 6.

С датчика поступает цифровой сигнал, который умножением на некоторый коэффициент и смещением (калибровка) преобразуется в угловую скорость (Рис.5). Значения угловой скорости за несколько измерений суммируются, после чего эта сумма делится на количество измерений, таким образом, на выходе получаем усреднённое значение угловой скорости. Усреднение производится для того, чтобы уменьшить шум измерений и уменьшить число вычислений программы в единицу времени. У программы есть верхний предел по частоте выходных данных 100Гц, выше этого значения программа не успевает обрабатывать поступающие с датчика данные, обработка в режиме реального времени становится невозможной. И поэтому при установке параметров работы программы нужно отдельно обратить внимание на усреднение: зная частоту подачи данных устанавливать такое усреднение (то есть выбрать количество измерений для усреднения), чтобы частота выходных данных программы не превышала предела.

Усреднённая угловая скорость поступает на вход сразу трём программным модулям: интегрирование кинематического уравнения, Фильтр Калмана (ФК) модели перманентного вращения, ФК модели крутильных колебаний. На выходе соответственно имеем: результат интегрирования, апостериори оценки угла отклонения макета от положения равновесия и угловой скорости двух ФК. Все эти значения плюс сама усреднённая угловая скорость подаются на вход модуля программы “Уточнение ковариации”, который работает по алгоритму, описанному в Табл.3. Он с некоторым периодом подставляет в фильтры Калмана оценённое значение матрицы шума системы Q. Все выходные значения программы записываются в файл с названием “xxxxxResults.txt” именно в том порядке, как изображено на Рис.5. “xxxxx” в названии файла – это случайное число, используемое здесь для того, чтобы не допустить переписывания файла с результатами предыдущего эксперимента. Каждый блок выходных значений записывается в отдельной строке. В конце каждого блока выходной информации записывается время с начала работы программы.

4.2 Интерфейс программы

Главное окно программы содержит две области (Рис.6):

• 
  Port settings – установка рабочего датчика, выбор порта, через который будет поступать информация, и установка усреднения. Задав эти параметры программы можно нажать кнопку “Open port”, и автоматически начнется процесс вычисления.
  
• 
  Some information – содержит информацию о частоте входных значений с датчика, о выходной частоте программы, отображает время работы программы и число принятых байт в одной посылке.
  

• 
  File – выход из программы.
  
• 
  Sensor calibration – установка калибровочных коэффициентов для датчиков VG-910D и ADIS16100.
  
• 
  Initial conditions – задание начального вектора состояния макета.
  
• 
  Dynamic model – задание периода крутильных колебаний.
  
• 
  Model of measuring – задание параметров модели измерений (квадрата ошибки измерений и значений элементов матрицы ошибки для ФК) и размера выборки N для алгоритма “Уточнение ковариации”.
  
• 
  Graphics – вывод в режиме реального времени графиков угловой скорости и угла отклонения от положения равновесия. Графики отображают все выходные данные программы.
  
• 
  Output values – вывод на экран выходных значений 2-х фильтров и результата интегрирования.
  

Управление ПО осуществляется при помощи интерфейса программы (Рис.6). Перед запуском программы необходимо произвести следующие действия:

1. 
   В области Port settings в выпадающем окошке Choose sensor выбрать датчик, с которого будет производиться чтение (VG-9, волоконно-оптический датчик или Mems, микромеханический датчик)
   
2. 
   В окошке Number of port задать COM-порт, откуда будет считываться информация (для VG-9 – СOM1, для Mems – COM2).
   
3. 
   Установить значение усреднения Averaging.
   
4. 
   В разделе меню Initial conditions задать начальный угол отклонения от положения равновесия в градусах (если макет закручен по часовой стрелке, то добавляется знак минус).
   
5. 
   В разделе меню Model of measuring установить значение квадрата ошибки для заданного усреднения датчика и размер выборки N для алгоритма “Уточнение ковариации”. Если N=0, то “Уточнение ковариации” отключено.
   
6. 
   В разделе меню Dynamic model задать период крутильных колебаний.
   
7. 
   Запустить программу, нажав на кнопку Open port. ПО начнет производить вычисления.
   

1. 
   Проверить, что аккумуляторы надежно соединены проводами с платой питания. Проверить правильность подключения аккумуляторов: черный провод всегда соответствует минусу, поэтому он должен соединяться с черным проводом аккумулятора и входить в разъем платы с подписью “–”; провода с любым другим цветом (кроме белого, который тоже иногда соответствует “–”) обозначают полярность “+” и должны подключаться соответственно.
   
2. 
   Включить БК. Проверить, что на макете зажглась желтая лампочка –индикатор включения питания.
   
3. 
   Включить стационарный компьютер. После загрузки, нажать левой кнопкой мыши два раза по значку Wi-Fi , в открывшемся диалоговом окне нажать на кнопку Обновить. В списке Беспроводные сети должна появиться сеть Gyro, её необходимо выделить и нажать кнопку Подключить. Далее зайти Пуск-> Программы->Стандартные->Связь->Подключение к удалённому рабочему столу. В появившемся окне необходимо набрать адрес макета \\10.10.10.4 и нажать кнопку Подключить. При подключении к удалённому рабочему столу БК макета необходимо будет ввести User Name - Administrator и Password – “1”.
   
4. 
   Запустить программу “Gyro”, ярлык которой находится на рабочем столе БК. В окошке “Choose sensor” выбрать “VG-9”, указав тем самым, что считывание будет производиться с оптоволоконного датчика угловой скорости, выбрать “COM1” в окошке “Number of port”, установить усреднение – 10. Нажать “Open port”. В левом нижнем углу окна должна появиться надпись File open successfully. Зайти в меню “Graphics” и выбрать “Graph of Rate of Rotation”. График будет отображать текущее значение выдаваемой датчиком угловой скорости и оценки угловой скорости фильтров Калмана. Если график ничего не отображает или застыл (не показывает текущих значений), нажать кнопку “Close port”, установить большее усреднение и вновь запустить программу. Так как макет находится в неподвижном состоянии (лежит на столе), то показания датчика будут колебаться около нуля (белый шум). По отклонениям от нуля оценить квадрат среднеквадратичной ошибки - . Для этого отметить на графике наибольшее и наименьшее зачение, найти разницу между ними, поделить на два и возвести в квадрат, то есть. Запишите результат.
   
5. 
   Нажать кнопку “Close port”. В окошке “Choose sensor” выбрать “Mems”, “Number of port” – “COM2”. Рассчитать минимальное усреднение для “Mems”, учитывая максимальную частоту выходных данных ПО (см. пункт 4.1 Руководства) и зная, что выходная частота данных для этого датчика составляет 1400Гц. Установить усреднение. Запустить программу. Оценить аналогичным образом (пункт 4) для микромеханического гироскопа. Записать результат.
   
6. 
   Выключить БК, нажав комбинацию клавиш Alt+F4 и выбрав Shut down в выпадающем окне, после чего отключить питание макета. Кликнув два раза по значку в нижнем правом углу рабочего стола стационарного компьютера, выделить сеть Gyro и нажать кнопку Разъединить.
   
7. 
   Подвесить макет на струну, найти положение равновесия и закрутить макет на один оборот от положения равновесия. Убедиться по формуле (19), что погонный угол закрутки мал. Засечь время двух полных колебаний макета, после чего рассчитать средний период колебаний Т. Записать полученный результат.
   
8. 
   Провести повторную проверку согласно пункту 1. Включить БК, проследить, что зажглась индикаторная лампочка подачи питания. Подключиться к удалённому рабочему столу БК (пункт 3), запустить программу “Gyro” и задать согласно пункту 4.3 настоящего руководства рабочий датчик “VG-9”, порт считывания – “COM1”, усреднение, установленное в пункте 4, квадрат среднеквадратичной ошибки для этого усреднения , начальный угол отклонения от положения равновесия – 360, измеренный период крутильных колебаний (пункт 7), размер выборки для алгоритма уточнения ковариации шума N=0, то есть этот алгоритм отключен.
   
9. 
   Закрутить макет ровно на один оборот от положения равновесия против часовой стрелки. Отпустить макет, после чего нажать на кнопку Open port ПО, начав тем самым процесс вычисления. Важно: макет не должен сильно раскачиваться на струне и колебаться, эти движения должны быть пренебрежимо малы по сравнению с крутильными, иначе нельзя считать движение одномерным. Этого можно достичь, слегка придерживая макет до начала запуска вычислений, устраняя тем самым колебания и раскачивания.
   
10. 
    После того, как макет сделает половину или одно колебание, нажать кнопку Close port, нажать в верху рабочего стола на кнопку , зайти со стационарного компьютера в Пуск-> Выполнить, открыть \\10.10.10.4, если потребуется, ввести имя и пароль (Пункт 3), после зайти в папку Gyro, скопировать из этой папки файл “ххххResults .txt”, и сохранить в отдельную папку на стационарном компьютере, например, “1 Experiment”. Записать имя файла с результатами эксперимента, указав параметры этого эксперимента. Файл “ххххResults .txt” из папки Gyro удалить.
    
11. 
    Изменить в программе согласно пункту 4.3 настоящего руководства размер выборки для алгоритма уточнения ковариации ошибки на N=100. Повторить эксперимент (Пункты 8-10).
    
12. 
    Повторить эксперимент с датчиком “Mems”, установив для него порт считывания “COM2”, , соответствующее усреднение, N=100.
    
13. 
    Выключить БК (Пункт 6), снять макет со струны.
    
14. 
    Написать программу отображения графиков полученных результатов в среде Matlab. Входным значением программы должно быть имя файла “ххххResults .txt”, содержащего результаты эксперимента, на выходе программа должна выдать 2 графика:
    

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

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