CC BY
3УДК 004.896 Исса М., Ахмад А.
Исса М.
инженер Федерального научного агроинженерного центра ВИМ, магистрант кафедры «систем автоматического управления» Московский государственный технический университет
им. Н.Э. Баумана (г. Москва, Россия)
Ахмад А.
младший научный сотрудник Федерального научного агроинженерного центра ВИМ, ассистент кафедры "СМ-7" Робототехнические Системы И Мехатроника Московский государственный технический университет
им. Н.Э. Баумана (г. Москва, Россия)
ОЦЕНКА ОРИЕНТАЦИИ С ПОМОЩЬЮ СЛИЯНИЯ ИНЕРЦИАЛЬНЫХ ДАННЫХ НА ОСНОВЕ НЕЛИНЕЙНЫХ ФИЛЬТРОВ (МАХОНИ И МЭДЖВИК)
Аннотация: в статье рассматривается задача точного определения ориентации в реальном времени с использованием инерциальных и магнитометрических данных, что имеет важное значение в таких областях, как робототехника и анализ движения человека. Цель исследования — разработка надежных методов оценки ориентации путем объединения показаний акселерометра, магнитометра и гироскопа. Представлен подробный анализ фильтров Калмана, Махони и Мэджвика для слияния данных, поддерживаемый моделированием в MATLAB. Результаты показывают, что фильтры Махони и Мэджвика значительно повышают точность оценки ориентации, что делает их пригодными для приложений, чувствительных к погрешностям датчиков и вибрациям окружающей среды.
Ключевые слова: оценка ориентации, инерциальные датчики, фильтр Калмана, фильтр Махони, фильтр Мэджвика, робототехника, навигационные системы.
Введение.
Оценка ориентации с помощью инерциальных и магнитометрических измерений на сегодняшний день является хорошо изученной проблемой, имеющей применение, например, в анализе движения человека и робототехнике. В последние годы растет спрос на алгоритмы, которые не требуют больших вычислительных затрат и могут оценивать ориентацию в реальном времени на микропроцессоре [1, с.1].
При разработке робототехнических систем часто возникает задача определения ориентации некоторого объекта в пространстве. Технический прогресс привел к созданию множества датчиков, способных, опираясь на измерение различных физических величин, определить положение тела в пространстве. Однако получение с них данных является недостаточным для точного определения искомых величин. Необходимо применять различные алгоритмы обработки принимаемых данных, чтобы в результате получить пригодные для дальнейшего использования значения параметров ориентации. В свою очередь, развитие математического аппарата привело к появлению нескольких способов описания ориентации в пространстве. Все они обладают разной степенью наглядности и удобства для человека, а также различными сферами применения [2, с.1].
Акселерометр и магнитометр измеряют гравитационное и магнитное поле Земли соответственно, и абсолютный эталон ориентации может быть получен непосредственно из этих измерений. Однако, скорее всего, они подвержены высоким уровням внешних возмущений. Например, вибрации, вызванные колебаниями движения, искажают углы тангажа и крена, рассчитанные на основе вектора силы тяжести акселерометра, а магнитные возмущения влияют на расчеты угла рысканья. Также хорошо известно, что ориентацию можно получить с помощью дискретно-временного интегратора, применяемого к наблюдениям гироскопа, в случае если начальные условия заданы заранее. Тем не менее, интегрирование наблюдений гироскопа с нулевым перекосом приведет к накоплению ошибки в вычисленной ориентации. Таким образом, ни
комбинированный акселерометр/магнитометр, ни гироскоп не могут быть использованы в качестве самостоятельного измерительного устройства. Поэтому необходимо вычислить единую оценку ориентации с помощью оптимального фильтра слияния измерений акселерометра, магнитометра и гироскопа [3, с. 174186].
Фильтр Калмана, фильтр Махони и фильтр Мэджвика - наиболее известные фильтры, используемые для оценки ориентации по инерциальным данным.
Фильтр Махони
Фильтр Махони использует явный комплементарный фильтр для вычисления ошибки ориентации по результатам предыдущего измерения с помощью акселерометра и магнитодатчика. Значение ошибки представляет собой сумму перекрестных произведений между оцененным направлением и измеренным направлением полей, которая затем используется в качестве обратной связи для вычисления обновленного измерения гироскопа [4, с.1].
Для математического описания фильтров, которые мы будем использовать, используется нотация, описанная Мэджвиком в [5, с.7]. Например: символ А обозначает нормированный вектор, символ * - сопряженный вектор, оператор ® - акватернионное произведение, - ориентация рамки В относительно рамки А, Ау - вектор в рамке А [6, с.4].
Ек представляет собой направление магнитного поля в рамке Земли, вычисленное с помощью кватернионного произведения предыдущей оценки ориентации с нормированным магнитометрическим измерением и с сопряженным кватернионом предыдущей оценки ориентации, как видно из уравнения (1):
% = й® (Ш®!Г) (1)
где т - измерение, полученное от магнитометра.
Кватернион определяется уравнением (2).
еЧ = [41 42 Чз 44 ] (2)
Влияние ошибочного наклона измеренного направления магнитного поля Земли может быть исправлено, получая только компоненты на осях х и ъ Земли, как показано в уравнении (3).
% = [0 ^Й^ТЙ2 0 к2]
(3)
где ЕЬ{ - это нормализация Й.
как:
как:
Расчетное направление гравитационного поля может быть рассчитано
Ед = [2(^2^4 - Ч1Ч3) , 2(Я1Я2 - чзчА) , -ч2-ч3 + ч1] (4) Предполагаемое направление магнитного поля может быть рассчитано
=
2Ь2(0.5 -ц1- &) + 2Ь4(Ц2Я4 - Я1Я3) 2Ь*(Ч2Чз - Ч1Ч4) + 2Ы^2 - ЯзЧ4) 2Ь2(Ч1Чз - Ч2Ч4) + 2&4(0.5 $)
(5)
После этого с помощью уравнения (6) вычисляется ошибка, которая является результатом суммы перекрестного произведения между оцененным направлением и измеренным направлением векторов поля.
Ее = а X Ер + т X Ем/ (6)
где а - измерение акселерометром.
Фильтр Махони используется, в частности, потому, что он позволяет корректировать смещение гироскопа, применяя интегральный и пропорциональный регулятор.
• • 1 Р Л
1к = 1к-1 + ье . яр
9к = 9к-1 + кр . Её + кг. Iк
где Бр - частота дискретизации (б), дк-1 - измерение, производимое гироскопом.
Наконец, скорость изменения кватерниона вычисляется с помощью уравнения (8) и интегрируется для получения кватерниона с помощью уравнения (9), который затем преобразуется в углы Эйлера.
¿1 = 0.5 Й ® Ед, (8)
ЕЯк = ЕЯк-1 + Ч-(9) Предыдущие шаги кратко описаны на следующей схеме [8]:
Рис. 1. Блок-схема фильтра Махони.
Фильтр Мэджвика.
Фильтр основан на введенном дополнительном фильтре, но в нем используются угловые скорости, а не телесные углы, как в дополнительном фильтре. Для реализации фильтра используются данные акселерометра, при этом предполагается, что ускорение, измеренное в кадре датчика (уравнение 27), совпадает с глобальным направлением гравитационного вектора (уравнение 26). Система координат Земли (обозначается Е) выбирается таким образом, чтобы вектор гравитации совпадал с осью Ъ инерциальной системы координат [7, с.14]:
Ед = [0 0 0 1] (10)
Ба = [0 ах ау аг ] (11)
Глобальная гравитационная ось может быть преобразована в локальную рамку датчика S следующим образом:
Бд = !П®Е9® ЮТ (12)
Комбинация гравитационного кватерниона Ед (уравнение 10) и нормированного кватерниона ускорения Ба (уравнение 11) приводит к упрощенной объективной функции f (уравнение 13), определяемой уравнением:
КМ.= т®Ед® йг - 5ъ (13)
Этот фильтр характеризуется добавлением корректирующего этапа, использующего алгоритм градиентного спуска для минимизации f. Минимизация требует матрицы Якобиана (уравнение 14) f для вычисления направления ошибки на поверхности решения (уравнение 15). Матрица Якобиана используется для изменения переменной при компенсации изменения системы координат.
-2яу 2qz 2цх
2дх 2^ 2Чг 2ЧУ (14)
0 -4Чх -4ЧУ О
УШЯ. ¡(§щу д 5П) (15)
Как мы видели в фильтре Махони, магнитное поле Земли можно рассматривать как имеющее компоненты на одной горизонтальной и вертикальной осях, вертикальная компонента обусловлена наклоном поля, который в Великобритании составляет от 65° до 70° к горизонтали [5, с.7].
(16)
J(iq) =
(17)
(18)
ЕЬ = [0 Ьх 0 bz] sm = [0 тх ту mz] fb(§q, Eb, sm) = §q®Eb® §q* - sm
'2bx(0.5 -ql- qX) + 2bz- q±q3) - mx 2bx(q2q-3 - q1q4) + 2bz(qtq2 - q3qA) - my
-2bx(qiq3 - q2q4) + 2bz(0.5 - q2 - q^) - mz_
Jb (iï, Eb)
-2bzq3 2qzqA -4bxq3- 2bzqi -4bxqA + 2bzq2
-2bxq4 + 2bzqx 2bxq3 + 2bzqi 2bxqx + 2bzq4 -2bxqi + 2bzq3 2bxq3 2bxq4 - 4bzqx 2bxqi- 4bzq3 2bxqx
Градиент снова рассчитывается по уравнениям (15),(18) и (19)
следующим образом:
Vf(sEq, sa, sm) = Vf(sEci, so) + ( JTb (sEq, Eb) ® fb{sEq, Eb, sm) ) (20)
Уравнения (21) и (22) объединяют измерения гравитации и магнитного
поля Земли, чтобы обеспечить уникальную ориентацию.
(19)
fg,b{SE4, Sa, Eb, sm) =
fg (So)
[fb (Й, ЕЪ, sm)
fg,b(iq> Щ =
fg (Й)
fgT (Й, ЕЪ)
(21)
(22)
Дрейф гироскопа компенсируется в зависимости от частоты данных и коэффициента дзета £ следующим образом:
дугоеггог = 2(!с1® Vf(|а, Бт)) (23)
9Угов1а5 = дугоеггог. 5р. ( (24)
= - дуготаз (25) где яр - частота дискретизации (б), Ед - измерение, производимое гироскопом,
£ представляет собой скорость сходимости для устранения ошибок
измерения гироскопа.
После этого вычислите скорость изменения с помощью уравнения (26).
q = 0.5(Йь-! ® ) - ß- V/(Й, Ч 5т) (26)
где Д- коэффициент усиления пропорционального регулятора.
Наконец, скорость изменения кватерниона интегрируется с помощью уравнения (9), и этот результат затем преобразуется в углы Эйлера.
Предыдущие шаги кратко описаны на следующей схеме [8, с.4]:
Рис. 2. Блок-схема фильтра Мэджвика.
Коэффициент усиления фильтра представляет все средние нулевые ошибки измерения гироскопа, выраженные в виде величины производной кватерниона [9, с.3]. Источниками ошибок являются: шум датчика, алиасинг сигнала, ошибки квантования, ошибки калибровки, несоосность датчика, неортогональность оси датчика и характеристики частотной характеристики. Коэффициент усиления фильтра представляет собой скорость сходимости для удаления ошибок измерения гироскопа, которые не являются средним нулем, также выраженным как величина производной кватерниона. Эти ошибки представляют собой смещение гироскопа. Удобно определить ¡3 и £ с помощью угловых величин оор и (Ъ^ соответственно, где сор представляет собой оценочную среднюю нулевую ошибку измерения гироскопа по каждой оси, а (Ъ^ представляет собой оценочную скорость дрейфа смещения гироскопа по каждой оси.
в может быть определена уравнением (27), где с[ - любой единичный кватернион. Аналогично, £ может быть описан уравнением (28).
(27)
Р =
-ц ® [0 (Ор Шр сор]
N
-(¿р
с =
N
(28)
— О) ^
Практические результаты и моделирование с использованием МЛТЬЛБ.
Предположим, что инерционные датчики являются идеальными датчиками, и смоделируем измерения инерциальной системы без шума. Мы использовали библиотеку программного обеспечения <ат^ешоп> для определения характеристик инерционных датчиков, а также магнитного датчика, и ниже мы кратко объясним эту библиотеку программного обеспечения.
• Генерация путей с помощью библиотеки imuSensor
Смоделируем выход датчика 1Ми для простого вращательного движения вокруг оси 7.
Параметры для вращения: угловая скорость = 2 * pi / 10, продолжительность = 10, временной шаг = 0.01,
На следующем рисунке показаны выходные данные всех датчиков, что соответствует круговому пути:
Данные акселерометра
ось X ось Y ось Т
1 23456789 10
Время (сек) Данные гироскопа
п-1-1-1-1-1-1-1- I _
ось X ось Y ось Т
1 2 3 4 5 6 7 8 9 10
Время (сек) Показания магнитометра
"1-Т-1-1-1-1-Т-Т- I
ось X ось Y ось Т
1 2 3 4 5 6 7 8 9 10
Время (сек)
Рис. 3. Выходной сигнал всех датчиков, соответствующий круговому пути.
• Практические результаты моделирования навигационного алгоритма
* Оценка тренда с помощью фильтра Махони: Инициализация фильтра Махони:
Период дискретизации = 1/100, Кватернион = [1 0 0 0], ^ = 0.1, Ю = 0.01,
На рисунке (4) показаны компоненты ориентации.
01 23456789 10
Время (сек)
Время (сек)
Время (сек)
Рис. 4. Компоненты отношения (крен, тангаж, рыскание) пути с помощью фильтра Махони.
* Оценка тренда с помощью фильтра Мэджвика:
Инициализация фильтра Мэджвика:
qEst = [1 0 0 0],
частота обновления = 100,
beta = 0.1,
zeta = 0.01,
На рисунке (5) показаны компоненты ориентации.
с
2 >
-200 _I_I_I_I_I_Т—_I_I_I_
0123456789 10
Время (сек)
Рис. 5. Компоненты отношения (крен, тангаж, рыскание) пути с помощью фильтра Мэджвика.
Заключение.
В данной работе описывается стандартное исследование инерциальных навигационных систем, в особенности бесплатформенных. Исследовалась оценка ориентации с помощью слияния инерциальных данных на основе фильтров, где сначала были изучены основные математические принципы, а затем подробно рассмотрены фильтры Махони и Маджвика. Проведено практическое моделирование навигационной системы с помощью МАТЬАВ, а также сгенерированы виртуальные данные для инерциальных датчиков с помощью библиотеки т^тиЫог МАТЬАВ. Угловое положение блока
инерциальных датчиков оценивалось с помощью фильтров Махони и Маджвика и сравнивалось с применяемым навигационным алгоритмом.
СПИСОК ЛИТЕРАТУРЫ:
1. Manon Kok and Thomas B. Schon. A Fast and Robust Algorithm for Orientation Estimation using Inertial Sensors", I E E E Signal Processing Letters, 2019. DOI: 10.1109/LSP.2019.2943995. 1 c;
2. И.А. Брич. СРАВНИТЕЛЬНЫЙ АНАЛИЗ АЛГОРИТМОВ ФИЛЬТРАЦИИ ДАННЫХ ИНЕРЦИАЛЬНЫХ ДАТЧИКОВ. Политехнический молодежный журнал. 2023. № 02. DOI: 10.18698/2541-8009-2023-02-864. 1 с;
3. Amjed S. Al-Fahoum, Momtaz S. Abadir. Design of a Modified Madgwick Filter for Quaternion-Based Orientation Estimation Using AHRS. April 15, 2018. doi: 10.17706/ijcee.2018.10.3.174-186;
4. R. Kadir, M. Sahal. Application of Kalman Filter in Fine Alignment of INS Assisted by Magneto Sensors. 978-1-7281-7413-6/20/$31.00 ©2020 I E E E. 1 c;
5. Sebastian O.H. Madgwick. An efficient orientation filter for inertial and inertial/magnetic sensor arrays. p. 32. Report x-io and University of Bristol, Bristol, UK (2010). 7 c;
6. David Segarra, Jessica Caballeros. Orientation Estimation Using Filter-Based Inertial Data Fusion for Posture Recognition. Springer Nature Switzerland AG 2019. 4 c;
7. Lea Alina Henrich. Evaluation of IMU Orientation Estimation Algorithms Using a Three-Axis Gimbal. Friedrich-Alexander-Universitat Erlangen-N " urnberg (FAU). October 2019. 14 c;
8. Simone A. Ludwig. Optimization of Control Parameter for Filter Algorithms for Attitude and Heading Reference Systems. North Dakota State University. 4 c;
9. Simone A. Ludwig, Kaleb D. Burnham. Comparison of Euler Estimate using Extended Kalman Filter, Madgwick and Mahony on Quadcopter Flight Data. North Dakota State University, Fargo, ND, USA. 3 c.
Международный научный журнал «ВЕСТНИК НАУКИ» № 11 (80) Том 1. НОЯБРЬ 2024 г. Issa М., Ahmad А.
Issa М.
Engineer of the Federal Scientific Agroengineering Center of VIM, Master's student of the Department of "Automatic Control Systems", Bauman Moscow State Technical University (Moscow, Russia)
Ahmad А.
Junior Research Associate of the Federal Scientific Agroengineering Center of VIM, Assistant of the SM-7 Department of Robotics Systems and Mechatronics at Bauman Moscow State Technical University (Moscow, Russia)
ORIENTATION ESTIMATION USING INERTIAL DATA FUSION BASED ON NONLINEAR FILTERS (MAHONEY AND MADGWICK)
Abstract: this paper addresses the problem of accurate real-time orientation estimation using inertial and magnetometric data, which is important in areas such as robotics and human motion analysis. The aim of the study is to develop robust orientation estimation methods by combining accelerometer, magnetometer and gyroscope readings. A detailed analysis of Kalman, Mahoney and Madgwick filters for data fusion is presented, supported by simulations in MATLAB. The results show that the Mahoney and Madgwick filters significantly improve the accuracy of orientation estimation, making them suitable for applications sensitive to sensor errors and environmental vibrations.
Keywords: orientation estimation, inertial sensors, Kalman filter, Mahoney filter, Madgwick filter, robotics, navigation systems.
