[Перевод] Фильтр Маджвика05.06.2015 12:18

Предисловие от переводчика Здесь представлен один из новейших методов расчёта ориентации в пространстве по показаниям датчиков акселерометра, гироскопа и компаса — фильтр Маджвика, который, по словам автора, даёт результат лучший, чем применение фильтра на основе метода Калмана в результатах и производительности. Автор — Себастьян Маджвик (его интернет-магазин). Метод описан в статье на английском. Данная работа защищена в Университете г. Бристоля Перевода я не нашёл. Переводчик из меня так себе, особенно таких сложных текстов. Но нам же интересно, что за метод? Кое где буду от себя добавлять — там текст выделен курсивом. Мною найдено более 10 опечаток в оригинальном тексте. Вообще было довольно трудно, поэтому помощь приветствуется — пишите в комментариях, где перефразировать нужно, в общем, где что не так.

Фильтр массивов данных инерционных и инерционно-магнитных датчиков для определения ориентации 30 апреля 2010От автора Данная статья описывает новый фильтр показаний датчиков для определения ориентации в пространстве в двух вариантах. Первый вариант применим к инерционным навигационным системам (ИНС), включающих акселерометр и гироскоп. Второй вариант применим к ИНС, включающих дополнительно 3-х осевой магнитометр (аббревиатура MARG — расшифровывается как «Magnetic, Angular Rate and Gravity»). Реализация ИНС с магнитометром подразумевает компенсацию магнитных искажений и компенсацию смещения гироскопа. В качестве инструмента используются кватернионы, которые позволяют использовать данные акселерометра и магнитометра для аналитических вычислений и оптимизации методом градиентного спуска в получении погрешности направления гироскопа в виде производного кватерниона. Преимущества включают в себя: дешевизна по вычислительным ресурсам — 277 простых арифметических операций каждое обновление фильтра (109 операций без магнитометра); эффективность при низких частотах дискретизации (например 10 Гц); Содержит 1 (без магнитометра) или 2 настраиваемых параметра, определяемых на наблюдаемых характеристиках системы. Точность оценивалась эмпирически с использованием коммерчески доступных датчиков ориентации. Эталонные значения ориентации получены с помощью оптической системы измерения. Простой метод калибровки для использования оптического измерительного оборудования представлен в данной работе.Точность также сопоставлена с патентованным фильтром на основе метода Калмана для датчиков ориентации. Результаты показывают, что данный фильтр достигает уровня точности, превышающий фильтр на основе метода Калмана: В следствие низкой вычислительной нагрузки и способности работать на низких частотах дискретизации открываются новые возможности применения ИНС для устройств реального времени с малыми вычислительными способностями и требованиями к высокой частоте выборок.1. Введение Точное определение ориентации в пространстве играет важную роль во многих областях, включая: Аэрокосмическую [1, 2, 3]; Робототехнику [4, 5]; Навигацию [6, 7]; Анализ движений человека [8, 9] Взаимодействие движений человека и машины [10]. В то время как различные технологии позволяют измерять ориентацию, инерционные сенсорные системы имеют преимущество в виде полной автономности — измеряемый объект не ограничен в перемещениях, не ограничен какой-то конкретной средой или расположением. Блок инерциальных измерений (БИИ, или от англ. IMU — Inertial Measurement Unit) состоит из гироскопов и акселерометров позволяющих отслеживать вращательные и поступательные движения. Для того, чтобы делать 3D замеры, требуется чтобы оси датчиков были взаимно перпендикулярны. АГМ представляет собой гибрид БИИ, который включает в себя трехосный магнитометр. Система без магнитометра может определять ориентацию относительно направления силы тяжести, что достаточно для многих приложений [4, 2, 8, 1]. Инерциальные навигационные системы используют систему отчёта, известную как «курс, тангаж, крен» (англ. AHRS — Attitude and Heading Reference Systems), и в состоянии обеспечить полное измерение ориентации относительно силы тяжести и земного магнитного поля.Гироскоп измеряет угловую скорость, которую при известных начальных условиях можно интегрировать с течением времени, чтобы получать ориентацию датчика [11, 12]. Точные гироскопы, например кольцевой лазер, слишком дороги и громоздки для большинства приложений. С другой стороны менее точные MEMS-датчики (Micro Electrical Mechanical System — микромеханические электронные системы) используются в большинстве приложений [13]. Интеграция ошибок измерения приведёт к накоплению ошибки в вычислении ориентации.Таким образом гироскопы, сами по себе, не могут обеспечить абсолютное измерение ориентации. Акселерометр и магнетометр измеряют гравитационные и магнитные поля нашей планеты, и соответственно могут определять абсолютное значение ориентации в пространстве. Тем не менее и они вероятнее всего будут подвержены воздействию высоких уровней шума. Например ускорение из-за движения приведёт к сдвигу контролируемого направления силы тяжести. Задача фильтра ориентации состоит в вычислении единой оценки ориентации полученной путём измерений акселерометра, гироскопа и магнитометра.

