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

Не буду вдаваться в тонкости устройства и работы современных MEMS и классических гироскопов. Об этом можно прочитать в википедии. Так же не буду говорить о сверхточных гироскопах по той причине, что их цена зашкаливает далеко за разумные пределы и купить их не так просто. Ограничимся теми, что можно найти «в соседнем магазине».

Среди прочих на ней установлен и гироскоп ITG-3205. Поскольку в общем случае сенсоры этой ценовой категории не сильно отличаются друг от друга, на него и будем ориентироваться. В конце концов, на данном этапе конкретная модель не особо важна. Важно то, что эти гироскопы возвращают не углы по осям, а угловые скорости. Для того чтобы вычислить угол относительно горизонта мы должны сделать следующее:

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

2.    С некоторой периодичностью будем снимать показания с гироскопа и постоянно интегрировать.

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

Вторая проблема нас поджидает в самом  начале: как бы так с высокой точностью в начальный момент времени выставить сенсор по горизонту? Представьте себе, что перед запуском коптера Вам придется долго и муторно выставлять его по уровню (и все равно получить некоторую достаточно большую погрешность). А в полете получить ко всему прочему еще и влияние этой начальной погрешности на управляемость аппаратом. Ну как-то совсем не оптимистично получается…

На указанной выше плате установлен BMA180 – пусть будет он – не принципиально.
Так же не буду вдаваться в подробности устройства этого сенсора. Скажу лишь, что углы можно мерять и при помощи него. Именно так в начале и поступали…
Акселерометр – это сенсор, который реагирует на все силы, на него действующие. Кроме силы тяжести. Так в состоянии покоя, снимая с него показания, мы получим вектор, по модулю равный g и направленный перпендикулярно вверх от поверхности земли – силу реакции опоры. Т.е. этот агрегат всегда (почти всегда) знает, где горизонт. Уже хорошо.
В случае, когда к акселерометру приложены какие-то внешние силы и наш объект приобретает ускорение, на выходе акселерометра мы получим суммарный вектор этого ускорения. Причем получим мы его в проекции на локальную систему координат.
Имеем локальную систему координат, имеем проекцию ускорения свободного падения. Соответственно можем вычислить угол поворота относительно вертикали:

В одномерном случае все достаточно просто. Но главная проблема заключается в том, что акселерометр дает очень высокий уровень шума. И шум этот высокочастотный. Это означает, что получаемый на выходе вектор будет сильно колебаться и наш аппарат в абсолютной тишине будет чувствовать себя как при n-бальном шторме. Такое положение вещей тоже далеко от желаемого.

Фильтры

Итого, каждый из сенсоров по отдельности задачу решить не может. Так пусть решают сообща!

И эта задача уже решаема с достаточной нам точностью. Алгоритмов существует много и рассматривать их все не будем. Я хочу остановиться на алгоритме под названием «комплементарный фильтр» или он же «альфа-бета фильтр». Почему? Он достаточно прост в понимании и реализации, требует достаточно мало вычислительных ресурсов. А его точность… Проект MultiWii имеет достаточно большую популярность, которую бы он не получил при недостаточной точности заложенных в него алгоритмов. Проект открытый и любой желающий может покопаться в исходниках и извлечь из его недр тот самый комплементарный фильтр (см. файл IMU.ino).
Основная идея состоит в том, чтобы дрейф нуля гироскопа компенсировать показаниями акселерометра:

Не правда ли очень похоже на ФНЧ? Коэффициент w здесь обычно подбирается в пределах 0.01:0.05. Здесь первая часть формулы за счет достаточно большого коэффициента убирает низкочастотный шум гироскопа, а очень маленький коэффициент во второй части убирает высокочастотный шум акселерометра.
Вторая проблема решается калибровкой акселерометра. Принимая во внимание, что акселерометр в состоянии покоя показывает вектор  в 1g, направленный перпендикулярно горизонту, достаточно легко можно определить начальную позицию объекта.
Правда есть одно «но». Акселерометр даст нам компенсацию только для двух углов. Вращение вокруг вертикальной оси он скомпенсировать не может. Для этого нам нужен второй базовый вектор, ортогональный первому. Получить его мы можем из цифрового компаса. При этом алгоритм работы фильтра будет аналогичным.
В итоге получим все три угла Эйлера. Далее имеет смысл переходить к кватернионам.
Кватернион – четверка чисел, гиперкомплексное число, четырехмерный вектор… все подробности можно прочитать в википедии. Нам же он интересен тем, что через кватернион удобнее всего описывать поворот объекта в пространстве. Так же операции с кватернионами достаточно экономичны с точки зрения затрат процессорного времени, что позволяет повысить скорость пересчетов в целом. Именно поэтому кватернионы получили широкое распространение в различных программно-аппаратных средствах, работающих с трехмерным пространством. Они используются в OpenGL, DirectX и большинстве прошивок для мультироторных систем.
При переходе к кватернионам получим картину для трехмерного случая:

К кватернионам применимы все основные правила векторной алгебры и далее рассчитать необходимый поворот становится делом техники. Единственное что, перед проведением операций, кватернион надо нормализовать.
Вот по этой ссылке я нашел очень неплохое FAQ по операциям с матрицами вращения и кватернионами.

Итог

Мы можем получить текущее положение нашего объекта в пространстве и можем посчитать необходимый поворот в пространстве, для приведения этого объекта в желаемое положение. Осталось это воплотить. Но тут тоже все не так просто и решается эта задача при помощи ПИД-регулятора. Этой теме я планирую посвятить следующую статью.