Оценка угловой скорости вращения тела при помощи системы трекинга

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

Авторы приносят благодарность ведущему научному сотруднику лаборатории управления и навигации Н.Б. Вавиловой за конструктивные замечания.

СПИСОК ЛИТЕРАТУРЫ

1. Голован А.А., Парусников Н.А. Математические основы навигационных систем. Ч. II: Приложения методов оптимального оценивания к задачам навигации. М.: Изд-во МГУ, 2008.

2. Вавилова Н.Б., Парусников Н.А., Сазонов И.Ю. Калибровка бескарданных инерциальных навигационных систем при помощи грубых одностепенных стендов // Современные проблемы математики и механики. Т. I: Прикладные исследования. М.: Изд-во МГУ, 2009.

3. Голован А.А., Парусников Н.А. Математические основы навигационных систем. Ч. I: Математические модели инерциальной навигации. М.: МАКС Пресс, 2011.

Поступила в редакцию 18.12.2012

УДК 51-72

ОЦЕНКА УГЛОВОЙ СКОРОСТИ ВРАЩЕНИЯ ТЕЛА ПРИ ПОМОЩИ

СИСТЕМЫ ТРЕКИНГА

Д. И. Бугров1, А. В. Лебедев2, В. А. Чертополохов3

Рассмотрена задача о нахождении угловой скорости вращения тела по записям матрицы ориентации, полученным с помощью системы трекинга. Проведено сравнение двух способов решения задачи — путем представления функции в виде частичной суммы Фурье и с использованием фильтра Савицкого-Голея.

Ключевые слова: угловая скорость, система трекинга, частичная сумма Фурье, фильтр Савицкого-Голея.

The problem of determining the rotational angular velocity of a body using the records of the matrix orientation supplied by a tracking system is considered. Two methods of solving this problem are compared. One of them is based on the representation of the function in the form of a partial sum of the Fourier series and the second one is based on the Savitzky-Golay filter.

Key words: angular velosity, tracking system, partial sum of Fourier series, Savitzky-Golay

filter.

Одним из направлений совместной работы Института математических исследований сложных систем и Института человека МГУ является изучение особенностей функционирования вестибулярной системы человека. Требуемую информацию можно получать, в частности, изучая записи движений глаз при поворотах головы. Для получения таких записей использовались электроокулограф, регистрирующий движения глаз, и система трекинга ARTtrac производства компании A.R.T. GmbH (Германия), включающая в себя две камеры, работающие в инфракрасном диапазоне, два соответствующих излучателя, набор отражающих маркеров и специальное программное обеспечение. Указанное программное обеспечение осуществляет первичную обработку отраженных от маркеров сигналов и с высокой точностью вычисляет

1 Бугров Дмитрий Игоревич — канд. физ.-мат. наук, ст. науч. сотр. лаб. математического обеспечения имитационных динамических систем мех.-мат. ф-та МГУ, e-mail: dima-bugrov@yandex.ru.

2 Лебедев Антон Викторович — канд. физ.-мат. наук, ст. науч. сотр. лаб. математического обеспечения имитационных динамических систем мех.-мат. ф-та МГУ, e-mail: antohal@rambler.ru.

3 Чертополохов Виктор Александрович — студ. каф. прикладной механики и управления мех.-мат. ф-та МГУ, e-mail: andorantes@gmail.com.

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

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

Рассмотрим следующую постановку задачи. Имеется зашумленная запись значений Л^, г = 0,...,п, матрицы ориентации Л = {а^} с постоянным шагом АЬ = ^ — Ь-1, г = 1,...,п, по времени. Частота записи 60 Гц. Требуется найти угловую скорость вращения "тела". Сделать это можно, используя кинематические соотношения, связывающие изменение во времени параметров, определяющих ориентацию "тела", и угловую скорость его вращения.

Разновидностью таких соотношений являются уравнения Пуассона. Они задают изменение матрицы ориентации Л в зависимости от угловой скорости и вращения твердого тела [3]:

A =

/ 0 wz -Wy\

—uz 0 ux \ Uy -Ux 0 J

A,

где Ux, Uy, Uz — проекции вектора и на жестко связанные с телом оси.

В каждый момент времени можно численно определить (вычислить через первую разность) производную матрицы ориентации и в силу уравнений Пуассона найти значения проекций угловой скорости и на оси тела в моменты времени t¿: Uxi, Uyi, Uz¿. Однако работа с несглаженными данными приводит к большим ошибкам в определении производной. В то же время сглаживание матрицы ориентации требует особой аккуратности и имеет свои особенности из-за необходимости поддержания ортонормальности ее строк и столбцов. Поэтому было предложено вместо уравнений Пуассона использовать кинематические уравнения для углов Эйлера-Крылова поворота тела а, в, Y (следуя терминологии [4], угловое перемещение первого рода), сглаживать данные и потом уже находить угловые скорости. Для оценки полученных результатов сравнивались исходная запись во времени значений матрицы ориентации и результаты интегрирования уравнений Пуассона при найденных значениях проекций угловой скорости и на оси "тела". Использовались соотношения, связывающие элементы матрицы ориентации и углы поворота, в случае в = п/2:

sin а cos в = —a32, sin в = a31, sin y cos в = —a21,

и выражения для угловой скорости через углы поворота и их производные [4]:

ux = cacos в cos y + в sin y, uy = —a cos в sin y + в cos y, uz = Y + а sin в

Сглаживание углов поворота проводилось двумя способами.