Фильтр Калмана [14] стал признанной основой для построения большинства алгоритмов определения ориентации [4, 15, 16, 17] и коммерческих систем ориентации и инерциальных модулей:

— все они основаны на его использовании. Широкое использование решений Калмана являются доказательством их точности и эффективности, однако они имеют ряд недостатков. Они могут быть сложны в реализации, что показано в имеющейся литературе [3, 4, 15, 16, 17, 24, 25, 26, 27, 28, 29, 30, 31, 32]. Линейная регрессия итерации, является основополагающим для процессов Калмана, требования к частоте дискретизации, значительно превышающие пропускную способность объекта. Например, частота дискретизации между 512 Гц и 30 кГц может быть использована в приложениях захвата движения. Состояние отношения, описывающего вращающеюся кинематику в трёх измерениях, как правило, требуют больше векторов состояния и реализации расширенного фильтра Калмана [4, 17, 24] для линеаризации задачи.Эти проблемы требуют большой вычислительной нагрузки для реализации решений Калмана, но и обеспечения чётких мотивов для реализации альтернативных подходов. Многие предыдущие подходы к решению этих вопросов были основаны либо на нечёткой обработке [2, 5] или на фиксации фильтра [33] в пользу акселерометра для определения ориентации на малых угловых скоростях и интегрирование измерений гироскопа при обнаружении высоких угловых скоростей. Такой подход прост, но может быть эффективным только при ограниченных условиях эксплуатации. Бахман и другие [34] предложили альтернативный подход, при котором фильтр достигает оптимального синтеза данных измерений на всех угловых скоростях. Тем не менее, процесс требует аппроксимации методом наименьших квадратов, что также добавляет вычислительной нагрузки. Махони и другие [35] разработали комплиментарный фильтр, который, как показывает практика, является эффективным и действенным решением. Однако, точность годится только для ИНС без магнитометра.

Эта статья описывает новый фильтр ориентации, который применим как к ИНС без магнитометра, так и к ИНС с магнитометром. Фильтр занимается обработкой массивов данных поступающих с датчиков и снимает проблемы точности и настройки параметров фильтров, основанных на подходах Калмана. Фильтр использует кватернион для представления ориентации (например [34, 17, 24, 30, 32]), чтобы описать положение в пространстве в трёх измерениях и не содержит проблем, связанных с описанием положения углами Эйлера (складывание рамок). В статье представлен полный вывод и эмпирические оценки нового фильтра. Его точность сопоставлена с уже существующими промышленными фильтрами и проверены системой оптического измерения. Инновационные аспекты предлагаемого фильтра включают в себя:

