Theory

Table of Contents Система координат Плоскость скоростей Пример Данные с сенсоров Гироскоп Акселерометр Реализация реакции на смещения в xOy Получение угла из данных акселлерометра и гироскопа Реализация реакции на вращения

Система координат

Во всем проекте под системой координат с осями 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.

Реализация реакции на смещения в xOy

Вывод уравнения (m — масса квадрокоптера, F_вн. — внешняя сила, g — ускорение свободного падения):

1. II закон Ньютона для ЦМ (до коррекции имеется ускорение a, цель — убрать его): ma = F_ABCD + F_вн. + mg
2. II закон Ньютона (после коррекции; считаем, что за время поворота внешняя сила изменилась несильно): 0 = F_ABCD' + F_вн. + mg
3. Отсюда F_ABCD' = F_ABCD - ma, т. е. ΔF_ABCD = -ma

То есть, чтобы знать, как нужно изменить силу (т.е. куда поворачиваться), необходимо узнать ускорение a. Но из принципа эквивалентности следует, что в общем виде это невозможно (всегда можно узнать только разность a - g, которую и показывает акселерометр). Так как реагировать следует на вектор ускорения, необходимо узнать направление g (модуль известен). Но из показаний гироскопа и акселлерометра можно получить это направление, воспользовавшись допущением (см. раздел ниже).

Получение ускорения: Из-за зашумленных данных с акселерометра, полученное ускорение также непригодно для использования. Рассматривается вариант использования еще одного фильтра.

При больших мощностях двигателей фильтровать бессмысленно:

Получение угла из данных акселлерометра и гироскопа

На графике видно, что фильтр получает угол даже при сильных шумах.

Очевидным способом получения угла является интегрирование угловой скорости. Но поскольку гироскоп не идеален, простое интегрирование приведет к тому, что полученный угол будет довольно далек от реального. Например, если вращать устройство по одной из осей с достаточно малой угловой скоростью (которую гироскоп не фиксирует), то интеграл будет равен нулю, а настоящий угол — нет.

Также из довольно простых соображений можно было бы получить угол поворота, если бы ускорение a было равно нулю. В таком случае акселерометр показывал бы просто проекции g на оси.

Подведем итог: гироскоп полезен для быстрых изменений (больших угловых скоростей), но если использовать только его, появится нарастающая ошибка. Акселерометр хорошо подходит для ситуации без ускорения, а при появлении ускорения угол при его помощи в одной итерации узнать невозможно.

Решением является совместное использование двух датчиков. Сделав приближение, согласно которому случайные силы F_вн., действующие на квадрокоптер, являются недолговременными, можно получить угол, фильтруя высокие частоты из показаний акселерометра. В данный момент используется итерационная формула RC-фильтра, пропускающего низкие частоты. Идея взята из статьи про стабилизацию.

Реализация: angle' = (1 - α) * angle + α * accel_angle * + gyro * Δt, где

• α = Δt / (Δt + T / (2π)),
• T — константа RC для фильтра,
• angle — текущий угол (оставшийся с прошлой итерации),
• angle' — новый угол (для текущей итерации),
• Δt — время между текущей и предыдущей итерацией,
• accel_angle — углы поворота вектора показаний акселерометра относительно вертикали,
• gyro — показания гироскопа.

Член gyro * Δt (численный интеграл угловой скорости) необходим для учета быстрых изменений, которые не проходят через RC-фильтр.

Реализация реакции на вращения

Производится реакция на угловую скорость. gyro_correcton вычисляется как const * gyro_data