Оценивание пространственной ориентации, или Как не бояться фильтров Махони и Маджвика04.02.2019 12:34

О чём речь

Появление на Хабре поста о фильтре Маджвика было по-своему символическим событием. Видимо, всеобщее увлечение дронами возродило интерес к задаче оценивания ориентации тела по инерциальным измерениям. При этом традиционные методы, основанные на фильтре Калмана, перестали удовлетворять публику — то ли из-за высоких требований к вычислительным ресурсам, неприемлемых для дронов, то ли из-за сложной и неинтуитивной настройки параметров.

Пост сопровождался весьма компактной и эффективной реализацией фильтра на C. Однако судя по комментариям, физический смысл этого кода, а равно и всей статьи, для кого-то остался туманным. Что ж, признаем честно: фильтр Маджвика — самый замысловатый из группы фильтров, основанных в общем-то на очень простых и элегантных принципах. Эти принципы я и рассмотрю в своём посте. Кода здесь не будет. Мой пост — не рассказ о какой-то конкретной реализации алгоритма оценивания ориентации, а скорее приглашение к изобретению собственных вариаций на заданную тему, которых может быть очень много.

Представление ориентации

Вспомним основы. Чтобы оценить ориентацию тела в пространстве, нужно для начала выбрать какие-то параметры, которые в совокупности однозначно определяют эту ориентацию, т.е. по сути ориентацию связанной системы координат $xyz$ относительно условно неподвижной системы — например, географической системы NED (North, East, Down). Затем нужно составить кинематические уравнения, т.е. выразить скорость изменения этих параметров через угловую скорость от гироскопов. Наконец, нужно ввести в расчёт векторные измерения от акселерометров, магнитометров и т.д. Вот самые употребительные способы представления ориентации:

Углы Эйлера — крен (roll, $\phi$ ), тангаж (pitch, $\theta$ ), курс (heading, $\psi$ ). Это самый наглядный и самый лаконичный набор параметров ориентации: количество параметров в точности равно количеству вращательных степеней свободы. Для этих углов можно записать кинематические уравнения Эйлера. Их очень любят в теоретической механике, но в задачах навигации они малопригодны. Во-первых, знание углов не позволяет напрямую преобразовать компоненты какого-либо вектора из связанной в географическую систему координат или наоборот. Во-вторых, при тангаже ±90 градусов кинематические уравнения вырождаются, крен и курс становятся неопределёнными.

Матрица поворота — матрица $\mathbf{C}$ размера 3×3, на которую нужно умножить любой вектор в связанной системе координат, чтобы получить тот же вектор в географической системе: $\mathbf{r}_{NED}=\mathbf{C}\mathbf{r}_{xyz}$ . Матрица всегда ортогональна, т.е. $\mathbf{C}=\mathbf{C}^{T}$ . Кинематическое уравнение для неё имеет вид $\dot{\mathbf{C}}=\mathbf{C}\mathbf{\Omega}$ .
Здесь $\mathbf{\Omega}$ — матрица из компонент угловой скорости, измеренных гироскопами в связанной системе координат:

$\mathbf{\Omega}= \begin{bmatrix} 0 & -\omega_{z} & \omega_{y} \\ \omega_{z} & 0 & -\omega_{x} \\ -\omega_{y} & \omega_{x} & 0 \\ \end{bmatrix}$

Матрица поворота чуть менее наглядна, чем углы Эйлера, зато в отличие от них позволяет непосредственно преобразовывать векторы и ни при каком угловом положении не лишается смысла. С вычислительной точки зрения её главный недостаток — избыточность: ради трёх степеней свободы вводятся сразу девять параметров, и все их нужно обновлять согласно кинематическому уравнению. Задачу можно слегка упростить, воспользовавшись ортогональностью матрицы.

