Алгоритм начальной инициализации кватерниона пространственной ориентации в параметрах Родрига-Гамильтона

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

Материалы и методы. Для отработки алгоритма коррекции был использован датчик MPU6050, выполненный по микроэлектромеханической технологии и имеющий в одном корпусе три ортогонально расположенных измерителя угловой скорости и три ортогонально расположенных акселерометра. Информацию, поступающую с MPU6050, обрабатывает микроконтроллер dsPIC33EP256MU806. Пространственная ориентация вычисляется чере параметры Родрига-Гамильтона в компонентах кватерниона, результат вычислений передается в программный пакет Matlab, выполняющий программу для визуализации зависимостей от времени четырёх компонент кватерниона в виде графиков.

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

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

1. Современные информационные технологии в задачах навигации и наведения беспилотных маневренных летательных аппаратов / К. К. Веремеенко [и др.], — Москва: ФИЗМАТЛИТ, 2009 г., 556 с.

2. Бранец, В. Н. Применение кватернионов в задачах ориентации твердого тела / В. Н. Бранец, И. П. Шмыглевский. — Москва : Наука, 1973. — 320 с.

3. Бранец, В. Н. Введение в теорию бесплатформенных инерциальных навигационных систем / В. Н. Бранец, И. П. Шмыглевский. — Москва : Наука, 1992. — 280 с.

4. Madgwick S.O.H. An efficient orientation filter for inertial and inertial/magnetic sensor arrays / S.O.N. Madgwick // Report x-io and University of Bristol 2010. — 32 p.

5. Hamilton, W.R. On quaternions, or on a new system of imaginaries in algebra / W.R. Hamilton // Philosophical Magazine 1844. — Vol. XXV, — P. 10–13.

6. Ватульян, А. О. Кватернионы / А. О. Ватульян // Соросовский образовательный журнал. — 1999. — № 5. — С. 117–120.

7. Гордеев, В. Н. Кватернионы и трехмерная геометрия / В. Н. Гордеев. — Киев: Сталь, 2012. — 318 с.

8. Mahony, R. Complementary filter design on the special orthogonal group / Robert Mahony, Tarek Hamel, Jean-Michel Pflimlin // Proceedings of the 44th IEEE Conference on Decision and Control, and the European Control Conference 2005 Seville, Spain, December 12-15, 2005. P. 1477–1484.

9. Roberto G. Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMUs and MARGs / Roberto G. Valenti, Ivan Dryanovsky, Jizhong Xiao // Sensors – Open Access Journal, 2015 P. 29.

10. Grant, B. Complementary filter design on the Special Euclidean group SE / Grant Baldwin, Robert Mahony, Jochen Trumpf, Tarek Hamel, Thibault Cheviron // Control Conference (ECC), 2007 European, Kos, Greece, July 2-5, 2007. P. 8.