Search in topic

Стабилизация квадрокоптера (PID)

1. У гироскопа надо выставить минимум 2000 рад в сек. Я столкнулся с тем что при резких наклонах
   был выход за пределы диапазона +32767 -32767 . У меня всё в общем цикле и не заморачиваюсь на таймеры.
   Данные гироскопа это самые главные данные . Просто подбираю коэффициент умножения.

Угол_наклона=Угол_наклона + дата_гир_х * коэффициент
ReadAcc();
ug_xg = ug_xg + xg * 0.000098;

Начинать надо обязательно просто с вывода нужных данных на монитор . Вывод через определённое цисло циклов .
К примеру наклоняем плату на 90 град и смотрим что насчитала наша программа.

2. Все данные гир и акселя надо калибровать. Смотрим сырые данные с акселя по всем осям . При наклонах 90 и 180 град.
   должна быть симметрия данных по осям , если нет добавляем после считывания поправку.
   Также обязательно находить средние значения всех данных при покое . Например я считываю 10тысяч раз данные с гироскопа и акселя по всем осям
   нахожу среднее и эти значения вычитаю каждый раз при считывании данных. Для акселя коптер надо выставить по уровню по всем осям.
   Так мы найдем настоящий горизонт. Всё занимает 9 сек.

void Aksel_Kalibr() // функция калибровки акселерометра , делаем 10000 измерений и вычисляем среднее , записываем в память
{ P_xa = 0; P_ya = 0; k=0;
ug_xa = 0 ; ug_ya = 0 ; i=11;
for (int y=1; y <= 10000; y++) { ReadAcc(); ug_xa = ug_xa + xa*0.0001 ; // вычисление среднего акселя
ug_ya = ug_ya + ya*0.0001 ; //
k=k+1; if (k==1000) { k=0; i=i-1; G_GPS_S[2]=i; Serial.write(G_GPS_S,5); }
}
P_xa = ug_xa; ug_xa = ug_xa*10; EEPROM.write(9 ,int(ug_xa)); EEPROM.write(10,int(ug_xa)>>8);
P_ya = ug_ya; ug_ya = ug_ya*10; EEPROM.write(11,int(ug_ya)); EEPROM.write(12,int(ug_ya)>>8);
}//================================================================//

void ReadAcc() //_________________________________________________________________________//
{ Wire.beginTransmission(MPU6050_ADDRESS); Wire.write(MPU6050_OUT ); Wire.endTransmission();
Wire.requestFrom (MPU6050_ADDRESS, 14); //| (1 << 7)

xa = Wire.read()<<8|Wire.read();
ya = Wire.read()<<8|Wire.read();
za = Wire.read()<<8|Wire.read();
Tmp = Wire.read()<<8|Wire.read();
yg = Wire.read()<<8|Wire.read();
xg = -(Wire.read()<<8|Wire.read());
zg = Wire.read()<<8|Wire.read();
Wire.endTransmission();
xg = xg - P_xg; xa = xa - P_xa;
yg = yg - P_yg; ya = ya - P_ya;
zg_f = float(zg) - P_zg; za += 100;
} //________________________________________________________________________________________//

Этот процес у меня через меню на пульте. Мне проще , пульт у меня тушка турниги с ардуиной и модемом 3DRadio.
Софт на пульте тоже свой. Есть монитор светодиодный 4х20 символов. 3Dradio это ком порт с двухст. связью.
Могу написать любое меню. Принцип такой : нажимаем кнопку на пульте , шлем команду на вход меню в коптере.
При получении команды обрабатываем любой стик на движение напримр Y , организуем счётчик . На каждое число свое условие на передачу например
текущих P или D на пульт. Крутим переменник на пульте , данные с него преобразуем в нужное значение P , стик X вправо и новое P записано
в память на ПК.
3. Угол наклона по акселю также нахожу через ug_xa = atan2( xa,za )*57.3;
Но родная функция atan2 жутко тормозная , я написал свой atan_moi .

float arctan( float aa,float bb)//++++++++++++++++++++++++++++++++++++++++++++++++++++++//
{ float result,perem;
perem=aa/bb;
if ( abs( perem)<=0.3 ) { result= perem * 55.7 ; } // 16.71
else { if ( perem > 0.3 && perem <= 0.6) { result= 2.46 + perem * 47.5 ; } // 16.71 – 31.1
else if ( perem > 0.6 ) { result= 10.1 + perem * 35 ; } // 31.1 – 45.1
if ( perem <-0.3 && perem >=-0.6) { result= -2.46 + perem * 47.5 ; }
else if ( perem <-0.6 ) { result=-10.1 + perem * 35 ; }
}
return result;
}//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
float atan_moi( float cc,float zz)
{ float ugoll;
if (abs(cc)<abs(zz)) { if ( zz>0 ) ugoll = arctan(cc,zz);
else { ugoll = 180 + arctan(cc,zz); if (ugoll>180) ugoll-=360; }
}
else { if ( cc>0 ) ugoll = 90 - arctan(zz,cc);
else ugoll =-90 - arctan(zz,cc);
}
return ugoll;
}//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//