Кватернион поворота — радикальное, но очень неинтуитивное средство против избыточности и вырождения. Это четырёхкомпонентный объект $\mathbf{q}=q_{0}+q_{1}\mathbf{i}+q_{2}\mathbf{j}+q_{3}\mathbf{k}$ — не число, не вектор и не матрица. На кватернион можно смотреть с двух ракурсов. Во-первых, как на формальную сумму скаляра $q_{0}$ и вектора $q_{1}\mathbf{i}+q_{2}\mathbf{j}+q_{3}\mathbf{k}$ , где $\mathbf{i}, \mathbf{j}, \mathbf{k}$ — единичные векторы осей (что, конечно, звучит абсурдно). Во-вторых, как на обобщение комплексных чисел, где теперь используется не одна, а три разных мнимых единицы $\mathbf{i}, \mathbf{j}, \mathbf{k}$ (что звучит не менее абсурдно). Как кватернион связан с поворотом? Через теорему Эйлера: тело всегда можно перевести из одной заданной ориентации в другую одним конечным поворотом на некоторый угол $\alpha$ вокруг некоторой оси с направляющим вектором $\mathbf{u}$ . Эти угол и ось можно объединить в кватернион: $\mathbf{q}=\mathrm{cos}(\alpha/2)+\mathbf{u}\mathrm{sin}(\alpha/2)$ . Как и матрицу, кватернион можно использовать для непосредственного преобразования любого вектора из одной системы координат в другую: $\mathbf{r}_{NED}=\mathbf{q}\mathbf{r}_{xyz}{\mathbf{q}}^{-1}$ . Как видно, кватернионное представление ориентации тоже страдает от избыточности, но намного меньше, чем матричное: лишний параметр всего один. Обстоятельный обзор кватернионов уже был на Хабре. Там шла речь о геометрии и 3D-графике. Нас же интересует ещё и кинематика, поскольку скорость изменения кватерниона нужно связать с измеряемой угловой скоростью. Соответствующее кинематическое уравнение имеет вид $\dot{\mathbf{q}}=1/2\mathbf{q}\mathbf{\omega}$ , где вектор $\mathbf{\omega}$ тоже считается кватернионом с нулевой скалярной частью.

Схемы фильтров

Самый наивный подход к вычислению ориентации — вооружиться кинематическим уравнением и обновлять в соответствии с ним любой понравившийся нам набор параметров. Например, если мы выбрали матрицу поворота, то можем написать цикл с чем-нибудь в духе C += С * Omega * dt. Результат разочарует. Гироскопы, особенно MEMS, имеют большие и нестабильные смещения нуля — в результате даже в полном покое вычисляемая ориентация будет иметь неограниченно накапливающуюся ошибку (дрейф). Все ухищрения, придуманные Махони, Маджвиком и многими другими, не исключая и меня, были направлены на компенсацию этого дрейфа за счёт вовлечения измерений от акселерометров, магнитометров, приёмников GNSS, лагов и т.д. Так родилось целое семейство фильтров ориентации, опирающихся на простой базовый принцип.

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

Здесь заключён совершенно иной подход, чем в построении корректирующего слагаемого фильтра Калмана. Главное отличие именно в том, что управляющая угловая скорость — не слагаемое, а множитель при оцениваемой величине (матрице или кватернионе). Отсюда вытекают важные преимущества:

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

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