1. Быстрое преобразование Фурье (БПФ) и фильтр высоких частот. С помощью БПФ можно представить сглаживаемую функцию частичной суммой ряда Фурье и найти производную. В качестве тестовой использовалась запись следующего движения: сначала "тело" неподвижно, потом оно совершает колебательные движения вокруг вертикальной оси, затем опять неподвижно, опять цикл колебаний, и снова неподвижно. Запись содержит n = 9203 точек. График зависимости угла поворота Yi = Y (ti) от времени представлен на рис. 1. Углы а и в остаются малыми в течение всего эксперимента.

С помощью процедур пакета программ Matlab было выполнено БПФ и построены частичные суммы рядов (xf (t), вf (t), Yf (t), в которых отброшены все члены с частотой v выше vc. Сравнивались результаты для vc = 1; 3; 10 Гц. Во всех случаях можно говорить о достаточно хорошем качестве приближения, при этом оно улучшается (с точки зрения минимума максимального значения модуля погрешности) при переходе от vc = 1 Гц к vc = 3 Гц и остается практически неизменным при переходе от vc = 3 Гц к vc = 10 Гц.

70

вестн. моск. ун-та. сер. 1, математика. механика. 2014. № 1

Далее вычислялись величины ái, Д, Y i как производные частичных сумм af (t), (t), (t) в точках ti, i = 1,...,n; a0, во, Yo полагались равными нулю. С помощью соотношения (2) интегрировались уравнения Пуассона, при этом использовалась кубическая интерполяция для угловых скоростей ux(t), Шу(t), uz(t) между узлами. После этого в соответствии с (1) находились af, в(, — восстановленные по угловой скорости значения углов поворота. Можно повторить сделанный ранее вывод о том, что во всех рассмотренных случаях имеется достаточно хорошее приближение углов поворота, при этом оно улучшается (с точки зрения минимума максимального значения модуля погрешности) при переходе от vc = l Гц к vc = 3 Гц и остается практически неизменным при переходе от vc = 3 Гц к vc = 10 Гц. В то же время можно отметить значительное отличие (с точки зрения максимального значения модуля угловой скорости) в полученных аппроксимациях для угловых скоростей ux(t), Шу(t), uz(t) при переходе от vc = 1 Гц к vc = 3 Гц и к vc = 10 Гц. Поэтому был сделан вывод о необходимости предварительного определения (из априорных данных о характере движения) максимальной частоты vc в случае использования данного подхода в исследуемой задаче.

f

Разность между исходным y i и восстановленным jf сигналами для vc = 3 Гц представлена на рис. 2.

Рис. 1. График зависимости угла поворота 7 от времени

Рис. 2. Разность между исходным и восстановленным сигналами по углу 7 для Vе = 3 Гц

Рис. 3. Разность между исходным и восстанов- Рис. 4. Вычисленная проекция вектора уг-

ленным сигналами по углу 7 ловой скорости

2. Фильтр Савицкого-Голея [5]. Метод состоит в следующем. Выбираются размеры окна (количество точек, по которым вычисляются коэффициенты, должно быть нечетным) и порядок полинома, которым приближаются данные. Коэффициенты полинома ищутся методом наименьших квадратов. Были выбраны следующие значения параметров: для угла 7 сглаживание проводилось по 15 точкам (соответствует интервалу времени 0,25 с в записи), степень полинома 4; для углов а и в сглаживание проводилось по 61 точке (соответствует интервалу времени 1 с в записи), степень полинома 2. С помощью фильтра Савицкого-Голея были найдены производные аг и вг в точках г = 1,...,и — 1; предполагалось, что ао = во = ап = вп = 0. Далее с помощью соотношений (2) вычислялись их(и), иу(¿¿), (и), интегри-

ровались уравнения Пуассона, при этом, как и ранее, для угловых скоростей использовалась кубическая интерполяция между узлами. После этого в соответствии с (1) находились восстановленные по угловой скорости значения углов поворота av (ti), ßv(ti) и jv (ti). Разность между исходным j(ti) и восстановленным Yv (ti) сигналами представлена на рис. 3. Отметим, что максимальная разность между исходными и восстановленными значениями угла y (диапазон изменения которого в эксперименте значительно больше остальных двух углов) на рис. 3 почти в 3 раза меньше, чем на рис. 2.

Восстановленные значения углов достаточно близки к измеренным, что позволяет сделать вывод о возможности использования данного метода для нахождения угловой скорости. На рис. 4 представлена вычисленная этим способом проекция вектора угловой скорости uz(t). Конечно, следует отметить, что для данного метода также требуются априорные данные о характере движения, вместо максимальной частоты VС следует задавать степень интерполирующего полинома и количество точек сглаживания.

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

Работа выполнена при финансовой поддержке Министерства образования и науки РФ, контракт 11.519.11.2045, и РФФИ, грант № 13-01-00515.

СПИСОК ЛИТЕРАТУРЫ

1. Хэмминг Р.У. Цифровые фильтры. М.: Советское радио, 1980.

2. Бобылев А.Н., Болотин Ю.В., Воронов А.В., Кручинин П.А. О двух модификациях метода наименьших квадратов в задаче восстановления утерянной информации системы видеоанализа по показаниям акселерометра // Рос. журн. биомех. 2012. 16, № 1. 89-101.

3. Парусников Н.А., Морозов В.М., Борзов В.И. Задача коррекции в инерциальной навигации. М.: Изд-во МГУ, 1982.

4. Ишлинский А.Ю. Ориентация, гироскопы и инерциальная навигация. М.: Наука, 1976.

5. Savitzky A., Golay M.J.E. Smoothing and differentiation of data by simplified least squares procedures // Anal. Chem. 1964. 36, N 8. 1627-1639.

Поступила в редакцию 13.02.2013