два регулируемых параметра (один для реализации без магнитометра) определяемых наблюдаемыми характеристиками системы; аналитические вычисления и оптимизация методом градиентного спуска, что повышает точность при малых частотах дискретизации; компенсация магнитных искажений и компенсация смещения нуля гироскопа в режиме реального времени. 2. Кватернионы Этот раздел статьи не так интересен — в нём описывается, что такое кватернион и некоторые базовые операции. Всё это можно посмотреть в статье с картинками. Нам важно обратить внимание на используемую автором систему обозначений.Рис. 1. Ориентация осей В достигается путём вращения осей А вокруг оси в системе отчёта А на угол θ.Кватернион — это четырехмерное комплексное число, которое может быть использовано для представления ориентации остроконечного тела или координат в трехмерном пространстве. Описать ориентацию системы отчёта B по отношению к системе отчёта A можно с помощью поворота на угол θ вокруг оси в системе отчёта А. Это изображено на рисунке 1, где взаимно ортогональные орты , и  определены главной осью систем координат А и В соответственно. Кватернион, описывающий эту ориентацию определяется уравнением (1), где Rx, Ry и Rz являются компонентами вектора в соответствующих осях X, Y и Z системы отчёта А. Для именования переменных, систем отчёта и векторов принята система обозначений надстрочными и подстрочными индексами по Крэйгу [37].

Впереди стоящий нижний индекс обозначает целевую систему отчёта, а впереди стоящий верхний индекс обозначает систему отчёта относительно которой задана переменная. Далее в тексте индекс S обозначает систему отчёта датчика, а индекс E систему отчёта Земли. Например, описывает ориентацию осей B по отношению к осям A, а  представляет собой вектор в системе отчёта А. Кватернионная арифметика часто требует, чтобы кватернион был нормализован к единице. Поэтому обычно все кватернионы описывающие ориентацию имеют длину равную единице.

Обратите внимание — в данной статье компонента W кватерниона идёт первой (нередко её помещают в конце).Противонаправленные кватернионы обозначаются * (звёздочкой) и используются для изменения системы отчёта. Например противонаправлен по отношению к * и описывает ориентацию системы отчёта A по отношению к B. Противонаправленный вектор к  определяется уравнением (2).

Кватернион, получаемый в результате операции может быть использован для определения составных ориентаций (серии поворотов). Например, для двух ориентаций и  может быть найдена по формуле (3).

Результат умножения для двух кватернионов a и b может быть найден с помощью правила Гамильтона и определяется формулой (4). При перемене мест множителей результат разный (умножение кватернионов ассоциативно, но не коммутативно).

Трехмерный вектор может быть повернут на кватернион с помощью отношения, описанного в уравнении (5) [36]. и  это векторы в системе координат А и B соответственно, где каждый вектор содержит 0 в качестве компоненты W, чтобы сделать их 4 компонентными векторами (кватернионами).

Ориентация описываемая кватернионом может быть описана в виде матрицы вращения определяемой по формуле (6) [36].

Углы Эйлера ψ, θ и φ в так называемой аэрокосмической последовательности [36] описывают ориентацию осей достигаемую за счет последовательных вращений относительно системы отчёта А, с помощью угла ψ вокруг оси Z, θ вокруг оси Y, и φ вокруг оси X. Такие углы Эйлера можно получить из кватерниона с помощью уравнений (7), (8) и (9).

