Стабилизация квадрокоптера (PID)
ug_xa не изменится в начале падения, когда квадрокоптер уравновесится сопротивлением воздуха очень даже изменится и общий угол неправильным будет. Комп. фильтр нужен чтоб скомпенсировать дрейф гироскопа, если дрейф маленький то и коэффициент у акселя меньше в формуле. Если интересуют мгновенные изменения углов для расчета пидов, аксель вообще можно не брать в расчет, вот рейсеры вообще отключают и без него летают)
Да, моё утверждение неверное. Значит повезло, что он хоть как-то держался в нескольких см.от пола)
Вот что значить править чужой код и не понимать основ. Какие данные у нас на входе, как их обрабатывать и что потом с ними делать.
Данные с гир это основные данные, они не меняются от ускорения обьекта , меньше шумят и более точные.
Я пошёл по пути перевода всё в градусы. Интегрируя данные с гир получаем угол наклона в градусах по осям.
Но со временем они уплывают даже с учетом калибровки. Это примерно несколько минут. Также ошибка накапливается при вибрациях и
манёврах. По данным аксел. токже вычисляем наклон в градусах. Но если ПК водить по столу угол будет сильно менятся из за ускорений.
Данные с акселя нужны чтобы знать горизонт.
Надо учитывать сильно усреднённые данные с акселя . А угол с гироскопа корректировать по ним .
Нафига нам тратить ресурсы на усреднение по акселерометру если проще учитывать их порциями .
ug_xg = ug_xg + xg * 0.000098; Это получаем наклон в градусах по гироскопу.
ug_xg = ug_xg*0.999 + ug_xa*0.001; Наклон по гироскопу корректируем углом по акселю.
За 1 секунду у меня 320 раз проходит весь цикл программы.
Чтобы угол по гир. стал равным углу по акселю надо строку ug_xg = ug_xg*0.999 + ug_xa*0.001; выполнить более 1000 раз.
Это примерно 3-4 сек. Даже если при ускорениях несколько показаний угла ug_xa будут неверные , они сгладятся другими данными.
угол наклона = ug_xa - ug_xg. Забудь про это.
Если у тебя аксель будет ускорятся ,угол он исказит , и эти данные мгновенно по твоей формуле пойдут на ПИД . И коптер
отработает наклон которого нет.
Я все время испытываю коптер в руках и нет проблем с настройками. В идеале он сопротивляеся наклонам как раскрученный гироскоп.
Насчёт P и D . У меня P=1 . Это всё условно , нужно пересчитывать в идеале в прибавку тяги на ось. Самый главный тут D. Он постоянно
отрабатывает малейшие изменения угла по датчикам. Это как на стекле удерживать шарик в ценре. Ты постоянно управляеш краями , чтобы
он не скатился. А положение его от центра зависит от P. А I это как бы поправка на край если ты часто этот край задействуеш.
Это ты глазами видиш где шар, а гире нужна опора в виде акселеромерта , для определения уровня горизонта.
Самый простой способ : на полу уровнем найти идеальную площадку по 2-м осям. Если позволяет конструкция прижать коптер к полу и запустить калибровку. Можно на фанеру прибить 4-е шпильки , чтоб на них ставить коптер лучами. Шпильки выставляем по уровню и калибруем аксель.
Гиры калибруются в любом положении . Но средние данные могут зависеть от температуры .
Далее если мы летим т.е наклоняем его , то надо добавлять газ. Это можно сделать автоматически по формуле .
Также надо учитывать много ньюансов. К примеру ты дал газу и он улетел, потеряв связь . Нужно заводить счетчик на потерю данных с пульта.
При сработке нужен алгоритм на спасение. 1 надо снизить газ до уровня висения ( тоже проблема как определить , напряжение меняется )
2. Начать снижение по баромерту до заданной высоты , при достижении выключить двигатели.
Также надо обязательно ввести защиту от критических углов наклона. Например у меня при 60 гр. двигатели блокируются.
Заставить его висеть ровно по горизонту это все фигня. Нужны ещё ПИД по оси Z , по высоте , алгоритмы на аварии и.т.д.
То что написано про аксел выше бред, аксель мгновенно показывает изненение по осям. Да он сильно шумит , но основе среднего шума
мы определяем горизонт.
Чтобы были полёты не только по осям нужен особый алгоритм для коррекции осей по Z . Придумал такой метод проверки.
На коптер сверху крепим шарнир с длинной палкой.Он висит на палке и вращается по осям и по Z. Даём газ висения и управляем стиками .
Сильно упрощает настройку по сравнению с реальными полетами.
Пропы должны быть жесткие. Я ранее потратил много времени не понимая что гибкие пропы на 11" у меня флатерят.
Думал что у меня наводки или вибрации на ПК. Он периодически резко вздрагивал по осям.
M1=constrain(gaz2+DM1+DRZ,123,251); M3=constrain(gaz2+DM3-DRZ,123, 251); // DRZ для поворота по оси
M2=constrain(gaz2+DM2-DRZ,123,251); M4=constrain(gaz2+DM4+DRZ,123, 251); // Разница в шим по диагонали
DRZ это и есть стабилизация по Z и вращение одновременно.
Чтобы развернуть коптер ещё нужно специально ослабить стабилизацию по Z. У нас нет привязки к азимуту и
P не нужен. И данные гир по Z нужно сильно уменьшить ,в сотни раз. Стабилизация по Z должна работать и при нуле газа,
т.е. при резких поворотах вокруг оси должны включатся противоположные диагонали.
То что написано про аксел выше бред, аксель мгновенно показывает изненение по осям.
Если прибить к полу квадрокоптер, то да все мгновенно, но в полете квадрокоптеру не на что опереться кроме как на воздух. Это классическая ошибка.
Причём тут воздух. Коптер летает в режимах стабилизации от 0 до 30-40 гр. И когда он летит равномерно аксель показывает данные как на столе,
только более шумные из за вибраций. И только в моменты ускорений и торможений угол будет ошибочным.
И только в моменты ускорений и торможений угол будет ошибочным.
Просто не понял где в моих словах бред) Да, также алгоритм не будет работать при постоянных полетах по кругу, горизонт уедет в сторону.
Ну попробуйте мысленный эксперимент. Коптер завис на одном месте. Показания акселерометра по осям 0,0,g. Повернем коптер на 90 градусов по питчу или ролу. Показания акселя как ни странно не изменились 0,0,g. Коптер начинает разгонятся в сторону куда его повернули(двигатели) и одновлеменно падать вниз (гравитация) и только когда сопротивление воздуха уравновесит эти силы аксель покажет правильный угол по горизонту. Теперь понятно почему аксель только коректирует гиру?
Во первых ось Z всегда показывает ускорение своб. пад. 9.8 м.с.кв , при настройке в 16гр это число около 2000 а не 0.
При наклоне это ускорение появляется на оси X или Y . 2000 появится при наклоне 90 гр., на оси которую наклонили а на Z будет 0. Чтобы по Z был 0 при горизонте , надо скинуть коптер с крыши без пропов. А при ускорении вверх Z наоборот возрастает .
Да, также алгоритм не будет работать при постоянных полетах по кругу, горизонт уедет в сторону.
Это по чему ? Я не привёл самый основной кусок кода где есть коррекция по осям от вращения.
Вот что значить править чужой код и не понимать основ.
Да не любитель я брать чужое. Смотрю как люди делают, пишу сам и пробую по-разному. Углы Эйлера только взял чужой код для расчета севера по магнитометру. и Всё!
Основы (статья, книга) мне на глаза не попались. Смотрел чужой код, на сырые данные и в итоге написал вот это вот… но ничего, перепишу!
Спасибо за разъяснение!
Во первых ось Z всегда показывает ускорение своб. пад. 9.8 м.с.кв
А у меня что ?0,0,g -это x=0,y=0,z=g.
При наклоне это ускорение появляется на оси X или Y . 2000 появится при в 90 гр. на оси которую наклонили.
Не сразу, а как только квадрик будет двигаться без ускорений, тобишь сопротивление воздуха скомпенсирует силу моторов и гравитации. Квадрокоптер в итоге превращается в самолет и по z в итоге будет 0, вы же летали в самолете, вот квадрокоптер повернутый на 90 градусов это самолет)
Я не привёл самый основной кусок кода где есть коррекция по осям от вращения.
Если Вы знаете как скомпенсировать дрейф гироскопа при полетах по кругу только с помошью акселерометра, то это нобелевка как минимум.
Квадрокоптер в итоге превращается в самолет и по z в итоге будет 0, вы же летали в самолете, вот квадрокоптер повернутый на 90 градусов это самолет)
Да какого хрена ? По Z данные будут меньше пропорционально углу наклона . При наших небольших углах наклона это 2000* cos(35Гр )= 1638
Если фристайл то там управление по гирам и полностью ручное , и пофиг что нам выдаёт аксель. Опишите для начала диаметр круга и скорость, и способ поворота . Если угол запредельный , например 45-50 гр, то и скорость будет максимальная , такие режимы тоже ручные.
Ранее я приводил код.
if ( abs(ug_xa)<45 && abs(ug_ya)<45 ) { ug_xg = ug_xg*0.999 + ug_xa*0.001; // комплиментарный фильтр
ug_yg = ug_yg*0.999 + ug_ya*0.001; // коррекция гир по акселю 0.001
}
Как видно коррекция только до 45 гр.
Опишите для начала диаметр круга и скорость, и способ поворота .
При полетах постоянных полетах по одному кругу возникает постоянная центробежная сила, действующая на акселерометр, приводит к дрейфу гироскопа в сторону мнимого горизонта по акселю. Это, кстати, первая причина ухода горизонта у подвесов и причина подключения их к полетникам для передачи инфы по горизонту.
Пропы должны быть жесткие. Я ранее потратил много времени не понимая что гибкие пропы на 11" у меня флатерят.
Думал что у меня наводки или вибрации на ПК. Он периодически резко вздрагивал по осям.
Вот я усредняю данные потому что заметил от гироскопа мощные импульсы 1-2 раза в секунду на оборотах 40%. Далее вроде учащаются случаи. И подергивается немного коптер от каждого импульса. После усреднения проблема пропала. Думал отложить решение этой проблемы, но лучше сразу найти причину.
На счет безопасности, у меня через 2 секунды ПК отключает мотор, если не получил команду с “земли”. Плавный спуск потом уж добавлю, когда выше 10см взлетит)
Стенд с шарниром - отличное решение!
Не представляю реальные задачи для таких маневров. Например при 5м/сек и радиусе в 50 метров ускорение 0.5м/сек.кв
Это даст погрешность в угле 2-3 гр всего atan2(100,2000) , Просто довернём рысканьем по Z и все.
Можно подумать на это способен в автоматическом режиме стандартный ПК, приведите пример.
Выложу ещё одну фичу. У меня на ПК через делитель сопротивления идёт бортовое питание.
U = analogRead(0)*0.0251;
Сразу получаем напряжение батареи в вольтах . Если умножить напряжение на газ висения , получим отличную переменную.
Я даю газ чтобы зависнуть , нажимаю кнопку и эту переменную записываю в память.
В любой момент времени деля переменную на текущее питание батареи получаем газ висения.
Пригодится для любых случаев.
Пригодится для любых случаев.
Те рекомендации реально помогли, спасибо! Реализовал с кватернионами и при вращении по одной оси ошибок почти нет, а при вращении по кругу (x,y,-x,-y) появляется ошибка 3-8 градусов наверное за 8 секунд. Это нормально? И калибровку акселерометра по z не сделать внутри коптера? Стенд надо городить?насколько это критично?
Кроме того, что было Вами сказано, в ПК больше ничего не надо добавлять?
Кватернионы взял из статьи: m.habr.com/ru/post/255005/
Вращение по кругу это как ? Если вокруг оси с пульта или руками то гиры по х и у не должны выдавать данные наклона.
Если только сам чип гироскопа не в центре осей коптера . Проведи эксперимент крути ПК на столе вокруг оси и смотри что даёт гира по дгугим осям.
Если ПК наклонить ровно на 90 гр по X или Y , что выдаст софт ? Если опросов с гир мало , могут быть пропуски данных и углы будут с ошибками.
Ошибку можно уменьшить по акселю , изменив фильтр. Стенд нужен только для калибровки осей X и Y акселерометра , чтобы знать точный горизонт.
По Z акселерометра калибровка не нужна. Калибровка X и Y акселерометра критична . Если ног нет можно положить на пол, проверив уровнем .
В принципе можно и без калибровки , использовать триммер. Тример на пульте, и пульт всегда шлёт угол на нужную ось.
Но лучше тример в мозгах , запоминаем переменную и её вычитаем из PID
oshibka_x =ug_xg - dx - trim_x
Без калибровки по гирам не обойтись совсем , главное в момент калибровки не трогать , положение коптера любое.
Попробу включи ПК и не трогай , посмотри что по осям .
В ПК добавлять ничего не надо , он висит и летает только за счет софта . Софт самое главное.
Если ПК наклонить ровно на 90 гр по X или Y , что выдаст софт ?
Выдаст 90 гр, как и должно быть. когда кручу по 1-2 осям - всё ОК. А когда по трем, то начинаются смещения.
Например, если плата в самолете, то держу за хвост и кручу за передний мотор по кругу. получается если не кренить, то задействованы оси x и z. И когда самолет возвращаю к начальному состоянию, все смещения показывают 0.
Но если при кручении добавить крен (ось y), то когда самолет возвращаю в начальное состояние, то по оси y будет не ноль, как должно быть, а там копится ошибка.
Так и должно быть или что-то ни так?
zealot01 ранее писал что при движении по кругу аксель начинает показывать
центростремительное ускорение .Появляется ложный угол наклона, который
вносит ошибку в коррекцию гироскопа. Как выриант можно уменьшать переменную коррекции гир по акселю в зависимости от данных Z пульта ( Курс, Yaw) . Но у тебя при отсутствии крена ошибка 0,
что непонятно. И в реале такие манёвры не возможны, очень маленький радиус вращения. Таже все данные верны если вращения идут через центр чипа гироскопа. При центре вращения сбоку гиры могут выдавать что угодно.
И только от софта зависит как это правильно обработать. И некоторые гироскопы могут выдавать ложные данные по X Y при вращении по Z.
Проведи эксперимент. Плата на столе , всё по 0. Подними наклони по X Y ( не более 40 гр ) с одновременным вращение по Z ( один оборот ) и верни на стол, что будет по осям.
Так и должно быть или что-то ни так?
Если считал только по калиброванному гироскопу, то дрейф от шумов и вибраций будет уводить показания. Нужно также поправлять по акселерометру. И даже тогда вектор направления будет крутиться вокруг оси z, нужно поправлять по компасу.) И таки да, ускорения негативно влияют на показания гироскопа.
Проведи эксперимент. Плата на столе , всё по 0. Подними наклони по X Y ( не более 40 гр ) с одновременным вращение по Z ( один оборот ) и верни на стол, что будет по осям.
Если вернуть плату “на место”, то по Y и Z будут отклонения.
Я перепроверил и оказалось, что ошибки не появляется только в случае вращения по одной оси.
Без исходного кода тут не разобраться. Я делал сначала глобальную переменную кватерниона нулевого вращения в памяти. Потом в цикле луптайма читал гироскоп по трем осям и определял по времени между циклами куда все отклонилось на последнем цикле, формировал по этим данным кватернион цикла. Потом перемножал глобальный кватернион и кватернион цикла и перезаписывал глобальный кватернион произведением. И так далее по циклу…