Search in topic

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

Добрый день!
Пишу программу для квадрокоптера. И уже совсем закопался.
Плата у меня примерно такая: github.com/fademike/Fly_MCU
моторы 1306, 4 шт. пропеллеры 5030, аккум: 2s 1000mah (но пока питаюсь от внешнего питания).
Подвесил проводом по крену, а по тангажу коптер никак не может стабилизироваться.
Когда включаю только P, коптер начинает раскачиваться (период примерно секунда), включаю D он в лучшем случае уменьшает амплитуду и тоже дико качается и среднее значение далеко от горизонтального. отключаю P и он продолжает качаться.

записал видео. P=4.0, I=0.
у видео VID_20200528_121556_conv.mp4 D=0
у видео VID_20200528_121744_conv.mp4 D=2.0 хотя тоже и при 4.0, 8.0.
github.com/fademike/Copter

Такое чувство, что часть с D неадекватно работает. Такое предположение, словно ESC слишком инерционные и меняют значение позже чем надо в данном случае (хотя они вроде шустрые. Прошивку взял тут: github.com/bitdump/BLHeli).

Гироскоп MPU9255 считываю 10-20 раз в секунду, фильтры менял на нем 40-5Гц. так визуально, по-моему гироскоп с акселерометром хорошо работают. в чем же может быть дело?

Часть кода с формулой точно нормальная и не нужно дополнений?

SET_StabKoeffCh1, SET_StabKoeffCh2, SET_StabServoMax2 - переменные, которые регулирую от 0 до 255.
dt - время в секундах после прошлого считывания
Calc_Pitch, Calc_Roll данные от MPU9255

			float P = (float)SET_StabKoeffCh1/10, I=SET_StabKoeffCh2/10, D=(float)SET_StabServoMax2/10;
			//float dt = 0;
			static float errorPitch_was = 0;
			static float errorRoll_was = 0;

			float errorPitch = 0-Calc_Pitch;
			static float integral_Pitch = 0;
			integral_Pitch += errorPitch * I *dt;
			float force_Pitch = P*errorPitch + integral_Pitch + D*((errorPitch_was-errorPitch)/dt);
			force_Pitch=-force_Pitch;

			static float errorRoll = 0-Calc_Roll;
			static float integral_Roll = 0;
			integral_Roll += errorRoll * I *dt;
			float force_Roll = P*errorRoll + integral_Roll + D*((errorRoll_was-errorRoll)/dt);
			force_Roll=-force_Roll;

			errorPitch_was = errorPitch;
			errorRoll_was = errorRoll;

			int motor1 = 1000 + ((SETmainMotorValue*1000)/0xFF) - force_Roll;
			int motor2 = 1000 + ((SETmainMotorValue*1000)/0xFF) - force_Pitch;
			int motor3 = 1000 + ((SETmainMotorValue*1000)/0xFF) + force_Roll;
			int motor4 = 1000 + ((SETmainMotorValue*1000)/0xFF) + force_Pitch;

странно. изменил где тангаж (force_Pitch), дискрет с плюса на минус, и по тангажу вроде заработало с параметрами P=1.0, D=0.1.

Тоже сделал у крена, но не прокатило(
Может и добью я этот коптер чисто своим трудом))

оказывается с коэффициентом 1.0 он какой-то унылый и на нормальных оборотах держится под углом. Увеличил до P=4.0, D=0.5. В итоге отрабатывает плата замечательно, а вот его все равно болтает. Может значение P слишком большое?! Хотя как иначе то…
Инерционно до такой степени уж врятли…

Скорость гироскопа маленькая , у меня 320 раз в секунду.(Arduino nano + гира 6050).
Если использовать только D то раскачки не должно быть совсем. Коптер будет сопротивлятся только резким наклонам.
У меня P=1 D=30 , но все расчёты у меня в градусах , а выход из ПИД значения шим для analogWrite .
Проще писать программу с нуля, либо досканально разбираться в чужой. Код у тебя непонятный и не читаемый.
( опиши все типы переменных заране ).