3. Фильтр 3.1. Ориентация из угловой скорости Трёхосевой гироскоп измеряет угловые скорости ωx, ωy и ωz относительно осей X, Y, Z соответственно, в системе отчёта датчика. Если значения этих скоростей (рад/сек) преобразовать в кватернион определённым уравнением (10), то производный кватернион, описывающий скорость в системе отчёта Земли по отношению к системе отчёта датчика может быть вычислен [38] уравнением (11): Откуда взялась ½? Долго не мог понять — откуда взялась ½-я? Это связано с определением кватерниона: … кватернион q определяется как q = w + xi + yj + zk = w + (x, y, z) = cos (a/2) + u*sin (a/2), где u — единичный вектор.В статье о угловой скорости имеется формула, которая выражается из (11): Если для описания поворота используется кватернион, выражаемый через угол a и орт оси поворота v как q = (cos (a/2), v*sin (a/2)), то угловая скорость находится из выражения: , где обозначения в формуле совпадают с обозначениями в статье — кватернион с точкой описывает угловую скорость в системе отчёта Земли, а кватернион с чёрточкой описывает поворот, необходимый для совмещения системы отчёта датчика с системой отчёта Земли. Ориентация в глобальной системе отчёта по отношению к локальной системе отчёта датчика в момент t это , может быть вычислена путем численного интегрирования кватернионов производных , так как описано в уравнениях (12) и (13), при условии, что начальная ориентация в пространстве известна.где  — угловая скорость, измеренная датчиком в момент времени t; ∆t — задержка между измерениями (период дискретизации);  — предыдущий результат оценки ориентации.Индекс ω указывает, что кватернион вычисляется из угловых скоростей.3.2. Ориентация из векторных наблюдений Трёхосевой акселерометр измеряет величину и направление поля тяготения в локальной системе координат вместе с линейными ускорениями вследствии движения датчика. Точно так же трёхосевой магнитометр измеряет величину и направление магнитного поля земли в локальной системе отчёта вместе с локальными магнитными искажениями. В контексте фильтра ориентации, он в начальный момент времени будет предполагать, что акселерометр будет измерять только гравитацию, магнитометр измеряет только магнитное поле Земли (прибор должен быть неподвижен какое-то время).Если направление поля Земли известно в её системе координат, то измерение направления в системе координат датчика позволит рассчитать положение системы координат датчика относительно системы координат Земли. Однако для любого такого измерения не будет единственного решения, а будет бесконечное число решений, представленные всеми теми ориентациями, достигаемыми за счёт вращения ориентации вокруг вращения, параллельного направлению поля (магнитного или гравитации). В некоторых случаях можно решение представить в виде углов Эйлера, где будет два известных угла и один неизвестный. Неизвестный угол будет вращаться вокруг оси параллельной направлению поля. Для представления решения кватернионом требуется комплексное решение. Этого можно достичь путём задачи оптимизации, где ориентация датчика это то, что выравнивает определённое заранее направление поля в земной системе координат , с измеренным направлением в системе координат датчика с помощью операции вращения, определяемой уравнением (5). Поэтому может быть найдено как решение уравнения (14), где уравнение (15) определяет целевую функцию. Компоненты каждого вектора определяются в уравнениях (16), (17) и (18).Существует множество алгоритмов оптимизации, но метод градиентного спуска является одним из простейших в реализации и вычислениях. Уравнение (19) описывает данный метод для n итераций в результате оценки ориентации на основе «начального приближения» ориентации и размера шага µ. Уравнение (20) вычисляет градиент поверхности решений, определяемой целевой функцией и её Якобиан, упрощённый к трёхкомпонентным векторам, определённых в уравнениях (21) и (22) соответственно.Уравнения (19) — (22) описывают общий вид алгоритма.применимого к полю, изначально ориентированного в любом направлении. Однако, если направление поля можно считать только в одной или двух осях глобальной системы координат, уравнение упрощается. Соответствующее соглашение будет предполагать, что направление силы тяжести направлено вертикально, вдоль оси Z, как показано в уравнении (23). Подставляя кватернион и нормализованные измерения акселерометра , и  соответственно в уравнения (21) и (22) получим уравнение (25) и (26).

Направление магнитного поля земли можно считать также располагается в одной плоскости и измеряется в горизонтальной и вертикальной осях. Вертикальная составляющая зависит от точки на Земном шаре, в котором происходит измерение. Для Англии это значение находится в пределах между 65 и 70 градусами по отношению к горизонту [39]. Это может быть представлено уравнением (27). Подставляя и нормализованные значения измерений , и  соответственно в уравнения (21) и (22) получим уравнения (29) и (30).

Как уже обсуждалось, измерение силы тяжести или магнитного поля Земли сами по себе не обеспечивают уникальную ориентацию датчика. Чтобы сделать это, измерения и отношения направления обоих полей должны быть объединены, как описано в уравнениях (31) и (32). В то время как поверхность решений, созданная целевыми функциями в уравнениях (25) и (29) имеют, как минимум, определенный в соответствии с поверхностью решений определённом с помощью уравнения (31), так и минимум определённый по одной точке, при условии, что .