Фильтр Махони. Вся зубодробительная математика оригинальной статьи Махони написана ради обоснования несложных уравнений (32). Перепишем их в наших обозначениях. Если отвлечься от оценивания смещений нуля гироскопов, то останутся два ключевых уравнения — собственно кинематическое уравнение для матрицы поворота (с управляющей угловой скоростью в виде матрицы $\mathbf{\Omega}'$ ) и закон формирования этой самой скорости в виде вектора $\mathbf{\omega}'$ . Предположим для простоты, что ни ускорений, ни магнитных наводок нет, и благодаря этому нам доступны измерения ускорения свободного падения $-\hat{\mathbf{g}}_{xyz}$ от акселерометров и напряжённости магнитного поля Земли $\hat{\mathbf{m}}_{xyz}$ от магнитометров. Оба вектора измеряются датчиками в связанной системе координат, а в географической системе их положение заведомо известно: $-\mathbf{g}_{NED}$ направлен вверх, $\mathbf{m}_{NED}$ — на магнитный север. Тогда уравнения фильтра Махони будут выглядеть так:

$\dot{\mathbf{C}}=\mathbf{C}(\mathbf{\Omega}+\mathbf{\Omega}') \\ \mathbf{\omega'}=k_{g}\hat{\mathbf{g}}_{xyz}\times \mathbf{C}^{T}\mathbf{g}_{NED}+k_{m}\hat{\mathbf{m}}_{xyz}\times \mathbf{C}^{T}\mathbf{m}_{NED}$

Посмотрим внимательно на второе уравнение. Первое слагаемое в правой части — это векторное произведение. Первый множитель в нём — измеренное ускорение свободного падения, второй — истинное. Поскольку множители обязаны быть в одной системе координат, то второй множитель преобразуется к связанной системе умножением на $\mathbf{C}^{T}$ . Угловая скорость, построенная как векторное произведение, перпендикулярна плоскости векторов-множителей. Она позволяет поворачивать расчётное положение связанной системы координат, пока векторы-множители не совпадут по направлению — тогда векторное произведение обнулится и поворот прекратится. Коэффициент $k_{g}$ задаёт жёсткость такой обратной связи. Второе слагаемое выполняет аналогичную операцию с магнитным вектором. По сути фильтр Махони воплощает хорошо известный тезис: знание двух неколлинеарных векторов в двух разных системах координат позволяет однозначно восстановить взаимную ориентацию этих систем. Если векторов больше двух, то это даст полезную избыточность измерений. Если вектор всего один, то одну вращательную степень свободы (движение вокруг этого вектора) зафиксировать не удастся. Например, если дан только вектор $-\mathbf{g}$ , то можно скорректировать дрейф крена и тангажа, но не курса.

Разумеется, в фильтре Махони необязательно пользоваться матрицей поворота. Есть и неканонические кватернионные варианты.

Виртуальная гироплатформа. В фильтре Махони мы прилагали управляющую угловую скорость $\mathbf{\omega'}$ к связанной системе координат. Но можно приложить её и к расчётному положению географической системы координат. Кинематическое уравнение тогда примет вид

$\dot{\mathbf{C}}=\mathbf{C}\mathbf{\Omega}-\mathbf{\Omega}'\mathbf{C}$

Оказывается, такой подход открывает путь к очень плодотворным физическим аналогиям. Достаточно вспомнить то, с чего начиналась гироскопическая техника, — курсовертикали и инерциальные навигационные системы на основе гиростабилизированной платформы в кардановом подвесе.

www.theairlinepilots.com

Задачей платформы там была материализация географической системы координат. Ориентация носителя измерялась относительно этой платформы датчиками углов на рамах подвеса. Если гироскопы имели дрейф, то вслед за ними дрейфовала и платформа, и в показаниях датчиков углов накапливались ошибки. Чтобы эти ошибки устранить, вводилась обратная связь от акселерометров, установленных на платформе. Например, отклонение платформы от горизонта вокруг северной оси воспринималось акселерометром восточной оси. Этот сигнал позволял задать управляющую угловую скорость $\mathbf{\omega'}$ , возвращающую платформу в горизонт.

Теми же самыми наглядными понятиями мы можем пользоваться и в своей задаче. Выписанное кинематическое уравнение нужно тогда читать так: скорость изменения ориентации представляет собой разность двух вращательных движений — абсолютного движения носителя (первое слагаемое) и абсолютного движения виртуальной гироплатформы (второе слагаемое). Аналогию можно распространить и на закон формирования управляющей угловой скорости. Вектор $-\hat{\mathbf{g}}_{NED}=-\mathbf{C}\hat{\mathbf{g}}_{xyz}$ олицетворяет показания акселерометров, якобы стоящих на гироплатформе. Тогда из физических соображений можно написать:

$\omega'_{N}=-gk_{g}\hat{g}_{E},\; \omega'_{E}=gk_{g}\hat{g}_{N}$

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

Первый намёк на полезную аналогию платформенной и бесплатформенной инерциальной навигации появляется, видимо, в древнем патенте «Боинга». Затем эта идея активно разрабатывалась Салычевым, а в последнее время — и мной тоже. Очевидные преимущества такого подхода:

Управляющую угловую скорость можно формировать на основе понятных физических принципов.
Естественным образом оказываются разделены горизонтальные и курсовой каналы, очень различные по своим свойствам и способам коррекции. В фильтре Махони они смешаны.
Удобно компенсировать влияние ускорений за счёт привлечения данных GNSS, которые выдаются именно в географических, а не связанных осях.
Легко обобщить алгоритм на случай высокоточной инерциальной навигации, где приходится учитывать форму и вращение Земли. Как это сделать в схеме Махони, я не представляю.

Фильтр Маджвика. Маджвик избрал трудный путь. Если Махони, судя по всему, интуитивно пришёл к своему решению, а потом обосновал его математически, то Маджвик с самого начала проявил себя формалистом. Он взялся решать задачу оптимизации. Рассудил он так. Зададим ориентацию кватернионом поворота. В идеальном случае расчётное направление какого-нибудь измеряемого вектора (пусть у нас это будет $-\mathbf{g}$ ) совпадает с истинным. Тогда будет ${\mathbf{q}}^{-1}\mathbf{g}_{NED}\mathbf{q}=\hat{\mathbf{g}}_{xyz}$ . В реальности это не всегда достижимо (особенно если векторов больше чем два), но можно попробовать минимизировать отклонение $\mathbf{F}={\mathbf{q}^{-1}\mathbf{g}_{NED}\mathbf{q}}-\hat{\mathbf{g}}_{xyz}$ от точного равенства. Для этого введём критерий минимизации

$E=\frac{1}{2}|\mathbf{F}|^{2}\rightarrow \mathrm{min}$

Минимизация требует градиентного спуска — движения маленькими шагами в сторону, противоположную градиенту $\nabla E$ , т.е. противоположную наискорейшему возрастанию функции $E$ . Кстати, Маджвик допускает ошибку: во всех своих работах он вообще не вводит $E$ и настойчиво пишет $\nabla \mathbf{F}$ вместо $\nabla E$ , хотя фактически вычисляет именно $\nabla E$ .

Градиентный спуск в итоге приводит к следующему условию: для компенсации дрейфа ориентации нужно добавить к скорости изменения кватерниона из кинематического уравнения новое отрицательное слагаемое, пропорциональное $\nabla E$ :

$\dot{\mathbf{q}}=\frac{1}{2}\mathbf{q}\mathbf{\omega}-\beta \frac{\nabla E}{|\nabla E|}$

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

Влияние ускорений

До сих пор предполагалось, что истинных ускорений нет и акселерометры измеряют только ускорение свободного падения $-\mathbf{g}$ . Это позволяло получить эталон вертикали и с его помощью скомпенсировать дрейф крена и тангажа. Однако в общем случае акселерометры, независимо от своего принципа действия, измеряют кажущееся ускорение — векторную разность истинного ускорения и ускорения свободного падения $\mathbf{f}=\mathbf{a}-\mathbf{g}$ . Направление кажущегося ускорения не совпадает с вертикалью, и в оценках крена и тангажа появляются ошибки, вызванные ускорениями.

Это легко проиллюстрировать с помощью аналогии виртуальной гироплатформы. Её система коррекции устроена так, что платформа останавливается в том угловом положении, в котором обнуляются сигналы акселерометров, якобы установленных на ней, т.е. когда измеряемый вектор $\mathbf{f}$ становится перпендикулярен осям чувствительности акселерометров. Если ускорений нет, это положение совпадает с горизонтом. Когда возникают горизонтальные ускорения, гироплатформа отклоняется. Можно сказать, что гироплатформа похожа на сильно задемпфированный маятник или отвес.

В комментариях к посту о фильтре Маджвика промелькнул вопрос о том, можно ли надеяться на то, что этот фильтр менее восприимчив к ускорениям, чем, например, фильтр Махони. Увы, все описанные здесь фильтры эксплуатируют одни и те же физические принципы и поэтому страдают от одних и тех же проблем. Обмануть физику математикой нельзя. Что же тогда делать?

Самый простой и грубый способ придумали ещё в середине прошлого века для авиационных гировертикалей: уменьшать или вовсе обнулять управляющую угловую скорость при наличии ускорений или угловой скорости курса (которая свидетельствует о входе в вираж). Тот же метод можно перенести и в нынешние бесплатформенные системы. Об ускорениях при этом нужно судить по значениям $\hat{f}_{N}, \hat{f}_{E}$ , а не $\hat{f}_{x}, \hat{f}_{y}$ , которые в вираже сами по себе нулевые. Однако в величине $\hat{\mathbf{f}}_{NED}$ не всегда можно отличить истинные ускорения от проекций ускорения свободного падения, обусловленных тем самым наклоном гироплатформы, который требуется устранить. Поэтому метод работает ненадёжно — зато не требует никаких дополнительных датчиков.

Более точный способ основан на использовании внешних измерений скорости от приёмника GNSS. Если известна скорость $\mathbf{v}$ , то её можно численно продифференцировать и получить истинное ускорение $\dot{\mathbf{v}}$ . Тогда разность $\hat{\mathbf{f}}_{NED}-\dot{\mathbf{v}}$ будет в точности равна $-\hat{\mathbf{g}}_{NED}$ независимо от движения носителя. Ей можно пользоваться как эталоном вертикали. Например, можно задать управляющие угловые скорости гироплатформы в виде

$\omega'_{N}=gk_{g}(\hat{f}_{E}-\dot{v}_{E}),\; \omega'_{E}=-gk_{g}(\hat{f}_{N}-\dot{v}_{N})$

Смещения нуля датчиков

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

Гироскопы. Разберёмся со смещениями нуля гироскопов $\delta{\mathbf{\omega}}$ . Расчётное положение связанной системы координат уходит от своего истинного положения с угловой скоростью, определяемой двумя противодействующими факторами — смещениями нуля гироскопов и управляющей угловой скоростью: $\delta{\mathbf{\omega}}-\mathbf{\omega'}$ . Если системе коррекции (например, в фильтре Махони) удалось остановить уход, то в установившемся режиме окажется $\delta{\mathbf{\omega}}=\mathbf{\omega'}$ . Иными словами, в управляющей угловой скорости $\mathbf{\omega'}$ заключена информация о неизвестном действующем возмущении $\delta{\mathbf{\omega}}$ . Поэтому можно применить компенсационное оценивание: мы не знаем величины возмущения непосредственно, однако знаем, какое корректирующее воздействие нужно, чтобы его уравновесить. На этом основано оценивание смещений нуля гироскопов. Например, у Махони оценка обновляется по закону

$\dot{\hat{\delta\mathbf{\omega}}}=k_{b}\mathbf{\omega'}$

Однако результат у него получается странный: оценки достигают 0,04 рад/с. Такой нестабильности смещений нуля не бывает даже у самых скверных гироскопов. Подозреваю, проблема связана с тем, что Махони не использует GNSS или другие внешние датчики — и в полной мере страдает от влияния ускорений. Только по вертикальной оси, где ускорения не вредят, оценка выглядит более или менее здравой:

Mahony et al., 2008

Акселерометры. Оценить смещения нуля акселерометров $\delta{\mathbf{f}}$ намного сложнее. Информацию о них приходится извлекать из той же управляющей угловой скорости $\mathbf{\omega'}$ . Однако в прямолинейном движении эффект смещений нуля акселерометров неотличим от наклона носителя или перекоса установки блока датчиков на нём. Никакой добавки к $\mathbf{\omega'}$ акселерометры не создают. Добавка появляется только при развороте, что и позволяет разделить и независимо оценить погрешности гироскопов и акселерометров. Пример того, как это можно сделать, есть в моей статье. Вот картинки оттуда:

Вместо заключения:, а что же с фильтром Калмана?

У меня нет сомнения, что описанные здесь фильтры почти всегда будут иметь преимущество перед традиционным фильтром Калмана в отношении быстродействия, компактности кода и удобства настройки — для этого они и создавались. Что касается точности оценивания, то здесь всё не столь однозначно. Мне встречались неудачно спроектированные фильтры Калмана, которые и по точности заметно проигрывали фильтру с виртуальной гироплатформой. Маджвик также доказывал выгоды своего фильтра относительно каких-то калмановских оценок. Однако для одной и той же задачи оценивания ориентации можно соорудить не менее десятка разных схем фильтра Калмана, и у каждой будет бесчисленное количество вариантов настройки. У меня нет никаких поводов думать, что фильтр Махони или Маджвика окажется точнее лучшего из возможных фильтров Калмана. И конечно, за калмановским подходом всегда останется преимущество универсальности: он не налагает никаких жёстких ограничений на конкретные динамические свойства оцениваемой системы.