Предыдущая статья: Микроконтроллеры в управлении летательными аппаратами. Фильтр низких частот.
В данной статье пойдет речь о применении датчиков (гироскопа, акселерометра, компаса). Надеюсь, эта информация будет полезна кому-то при выборе контроллера для своего мультикоптера. Все ниже описанное имеет практическую реализацию в прошивке MultiWii. Ее код является открытым и, я надеюсь, эта статься поможет желающим в нем разобраться.
Так же эта статья будет полезна старшеклассникам и студентам (коих здесь не мало), т.к. в ней наглядно показано практическое применение "вышки" в нашем с Вами хобби.
При стабилизации объекта в пространстве одной из самых распространенных задач является определение положения этого объекта относительно горизонта. Т.е. мы хотим в любой момент времени знать углы наклона нашего объекта относительно земли. Первое, что приходит на ум из курса школьной физики – использовать гироскоп.
Гироскоп
Не буду вдаваться в тонкости устройства и работы современных MEMS и классических гироскопов. Об этом можно прочитать в википедии. Так же не буду говорить о сверхточных гироскопах по той причине, что их цена зашкаливает далеко за разумные пределы и купить их не так просто. Ограничимся теми, что можно найти «в соседнем магазине».
Я использую для экспериментов вот такую плату:
Среди прочих на ней установлен и гироскоп ITG-3205. Поскольку в общем случае сенсоры этой ценовой категории не сильно отличаются друг от друга, на него и будем ориентироваться. В конце концов, на данном этапе конкретная модель не особо важна. Важно то, что эти гироскопы возвращают не углы по осям, а угловые скорости. Для того чтобы вычислить угол относительно горизонта мы должны сделать следующее:
1. В начальный момент времени, когда гироскоп находится в состоянии покоя и его показания по каждой оси равны нулю, предположим, что он установлен горизонтально. Все последующие вычисления будем вести относительно этого положения.
2. С некоторой периодичностью будем снимать показания с гироскопа и постоянно интегрировать.
Так мы сможем узнать интересующие нас углы. Но при интегрировании мы имеем некоторую погрешность. Со временем эта погрешность будет накапливаться. Возьмем гироскоп в руки и некоторое время покрутим его. А потом вернем в исходное положение. Хоть сенсор в начале и в конце наших манипуляций и занимает одно и то же положение, углы будут отличаться. Это явление принято называть дрейфом нуля. Так наш объект будет постоянно вращаться в пространстве по всем трем осям. Далеко не самая приятная новость…
Вторая проблема нас поджидает в самом начале: как бы так с высокой точностью в начальный момент времени выставить сенсор по горизонту? Представьте себе, что перед запуском коптера Вам придется долго и муторно выставлять его по уровню (и все равно получить некоторую достаточно большую погрешность). А в полете получить ко всему прочему еще и влияние этой начальной погрешности на управляемость аппаратом. Ну как-то совсем не оптимистично получается…
Акселерометр
На указанной выше плате установлен BMA180 – пусть будет он – не принципиально.
Так же не буду вдаваться в подробности устройства этого сенсора. Скажу лишь, что углы можно мерять и при помощи него. Именно так в начале и поступали…
Акселерометр – это сенсор, который реагирует на все силы, на него действующие. Кроме силы тяжести. Так в состоянии покоя, снимая с него показания, мы получим вектор, по модулю равный g и направленный перпендикулярно вверх от поверхности земли – силу реакции опоры. Т.е. этот агрегат всегда (почти всегда) знает, где горизонт. Уже хорошо.
В случае, когда к акселерометру приложены какие-то внешние силы и наш объект приобретает ускорение, на выходе акселерометра мы получим суммарный вектор этого ускорения. Причем получим мы его в проекции на локальную систему координат.
Имеем локальную систему координат, имеем проекцию ускорения свободного падения. Соответственно можем вычислить угол поворота относительно вертикали:
a = atan2(gx, - gy)
В одномерном случае все достаточно просто. Но главная проблема заключается в том, что акселерометр дает очень высокий уровень шума. И шум этот высокочастотный. Это означает, что получаемый на выходе вектор будет сильно колебаться и наш аппарат в абсолютной тишине будет чувствовать себя как при n-бальном шторме. Такое положение вещей тоже далеко от желаемого.
Фильтры
Итого, каждый из сенсоров по отдельности задачу решить не может. Так пусть решают сообща!
Угол = f(гироскоп; акселерометр)
И эта задача уже решаема с достаточной нам точностью. Алгоритмов существует много и рассматривать их все не будем. Я хочу остановиться на алгоритме под названием «комплементарный фильтр» или он же «альфа-бета фильтр». Почему? Он достаточно прост в понимании и реализации, требует достаточно мало вычислительных ресурсов. А его точность… Проект MultiWii имеет достаточно большую популярность, которую бы он не получил при недостаточной точности заложенных в него алгоритмов. Проект открытый и любой желающий может покопаться в исходниках и извлечь из его недр тот самый комплементарный фильтр (см. файл IMU.ino).
Основная идея состоит в том, чтобы дрейф нуля гироскопа компенсировать показаниями акселерометра:
a[i+1] = (1 – w) * (a[i] + gyro * delta_t) + w * atan2(gx,– gy)
Не правда ли очень похоже на ФНЧ? Коэффициент w здесь обычно подбирается в пределах 0.01:0.05. Здесь первая часть формулы за счет достаточно большого коэффициента убирает низкочастотный шум гироскопа, а очень маленький коэффициент во второй части убирает высокочастотный шум акселерометра.
Вторая проблема решается калибровкой акселерометра. Принимая во внимание, что акселерометр в состоянии покоя показывает вектор в 1g, направленный перпендикулярно горизонту, достаточно легко можно определить начальную позицию объекта.
Правда есть одно «но». Акселерометр даст нам компенсацию только для двух углов. Вращение вокруг вертикальной оси он скомпенсировать не может. Для этого нам нужен второй базовый вектор, ортогональный первому. Получить его мы можем из цифрового компаса. При этом алгоритм работы фильтра будет аналогичным.
В итоге получим все три угла Эйлера. Далее имеет смысл переходить к кватернионам.
Кватернион – четверка чисел, гиперкомплексное число, четырехмерный вектор… все подробности можно прочитать в википедии. Нам же он интересен тем, что через кватернион удобнее всего описывать поворот объекта в пространстве. Так же операции с кватернионами достаточно экономичны с точки зрения затрат процессорного времени, что позволяет повысить скорость пересчетов в целом. Именно поэтому кватернионы получили широкое распространение в различных программно-аппаратных средствах, работающих с трехмерным пространством. Они используются в OpenGL, DirectX и большинстве прошивок для мультироторных систем.
При переходе к кватернионам получим картину для трехмерного случая:
q[i+1] = (1 – w) * (q[i] + ½*q[i]*(ax * delta_t, ay * delta_t, az * delta_t, 0)) + w * ½ * q[i] * (ex * delta_t, ey * delta_t, ez * delta_t, 0)
К кватернионам применимы все основные правила векторной алгебры и далее рассчитать необходимый поворот становится делом техники. Единственное что, перед проведением операций, кватернион надо нормализовать.
Вот по этой ссылке я нашел очень неплохое FAQ по операциям с матрицами вращения и кватернионами.
Итог
Мы можем получить текущее положение нашего объекта в пространстве и можем посчитать необходимый поворот в пространстве, для приведения этого объекта в желаемое положение. Осталось это воплотить. Но тут тоже все не так просто и решается эта задача при помощи ПИД-регулятора. Этой теме я планирую посвятить следующую статью.
Все на углах этого вашего Эйлера, поэтому работает только при углах по Х и У до 90. Примитив короче. Но кватернионы я не осилил. Проблема в том что при движении в сторону акселерометр из за наложения ускорения неправильно меряет угол. И вот я незнаю что делать.
Вот пример тому из мультивия:
validAcc = 72 < accMag && accMag < 133;
// Apply complimentary filter (Gyro drift correction)
// If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
// To do that, we just skip filter, as EstV already rotated by Gyro
for (axis = 0; axis < 3; axis++) {
if ( validAcc )
EstG.A[axis] = (EstG.A[axis] * GYR_CMPF_FACTOR + imu.accSmooth[axis]) * INV_GYR_CMPF_FACTOR;
Материал хороший. Я не часто ставлю "лайки", но тут совершенно иной случай. Спасибо. Пишите больше.
Немного отвлекаясь от темы.
Я тут как раз дополз до квадрокоптера.
У меня как раз обе мультивишки, 328-ая, указанная на снимке выше и "прошка" (MultiWii Pro). Получилось так потому, что на момент заказа "прошек" не было на стоке, взял, что было, а руки так и не дошли до сборки аппарата. Теперь вот дошли и выясняется, что что-бы понять, как это квадрик настроить, какие есть прошивки, ПИДы там всякие, Ардуино и много всяких новых слов и понятий - надо перечитать километры разных ресурсов. И ладно бы народ писАл конкретно и по делу... начинают холивары разводить, какая прошивка круче, от Маховика RC2 или RC1, либо вовсе штатная и тому подобная болтовня. Народ путём долгих мук и сомнений достигает результата и потом глумится над чайниками в этом вопросе, типа меня. :-))) Ан нет, взять и всё расписать, что и к чему, так сказать представить законченное коробочное решение. Вот чего не хватает. И получается, что каждый топает своим тернистым путём.
Это я к чему. Други, давайте не будем лениться, будем выкладывать больше полезной и доступной для других участников информации. Вот яркий тому пример - эта статья. Ещё раз спасибо.
ЗЫ: в свою очередь обещаю, что напишу небольшой труд по ФПВ, как только закончу экспериментировать с Х-8 и навесным на него оборудованием. С раптором и 5.8ГГц всё уже и так ясно. Игрушка одним словом. А вот Х-8 - тут даже не поле, тут просто океан для творчества.
C какого бодуна?
При стабилизации на хвосте стабильность была выше.
Угловой? Гироскоп? А может датчик ускорений?
Возможно просто условия второго полета существенно отличались от условий первого?
Какая именно система была при этом использована?