Стабилизация квадрокоптера (PID)
Без исходного кода тут не разобраться. Я делал сначала глобальную переменную кватерниона нулевого вращения в памяти. Потом в цикле луптайма читал гироскоп по трем осям и определял по времени между циклами куда все отклонилось на последнем цикле, формировал по этим данным кватернион цикла. Потом перемножал глобальный кватернион и кватернион цикла и перезаписывал глобальный кватернион произведением. И так далее по циклу…
Здравствуйте, если кому интересно тоже написал прогу для стабализации на меге8 и двухстороннюю связь с пультом, тоже со со своей прогой. Обмен 32 байтами на скорости 1мб/с. На пульте отображаются любые параметры коптера, которые умещаются в 32 байта и в экран 16×4 символов. Правда изза того что у пульта не работал стик по рысканью, то управление только по 2 осям. И когда наконец когда понял что для полноцеенного управления по 3 осям нужны матрицы поворота или кватернионы, то понял что сил и времени не хватит. Кстати вычисления с плавающей точкой, время цикла 3 мс при 8 мгц.
Работа с nrf24l01 по spi на аппаратном прерывании, могу выложить, если интересно
У меня получилось без матриц. Тупо обычная тригонометрия и синусами и тангенсами . Давайте обсудим как стабильно удерживать высоту. У меня подозрение что в мавиках используют оптическую стабилизацию. Ну как в оптических мышах. Сравнивают кадры и по вектору смещения выдают перемещение по Х и Y. Вопрос где происходит сравнение в камере или отдельный процессор. И эти данные ПК использует для удержания позиции.
Ну просто не возможно висеть не подвижно в ветер в одной точке. Ни аксель ни барометр на такое не способен. Вот если камеру там закрыть и проверить на удержание , то все станет понятно. Что скажете.
Вот если камеру там закрыть и проверить на удержание все станет понятно. Что скажете.
Зачем закрывать? Мавик держит позицию в ветер ночью на любой высоте. Камера там точно не работает
Загуглил мавик эир, пишут:
точность позиционирования в вертикальной плоскости: ±0,1 м (визуальное) или ±0,5 м (спутниковое);
точность позиционирования в горизонтальной плоскости: ±0,1 м (визуальное) или ±1,5 м (спутниковое);
Там же сонар для определения высоты. Он по идее должен при любом освещении работать. Хотя както ночью чуть об землю не приложил. А вот позиция работает только при хорошем освещении и хорошо отличимом «рисунке» на земле.
У меня получилось без матриц.
И что, летает хорошо в маневрах полицейского разворота? Я просто уперся в то что если поднять квадр на бок, повернуть и положить на пол, то по факту он повернулся по z, а гироскоп естественно по y вернулся в то же положение, повернулся по х, а по Z гироскоп не поворачивался. Получается на боку вращение по х должно превращаться во вращение по Z. Просто жаль, что получилась очень не плохая телеметрия для визуального полета, но не уверен что это будет летать по 3 плоскостям.
Там же сонар для определения высоты. Он по идее должен при любом освещении работать.
Так сонар и работает при любом освещении, но только до высоты 15 метров. И сонар только определяет высоту, в отличии от камер, которые могут “зацепиться” за точку на поверхности.
Мне кажется, что у Мавиков все вычисления производятся мощным процессором, без помощи камер и сонаров - просто по данным с IMU
А вот позиция работает только при хорошем освещении и хорошо отличимом «рисунке» на земле.
Вот с этим “рисунком на земле” чуть не приложил один раз Мавик к дереву…
Ситуация была такая: Мавик плавно снижался на точку взлета и на высоте около метра что то на поверхности не понравилось, и он резко рванул в бок. После чего я отключил точное позиционирование от греха подальше
И что, летает хорошо в маневрах полицейского разворота? Я просто уперся в то что если поднять квадр на бок, повернуть и положить на пол, то по факту он повернулся по z, а гироскоп естественно по y вернулся в то же положение, повернулся по х, а по Z гироскоп не поворачивался. Получается на боку вращение по х должно превращаться во вращение по Z. Просто жаль, что получилась очень не плохая телеметрия для визуального полета, но не уверен что это будет летать по 3 плоскостям.
Надо добавить всего три формулы для переводя вращения по Z в изменения по осям X и Y. К примеру наклонили в право по X на 30 гр. Если развернём по Z на 90 гр по часовой стрелке по по X будет 0 гр. а на Y 30 гр. Вот это преобразование и надо реализовать в формулах. В них учитывать угол поворота по Z , квадрант и угол наклона по X и Y.
Мне кажется, что у Мавиков все вычисления производятся мощным процессором, без помощи камер и сонаров - просто по данным с IMU
Дело не в мощном процессоре, а в дополнительных вводных данных получаемых с сонаров и камер.
Ситуация была такая: Мавик плавно снижался на точку взлета и на высоте около метра что то на поверхности не понравилось, и он резко рванул в бок. После чего я отключил точное позиционирование от греха подальше
Не было никогда таких проблем. Только иногда мог ругаться на поверхность и запрашивал посадку.
Надо добавить всего три формулы для переводя вращения по Z в изменения по осям X и Y. К примеру наклонили в право по X на 30 гр. Если развернём по Z на 90 гр по часовой стрелке по по X будет 0 гр. а на Y 30 гр. Вот это преобразование и надо реализовать в формулах. В них учитывать угол поворота по Z , квадрант и угол наклона по X и Y.
Это все верно, но проблема в том, что оси гироскопа по сути математически равнозначны и вращение вокруг одной оси вызывает вращение двух других. Если мы рассуждаем, что вращение по Z переводит X в Y, мы также должны принять, что вращение по X меняет положение осей Z и Y, а вращение по Y меняет X и Z. Если этого не учитывать, то к дрейфу гироскопа прибавится ошибка вычислений.
Я думал насчет преобразований в этом смысле. То есть степень перехода одной оси в другую зависит от наклона третьей, типа синусов-косинусов. То есть если по Х у нас 45 градусов, то есть он стоит на полу на двух моторах под углом 45 к полу по осиХ. Я его вращаю не отрывая от пола и не меняя угла по Х. Получаю что относительно самого себя по Z я его не вращал, потому что оба мотора на полу, а показывает что вращал, хотя по факту вращал и по У и по Z… Я когда его крутил во все стороны и смотрел углы именно чисто гироскопа на экране пульта, то что то складывалось , а сейчас чот кружится все в голове от 3д картинок, надо мож было попробовать все таки управление по 3ей оси дописать и попробовать его в деле…
Такое ощущение что мы с вами крутимся вокруг этих матриц поворота только своими словами. Вообще заметил что все улучшения сейчас сводятся к введению зависимостей пидов от газа, угла наклона ручек, скорости их наклона, даже от ускорения их наклона и фильтрации шумов. Мне в принципе хватило бы и своей проги, но было желание летать в очках, но без осд думаю это не просто. Поэтому к сожалению уже дошел до матека 722 и прям балдею от кучи параметров полета в очках. Помню как еще во времена мультивия(господина Маховия) чуть ли не всем форумом пытались дотачивали его, хотя 90% писал он. Были времена…
Я думал насчет преобразований в этом смысле. То есть степень перехода одной оси в другую зависит от наклона третьей, типа синусов-косинусов. То есть если по Х у нас 45 градусов, то есть он стоит на полу на двух моторах под углом 45 к полу по осиХ. Я его вращаю не отрывая от пола и не меняя угла по Х. Получаю что относительно самого себя по Z я его не вращал, потому что оба мотора на полу, а показывает что вращал, хотя по факту вращал и по У и по Z…
Такие манёвры не возможны в принципе, только руками . Если будет наклон 45гр то он должен лететь прямо. Без разворота по Z у нас будет управление крестиком. Вправо , в лево ,прямо и т.д. Если по осям нули то вращение по Z вообще без проблем. А если есть угол наклона то измеряя вращение по Z мы
вносим коррекцию по X и Y и PID корректирует оси. Например при движении прямо под 45 гр с пульта даем дисбаланс на оси для вращения по Z. Гира по Z измеряет угол поворота и вносит изменения по углам X и Y , которые не зафиксировали гиры по Х и Y. PID докручивает каждую ось и наши передние моторы как бы остаются на полу , но мы меняем вектор движения .
Вот , вот, теперь начинает проясняться! Я не правильно выразился насчет полицейского. Как называется у нас разворот с ходу когда обе ручки влево или вправо?
Мне интересно ваш хорошо летает?
Летает хорошо .По камере пока не летал , то времени нет, то погода , то зрение изменилось и пришлось переделывать линзы в шлеме с 3диоптр. до 5 диоптр.
Как то попробовал но экран был слишко большой трудно было фокусировать зрение . Сейчас уменьшил картинку (залил прошивку с новым сканом по аналогу и из 7дюй. стало примерно 5дюймов по диагонали , четкость картинки стала лучше на порядок. ) и буду пробовать. Ну и сделал в размере 250 .
Был в краснодаре в августе, еслиб раньше я зарегистрировался можно было б встретится, пообщаться))
Начал дописывать код на АКРО режим. Сделала пока так : Убрал все коррекции по акселю и развороту.
В PID регуляторе обнуляю значения интегрирования гир по углам наклона в момент не нулевого значения данных по стикам X Y.
При положении стика в 0 коптер держит оси X Y . Если по оси есть данные
, эти данные становятся ошибкой и PID их отрабатывает. Интегрирование угла в этот момент обнуляю. Работает нормально, делает полные развороты , пока в руках.
Но в этот момент пропадает полностью стабилизация по этой оси, так как нет обратной связи . Кто может проверить данный момент на своём кваде.
Если дать немного стиком и в этот момент рукой вращать коптер по данной оси будет сопротивление или нет.
Мне кажется в акро вообще нет интегрирования, нам же нужна ошибка угловой скорости а не угла
И если вращать сильнее чем стик то должен сопротивляться. Мож пиды маловаты по Р
Мне кажется в акро вообще нет интегрирования, нам же нужна ошибка угловой скорости а не угла
И если вращать сильнее чем стик то должен сопротивляться. Мож пиды маловаты по Р
Ошибка=Интеграл_гир - Угол_пульта
При стаб режиме мы с пульта шлём угол и пид отрабатывает его пока ошибка
не станет равной 0. Для этого коптер должен повернутся и угол его поворота
будет в Интеграл_гир .
Сделал по другому.
Инт_угла=Инт_угла + Угол_пульта*кофф
Ошибка=Интеграл_гир - Инт_угла
При АКРО нам надо постоянное вращение на любое отклонение стика.
Надо просто проинтегрировать сам угол с пульта через коэффициент.
Мы получим как бы постоянное увеличение угла , до любых градусов.
И PID будет жестко следить за скоростью поворота, у будет стабильная скорость вращения . В этом варианте работает лучше .
Далее возможно 2 решения. Если эти углы не обнулять
там накопится что угодно и при возврате в стаб PID отработает эти углы .
И сами значения этих углов не важны 350 гр. или -180 гр , это не повлияет
на само удержание в нужном угле.
Лучше их обнулять кратковременно в момент возврата стика в 0 или при переключении в стаб .
Причём обнулять надо одновременно Интеграл_гир и Инт_угла .
Проверить есть интегрирование или нет просто. Надо в АКРО довести угол
наклона например до 45 гр и включить стаб. Если он сразу восстановит горизонт , значит высчитал угол . А если медленно , то корректируется по акселю.
Пока писал возникла идея корректировать по акселю одновременно Интеграл_гир и Инт_угла при стике в 0 .
Причем с учетом их ошибки , чтобы не сбивать PID. Допустим при флипе насчитали 350 гр.
в Интеграл_гир и Инт_угла . PID держит -10 гр. настоящий . В этот момент корректируем по акселю
Интеграл_гир и Инт_угла до -10г. Коррекция может быть более скоростной чем при стабе , так как она не влияет
на PID .
Просто суперское решение - корректировать по акселю одновременно Интеграл_гир и Инт_угла при стике в 0 .
Работает идеально !!! При возврате в стаб возвращается в горизонт сразу.
Есть иногда погрешности , т.как вращение происходит в руках .