Традиционный подход к оптимизации потребует нескольких итераций уравнения (19), чтобы вычислить результат для каждой новой ориентации и соответствующих измерений датчиков. Эффективные алгоритмы требуют также размер шага μ для корректировки результата на каждой итерации до оптимального значения, обычно получаемый на основе второй производной целевой функции, Гессе. Тем не менее, эти требования существенно повышают вычислительную нагрузку алгоритма и не являются необходимыми в нашем деле. Для нас приемлемо вычисление одной итерации на отсчет времени при условии, что скорость сходимости регулируемая µt равна или больше, чем физическая скорость изменения ориентации. Уравнение (33) вычисляет приблизительное направление вычисляемое в момент времени t на основе предыдущей оценки ориентации и целевой функцией градиента ∇f определяемой путём измерений датчиков и  в момент времени t. Форма ∇f выбирается в соответствии с датчиками в использовании, как показано в уравнении (34). Индекс ∇ означает, что кватернион вычисляется с использованием метода градиентного спуска.

Оптимальное значение µt может быть определено как то, которое обеспечивает скорость сходимости и ограничивается физической скоростью ориентации, так как это позволяет избежать превышения из-за излишне большого размера шага. Поэтому µt может быть вычислено с помощью уравнения (35), где ∆t — это время между замерами,  — это физическая скорость изменения ориентации, измеренная гироскопом, α — это увеличение µ для учёта шума при измерениях акселерометром и магнитометром.

3.3. Алгоритм объединяющего фильтра Мы видим, что ориентация датчика по отношению к Земле получается путём объединения расчётов ориентации и , рассчитываемые с помощью уравнений (13) и (33) соответственно. Объединение и  описывается уравнением (36), где γt and (1 − γt) — это веса, применяемые к каждому расчёту ориентации.Оптимальное значение γt может быть определено как то, при котором взвешенная расходимость равна взвешенной сходимости . Это представлено уравнением (37), где  — это скорость сходимости , а β — это скорость расхождения , выраженная в виде величины кватерниона, производного от соответствующей погрешности измерений гироскопа. Уравнение (37) может быть изменено, чтобы определить γt в уравнение (38).

Уравнения (36) и (38) обеспечивают оптимальное сочетание и  при условии, что скорость сходимости регулируется α, которая больше либо равна чем физическая скорость изменения ориентации. Поэтому α не имеет верхней границы. Если считать α очень большой, то µt определяется выражением (35), а также становится и очень большим значения в упрощённом уравнении фильтра ориентации. Большие значения µt использованные в уравнении (33) означают, что  становится пренебрежительно мало и уравнение можно переписать в виде выражения (39).

Уравнение (38), вычисляющее γt, может быть дополнительно упрощено путём принятия незначительности величин β в знаменателе и выражение тогда можно переписать в виде уравнения (40). Из уравнения (40) вполне возможно, что γt ≈ 0.

Подставляя уравнения (13), (39) и (40) в уравнение (36) получаем непосредственно уравнение (41). Обратите внимание, что γt в уравнении (41) заменяется и как выражение (39) и как 0.

Уравнение (41) можно упростить до уравнения (42), где  — это расчётная скорость изменения ориентации, определяемая выражением (43);  — это направление ошибки , определяемое выражением (44).

Из уравнений (42), (43) и (44) видим, что фильтр вычисляет ориентацию путем численного интегрирования расчетной скорости ориентации . Фильтр вычисляет как скорость изменения ориентации, измеренной гироскопом, то же, но ещё с ошибкой измерения гироскопа, β — это компенсация в направлении предполагаемой ошибки; вычисляется на основании измерений акселерометра и магнитометра. Рис. 2 показывает блок-схему полной реализации фильтра ориентации для ИНС.

Рис. 2. Блок-схема представляющая полную реализацию фильтра ориентации для ИНС

