-
Notifications
You must be signed in to change notification settings - Fork 7
Theory
Во всем проекте под системой координат с осями x, y, z и началом O подразумевается правосторонняя система координат (xxy=z), ось x которой направлена вдоль крепления мотора A, а ось y лежит в плоскости моторов.
Оборотами двигателя называется угловая скорость вращения винта относительно крепежа. В первом приближении сила тяги, развиваемая винтом, закрепленном на оси бесколлекторного двигателя, линейно зависит от его оборотов.
В рассматриваемой модели тяга развивается перпендикулярно плоскости двигателей. В каждый момент векторы силы тяги, приложенные к каждому из четырех моторов, должны иметь такие длины, что их концы относятся к какой-то одной плоскости. Наложение это условие позволяет избежать ситуации, в которой происходит потеря мощности в результате частичного противодействия различных моторов. Существует взаимно-однозначное соответствие между четверкой векторов оборотов и некоторой плоскостью, проходящей через концы этих векторов, называемой «плоскостью скоростей».
Таким образом основной принцип управления двигателями квадрокоптера можно назвать задание плоскости в пространстве (x, y, F), где (x, y) - плоскость моторов, F - шкала сил. Для простоты описания масштабы осей выбраны так, что вся модель вписана в единичный куб (центр нижней грани в (0, 0, 0); центр верхней грани в (0, 0, 1); ребра параллельно осям координат).
В качестве интерфейса управления скоростями в модели выбран такой радиус-вектор нормали к плоскости скоростей, конец которого относится к этой же плоскости (будем называть этот вектор «вектором управления», throttle vector).
Максимальная тяга коптера достижима при длинах векторов тяги, равных 1. Для воспроизведения ситуации требуется следующее задание вектора управления: (0, 0, 1).
Гироскоп возвращает угловую скорость вращения вокруг каждой из трех осей. Положительное направление — против часовой стрелки. Показания в радианах/сек (метод get_readings())
Показывает разность a - g, где a — ускорение (в datasheet'е называется динамическим ускорением), g — ускорение свободного падения (гравитационная сила, разделенная на массу, в datasheet'е называется статическим ускорением). Показания в единицах g.
На графике видно, что фильтр получает угол даже при сильных шумах.
Очевидным способом получения угла является интегрирование угловой скорости. Но поскольку гироскоп не идеален, простое интегрирование приведет к тому, что полученный угол будет довольно далек от реального. Например, если вращать устройство по одной из осей с достаточно малой угловой скоростью (которую гироскоп не фиксирует), то интеграл будет равен нулю, а настоящий угол — нет.
Также из довольно простых соображений можно было бы получить угол поворота, если бы ускорение a было равно нулю. В таком случае акселерометр показывал бы просто проекции g на оси.
Подведем итог: гироскоп полезен для быстрых изменений (больших угловых скоростей), но если использовать только его, появится нарастающая ошибка. Акселерометр хорошо подходит для ситуации без ускорения, а при появлении ускорения угол при его помощи в одной итерации узнать невозможно.
Решением является совместное использование двух датчиков. Сделав приближение, согласно которому случайные силы Fвн., действующие на квадрокоптер, являются недолговременными, можно получить угол, фильтруя высокие частоты из показаний акселерометра. В данный момент используется итерационная формула RC-фильтра, пропускающего низкие частоты. Идея взята из статьи про стабилизацию.
Реализация: angle' = (1 - α) * (angle+ gyro * Δt) + α * accel_angle, где
- α = Δt / (Δt + T / (2π)),
- T — константа RC для фильтра,
- angle — текущий угол (оставшийся с прошлой итерации),
- angle' — новый угол (для текущей итерации),
- Δt — время между текущей и предыдущей итерацией,
- accel_angle — углы поворота вектора показаний акселерометра относительно вертикали,
- gyro — показания гироскопа.
Вывод уравнения (m — масса квадрокоптера, Fвн. — внешняя сила, g — ускорение свободного падения):
- II закон Ньютона для ЦМ (до коррекции имеется ускорение a, цель — убрать его): ma = FABCD + Fвн. + mg
- II закон Ньютона (после коррекции; считаем, что за время поворота внешняя сила изменилась несильно): 0 = FABCD' + Fвн. + mg
- Отсюда FABCD' = FABCD - ma, т. е. ΔFABCD = -ma
Получение ускорения: Из-за зашумленных данных с акселерометра, полученное ускорение также непригодно для использования. Рассматривается вариант использования еще одного фильтра.
При больших мощностях двигателей фильтровать бессмысленно:
Производится реакция на угловую скорость при помощи ПИД-регулятора. Входные данные — угловая скорость (данные с гироскопа), выходные данные задают момент сил (в настоящее время через плоскость скоростей поворотом вектора управления).
Этот метод довольно распространен , но пока не дал полезного результата в проекте.
Производится реакция на угол, посчитанный с помощью RC-фильтра. Входные данные: угол вращения вокруг двух осей (x и y, или углы крена и тангажа), выходные данные: момент сил (в настоящее время производится поворот вектора управления). В этом методе также используется ПИД-регулятор.