Работает намного быстрее , точности с избытком.
Данные гироскопа приближаем к углу акселерометра через комплимент. фильтр

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
}
Примерно за 3-4 сек данные наклона по гирам приблизятся к данным акселерометра.
В ручных режимах полёта это не нужно . И далее углы по гирам кидаем на ПИД.

Такой простой алгоритм без калманов и векторов работает только для полётов по осям.
Если например мы летим прямо с наклоном по Y 30 гр. то при вращении по Z гиры не отработают изменение
по оси Y , произойдет смена осей и получится фигня. Тут надо добавить хитрый алгоритм который по гире Z изменял бы
данные по X и Y . Обычная пригонометрия за 7 кл.

4. На регуляторах обязательно должен быть включен Дампинг. Это активное торможение при снижении оборотов.
   Снижает обороты до заданных быстрее раз в 5. На стенде замерил реакцию на изменение тяги в динимике.
   Моторы 2204 2300Кв пропы 5х4.5х3 питание 3S , за 0.1 сек заканчивается процесс набора или снижения тяги.

5. У меня 10DOF на GY-86 стоит не дорого , напаял его сверху ардуины и ПК готов. Там есть точный барометр.
   Если скорость с датчиков маленькая и есть фильтры на эти данные то будет жуткое отставание между движением коптера по осям
   и отслеживание этого положения . У меня нет фильтров на сглаживание по осям. Даже если аксель шумит он вносит изменения
   в положение наклона с коэфиициентом 0.001 . А данные с гироскопа интегрируется с коэффициентом 0.000098 .
   Чем меньше скорость, тем больше будут эти коэфф. , а значить и влияние вибраций на стабилизацию .
   Фильтры у меня только по оси Z , на баромер, и на данные со стиков.
   Если резко поменять стик то будут остилляции .
   dx1=dx1*0.97 + dx*0.03;
   dx это угол с пульта, dx1 это новый угол для ПИД.

Спасибо за советы! Надеюсь они помогут коптеру удержаться на высоте!

Разве гиру надо не кватернионами считать, у mpu есть встроеный DMP, он так делает, правда неточно. Потом по положению в пространстве добавлять ПИДы. Я на своей СУ так делал. m.habr.com/ru/post/255005/
Если просто добавлять углы к осям, то это полеты только по одной оси.)

Кватернионы - тогда это уже следующий шаг после удержания.

Добрый день!
ug_xg = ug_xg + xg * 0.000098; - так получаете угол наклона, что в расчете PID
ug_xg = ug_xg*0.999 + ug_xa*0.001; - добавляете немного угла по акселерометру.
0.000098 - это коэффициент преобразования данных гироскопа в градусы?

Значит, акселерометр только корректирует данные гироскопа?
Почему не так: угол наклона = ug_xa - ug_xg; вместо комп.фильтра?

И гироскоп с акселерометром можно откалибровать только по смещению нуля. Иначе нужен же точный стенд чтоб угол наклона замерять!?

Данные с гироскопа у меня очень зашумленные. Чтоб получить более-менее адекватные, мне пришлось поставить фильтр 5Hz, и ещё делаю усреднение по 4-ём значениям. Считывание поставил в прерывание с периодом 5мс. Планирую купить MPU-6050(что и в GY-86). Может он почище выдает.

В итоге: опрос 200Гц; угол наклона = ug_xa - ug_xg; P=1;D=30: ФНЧ=5Гц, усреднение 4х значений и он реально уже держится и болтается в 1-4х сантиметрах от пола. Но крутится по оси Z и из-за этого не стал параметры никакие корректировать, выше поднимать. Мне нужно в программу добавить поворот по данным гироскопа по оси Z?

Если увеличить P до 2-3 (в идеале до 10), то его уже шатать начинает. Компенсировать нужно же параметром D? Или ни как не компенсировать?

Ну попробуйте мысленный эксперимент. Коптер завис на одном месте. Показания акселерометра по осям 0,0,g. Повернем коптер на 90 градусов по питчу или ролу. Показания акселя как ни странно не изменились 0,0,g. Коптер начинает разгонятся в сторону куда его повернули(двигатели) и одновлеменно падать вниз (гравитация) и только когда сопротивление воздуха уравновесит эти силы аксель покажет правильный угол по горизонту. Теперь понятно почему аксель только коректирует гиру?

Тогда подходит формула: угол наклона = ug_xa - ug_xg.
ug_xa - не изменится при резком скачке, а ug_xg покажет 90 градусный скачек!
Мне бы хотелось бы параметр P увеличить… до 10 в идеале. Реально ли?

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/