Вот мой кусок кода с ПИД : ug_xg , ug_yg углы наклона из датчиков по осям в град ,dx ,dy углы наклона с пульта в град , UV_ Управляющее воздействие по каждой оси

oshibka_x =ug_xg -dx-trim_x ; integral_x = constrain( (integral_x+oshibka_x * I_STAB-) ,-1,1 );
oshibka_y =ug_yg -dy-trim_y ; integral_y = constrain( (integral_y+oshibka_y * I_STAB-) ,-1,1 );

UV_X=P_STAB*oshibka_x + integral_x + D_STAB * (oshibka_x -old_oshibka_x); old_oshibka_x =oshibka_x;
UV_Y=P_STAB*oshibka_y + integral_y + D_STAB * (oshibka_y -old_oshibka_y); old_oshibka_y =oshibka_y;

DM1=constrain(-UV_Y +UV_X ,-20,20); DM3=constrain(-UV_Y - UV_X , -20 , 20); // 1 ^ 3 // нумерация двигателей
DM2=constrain( UV_Y +UV_X ,-20,20); DM4=constrain( UV_Y - UV_X , -20 , 20); // 2 4 // Микшер по осям

gaz2 = gaz +d_gaz_h ; // d_gaz_h для поддержания высоты с пид высоты ,
// gaz газ с пульта в единицах шим от 123 до 250

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); // Разница в шим по диагонали

if ( kom1 == 0 || stop_==1) { M1 = 123; M2 = 123; M3 = 123; M4 = 123; }

analogWrite( 10 , M1 ); analogWrite( 9 , M3 );
analogWrite( 11 , M2 ); analogWrite( 3 , M4 );

Добрый день! Спасибо за ответ!

Скорость гироскопа маленькая , у меня 320 раз в секунду.

• у меня 50-100, могу менять. а какая минимальная для нормальной работы? LPF используете?

Код у нас одинаковый, только: у Вас для “X”, а у меня пока для “+” схемы; “ug_xg -dx”, “oshibka_x -old_oshibka_x” у меня наоборот; в место x,y -> pitch, roll, и dt(разница по времени между чтением в секундах) наверное уберу. И “trim_x” я не понял, что это такое((

У меня начались подозрения на инерционность в ESC и на неверные показания MPU9255. Припаял светодиод, который горит при углах наклона >0 и гаснет при <0. В итоге на замедленной съемке увидел что есть небольшая ошибка. Вот как ее бы убрать!?(

Угол наклона же я правильно определяю?

#define RAD_TO_DEG (180/3.14159265)

double gyro_conv = (1000/32767);	// 1000dps установленное значение у гироскопа, 32767 - вся шкала.

double gyro1 = Est_G.x*gyro_conv;	//получаем смещение, dps по оси х
double ang1 = atan2(Est_A.y,Est_A.z)*RAD_TO_DEG;	//получаем направление, куда направлено ускорение, в градусах

double Calc_Pitch = ang1 + (gyro1*dt);	//dt - разница между считываемыми значениями, в секундах.

где:
Est_A - данные с акселерометра x,y,z в int16_t
Est_G - данные с гироскопа x,y,z в int16_t

Как бы ещё проверить правильность работы датчика положения? Как удобнее отлаживать?

Меня ещё удивляет, что обычно нигде не используют MPU9255. Обычно другие датчики. Не понятно почему…

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 гр. И когда он летит равномерно аксель показывает данные как на столе,
только более шумные из за вибраций. И только в моменты ускорений и торможений угол будет ошибочным.

И только в моменты ускорений и торможений угол будет ошибочным.

Просто не понял где в моих словах бред) Да, также алгоритм не будет работать при постоянных полетах по кругу, горизонт уедет в сторону.