3.4. Компенсация магнитных искажений Измерения магнитного поля Земли будут искажены наличием ферромагнитных источников вблизи датчика. Исследования по влиянию магнитных искажений на эффективность датчика ориентации показали, что существенную погрешность вносят: электроприборы, металлическая мебель и металлоконструкции использованные при строительстве зданий [40, 41]. Источники помех в системе отчёта датчика, могут быть компенсированы путём его калибровки [42, 43, 44, 45]. Источники помех в системе отчёта Земли, вызываемые например месторождениями железа, вызывают ошибки в контролируемом направлении магнитного поля Земли. Ошибки склонения, которые в горизонтальной плоскости по отношению к земной поверхности, не могут быть скомпенсированы без дополнительной информации о курсе. Ошибки наклона в вертикальной плоскости, могут быть скомпенсированы по акселерометру, который обеспечивает дополнительное измерение относительно датчика.Контролируемое направление магнитного поля Земли в земных координатах в момент времени t, , может быть вычислено как нормированное значение измерений магнитометра , вращаемое ориентацией, полученной объединяющим фильтром , как описано в уравнении (45). Эффект ошибочного наклона магнитного поля в контролируемом направлении Земли может быть исправлен, если относительное направление магнитного поля земли всё время имеет тот же самый наклон. Это достигается путём вычисления нормалей и  только на оси X и Y в системе отчёта Земли, что описано в уравнении (46).

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

3.5. Компенсация дрейфа нуля гироскопа Дрейф нуля гироскопа будет происходить от изменения температур, от движения и просто со временем. Любая практическая реализация ИНС должна учитывать это. Преимуществом Калмано-подобных решений является то, что они способны оценить смещение гироскопа в качестве дополнительного состояния в рамках модели системы [26, 30, 15, 24]. Тем не менее Махони и др. [35] показали, что дрейф нуля гироскопа также может быть скомпенсирован простыми фильтрами ориентации, представляя его как часть ошибки от скорости изменения ориентации. Аналогичный подход будет использоваться здесь.Нормированное направление расчетной ошибки в скорости изменения ориентации может быть выражена как угловая погрешность по каждой оси гироскопа с использованием уравнения (47) и получается как обратное отношение из уравнения (11). Смещение гироскопа представлено как DC-составляющая и поэтому может быть удалена, так как часть средневзвешенное соответствующим усилением ζ. Это даёт компенсацию измерений гироскопа как показано в уравнениях (48) и (49). Предполагается, что первый элемент всегда равен 0.

Скомпенсированные измерения гироскопа могут быть использованы вместо исходных измерений гироскопа в уравнении (11). Величина угловой погрешности в каждой оси равна кватерниону единичной длины. Поэтому встроенный коэффициент усиления ζ непосредственно определяет скорость сходимости предполагаемого смещения гироскопа выраженного в виде производного кватерниона. Так как этот процесс требует полной оценки ориентации фильтра это применимо только к реализации фильтра с магнитометром. Рис. 3 показывает блок-схему представления полной реализации фильтра для ИНС с магнитометром, включающий компенсацию магнитных искажений и дрейфа гироскопа.

Рис. 3. Блок-схема представляющая полный фильтр ИНС с магнитометром включающий компенсацию магнитного искажения (группа 1) и компенсацию дрейфа гироскопа (группа 2).

3.6. Коэффициенты усиления фильтра Коэффициент усиления фильтра β представляет все ошибки измерений нуля гироскопа, выраженные как величина производного кватерниона. Источники ошибки: шум датчика, сглаживающий фильтр, ошибки квантования, ошибки калибровки, ошибки установки и выравнивания датчика, неортогональность осей датчика и частотные характеристики. Коэффициент усиления фильтра ζ представляет собой скорость сходимости для удаления ошибок измерения гироскопа, не связанных с нулём и также выражен как величина производного кватерниона. Эти ошибки представляют собой смещение гироскопа. Определять β и ζ удобно с помощью угловых величин и  соответственно, где представляет оценку средней погрешности измерения нуля по каждой оси, а © Habrahabr.ru